// =======================================================================
// Function to drive the robot towards the IR beacon
// =======================================================================
void IR_TimeMove(int power, long maxtime)
{
step++; // increase step number for data log file
int limitReached = 0;
long current_power;
long starttime = nPgmTime;
ResetDriveEncoders();
motor[LF_motor] = power;
motor[RF_motor] = power;
motor[LR_motor] = power;
motor[RR_motor] = power;
current_power = power;
bool Done = false;
while(!Done)
{
if (nPgmTime> starttime + (maxtime* 1000))
{
Done = true; // the timer has run out, so stop the move
}
if(limitReached > 4)
{
Done = true;
}
float adj_power = (IR_Bearing-5.0)*IR_PROPORTION;
motor[LF_motor] = (current_power - (int)adj_power);
motor[RF_motor] = (current_power + (int)adj_power);
}
StopRobot();
}
//===================================================
// Function to turn the robot with gyro V2
//===================================================
void Gyro_TurnV2 (float degrees, int power, bool ConstOrRel)
{
relHeading=0;
if (degrees<0)
{
motor[LF_motor]=power; // start turning left
motor[LR_motor]=power;
motor[RF_motor]=power; // start turning left
motor[RR_motor]=power;
}
else
{
PlaySound(soundBeepBeep);
motor[LF_motor]=-power; // start turning right
motor[LR_motor]=-power;
motor[RF_motor]=-power; // start turning right
motor[RR_motor]=-power;
}
if(gyro_mux_status==false) // the GYRO mux seems to be active
{
if(ConstOrRel) // decide whether to turn relative to start position
// or relative to current robot orientation
{
while(abs((int)degrees) > abs((int)constHeading)){} // relative to field
}
else
{
while(abs((int)degrees) > abs((int)relHeading)){} // relative to robot start posn
}
}
else // we don't have a good gyro reading
{
}
StopRobot(); // once done we shutdown all drive motors
}
// =======================================================================
// Function to move the robot by the gyro by time sideways V2
// =======================================================================
void GyroTimeS_moveV2(int time, int power, bool light,bool StopWhenDone, bool adjust, bool ConstOrRel)
{
long adj_power;
long adj_deg;
long current_power;
int i = 0;
relHeading =0;
wait1Msec(200);
motor[LF_motor] = -power;
motor[RF_motor] = -power;
motor[LR_motor] = power;
motor[RR_motor] = power;
current_power = power;
ClearTimer(T1);
bool Done = false;
int addPower = 0;
while(!Done)
{
RequestedScreen=S_LIGHT; // force the light value screen to be displayed
if(light == true)
{
if(light_normalised > light_threshold) i++;
if(i > 5) Done = true; // was 10, but that may be too high, now 5 at 21 Nov 2012
}
if(time1[T1] > time)
{
Done = true;
}
if(adjust == true)
{
if(ConstOrRel) adj_deg = (long) constHeading;
else adj_deg = (long) relHeading;
adj_power = adj_deg*GYRO_PROPORTION;
addPower = (current_power*5)/100;
if(power > 0)
{
motor[LF_motor] = -(current_power-addPower); //more
motor[RF_motor] = -((current_power+addPower) - adj_power);
motor[LR_motor] = (current_power-addPower); //more
motor[RR_motor] = ((current_power+addPower) + adj_power);
}
else
{
motor[LF_motor] = -(current_power+addPower);
motor[RF_motor] = -((current_power-addPower) - adj_power);
motor[LR_motor] = (current_power+addPower);
motor[RR_motor] = ((current_power-addPower) + adj_power);
}
}
}
if(StopWhenDone==true)
{
StopRobot();
}
}
// =======================================================================
// Function to move the robot by the gyro by time V2
// =======================================================================
void GyroTime45_V2(int time, int power,bool StopWhenDone, bool adjust, bool ConstOrRel, bool LorR)
{
step++; // increase step number for data log file
long adj_power;
long adj_deg;
long current_power;
relHeading =0;
wait1Msec(20);
if(LorR)
{
motor[LF_motor] = 0;
motor[RF_motor] = power;
motor[LR_motor] = -power;
motor[RR_motor] = 0;
}
else
{
motor[LF_motor] = -power;
motor[RF_motor] = 0;
motor[LR_motor] = 0;
motor[RR_motor] = power;
}
current_power = power;
ClearTimer(T1);
bool Done = false;
while(!Done)
{
if(time1[T1] > time)
{
Done = true;
}
if(adjust == true)
{
if(ConstOrRel) adj_deg = (long) constHeading;
else adj_deg = (long) relHeading;
adj_power = adj_deg*GYRO_PROPORTION;
if(LorR)
{
motor[LF_motor] = 0;
motor[RF_motor] = (current_power + adj_power);
motor[LR_motor] = (-current_power - adj_power);
motor[RR_motor] = 0;
}
else
{
motor[LF_motor] = (-current_power - adj_power);
motor[RF_motor] = 0;
motor[LR_motor] = 0;
motor[RR_motor] = (current_power + adj_power);
}
}
}
if(StopWhenDone==true)
{
StopRobot();
}
}
// =======================================================================
// Function to move the robot by the gyro and the sonar sensors V2
// =======================================================================
void GyroSonar_moveV2(int time, int WhichSensors, int distanceX, int distanceY, int power,bool StopWhenDone, bool adjust, bool ConstOrRel)
{
step++; // increase step number for data log file
long adj_power; // Reset all the values
long adj_deg; //
int count = 0; //
int sonar_adj = 0; //-----------------
int lastvalue=9999;
long current_power;
relHeading =0; //-----------------
wait1Msec(20);
motor[LF_motor] = -power; //---------------
motor[RF_motor] = power; // Set beginning speeds
motor[LR_motor] = -power; // for the motors
motor[RR_motor] = power; //---------------
current_power = power;
ClearTimer(T1);
bool Done = false;
while(!Done)
{
if(WhichSensors != 2) //----------------
{ //
if(sonarLive2 > distanceY) // we have exceeded our target distance
{
if (sonarLive2!=lastvalue) // we have a new reading above the target distance
{
count++; // so increase our count of values above distance
lastvalue=sonarLive2; // and save the current reading for next time around
}
}
else count=0; // the measured distance is below the target distance
if(count > 2) Done = true; // once we get two readings above target we exit the loop
} //
else if(time1[T1] > time) Done = true; //----------------
if(adjust == true)
{
if(WhichSensors != 1) sonar_adj = ((distanceX - sonarLive)*SONAR_PROPORTION);
if(ConstOrRel) adj_deg = (long) constHeading;
else adj_deg = (long) relHeading;
adj_power = adj_deg*GYRO_PROPORTION;
/*
motor[LF_motor] = -(current_power + adj_power + sonar_adj);
motor[RF_motor] = current_power + adj_power - sonar_adj; // was +
motor[LR_motor] = -(current_power + adj_power - sonar_adj);
motor[RR_motor] = current_power + adj_power + sonar_adj;
*/
motor[LF_motor] = -(current_power - (adj_power + sonar_adj));
motor[RF_motor] = (current_power + (adj_power + sonar_adj));
motor[LR_motor] = -(current_power);
motor[RR_motor] = (current_power);
}
}
if(StopWhenDone==true)
{
StopRobot();
}
}
void SpinAlgorithm(void) {
vector V(0,0);
double omega=0.20, h=0; // body angular velocity in rad/sec
do {
V.x = 0.07*cos(h); // body x velocity vector in meters
V.y = 0.07*sin(h); // body y velocity vector in meters
h += omega; // turn velocity vector by omega
Vector_Drive(V, omega); // drive robot using direction vector and angular velocity
} while (SmartWait(90) == 0); // loop will be interrupted by penDown event
StopRobot();
}
void PenFollowAlgorithm(void) {
unsigned long start_time;
int pen_dist=30, dist, detected = False, lost = False, infi = 60, played = False;
// put pen facing wheel #1
// turn robot to the right until it finds the center of the pen
Turn(Right, Slow);
DisplayTxt("Scanning...");
do {
dist = Sensor_To_Range(Sensor(3));
if (dist <= pen_dist)
detected = True;
} while ( (detected == False) && (SmartWait(1)==0) );
StopRobot(); // stop robot
// begin tracking loop
do {
// ADJUST ANGULAR POSITION
dist = Sensor_To_Range(Sensor(3));
if (dist > infi) { // target lost turn left/right
played = False;
Turn(Right,Slow); Wait(50); StopRobot(); // turn right for awhile
dist = Sensor_To_Range(Sensor(3));
if (dist > infi) { // target is not to the right,
Turn(Left,Slow);// turn left
start_time = 0;
do {
dist = Sensor_To_Range(Sensor(3));
} while ((start_time++ < 1000) && (dist > infi) && (SmartWait(1)==0) ); // turn until found
dist = Sensor_To_Range(Sensor(3));
if (dist > infi) {
lost = True;
break; }
StopRobot(); } }
dist = Sensor_To_Range(Sensor(3));
// ADJUST LINEAR POSITION
if ( (dist < infi) && (dist < (pen_dist-5)) ) { // too close to target
played = False;
Move(RToServo3);
do {
dist = Sensor_To_Range(Sensor(3));
if (dist > infi) // lost angular tracking
break;
} while (dist < pen_dist); // get dist to pen_dist
StopRobot(); }
if ( (dist < infi) && (dist > (pen_dist+5)) ) { // too far
played = False;
Move(ToServo3);
do {
dist = Sensor_To_Range(Sensor(3));
if (dist > infi) // lost angular tracking
break;
} while (dist > pen_dist); // get dist to pen_dist
StopRobot(); }
if ( (dist < (pen_dist+5)) && (dist > (pen_dist-5)) && (played == False) ) {
printf("Yep\n");
played = True; }
} while ( (lost == False) && (SmartWait(10)==0) );
StopRobot();
}
main(int argc, char *argv[])
{
char *opt;
int i, n;
init_serial("/dev/ttyS0");
init_lircd("/var/tmp/lircd");
if (argc == 2 && strcmp(argv[1], "test") == 0) {
simple_test();
exit(0);
}
StopRobot(); /* turn it off to start with */
n = 0;
do {
opt = get_irkey();
if (opt) {
syslog(LOG_INFO, "PalmBot - %s\n", opt);
for (i = 0; command[i].key_name; i++)
if (strcmp(command[i].key_name, opt) == 0) {
n = i;
break;
}
}
if (command[n].key_name) {
syslog(LOG_INFO, "Run command '%s'\n", command[n].key_name);
(*command[n].func)();
}
} while (command[n].key_name);
StopRobot(); /* turn it off to end with */
exit(0);
}
//===================================================
// Function to turn the robot with gyro V2
//===================================================
bool Gyro_TurnV2 (float degrees, int power, bool ConstOrRel)
{
bool sawLine = false;
int i = 0;
step++; // increase step number for data log file
relHeading=0;
if (degrees<0)
{
motor[LF_motor]=power; // start turning left
motor[LR_motor]=power;
motor[RF_motor]=power; // start turning left
motor[RR_motor]=power;
}
else
{
PlaySound(soundBeepBeep);
motor[LF_motor]=-power; // start turning right
motor[LR_motor]=-power;
motor[RF_motor]=-power; // start turning right
motor[RR_motor]=-power;
}
if(gyro_mux_status==false) // the GYRO mux seems to be active
{
if(ConstOrRel) // decide whether to turn relative to start position
// or relative to current robot orientation
{
while(abs((int)degrees) > abs((int)constHeading))
{
if(light_normalised > light_threshold) i++;
if(i > 5) sawLine = true; // was 10, but that may be too high, now 5 at 21 Nov 2012
} // relative to field
}
else
{
while(abs((int)degrees) > abs((int)relHeading))
{
if(light_normalised > light_threshold) i++;
if(i > 5) sawLine = true; // was 10, but that may be too high, now 5 at 21 Nov 2012
} // relative to robot start posn
}
}
else // we don't have a good gyro reading
{
}
StopRobot(); // once done we shutdown all drive motors
return sawLine;
}
//========================================================================
// Gyro turn with only 3 motors, for now
//========================================================================
void Gyro3MT_V2 (float degrees, int power, bool ConstOrRel, bool LorR)
{
step++; // increase step number for data log file
relHeading=0;
if (degrees<0)
{
if(LorR == true)
{
motor[LF_motor]=power/2;
motor[RF_motor]=-1;
motor[RR_motor]=power/2;
motor[LR_motor]=power;
}
else
{
motor[LF_motor]=-1;
motor[RF_motor]=power/2;
motor[RR_motor]=power;
motor[LR_motor]=power/2;
}
}
else
{
PlaySound(soundBeepBeep);
if(LorR == true)
{
motor[LF_motor]=power/2;
motor[RF_motor]=-1;
motor[RR_motor]=power/2;
motor[LR_motor]=power;
}
else
{
motor[LF_motor]=-1;
motor[RF_motor]=power/2;
motor[RR_motor]=power;
motor[LR_motor]=power/2;
}
}
if(gyro_mux_status==false) // the GYRO mux seems to be active
{
if(ConstOrRel) while(abs((int)degrees) > abs((int)constHeading)){}
else while(abs((int)degrees) > abs((int)relHeading)){}
}
StopRobot(); // shutdown all drive motors
}
void
timed_turn(int dir, int speed)
{
fd_set rfds;
struct timeval tv;
Turn(dir, speed);
tv.tv_sec = 3;
tv.tv_usec = 0;
FD_ZERO(&rfds);
FD_SET(ir_fd, &rfds);
if (select(ir_fd + 1, &rfds, NULL, NULL, &tv) == 1)
return;
StopRobot();
}
void StopRun(void) {
if (bRecordFlag)
MoveRobot(XSPEED, 50, 0, 150, 0, ACCELERATION);
else if (bExplorationFlag) {
MoveRobot(XSPEED, 500, 0, EXPLORATION_SPEED, 0, ACCELERATION);
bExplorationFlag = FALSE;
} else if (bFastFlag) {
MoveRobot(XSPEED, 550, 0, 0, 0, ACCELERATION);
bFastFlag = FALSE;
} else {
MoveRobot(XSPEED, 100, 0, EXPLORATION_SPEED, 0, ACCELERATION);
// MoveRobot(XSPEED, 2500, 0, EXPLORATION_SPEED, 0, ACCELERATION);
}
StopRobot();
DisableMotor();
bRunFlag = FALSE;
bStopFlag = FALSE;
}
task main()
{
if (InitializeMain(true, true))
{
StartReadingTouchSensor(touchForklift);
SpeculativelyUpdateBlackboard();
//
for (;;)
{
if (touchForklift.fValue)
{
StopRobot();
break;
}
//
SendOneMotorPower(motorForklift, -2); // down *slow*
//
EndTimeSlice();
}
}
}
void TravelerAlgorithm(void)
{
vector Z(0,0);
double omega, vel=0.07; // vel is top speed of servos
int chosen_dir = 1, quit_loop=0;
int sensor[6], sensor_obstacle=30, sensor_clearview=40;
Move(chosen_dir);
do {
do { // drive until robot sees an obstacle using a current sensor
quit_loop = SmartWait(1); // obstacle detected
} while ( (Sensor_To_Range(Sensor(chosen_dir)) > sensor_obstacle) && (quit_loop == 0) && (chosen_dir!=0) );
sensor[1] = Sensor_To_Range(Sensor(1)); // get sensors
sensor[2] = Sensor_To_Range(Sensor(2));
sensor[3] = Sensor_To_Range(Sensor(3));
sensor[4] = sensor[1];
sensor[5] = sensor[2];
// sensor+1 -> obstalce & sensor+2 -> obstalce
if ( (sensor[chosen_dir+2] < sensor_obstacle) && (sensor[chosen_dir+1] < sensor_obstacle) ) {
Vector_Drive(Z, 0.500);
chosen_dir=0; }
else {
// sensor+1 -> clear & sensor+2 -> obstacle
if ( (sensor[chosen_dir+1] > sensor_clearview) && (sensor[chosen_dir+2] < sensor_obstacle) )
chosen_dir+=1;
// sensor+1 -> obstalce & sensor+2 -> clear
if ( (sensor[chosen_dir+1] < sensor_obstacle) && (sensor[chosen_dir+2] > sensor_clearview) )
chosen_dir+=2;
// sensor+1 -> clear & sensor+2 -> clear
if ( (sensor[chosen_dir+2] > sensor_clearview) && (sensor[chosen_dir+1] > sensor_clearview) )
// pick 2 for now, change to random later
chosen_dir+=2;
if ((chosen_dir) > 3)
chosen_dir-=3; // prevent overflow
Move(chosen_dir); }
Display(chosen_dir);
} while (quit_loop==0);
StopRobot();
}
// =======================================================================
// Function to move the robot by the gyro by time V2
// =======================================================================
void GyroTime_moveV2(int time, int power,bool StopWhenDone, bool adjust, bool ConstOrRel)
{
long adj_power;
long adj_deg;
long current_power;
relHeading =0;
wait1Msec(200);
motor[LF_motor] = -power;
motor[RF_motor] = power;
motor[LR_motor] = -power;
motor[RR_motor] = power;
current_power = power;
ClearTimer(T1);
bool Done = false;
while(!Done)
{
if(time1[T1] > time)
{
Done = true;
}
if(adjust == true)
{
if(ConstOrRel) adj_deg = (long) constHeading;
else adj_deg = (long) relHeading;
adj_power = adj_deg*GYRO_PROPORTION;
motor[LF_motor] = -(current_power - adj_power);
motor[RF_motor] = (current_power + adj_power);
motor[LR_motor] = -(current_power);
motor[RR_motor] = (current_power);
}
}
if(StopWhenDone==true)
{
StopRobot();
}
}
// =======================================================================
// Function to move the robot by the gyro and the sonar sensors V2
// =======================================================================
void GyroSonar_moveV3(int time, int WhichSensors, int distanceX, int distanceY, int power,int direction,bool StopWhenDone, bool adjust, bool ConstOrRel)
{
step++; // increase step number for data log file
long adj_power; // Reset all the values
long adj_deg; //
int count = 0; //
int sonar_adj = 0; //-----------------
int lastvalue=9999;
long current_power;
relHeading =0; //-----------------
wait1Msec(20);
current_power = power;
ClearTimer(T1);
bool Done = false;
while(!Done)
{/*
if(WhichSensors != ByTIME || WhichSensors != SIDE)//----------------
{ //
if(sonarLive2 > distanceY ) // we have exceeded our target distance
{
if (sonarLive2!=lastvalue) // we have a new reading above the target distance
{
count++; // so increase our count of values above distance
lastvalue=sonarLive2; // and save the current reading for next time around
}
}
else count=0; // the measured distance is below the target distance
if(count > 2) Done = true; // once we get two readings above target we exit the loop
} //
else if(time1[T1] > time) Done = true; //----------------*/
switch(WhichSensors)
{
case SIDE:
if(time1[T1] > time) Done = true;
break;
case SIDE_BACK:
if(sonarLive2 > distanceY ) // we have exceeded our target distance
{
if (sonarLive2!=lastvalue) // we have a new reading above the target distance
{
count++; // so increase our count of values above distance
lastvalue=sonarLive2; // and save the current reading for next time around
}
}
else count=0; // the measured distance is below the target distance
if(count > 2) Done = true;
break;
case BACK:
if(sonarLive2 > distanceY ) // we have exceeded our target distance
{
if (sonarLive2!=lastvalue) // we have a new reading above the target distance
{
count++; // so increase our count of values above distance
lastvalue=sonarLive2; // and save the current reading for next time around
}
}
else count=0; // the measured distance is below the target distance
if(count > 2) Done = true;
break;
case ByTIME:
if(time1[T1] > time) Done = true;
break;
}
if(adjust == true)
{
if(WhichSensors != 1) sonar_adj = ((distanceX - sonarLive)*SONAR_PROPORTION);
if(ConstOrRel) adj_deg = (long) constHeading;
else adj_deg = (long) relHeading;
adj_power = adj_deg*GYRO_PROPORTION;
if(WhichSensors == ByTIME) sonar_adj = 0;
SetDrive(direction+(-sonar_adj+adj_power),power);
}
}
if(StopWhenDone==true)
{
StopRobot();
}
}
// =======================================================================
// Function to move the robot by the gyro and the sonar sensors V2
// Note: The option to predict driven distance based on time (in the event
// of loss of sonar signals) is dependent on the sonar distance having been
// zero at the start of the move - for example, the robot should be positioned
// against a wall.
// =======================================================================
bool GyroSonar_moveV2(int time, int WhichSensors, int distanceX, int distanceY, int power,bool StopWhenDone, bool adjust, bool ConstOrRel, bool prediction = false)
{
step++; // increase step number for data log file
long adj_power; // Reset all the values
long adj_deg; //
int count = 0; //
int sonar_adj = 0; //-----------------
int lastvalue=9999;
long current_power;
bool seenOpponent = false;
long MoveStartTime = nPgmTime-1; // fudge start time to avoid devide by zero
float AverageSpeed = 0; // units of speed will be centimeters per millisecond
float PredictedTime = 999999;
int i = 0;
relHeading =0; //-----------------
wait1Msec(20);
motor[LF_motor] = -power; //---------------
motor[RF_motor] = power; // Set beginning speeds
motor[LR_motor] = -power; // for the motors
motor[RR_motor] = power; //---------------
current_power = power;
ClearTimer(T1);
bool Done = false;
while(!Done)
{
if(WhichSensors != SIDE) //----------------
{ //
if(sonarLive2 > distanceY) // we have exceeded our target distance
{
if (sonarLive2!=lastvalue) // we have a new reading above the target distance
{
count++; // so increase our count of values above distance
lastvalue=sonarLive2; // and save the current reading for next time around
AverageSpeed = sonarLive2/(nPgmTime-MoveStartTime);
PredictedTime = distanceY/AverageSpeed; // calculate predicted time it will take to drive desired distance
}
}
else // readings are still below our taregt distance
{
count=0; // reset our count since we are still seeing numbers below the target distance
}
if(WhichSensors == BACK)
{
if(sonarLive < 124)
{
i++;
if(i>8) seenOpponent = true;
}
}
if(count > 2) // we have seen two different values about our target distance
{
Done = true; // once we get two readings above target we exit the loop
}
} //
else // we are controlling based on time
{
if (time1[T1] > time) // if the timer exceeds the desired time then we exit the loop
{
Done = true; // timer will also stop the move
}
}//----------------
if ((time1[T1] > (long)PredictedTime) && (prediction == true)) // if speed based prediction is enabled then check the calculated value
{
Done = true; // optionally the predicted time will also stop the mve
}
if(adjust == true) // then we use sensors to maintain distance and orientation
{
if(WhichSensors != 1) // then we use the side sensor for wall alignment
{
sonar_adj = ((distanceX - sonarLive)*SONAR_PROPORTION); // calculate the correction based on current distance
}
if(ConstOrRel) // decide whether adjustment is this move, or field relative
{
adj_deg = (long) constHeading; // field relative, so calculate adjustment accordingly
}
else
{
adj_deg = (long) relHeading; // relative to start of this move, so calculate adjustment accordingly
}
adj_power = adj_deg*GYRO_PROPORTION; // now add together the two adjstment values
/*
motor[LF_motor] = -(current_power + adj_power + sonar_adj);
motor[RF_motor] = current_power + adj_power - sonar_adj; // was +
motor[LR_motor] = -(current_power + adj_power - sonar_adj);
motor[RR_motor] = current_power + adj_power + sonar_adj;
*/
motor[LF_motor] = -(current_power - (adj_power + sonar_adj)); // drive the motors using the adjustment and current power values
motor[RF_motor] = (current_power + (adj_power + sonar_adj));
motor[LR_motor] = -(current_power);
motor[RR_motor] = (current_power);
}
}
if(StopWhenDone==true)
{
StopRobot();
}
return(seenOpponent);
}
// =======================================================================
// Function to move the robot by the gyro by time sideways V2
// =======================================================================
void GyroTimeS_moveV2(int time, int power, bool light,bool StopWhenDone, bool adjust, bool ConstOrRel)
{
step++; // increase step number for data log file
long adj_power;
long adj_deg;
long current_power;
int i = 0;
relHeading =0;
wait1Msec(200);
motor[LF_motor] = -power;
motor[RF_motor] = -power;
motor[LR_motor] = power;
motor[RR_motor] = power;
current_power = power;
ClearTimer(T1);
bool Done = false;
int addPower = 0;
while(!Done)
{
RequestedScreen=S_LIGHT; // force the light value screen to be displayed
if(light == true)
{
if(light_normalised > light_threshold) i++;
if(i > 5) Done = true; // was 10, but that may be too high, now 5 at 21 Nov 2012
}
if(time1[T1] > time)
{
Done = true;
}
if(adjust == true)
{
if(ConstOrRel) adj_deg = (long) constHeading;
else adj_deg = (long) relHeading;
adj_power = adj_deg*GYRO_PROPORTION;
//======================================
//addPower = (current_power*5)/100;
//if(power > 0)
//{
// motor[LF_motor] = -(current_power-addPower); //more
// motor[RF_motor] = -((current_power+addPower) - adj_power);
// motor[LR_motor] = (current_power-addPower); //more
// motor[RR_motor] = ((current_power+addPower) + adj_power);
//}
//else
//{
// motor[LF_motor] = -(current_power+addPower);
// motor[RF_motor] = -((current_power-addPower) - adj_power);
// motor[LR_motor] = (current_power+addPower);
// motor[RR_motor] = ((current_power-addPower) + adj_power);
//}
//==========================================================
motor[LF_motor] = -current_power+adj_power; // adj_power will be either plus or minus based on GYRO rotation
motor[RF_motor] = -current_power+adj_power; // if any rotation has been detected then power to all four
motor[LR_motor] = current_power+adj_power; // wheels needs to be adjusted to correct for it
motor[RR_motor] = current_power+adj_power;
//==========================================================
}
}
if(StopWhenDone==true)
{
StopRobot();
}
}
//=========================================
// Main Program
//=========================================
task main()
{
initializeRobot();
waitForStart(); // wait for FCS to tell us to go!
if(calibrate != 2) // GYRO calibration hasn't been run during the wait time
{
gyroCalTime = 3; // so setup the default calibrate time
calibrate = 1; // start the calibration going
while(calibrate != 2) // and wait for it to complete before moving the robot
{
EndTimeSlice();
}
}
constHeading = 0; // reset the GYRO headings to eliminate any drift while we waited
relHeading = 0; // same thing for relative heading
wait1Msec(Start_Delay*1000); // implement the user configurable delay before moving
PlaySound(soundBeepBeep); // tell everyone we're about to start going
servo[wrist] = WRIST_CLOSED;
servo[shoulder] = SHOULDER_DOWN;
servo[right_servo] = RIGHT_GRIPPER_START;
servo[left_servo] = LEFT_GRIPPER_START;
switch(MissionNumber) // now go run whichever mission we have been asked to run
{
//================================================
// start on the right side of the blue dispenser delivers to IR, then stops
//================================================
case 1:
GyroSonar_moveV2(0, SIDE_BACK, 100, 108, -55,true, true, CONSTANT);
GyroTimeS_moveV2(750,40,true,false,false,true);
Gyro_TurnV2(36,-15,CONSTANT);
if(abs(IR_Bearing) <=1) column = 2;
if(IR_Bearing <-1 ) column = 1;
if(IR_Bearing > 1) column = 3;
wait1Msec(800);
GyroTime_moveV2(1200,-30,true,false,false);
GyroTimeS_moveV2(120,-15,true,false,false,false);
motor[motorA] = -20;
wait1Msec(1000);
motor[motorA] = -3;
GyroTimeS_moveV2(8000,18,true,true,false,false);
GyroTimeS_moveV2(8000,-15,true,true,false,false);
GyroTime_moveV2(1200,-30,true,false,false);
//if(abs(IR_Bearing) <=1) column = 2;
//if(IR_Bearing <-1 ) column = 1;
//if(IR_Bearing > 1) column = 3;
switch(column)
{
case 1:
GyroTimeS_moveV2(700,-30,false,true,false,false);
break;
GyroTimeS_moveV2(8000,30,true,true,false,false);
GyroTime_moveV2(250,-20,true,false,false);
GyroTimeS_moveV2(140,-15,false,true,false,false);
wait1Msec(1500);
servo[shoulder] = SHOULDER_UP;
wait1Msec(2000);
servo[wrist] = WRIST_OPEN;
wait1Msec(3000);
GyroTime_moveV2(300,30,true,false,false);
wait1Msec(3000);
servo[shoulder] = SHOULDER_DOWN;
wait1Msec(3000);
break;
case 2:
wait1Msec(1500);
servo[shoulder] = SHOULDER_UP;
wait1Msec(1000);
servo[wrist] = WRIST_OPEN;
wait1Msec(3000);
GyroTime_moveV2(400,25,true,false,false);
wait1Msec(3000);
servo[shoulder] = SHOULDER_DOWN;
wait1Msec(3000);
break;
case 3:
GyroTimeS_moveV2(700,30,false,true,false,false);
break;
GyroTimeS_moveV2(8000,-30,true,true,false,false);
GyroTime_moveV2(250,20,true,false,false);
GyroTimeS_moveV2(140,-15,false,true,false,false);
wait1Msec(1500);
servo[shoulder] = SHOULDER_UP;
wait1Msec(2000);
servo[wrist] = WRIST_OPEN;
wait1Msec(3000);
GyroTime_moveV2(300,30,true,false,false);
wait1Msec(3000);
servo[shoulder] = SHOULDER_DOWN;
wait1Msec(3000);
break;
}
break;
//================================================
// CENTER PEG
//================================================
case 2:
GyroSonar_moveV2(0, SIDE_BACK, 100, 108, -45,true, true, CONSTANT);
GyroTimeS_moveV2(750,40,true,false,false,true);
Gyro_TurnV2(36,-15,CONSTANT);
wait1Msec(800);
GyroTime_moveV2(1080,-30,true,false,false);
moveLightServo(DOWN);
GyroTimeS_moveV2(8000,-15,true,true,false,false);
GyroTimeS_moveV2(8000,18,true,true,false,false);
GyroTimeS_moveV2(8000,-15,true,true,false,false);
GyroTimeS_moveV2(90,10,true,false,false,false);
GyroTime_moveV2(1200,-30,true,false,false);
wait1Msec(1500);
servo[shoulder] = SHOULDER_UP;
wait1Msec(2000);
servo[wrist] = WRIST_OPEN;
wait1Msec(1000);
GyroTime_moveV2(400,25,true,false,false);
wait1Msec(1000);
servo[shoulder] = SHOULDER_DOWN;
switch(fieldRed)
{
case STOP:
StopRobot();
break;
case RED:
Gyro_TurnV2(42,-15,REL);
GyroTime_moveV2(1200,50,true,false,REL);
GyroTime45_V2(490,50,false, true, REL, true);
GyroTime_moveV2(400,50,true,false,REL);
break;
case BLUE:
Gyro_TurnV2(42,15,REL);
GyroTime_moveV2(1600,45,true,false,false);
break;
}
break;
//================================================
// LEFT PEG
//================================================
case 3:
GyroSonar_moveV2(0, SIDE_BACK, 100, 108, -45,true, true, CONSTANT);
GyroTimeS_moveV2(750,40,true,true,false,true);
Gyro_TurnV2(36,-15,CONSTANT);
wait1Msec(800);
GyroTime_moveV2(1080,-30,true,false,false);
motor[motorA] = -20;
wait1Msec(1000);
motor[motorA] = -3;
GyroTimeS_moveV2(8000,-15,true,true,false,false);
GyroTimeS_moveV2(8000,18,true,true,false,false);
GyroTimeS_moveV2(8000,-15,true,true,false,false);
Gyro_TurnV2(36,-15,CONSTANT);
GyroTime_moveV2(1300,-30,true,false,false);
GyroTime_moveV2(120,30,true,false,false);
GyroTimeS_moveV2(480,20,true,false,false,false);
GyroTimeS_moveV2(8000,15,true,true,false,false);
GyroTimeS_moveV2(8000,-15,true,true,false,false);
Gyro_TurnV2(36,-15,CONSTANT);
GyroTimeS_moveV2(80,-18,true,false,false,false);
GyroTimeS_moveV2(80,15,true,false,false,false);
GyroTime_moveV2(400,-30,true,false,false);
wait1Msec(1500);
servo[shoulder] = SHOULDER_UP;
wait1Msec(2000);
servo[wrist] = WRIST_OPEN;
wait1Msec(1000);
GyroTime_moveV2(600,25,true,false,false);
wait1Msec(3000);
servo[shoulder] = SHOULDER_DOWN;
switch(fieldRed)
{
case STOP:
StopRobot();
break;
case RED:
wait1Msec(3000);
Gyro_TurnV2(42,-15,REL);
GyroTime_moveV2(1200,50,true,false,REL);
GyroTime45_V2(490,50,false, true, REL, true);
GyroTime_moveV2(400,50,true,false,REL);
break;
case BLUE:
wait1Msec(3000);
Gyro_TurnV2(42,15,REL);
GyroTime_moveV2(1600,45,true,false,false);
break;
}
break;
//================================================
// RIGHT PEG
//================================================
case 4:
GyroSonar_moveV2(0, SIDE_BACK, 100, 108, -45,true, true, CONSTANT);
GyroTimeS_moveV2(750,40,true,true,false,true);
Gyro_TurnV2(36,-15,CONSTANT);
wait1Msec(800);
GyroTime_moveV2(1080,-30,true,false,false);
motor[motorA] = -20;
wait1Msec(1000);
motor[motorA] = -3;
GyroTimeS_moveV2(8000,-15,true,true,false,false);
GyroTimeS_moveV2(8000,18,true,true,false,false);
GyroTimeS_moveV2(8000,-15,true,true,false,false);
Gyro_TurnV2(36,-15,CONSTANT);
GyroTime_moveV2(1300,-30,true,false,false);
GyroTime_moveV2(100,30,true,false,false);
Gyro_TurnV2(42,-15,CONSTANT);
GyroTimeS_moveV2(460,-20,true,false,false,false);
GyroTimeS_moveV2(8000,-18,true,true,false,false);
GyroTimeS_moveV2(8000,18,true,true,false,false);
GyroTimeS_moveV2(8000,-18,true,true,false,false);
GyroTime_moveV2(240,-30,true,false,false);
wait1Msec(1500);
servo[shoulder] = SHOULDER_UP;
wait1Msec(2000);
servo[wrist] = WRIST_OPEN;
wait1Msec(1000);
GyroTime_moveV2(600,25,true,false,false);
wait1Msec(3000);
servo[shoulder] = SHOULDER_DOWN;
switch(fieldRed)
{
case STOP:
StopRobot();
break;
case RED:
wait1Msec(3000);
Gyro_TurnV2(42,-15,REL);
GyroTime_moveV2(1200,50,true,false,REL);
GyroTime45_V2(490,50,false, true, REL, true);
GyroTime_moveV2(400,50,true,false,REL);
break;
case BLUE:
wait1Msec(3000);
Gyro_TurnV2(42,15,REL);
GyroTime_moveV2(1600,45,true,false,false);
break;
}
break;
//================================================
// CENTER PED SECOND POSITION
//================================================
case 5:
GyroSonar_moveV2(0, BACK, 100, 120, -55,true, true, CONSTANT);
GyroTimeS_moveV2(750,-40,true,true,false,true);
Gyro_TurnV2(40,15,CONSTANT);
wait1Msec(800);
GyroTime_moveV2(1200,-30,true,false,false);
motor[motorA] = -50;
wait1Msec(1000);
motor[motorA] = -3;
GyroTimeS_moveV2(8000,-15,true,true,false,false);
GyroTimeS_moveV2(8000,18,true,true,false,false);
GyroTimeS_moveV2(8000,-15,true,true,false,false);
Gyro_TurnV2(36,-15,CONSTANT);
GyroTime_moveV2(360,-20,true,false,false);
wait1Msec(1500);
servo[shoulder] = SHOULDER_UP;
wait1Msec(2000);
servo[wrist] = WRIST_OPEN;
wait1Msec(1000);
GyroTime_moveV2(600,25,true,false,false);
wait1Msec(1000);
servo[shoulder] = SHOULDER_DOWN;
switch(fieldRed)
{
case STOP:
StopRobot();
break;
case RED:
Gyro_TurnV2(42,-15,REL);
GyroTime_moveV2(1200,50,true,false,REL);
GyroTime45_V2(490,50,false, true, REL, true);
GyroTime_moveV2(400,50,true,false,REL);
break;
case BLUE:
Gyro_TurnV2(42,15,REL);
GyroTime_moveV2(1600,45,true,false,false);
break;
}
break;
wait1Msec(1500);
servo[shoulder] = SHOULDER_UP;
wait1Msec(2000);
servo[wrist] = WRIST_OPEN;
wait1Msec(1000);
GyroTime_moveV2(400,25,true,false,false);
wait1Msec(3000);
servo[shoulder] = SHOULDER_DOWN;
switch(fieldRed)
{
case STOP:
StopRobot();
break;
case RED:
wait1Msec(3000);
Gyro_TurnV2(42,-15,REL);
GyroTime_moveV2(1200,50,true,false,REL);
GyroTime45_V2(490,50,false, true, REL, true);
GyroTime_moveV2(400,50,true,false,REL);
break;
case BLUE:
wait1Msec(3000);
Gyro_TurnV2(42,15,REL);
GyroTime_moveV2(1600,45,true,false,false);
break;
}
break;
//================================================
// RIGHT PEG SECOND POSITION
//================================================
case 6:
GyroSonar_moveV2(0, BACK, 100, 120, -55,true, true, CONSTANT);
GyroTimeS_moveV2(750,-40,true,true,false,true);
Gyro_TurnV2(40,15,CONSTANT);
wait1Msec(800);
GyroTime_moveV2(1200,-30,true,false,false);
motor[motorA] = -50;
wait1Msec(1000);
motor[motorA] = -3;
GyroTimeS_moveV2(8000,-15,true,true,false,false);
GyroTimeS_moveV2(8000,18,true,true,false,false);
GyroTimeS_moveV2(8000,-15,true,true,false,false);
Gyro_TurnV2(36,-15,CONSTANT);
GyroTime_moveV2(1300,-30,true,false,false);
GyroTime_moveV2(100,30,true,false,false);
GyroTimeS_moveV2(360,-20,true,false,false,false);
GyroTimeS_moveV2(8000,-18,true,true,false,false);
GyroTimeS_moveV2(8000,18,true,true,false,false);
GyroTimeS_moveV2(8000,-18,true,true,false,false);
GyroTime_moveV2(240,-30,true,false,false);
wait1Msec(1500);
servo[shoulder] = SHOULDER_UP;
wait1Msec(2000);
servo[wrist] = WRIST_OPEN;
wait1Msec(1000);
GyroTime_moveV2(600,25,true,false,false);
wait1Msec(3000);
servo[shoulder] = SHOULDER_DOWN;
switch(fieldRed)
{
case STOP:
StopRobot();
break;
case RED:
wait1Msec(3000);
Gyro_TurnV2(42,-15,REL);
GyroTime_moveV2(1200,50,true,false,REL);
GyroTime45_V2(490,50,false, true, REL, true);
GyroTime_moveV2(400,50,true,false,REL);
break;
case BLUE:
wait1Msec(3000);
Gyro_TurnV2(42,15,REL);
GyroTime_moveV2(1600,45,true,false,false);
break;
}
break;
//================================================
// LEFT PED SECOND POSITION
//================================================
case 7:
GyroSonar_moveV2(0, BACK, 100, 120, -55,true, true, CONSTANT);
GyroTimeS_moveV2(750,-40,true,true,false,true);
Gyro_TurnV2(40,15,CONSTANT);
wait1Msec(800);
GyroTime_moveV2(1200,-30,true,false,false);
motor[motorA] = -50;
wait1Msec(1000);
motor[motorA] = -3;
GyroTimeS_moveV2(8000,-15,true,true,false,false);
GyroTimeS_moveV2(8000,18,true,true,false,false);
GyroTimeS_moveV2(8000,-15,true,true,false,false);
Gyro_TurnV2(36,-15,CONSTANT);
GyroTime_moveV2(1300,-30,true,false,false);
GyroTime_moveV2(120,30,true,false,false);
GyroTimeS_moveV2(260,20,true,false,false,false);
GyroTimeS_moveV2(8000,15,true,true,false,false);
GyroTimeS_moveV2(8000,-15,true,true,false,false);
Gyro_TurnV2(36,-15,CONSTANT);
GyroTimeS_moveV2(80,-18,true,false,false,false);
GyroTime_moveV2(400,-30,true,false,false);
wait1Msec(1500);
servo[shoulder] = SHOULDER_UP;
wait1Msec(2000);
servo[wrist] = WRIST_OPEN;
wait1Msec(1000);
GyroTime_moveV2(600,25,true,false,false);
wait1Msec(3000);
servo[shoulder] = SHOULDER_DOWN;
switch(fieldRed)
{
case STOP:
StopRobot();
break;
case RED:
wait1Msec(3000);
Gyro_TurnV2(42,-15,REL);
GyroTime_moveV2(1200,50,true,false,REL);
GyroTime45_V2(490,50,false, true, REL, true);
GyroTime_moveV2(400,50,true,false,REL);
break;
case BLUE:
wait1Msec(3000);
Gyro_TurnV2(42,15,REL);
GyroTime_moveV2(1600,45,true,false,false);
break;
}
break;
//================================================
// DEFENSE
//================================================
case 8:
GyroTime_moveV2(1200,-50,true,false,REL);
GyroSonar_moveV2(0, BACK, 100, 120, -55,true, true, CONSTANT);
break;
//================================================
// OPPONENTS SIDE
//================================================
case 9:
for (int i=0;i<360;i++)
{SetDrive(i,50);
wait1Msec(10);
}
StopRobot();
wait1Msec(30000);
break;
//================================================
// HIGH LIGHT
//================================================
case 10:
for (int i=0;i<360;i++)
{SetDrive(i,50);
wait1Msec(10);
}
StopRobot();
wait1Msec(30000);
break;
//================================================
// HIGH LIGHT OPPONENTS
//================================================
case 11:
for (int i=0;i<360;i++)
{SetDrive(i,50);
wait1Msec(10);
}
StopRobot();
wait1Msec(30000);
break;
//================================================
// ROBOT EVADE
//================================================
case 12:
for (int i=0;i<360;i++)
{SetDrive(i,50);
wait1Msec(10);
}
StopRobot();
wait1Msec(30000);
break;
}
}
//=========================================
// Main Program
//=========================================
task main()
{
initializeRobot();
waitForStart(); // wait for FCS to tell us to go!
if(done)
{
StartTask(lightDiagnostic);
}
if(calibrate != 2) // GYRO calibration hasn't been run during the wait time
{
gyroCalTime = 3; // so setup the default calibrate time
calibrate = 1; // start the calibration going
while(calibrate != 2) // and wait for it to complete before moving the robot
{
EndTimeSlice();
}
}
constHeading = 0; // reset the GYRO headings to eliminate any drift while we waited
relHeading = 0; // same thing for relative heading
wait1Msec(Start_Delay*1000); // implement the user configurable delay before moving
PlaySound(soundBeepBeep); // tell everyone we're about to start going
step = MissionNumber*100; // this records the actual mission number that we ran within the data log file
LogData=true; // start saving data to the log file
servo[wrist] = WRIST_CLOSED;
servo[shoulder] = SHOULDER_DOWN;
servo[right_servo] = RIGHT_GRIPPER_START;
servo[left_servo] = LEFT_GRIPPER_START;
switch(MissionNumber) // now go run whichever mission we have been asked to run
{
//================================================
// start on the right side of the blue dispenser delivers to IR
//================================================
case 1:
IRside = position1();//go here
if(IRside < -75) column = 1;
else if(IRside < -25) column = 2;
else column = 3;
wait1Msec(800);
switch(column)
{
case 1://left
diagnosticBeeps(1);
leftPeg();
break;
case 2://center
diagnosticBeeps(2);
centerPeg();
break;
case 3://right
diagnosticBeeps(3);
rightPeg();
break;
}
switch(fieldRed)
{
case STOP:
StopRobot();
break;
case RED:
redReturn();
break;
case BLUE:
blueReturn();
break;
}
break;
//================================================
// CENTER PEG
//================================================
case 2:
position1();
centerPeg();
switch(fieldRed)
{
case STOP:
StopRobot();
break;
case RED:
redReturn();
break;
case BLUE:
blueReturn();
break;
}
break;
//================================================
// LEFT PEG
//================================================
case 3:
position1();
leftPeg();
switch(fieldRed)
{
case STOP:
StopRobot();
break;
case RED:
redReturn();
break;
case BLUE:
blueReturn();
break;
}
break;
//================================================
// RIGHT PEG
//================================================
case 4:
position1();
rightPeg();
switch(fieldRed)
{
case STOP:
StopRobot();
break;
case RED:
redReturn();
break;
case BLUE:
blueReturn();
break;
}
break;
//================================================
// IR SECOND POSITION
//================================================
case 5:
IRside = position2();//go here
if(IRside < -75) column = 1;
else if(IRside < -25) column = 2;
else column = 3;
wait1Msec(800);
switch(column)
{
case 1://left
diagnosticBeeps(1);
leftPeg();
break;
case 2://center
diagnosticBeeps(2);
centerPeg();
break;
case 3://right
diagnosticBeeps(3);
rightPeg();
break;
}
switch(fieldRed)
{
case STOP:
StopRobot();
break;
case RED:
redReturn();
break;
case BLUE:
blueReturn();
break;
}
break;
//================================================
// CENTER PED SECOND POSITION
//================================================
case 6:
position2();
centerPeg();
break;
//================================================
// LEFT PEG SECOND POSITION
//================================================
case 7:
position2();
leftPeg();
break;
//================================================
// RIGHT PED SECOND POSITION
//================================================
case 8:
position2();
rightPeg();
break;
//================================================
// DEFENSE RIGHT
//================================================
case 9:
GyroSonar_moveV2(0, BACK, 100, 124, -60,true, true, CONSTANT);
GyroTime_moveV2(700,-50,true,true,REL);
GyroTimeS_moveV2(1800,40,true,true,false,true);
break;
//================================================
// DEFENSE LEFT
//================================================
case 10:
GyroSonar_moveV2(0, BACK, 100, 124, -60,true, true, CONSTANT);
GyroTime_moveV2(700,-50,true,true,REL);
GyroTimeS_moveV2(1800,-40,true,true,false,true);
break;
//================================================
// OPPONENT IR
//================================================
case 11:
for (int i=0;i<90;i++)
{SetDrive(i,50,i);
wait1Msec(30);
}
StopRobot();
wait1Msec(30000);
break;
//================================================
// OPPONENT CENTER
//================================================
case 12:
opponentRight();
centerPeg();
break;
//================================================
// OPPONENT LEFT
//================================================
case 13:
opponentRight();
rightPeg();
break;
//================================================
// OPPONENT RIGHT
//================================================
case 14:
opponentRight();
leftPeg();
break;
//================================================
// OPPONENTL IR
//================================================
case 15:
opponentLeft();
leftPeg();
break;
//================================================
// OPPONENTL CENTER
//================================================
case 16:
opponentLeft();
centerPeg();
break;
//================================================
// OPPONENTL LEFT
//================================================
case 17:
opponentLeft();
rightPeg();
break;
//================================================
// OPPONENTL RIGHT
//================================================
case 18:
opponentLeft();
leftPeg();
break;
//================================================
// See Opponent test
//================================================
case 19:
sawOpponent = GyroSonar_moveV2(0, BACK, 100, 112, -50,true, true, CONSTANT);
StopRobot();
if(sawOpponent) diagnosticBeeps(6);
break;
//================================================
// See Opponent test2
//================================================
case 20:
sawOpponent = GyroSonar_moveV2(0, BACK, 100, 112, -50,true, true, CONSTANT);
StopRobot();
if(sawOpponent) diagnosticBeeps(6);
break;
}
LogData=false; // stop logging IR data and close the file
StopRobot();
}
//=========================================
// Main Program
//=========================================
task main()
{
initializeRobot();
waitForStart(); // wait for FCS to tell us to go!
if(done)
{
StartTask(lightDiagnostic);
}
if(calibrate != 2) // GYRO calibration hasn't been run during the wait time
{
gyroCalTime = 3; // so setup the default calibrate time
calibrate = 1; // start the calibration going
while(calibrate != 2) // and wait for it to complete before moving the robot
{
EndTimeSlice();
}
}
constHeading = 0; // reset the GYRO headings to eliminate any drift while we waited
relHeading = 0; // same thing for relative heading
wait1Msec(Start_Delay*1000); // implement the user configurable delay before moving
PlaySound(soundBeepBeep); // tell everyone we're about to start going
step = MissionNumber*100; // this records the actual mission number that we ran within the data log file
LogData=true; // start saving data to the log file
servo[wrist] = WRIST_CLOSED;
servo[shoulder] = SHOULDER_DOWN;
servo[right_servo] = RIGHT_GRIPPER_START;
servo[left_servo] = LEFT_GRIPPER_START;
switch(MissionNumber) // now go run whichever mission we have been asked to run
{
//================================================
// start on the right side of the blue dispenser delivers to IR
//================================================
case 1:
int IRside = position1();//go here
if(IRside < -75) column = 1;
else if(IRside < -25) column = 2;
else column = 3;
wait1Msec(800);
switch(column)
{
case 1://left
diagnosticBeeps(1);
leftPeg();
break;
case 2://center
diagnosticBeeps(2);
centerPeg();
break;
case 3://right
diagnosticBeeps(3);
rightPeg();
break;
}
switch(fieldRed)
{
case STOP:
StopRobot();
break;
case RED:
redReturn();
break;
case BLUE:
blueReturn();
break;
}
break;
//================================================
// CENTER PEG
//================================================
case 2:
position1();
centerPeg();
switch(fieldRed)
{
case STOP:
StopRobot();
break;
case RED:
redReturn();
break;
case BLUE:
blueReturn();
break;
}
break;
//================================================
// LEFT PEG
//================================================
case 3:
position1();
leftPeg();
switch(fieldRed)
{
case STOP:
StopRobot();
break;
case RED:
redReturn();
break;
case BLUE:
blueReturn();
break;
}
break;
//================================================
// RIGHT PEG
//================================================
case 4:
position1();
switch(fieldRed)
{
case STOP:
StopRobot();
break;
case RED:
redReturn();
break;
case BLUE:
blueReturn();
break;
}
break;
//================================================
// CENTER PED SECOND POSITION
//================================================
case 5:
GyroSonar_moveV2(0, BACK, 100, 114, -48,true, true, CONSTANT);
GyroTime_moveV2(400,-30,true,false,false);
GyroTimeS_moveV2(750,-40,false,true,true,CONSTANT);
Gyro_TurnV2(40,15,CONSTANT);
wait1Msec(800);
GyroTime_moveV2(1200,-30,true,false,false);
moveLightServo(DOWN);
//GyroTimeS_moveV2(8000,15,true,true,false,false);
GyroTimeS_moveV2(8000,-15,true,true,false,false);
GyroTimeS_moveV2(8000,18,true,true,false,false);
GyroTimeS_moveV2(8000,-15,true,true,false,false);
//GyroTimeS_moveV2(90,10,true,false,false,false);
GyroTime_moveV2(1200,-30,true,false,false);
wait1Msec(1500);
servo[shoulder] = SHOULDER_UP;
wait1Msec(2000);
servo[wrist] = WRIST_OPEN;
wait1Msec(1000);
GyroTime_moveV2(400,25,true,false,false);
wait1Msec(1000);
servo[shoulder] = SHOULDER_DOWN;
switch(fieldRed)
{
case STOP:
StopRobot();
break;
case RED:
Gyro_TurnV2(42,-15,REL);
GyroTime_moveV2(1200,50,true,false,REL);
GyroTime45_V2(490,50,false, true, REL, true);
GyroTime_moveV2(400,50,true,false,REL);
break;
case BLUE:
Gyro_TurnV2(42,15,REL);
GyroTime_moveV2(1600,45,true,false,false);
break;
}
break;
//================================================
// LEFT PEG SECOND POSITION
//================================================
case 6:
/*
GyroSonar_moveV2(0, BACK, 100, 114, -48,true, true, CONSTANT); // was this
GyroTime_moveV2(400,-30,true,false,false);
GyroTimeS_moveV2(750,-40,true,true,false,CONSTANT);
Gyro_TurnV2(40,15,CONSTANT);
*/
GyroSonar_moveV2(0, BACK, 100, 114, -48,true, true, CONSTANT); //trying this
GyroTime_moveV2(400,-30,true,false,false);
GyroTimeS_moveV2(750,-40,false,true,true,CONSTANT);
Gyro_TurnV2(40,15,CONSTANT);
wait1Msec(800);
GyroTime_moveV2(1200,-30,true,false,false);
moveLightServo(DOWN);
//GyroTimeS_moveV2(8000,15,true,true,false,false);
GyroTimeS_moveV2(8000,-15,true,true,false,false);
GyroTimeS_moveV2(8000,18,true,true,false,false);
GyroTimeS_moveV2(8000,-15,true,true,false,false);
Gyro_TurnV2(36,-15,CONSTANT);
GyroTime_moveV2(1300,-30,true,false,false);
GyroTime_moveV2(120,30,true,false,false);
GyroTimeS_moveV2(500,25,true,false,false,false);
GyroTimeS_moveV2(2000,15,true,true,false,false);
GyroTimeS_moveV2(8000,-10,true,true,false,false);
Gyro_TurnV2(36,-15,CONSTANT);
//GyroTimeS_moveV2(80,15,true,false,false,false);
GyroTimeS_moveV2(8000,-25,true,true,false,false);
GyroTimeS_moveV2(50,-20,true,false,false,false);
GyroTime_moveV2(400,-30,true,false,false);
wait1Msec(1500);
servo[shoulder] = SHOULDER_UP;
wait1Msec(2000);
servo[wrist] = WRIST_OPEN;
wait1Msec(1000);
GyroTime_moveV2(600,25,true,false,false);
wait1Msec(3000);
servo[shoulder] = SHOULDER_DOWN;
switch(fieldRed)
{
case STOP:
StopRobot();
break;
case RED:
break;
case BLUE:
wait1Msec(1000);
Gyro_TurnV2(30,15,CONSTANT);
GyroTime_moveV2(800,50,true,false,false);
break;
}
break;
//================================================
// RIGHT PED SECOND POSITION
//================================================
case 7:
/*
GyroSonar_moveV2(0, BACK, 100, 114, -48,true, true, CONSTANT); //was this
GyroTime_moveV2(400,-30,true,false,false);
GyroTimeS_moveV2(750,-40,false,true,true,CONSTANT);
Gyro_TurnV2(40,15,CONSTANT);
*/
GyroSonar_moveV2(0, BACK, 100, 114, -48,true, true, CONSTANT); //trying this
GyroTime_moveV2(400,-30,true,false,false);
GyroTimeS_moveV2(750,-40,false,true,true,CONSTANT);
Gyro_TurnV2(40,15,CONSTANT);
break;
wait1Msec(800);
GyroTime_moveV2(1200,-30,true,false,false);
moveLightServo(DOWN);
//GyroTimeS_moveV2(8000,-15,true,true,false,false);
GyroTimeS_moveV2(8000,18,true,true,false,false);
GyroTimeS_moveV2(8000,-15,true,true,false,false);
Gyro_TurnV2(36,-15,CONSTANT);
GyroTime_moveV2(1300,-30,true,false,false);
GyroTime_moveV2(120,30,true,false,false);
GyroTimeS_moveV2(260,20,true,false,false,false);
GyroTimeS_moveV2(8000,15,true,true,false,false);
GyroTimeS_moveV2(8000,-15,true,true,false,false);
Gyro_TurnV2(36,-15,CONSTANT);
GyroTimeS_moveV2(80,-18,true,false,false,false);
GyroTime_moveV2(400,-30,true,false,false);
wait1Msec(1500);
servo[shoulder] = SHOULDER_UP;
wait1Msec(2000);
servo[wrist] = WRIST_OPEN;
wait1Msec(1000);
GyroTime_moveV2(600,25,true,false,false);
wait1Msec(3000);
servo[shoulder] = SHOULDER_DOWN;
switch(fieldRed)
{
case STOP:
StopRobot();
break;
case RED:
wait1Msec(3000);
Gyro_TurnV2(42,-15,REL);
GyroTime_moveV2(1200,50,true,false,REL);
GyroTime45_V2(490,50,false, true, REL, true);
GyroTime_moveV2(400,50,true,false,REL);
break;
case BLUE:
wait1Msec(3000);
Gyro_TurnV2(42,15,REL);
GyroTime_moveV2(1600,45,true,false,false);
break;
}
break;
//================================================
// DEFENSE RIGHT
//================================================
case 8:
GyroSonar_moveV2(0, BACK, 100, 124, -60,true, true, CONSTANT);
GyroTime_moveV2(700,-50,true,true,REL);
GyroTimeS_moveV2(1800,40,true,true,false,true);
break;
//================================================
// DEFENSE LEFT
//================================================
case 9:
GyroSonar_moveV2(0, BACK, 100, 124, -60,true, true, CONSTANT);
GyroTime_moveV2(700,-50,true,true,REL);
GyroTimeS_moveV2(1800,-40,true,true,false,true);
break;
//================================================
// OPPONENTS SIDE
//================================================
case 10:
for (int i=0;i<360;i++)
{SetDrive(i,50);
wait1Msec(10);
}
StopRobot();
wait1Msec(30000);
break;
//================================================
// TEST ORBIT
//================================================
case 11:
for (int i=0;i<360;i++)
{SetDrive(i,50,i);
wait1Msec(10);
}
StopRobot();
wait1Msec(30000);
break;
//================================================
// HIGH LIGHT OPPONENTS
//================================================
case 12:
for (int i=0;i<50;i++)
{
moveLightServo(UP);
wait1Msec(1000);
moveLightServo(DOWN);
wait1Msec(1000);
}
break;
//================================================
// ROBOT EVADE
//================================================
case 13:
wait1Msec(30000);
break;
}
LogData=false; // stop logging IR data and close the file
StopRobot();
}
