*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;
}
