Spitter

From ZENotes
Jump to: navigation, search
/*
* spitter.nxc
* Version 1.1, December 2006
*
* Sivan Toledo
*
* Physical Robot Structure:
* spitting motor A turns the wheels
* throat   motor B opens the throat
* scanning motor C turns the US sensor
* light sensor senses paper pellets, port 1
* sound sensor waits for long quiet periods and complains, port 2
* US sensor searches for something close, turns to him/her
*    to ask for food, on port 4
* see http://www.tau.ac.il/~stoledo/lego/Spitter/ for details
*
* Version 1.0 was compiled successfully with beta 15 of NBC/NXC
* Version 1.1 work in progress for beta 16
*
*/

u8 y_sound_level        = 0;
u8 y_light_level        = 10;
u8 y_us_level           = 20;
u8 y_angle              = 30;
u8 y_time_since_eating  = 40;
u8 y_time_since_loud    = 50;
u8 y_time_since_calling = 60;
u8 x_number             = 72;

u32 quiet_timeout  = 15000;
u32 feedme_timeout = 30000;

u32 base_quiet_timeout  = 15000;
u32 base_feedme_timeout = 30000;

u32 max_quiet_timeout  = 150000;
u32 max_feedme_timeout = 300000;

u8 l;
mutex eating_state_mutex;
u8    eating_state = 0; // not eating
u32   eating_time  = 0; // already hungry!

task eating()
{
 //Follows(main);

 SetSensorType(IN_1, IN_TYPE_LIGHT_ACTIVE);
 SetSensorMode(IN_1, IN_MODE_PCTFULLSCALE);
 ResetSensor(IN_1);

 while (TRUE) {
   l = Sensor(IN_1);

   TextOut(0,        y_light_level, "light level: " );
   TextOut(x_number, y_light_level, "    " );
   NumOut (x_number, y_light_level, l );

   if (l > 40) {
     // the light sensor saw a white paper pellet

     Acquire(eating_state_mutex);
     StopSound(); // stop talking, if we're taking
     eating_state = 1;
     eating_time  = CurrentTick();
     Release(eating_state_mutex);

     PlayFile("yuck.rso");
     while (SoundFlags() != SOUND_FLAGS_IDLE);

     quiet_timeout  = base_quiet_timeout;
     feedme_timeout = base_feedme_timeout;

     // turn motor A on
     OnFwd(OUT_A, 100);
     // wait for them to spin fast
     Wait(2000);
     // open the throat
     OnFwd(OUT_B, 40);
     // wait for the throat to open fully
     Wait(500);
     Off(OUT_B);
     Wait(1000);
     // wait for the pellets to drop and be spitted out
     Wait(2000);
     // close the mouth and stop the wheels
     OnRev(OUT_B, 40);
     Coast(OUT_A);
     Wait(500);
     Off(OUT_B);
     Off(OUT_A);

     Acquire(eating_state_mutex);
     eating_state = 0;
     Release(eating_state_mutex);
   }
 }
}


task listening()
{
 u32 last_loud;
 u8  sound_level;
 SetSensorType(IN_2, IN_TYPE_SOUND_DB);
 SetSensorMode(IN_2, IN_MODE_PCTFULLSCALE);
 ResetSensor(IN_2);

 last_loud = CurrentTick(); // pretend it's loud when we start

 while (TRUE) {
   sound_level = Sensor(IN_2);
   if (sound_level > 20)
     last_loud = CurrentTick();
   if (sound_level > 50)
     quiet_timeout = base_quiet_timeout; // loud, Spitter gets impatient

   TextOut(0,        y_sound_level, "sound level: " );
   TextOut(x_number, y_sound_level, "    " );
   NumOut(x_number,  y_sound_level, sound_level);

   TextOut(0,        y_time_since_loud, "T loud: " );
   TextOut(x_number, y_time_since_loud, "    " );
   NumOut(x_number,  y_time_since_loud, (CurrentTick()-last_loud)/1000);

   if (CurrentTick() - last_loud > quiet_timeout) {
     last_loud = CurrentTick(); // I'm talking!
     PlayFile("itsquiet.rso" );
     quiet_timeout = 2*quiet_timeout;
     if (quiet_timeout > max_quiet_timeout)
       quiet_timeout = max_quiet_timeout;
   }

   Wait(500);
 }
}

task scanning()
{
 u8  usdist;
 u8  mindist;
 s16 scn_deg;
 s16 mindeg;
 u8  hungry = FALSE;
 u32 time_since_calling;
 u32 time_since_eating;
 u32 calling_time = 0;

 while (TRUE) {

   Acquire(eating_state_mutex);
   time_since_eating = CurrentTick() - eating_time;
   Release(eating_state_mutex);

   TextOut(0,        y_time_since_eating, "T feeding: " );
   TextOut(x_number, y_time_since_eating, "    " );
   NumOut(x_number,  y_time_since_eating, time_since_eating/1000);

   TextOut(0,        y_time_since_calling, "T calling: " );
   TextOut(x_number, y_time_since_calling, "    " );
   NumOut(x_number,  y_time_since_calling, (CurrentTick() - calling_time)/1000);

   if (time_since_eating > feedme_timeout &&
       (CurrentTick() - calling_time) > feedme_timeout) {

     calling_time = CurrentTick();

     mindist = 255;
     SetSensorLowspeed(IN_4);
     RotateMotor( OUT_C, 30, -100 );
     Wait(100);
     for (scn_deg=-100; scn_deg < 100; scn_deg += 10) {
       usdist = SensorUS(IN_4);
       if (usdist < mindist) {
         mindist = usdist;
         mindeg  = scn_deg;
       }

       TextOut(0,        y_us_level, "us distance: " );
       TextOut(x_number, y_us_level, "    " );
       NumOut(x_number,  y_us_level, usdist );

       TextOut(0,        y_angle, "angle: " );
       TextOut(x_number, y_angle, "    " );
       NumOut(x_number,  y_angle, scn_deg);

       RotateMotor( OUT_C, 30,  10 );
       Wait(100);
     }
     RotateMotor( OUT_C, 30, -100 );
     Wait(500);

     if (mindist < 255) {
       RotateMotor( OUT_C, 30,  mindeg );
       PlayFile("feedme.rso");
       //while (SoundState() != SOUND_STATE_IDLE) // || SoundFlags() != SOUND_FLAGS_IDLE)
       while (SoundFlags() != SOUND_FLAGS_IDLE);
       PlayFile("feedme.rso");
       while (SoundFlags() != SOUND_FLAGS_IDLE);
       PlayFile("feedme.rso");
       while (SoundFlags() != SOUND_FLAGS_IDLE);
       RotateMotor( OUT_C, 30,  -mindeg );
     }

     feedme_timeout = 2*feedme_timeout;
     if (feedme_timeout > max_feedme_timeout)
       feedme_timeout = max_feedme_timeout;

   }
   Wait(1000);
 }
}

task keepalive()
{
 while (true) {
   ResetSleepTimer();
   Wait(60000);
 }
}

// the main task only starts the other ones

task main()
{
 //Precedes(listening, eating, scanning, keepalive);
 Precedes(listening, eating, scanning, keepalive);
}
Personal tools
Namespaces
Variants
Actions
Navigation
Toolbox