Robots:DeskBotSource

From Cucumis

/*! \file   DeskBotV2.c
    \brief  wanders around my desk, keeping away from the edges.
    \author Cucumber <Cucumber@dargos.com>

    wanders around my desk, keeping away from the edges.
*/

#include <conio.h>
#include <unistd.h>
#include <stdlib.h>
#include <dsensor.h>
#include <dmotor.h>
#include <dsound.h>
#include <dkey.h>
#include <tm.h>

#define LIGHTSENS       SENSOR_2
#define DARK_THRESH     0x40
#define BRIGHT_THRESH   0x48

#define NORMAL_SPEED (2*MAX_SPEED/3)
#define TURN_SPEED   (MAX_SPEED)

int dir=1;
int clift=55;
int scanpause=0;
int aFwd = fwd;
int aRev = rev;
int cFwd = rev;
int cRev = fwd;

int fwdSpeed = MAX_SPEED;
int revSpeed = MAX_SPEED;


void straight_ahead( void )
{
   ds_active(&LIGHTSENS);
   motor_a_speed(fwdSpeed);
   motor_c_speed(fwdSpeed);
   motor_a_dir(aFwd);
   motor_c_dir(cFwd);
   cputs("fwwd ");
}


void backup( void )
{
   motor_a_speed(revSpeed);
   motor_c_speed(revSpeed);
   motor_a_dir(aRev);
   motor_c_dir(cRev);
   cputs("rev  ");
   msleep(700);
   motor_a_speed(fwdSpeed);
   motor_c_speed(fwdSpeed);
}


void turnLR( void )
{
   int ndir;

   // turn left or right?
   ndir = (random() % 100);

   // change directions?
   if( ndir < 10 )
   {
      ndir = (random() % 100);
      if( ndir < 50 ) dir=1; else dir=2;
   }

   // make the turn
   if(dir == 1)
   {
      motor_a_dir(aRev);
      motor_c_dir(cFwd);
      cputs("left ");
   }
   else
   {
      motor_a_dir(aFwd);
      motor_c_dir(cRev);
      cputs("right");
   }

   msleep(700);
}


void clift_recover( void )
{
   scanpause=1;
   backup();
   turnLR();
   scanpause=0;
   straight_ahead();
}

void config( int kp )
{
    if( kp & KEY_VIEW )
    {
      clift -= 5;
        lcd_int(clift);
        msleep(1500);
        cputs("     ");
    }
    if( kp & KEY_PRGM )
    {
      clift += 5;
        lcd_int(clift);
        msleep(1500);
        cputs("     ");
    }

   return;
}


int scanner()
{
   int sdir=0;
   int f1=0;
   int f3=0;
   long last=sys_time;

   motor_b_dir(fwd);

   for(;;)
   {
      if( scanpause != 1 )
      {
         motor_b_speed(MAX_SPEED/17);

         if( TOUCH_1 || f1 )
         {
            sdir=1; f1=0;
            last = sys_time;
            motor_b_dir(rev);
         }
         if( TOUCH_3 || f3 )
         {
            sdir=0; f3=0;
            last = sys_time;
            motor_b_dir(fwd);
         }
         if(  sys_time > (500 + last) )
         {
            dsound_system(DSOUND_BEEP);
            if( sdir ) f3=1; else f1=1;
         }
      }
      else
      {
         motor_b_speed(0);
      }
   }

   return(0);
}

int master()
{
   int kp=0;
   int edge=0;
   int doze=0;
   long clock=0;

   srandom( sys_time );
   doze = (((random() % 20) + 10)*1000);
   clock = sys_time + doze;

   // start moving
   straight_ahead();

   while(1)
   {
      edge = LIGHT(LIGHTSENS);
      if( edge <= clift ) clift_recover();
      if( (kp=dkey_pressed(KEY_ANY)) ) config(kp);
      if( sys_time > clock )
      {
         scanpause=1;
         ds_passive(&LIGHTSENS);
         motor_a_speed(off);
         motor_c_speed(off);
         for( doze=0; doze<45; doze++ )
         {
            cputs("sleep");
            sleep(1);
            if( (kp=dkey_pressed(KEY_ANY)) ) config(kp);
         }
         doze = (((random() % 20) + 10)*1000);
         clock = sys_time + doze;
         scanpause=0;
         straight_ahead();
      }
  }

  return 0;
}


int main(int argc, char *argv[])
{
   /* Spawn driver task */
   execi( &master, 0, NULL, 1, DEFAULT_STACK_SIZE );
   execi( &scanner, 0, NULL, 1, DEFAULT_STACK_SIZE );

   return 0;
}