Line-Following Code :: 12_20_2012

*Attempted to write line following code with two sensors. 3 Sensors was more reliable.
*Reduced lines of code in the main task and cleaned up functions
*Brainstormed fault detection: What will robot do if no lines are sensed?
*Reduce amount of hard-coding. Push calibration values to top of code
*Added int (debugstate) to allow monitoring of which section of code caused failure in routine
*To do: Write header and program explanation.
*Future: Continue routine. Move to second target

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in1,    midfollower,    sensorLineFollower)
#pragma config(Sensor, in3,    pressureSens,   sensorAnalog)
#pragma config(Sensor, in8,    leftfollower,   sensorLineFollower)
#pragma config(Sensor, in4,    rightfollower,  sensorLineFollower)
#pragma config(Sensor, I2C_1,  leftDrive,      sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_2,  rightDrive,     sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_3,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port1,           bumperStrafe,  tmotorVex269, openLoop, reversed)
#pragma config(Motor,  port2,           leftDrive,     tmotorVex393HighSpeed, openLoop, reversed, encoder, encoderPort, I2C_2, 1000)
#pragma config(Motor,  port3,           leftBottomLift, tmotorVex393, openLoop)
#pragma config(Motor,  port4,           leftTopLift,   tmotorVex393, openLoop, encoder, encoderPort, I2C_3, 1000)
#pragma config(Motor,  port5,           leftFeed,      tmotorVex393, openLoop)
#pragma config(Motor,  port6,           rightFeed,     tmotorVex393, openLoop)
#pragma config(Motor,  port7,           rightTopLift,  tmotorVex393, openLoop)
#pragma config(Motor,  port8,           rightBottomLift, tmotorVex393, openLoop)
#pragma config(Motor,  port9,           rightDrive,    tmotorVex393HighSpeed, openLoop, encoder, encoderPort, I2C_1, 1000)
#pragma config(Motor,  port10,          bootStrafe,    tmotorVex269, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//


const int linelength = 850;
int midthreshold = 1500;
int leftthreshold = 1500;
int rightthreshold = 1500;
void linefollower();
void turnleft();
void turnright();
void goforward();
void gobackward();
bool linedetector();
int debugstate = 0;
int linecrossdetected = 0;
int lastturn = 0;  //Left = 1 Right = 2 Forward = 3

void goforward()
{
motor[leftDrive] = 50;
motor[rightDrive] = 50;
}
void gobackward()
{
motor[leftDrive] = -30;
motor[rightDrive] = -30;
}
void turnleft()
{
motor[leftDrive] = 0;
motor[rightDrive] = 30;
}
void turnright()
{
motor[leftDrive] = 30;
motor[rightDrive] = 0;
}
void strafeleft()
{
motor[bootStrafe]=55;
   motor[bumperStrafe]=-55;
}
void straferight()
{
motor[bootStrafe]=-55;
   motor[bumperStrafe]=55;
}
void stopmotors()
{
motor[leftDrive] = 0;
motor[rightDrive] = 0;
motor[bootStrafe]=0;
  motor[bumperStrafe]=0;
}
void initialsearch()
{
//move to the left until lines are detected
//insert code here for deployment.
while(linedetector()==false)
  {
     strafeleft();
     goforward();
     debugstate = 1;
  }

  //turn  off strafe motor
  stopmotors();
  debugstate = 2;
  wait10Msec(20);
}
void findlinecrossing()
{
while (linecrossdetected == 0)
  {
  linefollower();
  }
  linecrossdetected = 0;
  debugstate = 100;
  stopmotors();
}
void followlinedistance(int length)
{
 nMotorEncoder[rightDrive] = 0;
  nMotorEncoder[leftDrive] = 0;

  while(nMotorEncoder[leftDrive]<length)
  {
      linefollower();
  }
}
// turn 180 degrees
void turnhalfcircle()
{
int initialleft = nMotorEncoder[leftDrive];
int initialright = nMotorEncoder[rightDrive];
while(nMotorEncoder[leftDrive]-initialleft<430&&nMotorEncoder[rightDrive]-initialright>-497)
{
motor[rightDrive] = -50;
  motor[leftDrive] = 50;
}
debugstate = 180;
}

//rotate 90 degrees left
//not working
void rotateleft()
{
int initialleft = nMotorEncoder[leftDrive];
int initialright = nMotorEncoder[rightDrive];

while(nMotorEncoder[leftDrive]-initialleft<215&&nMotorEncoder[rightDrive]-initialright<250)
{
motor[rightDrive] = 50;
  motor[leftDrive] = -50;
}
}

task main()
{
//robot correctly strafes left, finds line, moves forward, finds cross section and moves forward to target
initialsearch();
  findlinecrossing();
  followlinedistance(linelength);
  stopmotors();
  wait10Msec(200);
 // turnhalfcircle();
 // findlinecrossing();//not functioning
 // debugstate = 60;
 // rotateleft();
 // while(linedetector()==false)
 // {
// straferight();
 // }
 //followlinedistance(400);

}
void linefollower()
{  //Check to see if both left and right are on black if so, we are over the line
if(SensorValue[midfollower] < midthreshold)
    {
    //go forward
  debugstate = 10;
  lastturn = 3;
    goforward();
    }
    //check the left sensor if both aren't high
    else if(SensorValue[leftfollower]<leftthreshold)
    {
    lastturn = 1;
    debugstate = 20;
      turnright();
     }
     //if both aren't high and the left isn't low then it must be right
     else if(SensorValue[rightfollower]<rightthreshold)
    {
    lastturn = 2;
    debugstate=30;
    turnleft();
      }
      else
      {
      debugstate=50;
      if (lastturn == 1)  //last turn was left so turn right
      {
      debugstate=51;
      turnright();
      }
      if (lastturn == 2)
      {
      debugstate=52;
      turnleft();
      }
      if (lastturn == 3)
      {
      debugstate=53;
      gobackward();
      }
      }
      //both left and right trigger at the same time at crossing.
     if(SensorValue[leftfollower]<leftthreshold&&SensorValue[rightfollower]<rightthreshold)
     {
      debugstate = 40;
        linecrossdetected = 1;
     }
}
bool linedetector()
{
    if(SensorValue[leftfollower]<leftthreshold||SensorValue[rightfollower]<rightthreshold||SensorValue[midfollower]<midthreshold)
    {
      return true;
    }
    else
      return false;
}