//Anti-SLAM v16_final
//Convention: Left and Right refer to the SENSOR/MOTOR, NOT THE WIRING.
//See the wiring diagrams in Chapter 9.

#define LeftMotor OUT_B
#define RightMotor OUT_C
#define LeftEar S2
#define RightEar S3
#define LeftEye S1
#define RightEye S4

/*====="Level -1": Integration==================================================
Each of the terms (Sensitivity, Reverse, LeftBias, RightBias) is part of a later
level's connection to the motors. See the main task to see their defaults.
On its own, this task does nothing. However, it will function at every level
without modification.
*/

int Sensitivity,Reverse, LeftSpeed,RightSpeed, LeftBias,RightBias, LVis,RVis;
int Hearing;

task Drive(){
  while(true){
    //"Hearing/255" converts from responsive raw ultrasonic to % motor speed.
    RightSpeed = ((SensorUS(RightEar)*(Hearing-LeftBias)/255)+RVis)
                 * Sensitivity+Reverse;
    LeftSpeed = ((SensorUS(LeftEar)*(Hearing-RightBias)/255)+LVis)
                * Sensitivity-Reverse;
  }
}

/*=====Level 0: Drive===========================================================
Feed the distance from each ultrasonic sensor to the motor. The robot is wired
contralaterally, and thus avoids all walls equally. As a result, when it reaches
a corner, it slows down and ends up stopping in the corner "for free".
*/

task DriveRight(){
  while(true){
    OnFwd(RightMotor, RightSpeed);
  }
}

task DriveLeft(){
  while(true){
    OnFwd(LeftMotor, LeftSpeed);
  }
}

/*=====Level 1: Escape==========================================================
If the motors move less than a stated threshold over a delay period, the robot's
sensors are temporarily overriden (zero sensitivity) as it spins around. It ends
up pointing approx. 45 degrees from the corner when normal operation resumes.
*/

int Threshold, Delay;

void Spin(){
    ResetRotationCount(LeftMotor); ResetRotationCount(RightMotor);
    Sensitivity = 0; Reverse = 35; //Disable sensors, enable spin term
    Wait(4000);   //Time to spin in milliseconds
    Reverse = 0; Sensitivity = 1; //Return to default settings
    ResetRotationCount(LeftMotor); ResetRotationCount(RightMotor);
}

task Retreat(){
  long RotCount;//Tracks motor rotation.
  while(true){
    RotCount = MotorRotationCount(LeftMotor) + MotorRotationCount(RightMotor);
    Wait(Delay);
    if(((MotorRotationCount(LeftMotor)+MotorRotationCount(RightMotor)-RotCount)
        < Threshold) && Reverse==0){//If the motors slow down while not spinning
      PlayTone(440, 500); //Beep to indicate spinning and data point.
      Spin();
      Wait(500);
    }
  }
}

/*=====Level 2: Follow==========================================================
Introduce a bias to the robot's motors, causing it to "prefer" one motor (a sort
of "handedness"). The difference in motor power induces a turn, pushing the
robot closer to one wall. Level 0 will straighten it out after it gets close
enough, resulting in a robot that follows walls on one side.
*/

int Nearest(bool hand){ /*This returns the value of the sensor nearest the wall.
Note: "nearest" is defined by the robot's "preferred" side*/
  if (hand) {return SensorUS(RightEar);}
  else {return SensorUS(LeftEar);}
}

bool preferred; //Determines which side the robot prefers. True = right turns.
int bias; //The strength of the bias term. See the main task.

task Seek(){
//Set the bias to the appropriate side.
  while(true){
    if (preferred) {RightBias = bias; LeftBias = 0;}
    else {LeftBias = bias; RightBias = 0;}
  }
}

/*=====Level 3: Feature=========================================================
Enables and reads the light sensors (eyes) as a percentage based on "Vision"
(a sensitivity term), such that more light = more speed. Since the connection is
contralateral, this results in the robot turning toward sources of light.
However, level -1 weighs this visual sense with the earlier ultrasonic sense,
allowing both terms to influence the robot's final behaviour.
*/

int Vision; //The strength of the light sensors in percent.

task See(){
//Sets the strength of the robot's visual response to a scaled percentage.
  while(true){
    LVis = Sensor(LeftEye)*Vision/100;
    RVis = Sensor(RightEye)*Vision/100;
  }
}

//=====Main Task================================================================

task main(){
  //Set up ultrasonic sensors and speed calculation weights.
  SetSensorLowspeed(LeftEar);
  SetSensorLowspeed(RightEar);
  SetSensorMode(LeftEar, SENSOR_MODE_RAW);
  SetSensorMode(RightEar, SENSOR_MODE_RAW);
  Sensitivity = 1; //Level 0 connection: Ultrasonic sensitivity. Default 1.
  Reverse = 0; //Level 1 connection: Lets robot spin and escape. Default 0.
  LeftBias = 0; //Level 2 connection: Causes robot to prefer left turns. Def. 0.
  RightBias = 0;//Level 2 connection: As above, but prefers right turns. Def. 0.
  LVis = 0; //Impact of the left eye on movement. Zero at this level.
  RVis = 0; //Impact of the right eye on movement. Zero at this level.
  Hearing = 100; //Strength of ultrasonic sense. (Overridden at level 3.)
  start Drive; //Starts mapping the motor speeds to the collective input.
  //Level 0.
  start DriveRight; //Turn the right motor on.
  start DriveLeft;  //Turn the left motor on.
  //Level 1. (Delete below this line for a level 0 robot.)
  Threshold = 360; //Combined motor movement to be considered 'on'. Default 360.
  Delay = 5000; //How long the robot needs to have been stopped. Default 5000ms.
  start Retreat; //Allow the robot to escape corners.
  //Level 2. (Delete below this line for a level 1 robot.)
  preferred = true; //True for left-handed (right-following), false otherwise.
  bias = 40; //Fixed value for handedness bias. Default 40.
  start Seek; //Follow the wall on your preferred side.
  //Level 3. (Delete below this line for a level 2 robot.)
  //Set up eyes.
  SetSensorType(LeftEye, SENSOR_TYPE_LIGHT_INACTIVE);
  SetSensorMode(LeftEye, SENSOR_MODE_PERCENT);
  SetSensorType(RightEye, SENSOR_TYPE_LIGHT_INACTIVE);
  SetSensorMode(RightEye, SENSOR_MODE_PERCENT);
  Hearing = 40;// % of ultrasonic sense that feeds to the motors. Default 40.
  Vision = 60;// % of light sense that feeds to the motors. Default 60.
  start See;
}