// tim's mega-sumo bot for brickfest 05

//------------------------------------------------------------
// motors

#define LEFT  OUT_A  // left stepper motor
#define RIGHT OUT_B  // right stepper motor

int bounced=0;                // have we hit a border?
#define BorderReverseTime  50 // back up for xx/100 second
#define BorderTurnTime1   125 // turn    for xx/100 second
#define BorderTurnTime     75 // turn    for xx/100 second

#define BumpTurnTime        1 // turn    for xx/100 second

#define BackUpTime          1 // back up for xx/100 second

#define COAST 15     // when turning off, motors must
                     // coast for xx/100 second

// stepper settings

#define STEPPOWER 8  // power setting
#define PULSE 7      // time on
#define RECOIL 7     // time off
#define STEPMAX 2    // clicks to make motor go forward
#define STEPMIN -2   // clicks to make motor go backward

int L=0;        // the stepper counts of the motors
int R=0;
int LTarg=0;    // target for stepper levels
int RTarg=0;
int leftdir=0;  // sign of stepper pulses
int rightdir=0;

//------------------------------------------------------------
// sensors

// touch sensors

#define LTouch  SENSOR_1
#define RTouch  SENSOR_2
#define Touching 90 // touch sensor reads 100 when
                    // read as a light sensor

// light sensors

#define LLight  SENSOR_1 // piggyback light and touch
#define RLight  SENSOR_2
#define LIGHTMARGIN 7
int line=0;
int threshold=0;

// rotation sensor

#define Roller  SENSOR_3
#define RollerTimeout 50  // check rolling sensor every xx/10 seconds
#define BLINK   OUT_C   // sound the alarm!
int roll=0;
int rollwindowmin=0;
int rollwindowmax=0;
int backupnow=0;        // flag: we're stuck!

//------------------------------------------------------------
// program controls

#define MotorWatch   0   // task priority settings
#define SensorWatch  1
#define RollerWatch  2

int MotorUpdate=0; // flag: when set to 1, UpdateMotors is triggered
int dopulse=0;

//------------------------------------------------------------

task main()
{

  // initialize sensors
  SetSensor(LTouch,SENSOR_LIGHT);
  SetSensor(RTouch,SENSOR_LIGHT);
  SetSensor(Roller, SENSOR_ROTATION);

  SelectDisplay(DISPLAY_SENSOR_1); // display LLight reading at startup

  // calibrate left light sensor with right touch
  Calibrate();

  // another right touch says "go!"
  until(RTouch>Touching);     // wait for right touch
  until(RTouch<Touching);     // wait for right release
  PlaySound(SOUND_CLICK);     // acknowledge: "let's go!"

  // display rolling counter during run
  SelectDisplay(DISPLAY_SENSOR_3); 

  // set up motors
  SetPower(LEFT+RIGHT,STEPPOWER);   
  L = 0; R = 0;
  start WatchUpdate; // updates motors with setting changes

  // the required five-second delay
  Wait(500);

  // let's go!
  GoForward();

  // start rolling sensor task
  start SensorPoll;  // watch for light, touch sensor events
  ClearTimer(0);     // clear roller polling timer
  roll = 0;
  rollwindowmin = 0;
  rollwindowmax = 0;
  backupnow = 0;
  ClearSensor(Roller);
  start CheckRoller; // watch to see if we're actually moving

  // first bounce is ~180 turn, less after
  bounced = 0;

} // end main()

//------------------------------------------------------------
// set the light/dark sensor readings for line avoidance

void Calibrate()
{
  // this is the "read the dark" button
  until(RTouch>Touching);      // wait for right touch

  line=LLight;                 // read left light
  threshold=line+LIGHTMARGIN;  // set light sensor threshold

  until(RTouch<Touching);      // wait for right release
  PlaySound(SOUND_CLICK);      // acknowledge: "I got it"
}

//------------------------------------------------------------
// sensor behaviors

// watch for sensor events
task SensorPoll()
{
  SetPriority(SensorWatch);
  while(true)
    {
      // watch for sensor thresholds
      until((LLight<=threshold) ||
            (RLight<=threshold) ||
            (backupnow==1) ||
            (LTouch>Touching) ||
            (RTouch>Touching));
      acquire(ACQUIRE_USER_1)
        {
          if (LLight<=threshold) {
            LAvoid();  // avoid the line on the left
          } else {
            if (RLight<=threshold) {
              RAvoid();  // avoid the line on the right
            } else {
              if (backupnow==1) {
                BackUp();  // roller says we're stuck, back up and turn a bit, once
                backupnow = 0;
              } else {
                if ((LTouch>Touching) && (RTouch<Touching)) {
                  LTurn();  // turn toward opponent on the left
                } else {
                  if ((RTouch>Touching) && (LTouch<Touching)) {
                    RTurn(); // turn toward opponent on the right
                  } else {
                    if ((RTouch>Touching) && (LTouch>Touching)) {
                      GoForward(); // keep going forward
                    }
                  }
                }
              }
            }
          }
        }
    }
}

