//************************************************************************
//SteerCar
//software version 6
//
//by Gustav Jansson
//************************************************************************
// Sensor and Output constants
#define LIGHT SENSOR_1
#define FULLLEFTTOUCH SENSOR_3
#define FULLRIGHTTOUCH SENSOR_2
#define STEER_MOTOR OUT_A
#define DRIVE_MOTOR OUT_C
#define OFFEDGETIMER 0 // Timer(0) used to time since last saw edge
#define SteerRight Fwd(STEER_MOTOR)
#define SteerLeft Rev(STEER_MOTOR)
//************************************************************************
//Time constants
#define MAXOFFEDGE 85
#define SLOWDOWNTIME 20
#define REV_TIME 40
//Power constants
#define MAXPOWER 7
#define SLOWPOWER 4
//************************************************************************
//Globals
int lLeft, lRight;
int lMid;
int lLeftIn, lRightIn;
int lLeftOut, lRightOut;
//Use macros fHighOnLeft and fHighOnRight to check if high values are
//on left side of edge or viceversa.
#define fHighOnLeft (lLeft > lRight)
#define fHighOnRight (lRight > lLeft)
//************************************************************************
void SteerToFullRight()
{
SteerRight;
On(STEER_MOTOR);
until (FULLRIGHTTOUCH == 1);
Off(STEER_MOTOR);
}
void SteerToFullLeft()
{
SteerLeft;
On(STEER_MOTOR);
until (FULLLEFTTOUCH == 1);
Off(STEER_MOTOR);
}
//************************************************************************
//Init()
void Init()
{
SteerToFullLeft();
lLeft = LIGHT;
SteerToFullRight();
lRight = LIGHT;
//Note Steerting still at full right. Need to find edge before
//robot starts to move
//Calculate light threshold values
//These threshold values seem to work really well. When the outer
//threshold values are too far out, the robot has problems when the
//ambiant light varies from one part of the track to another.
lLeftOut = (3*lLeft+lRight)/4; //1/4th of range from lLeft to lRight
lLeftIn = (5*lLeft+3*lRight)/8; //3/8th
lMid = (lLeft+lRight)/2; //1/2
lRightIn = (3*lLeft+5*lRight)/8; //5/7th
lRightOut = (lLeft+3*lRight)/4; //3/4th
CreateDatalog(100);
AddToDatalog(lLeft);
AddToDatalog(lLeftOut);
AddToDatalog(lLeftIn);
AddToDatalog(lMid);
AddToDatalog(lRightIn);
AddToDatalog(lRightOut);
AddToDatalog(lRight);
//Use macros fHighOnLeft and fHighOnRight to check if high values are
//on left side of edge or viceversa.
}
//************************************************************************
//TrackEdgeHR and TrackEdgeHL
//These two routines are the same except HR tracks edge where
//the high light values are on the right and HL tracks edge where
//the high light values are on the left.
#if 1
//The code inside this #if tests the full left and full right buttons.
//The code is essentially the same as the code after the #else except
//that when the steering motor gets all the way to full left or right
//it turns off the steering motor.
task TrackEdge()
{
if (fHighOnRight) {
//High Light values are supposed to be on right side of edge. If
//Light sensor gets high values then it is too far to right and
//needs to turn left.
Off(STEER_MOTOR);
while(true) {
ClearTimer(OFFEDGETIMER); //Everytime we see edge, clear timer
if (LIGHT > lRightIn) {
SteerLeft; On(STEER_MOTOR);
until(LIGHT < lRightOut)
if (FULLLEFTTOUCH) {
SetPower(STEER_MOTOR,2);
until (LIGHT < lRightOut);
Off(STEER_MOTOR);
SetPower(STEER_MOTOR,7);
break;
}
Off(STEER_MOTOR);
} else if (LIGHT < lLeftIn) {
SteerRight; On(STEER_MOTOR);
until(LIGHT > lLeftOut)
if (FULLRIGHTTOUCH) {
SetPower(STEER_MOTOR,2);
until (LIGHT > lLeftOut);
Off(STEER_MOTOR);
SetPower(STEER_MOTOR,7);
break;
}
Off(STEER_MOTOR);Wait(1);
}
}
} else {
//High Light values are supposed to be on left side of edge. If
//Light sensor gets high values then it is too far to left and
//needs to turn right.
Off(STEER_MOTOR);
while(true) {
ClearTimer(OFFEDGETIMER); //Everytime we see edge, clear timer
if (LIGHT < lRightIn) {
SteerLeft; On(STEER_MOTOR);
until(LIGHT > lRightOut)
if (FULLLEFTTOUCH) {
SetPower(STEER_MOTOR,2);
until (LIGHT > lRightOut);
Off(STEER_MOTOR);
SetPower(STEER_MOTOR,7);
break;
}
Off(STEER_MOTOR);
} else if (LIGHT > lLeftIn) {
SteerRight; On(STEER_MOTOR);
until(LIGHT < lLeftOut)
if (FULLRIGHTTOUCH) {
SetPower(STEER_MOTOR,2);
until (LIGHT < lLeftOut);
Off(STEER_MOTOR);
SetPower(STEER_MOTOR,7);
break;
}
Off(STEER_MOTOR);
}
}
}
}
#else
task TrackEdge()
{
if (fHighOnRight) {
//High Light values are supposed to be on right side of edge. If
//Light sensor gets high values then it is too far to right and
//needs to turn left.
Off(STEER_MOTOR);
while(true) {
ClearTimer(OFFEDGETIMER); //Everytime we see edge, clear timer
if (LIGHT > lRightIn) {
SteerLeft; On(STEER_MOTOR);
until(LIGHT < lRightOut);
Off(STEER_MOTOR);
} else if (LIGHT < lLeftIn) {
SteerRight; On(STEER_MOTOR);
until(LIGHT > lLeftOut);
Off(STEER_MOTOR);
}
}
} else {
//High Light values are supposed to be on left side of edge. If
//Light sensor gets high values then it is too far to left and
//needs to turn right.
Off(STEER_MOTOR);
while(true) {
ClearTimer(OFFEDGETIMER); //Everytime we see edge, clear timer
if (LIGHT < lRightIn) {
SteerLeft; On(STEER_MOTOR);
until(LIGHT > lRightOut);
Off(STEER_MOTOR);
} else if (LIGHT > lLeftIn) {
SteerRight; On(STEER_MOTOR);
until(LIGHT < lLeftOut);
Off(STEER_MOTOR);
}
}
}
}
#endif
//************************************************************************
//WatchOffTrackTimer
//This task keeps track of the time that the robot spends away from the
//edge. The first action if too long since last saw edge is to slow down
//the robot. If later is still has not seen edge then it begins a 3-point
//turn.
task WatchOffTrackTimer()
{
while(true){
until(FastTimer(OFFEDGETIMER)>SLOWDOWNTIME);
SetPower(DRIVE_MOTOR, SLOWPOWER);
until(FastTimer(OFFEDGETIMER)>MAXOFFEDGE ||
FastTimer(OFFEDGETIMER)<SLOWDOWNTIME);
SetPower(DRIVE_MOTOR, MAXPOWER);
if (FastTimer(OFFEDGETIMER)<SLOWDOWNTIME) continue;
if(FastTimer(OFFEDGETIMER)>MAXOFFEDGE) {
Off(DRIVE_MOTOR);
ClearTimer(1);
until(FastTimer(1)>20 || FastTimer(OFFEDGETIMER) < MAXOFFEDGE);
//Check again in case TrackEdge found the edge again
if(FastTimer(OFFEDGETIMER)>MAXOFFEDGE) {
//Start 3-point turn
stop TrackEdge;
if (LIGHT > lMid) {
if (fHighOnRight)
SteerToFullRight();
else
SteerToFullLeft();
Rev(DRIVE_MOTOR);
ClearTimer(1);
On(DRIVE_MOTOR);
until(FastTimer(1)>REV_TIME || LIGHT < lMid);
Off(DRIVE_MOTOR);
} else {
if (fHighOnLeft)
SteerToFullRight();
else
SteerToFullLeft();
Rev(DRIVE_MOTOR);
ClearTimer(1);
On(DRIVE_MOTOR);
until(FastTimer(1)>REV_TIME || LIGHT > lMid);
Off(DRIVE_MOTOR);
}
Fwd(DRIVE_MOTOR);
start TrackEdge;
Wait(20);
ClearTimer(OFFEDGETIMER);
}
On(DRIVE_MOTOR);
}
}
}
task main()
{
//Init sensors
SetSensorType (LIGHT, SENSOR_TYPE_LIGHT);
SetSensorMode (LIGHT, SENSOR_MODE_RAW);
SetSensor (FULLLEFTTOUCH, SENSOR_TOUCH);
SetSensor (FULLRIGHTTOUCH, SENSOR_TOUCH);
SetPower(DRIVE_MOTOR, MAXPOWER);
Init();
start TrackEdge;
Wait(20);
start WatchOffTrackTimer;
On(DRIVE_MOTOR);
}
|
|