// avoid the black border on the left
void LAvoid()
{
  GoBackward();
  Wait(BorderReverseTime);

  RotateRight();
  if (bounced == 1) {
    Wait(BorderTurnTime);
  } else {
    Wait(BorderTurnTime1); // for first bounce, rotate ~180
    bounced = 1;
  }

  GoForward();
}

// avoid the black border on the right
void RAvoid()
{
  GoBackward(); 
  Wait(BorderReverseTime);

  RotateLeft();
  if (bounced == 1) {
    Wait(BorderTurnTime);
  } else {
    Wait(BorderTurnTime1); // for first bounce, rotate ~180
    bounced = 1;
  }

  GoForward();
}

// back up and turn a bit (for when we're stuck)
void BackUp()
{
  GoBackward();  Wait(BackUpTime);
  SetMotors(STEPMIN, 0); // left wheel back, right wheel off
  GoForward();
}

// turn toward opponent on the left
void LTurn()
{
  RotateLeft();  Wait(BumpTurnTime);
  GoForward();
}

// turn toward opponent on the right
void RTurn()
{
  RotateRight(); Wait(BumpTurnTime);
  GoForward();
}

//------------------------------------------------------------
// motor subroutines

void SetMotors (int left, int right) 
{
  LTarg=left;
  RTarg=right;
  MotorUpdate=1;
  while(MotorUpdate==1);  // return after update is done
}

void GoForward()
{
  SetMotors(STEPMAX, STEPMAX);
}

void RotateLeft()
{
  SetMotors(STEPMIN, STEPMAX);
}

void RotateRight()
{
  SetMotors(STEPMAX, STEPMIN);
}

void GoBackward()
{
  SetMotors(STEPMIN, STEPMIN);
}

//------------------------------------------------------------
// motor support

task WatchUpdate()
{
  SetPriority(MotorWatch);
  while(true)
    {
      // watch for the motor update flag
      until(MotorUpdate == 1) {
        UpdateMotors();
      }
      MotorUpdate = 0;
    }
}

void UpdateMotors()
{
  while ((L != LTarg) || (R != RTarg)) {

    dopulse=0;
    leftdir=0;
    rightdir=0;

    // make L count match target L count,
    // keeping track of which way to pulse
    if (L < LTarg) {
      L++;
      if (L <= STEPMAX) {
        dopulse=1;
        leftdir=1;
      } else {
        L = STEPMAX;
      }
    } else {
      if (L > LTarg) {
        L--;
        if (L >= STEPMIN) {
          dopulse=1;
          leftdir=-1;
        } else {
          L = STEPMIN;
        }
      }
    }

    // make R count match target R count,
    // keeping track of which way to pulse
    if (R < RTarg) {
      R++;
      if (R <= STEPMAX) {
        dopulse=1;
        rightdir=1;
      } else {
        R = STEPMAX;
      }
    } else {
      if (R > RTarg) {
        R--;
        if (R >= STEPMIN) {
          dopulse=1;
          rightdir=-1;
        } else {
          R = STEPMIN;
        }
      }
    }

    if (dopulse == 1) {
      // polarity switches are reversed from RCX output
      if (leftdir  ==  1) { OnRev(LEFT);  }
      if (leftdir  == -1) { OnFwd(LEFT);  }
      if (rightdir ==  1) { OnRev(RIGHT); }
      if (rightdir == -1) { OnFwd(RIGHT); }
      Wait(PULSE);
      Off(LEFT+RIGHT);
      Wait(RECOIL);
    }

    if ((L == 0) || (R == 0)) {
      PlaySound(SOUND_CLICK); // feedback for coast
      Wait(COAST);
    }
    
  } // while L != Ltarg or R != RTarg

} // void UpdateMotors

//------------------------------------------------------------
// watch the rolling sensor

task CheckRoller()
{
  SetPriority(RollerWatch);
  while(true) {
     // wait until the rolling timer goes off
     until(Timer(0) >= RollerTimeout);
     roll=Roller;
     if ((roll > rollwindowmin) && (roll < rollwindowmax)) {
       backupnow=1;
       SetPower(BLINK,8);
       OnFwd(BLINK); // turn on gratuitous blinker
     } else {
       backupnow=0;
       Off(BLINK);
     }

     rollwindowmin = roll-3;
     rollwindowmax = roll+3;
     
     ClearTimer(0);
     ClearSensor(Roller);
  }
}

// Local Variables:
// mode: c
// End: