// ========================================================================================================
// Function to move the robot to the next line in the requested angle maintaining the requested orientation
// ========================================================================================================
void GyroDrive(int time, int angle, int power, bool light,bool StopWhenDone)
{
step++; // increase step number for data log file
relHeading =0;
wait1Msec(200);
ClearTimer(T1); // initialise the overall timer
int i = 0; // initialise line seen counter
bool Done = false; // initialise loop control variable
int orientation = constHeading; // save the current robot orientation so it can be maintained
RequestedScreen=S_LIGHT; // force the light value screen to be displayed
SetDrive(angle,power,orientation); // start driving the requested angle
while(!Done) // and continue until something causes completion
{
if(light == true) // we are to check the light sensor for a line
{
if(light_normalised > light_threshold) i++; // we are seeing the line so increase count
if(i > 5) Done = true; // enough times then we're done
}
if(time1[T1] > time) // timer has expired
{
Done = true; // so we are done
}
SetDrive(angle, power, orientation); // all checks are done, so update drive motor speeds
}
if(StopWhenDone==true) // we are done, decide whether to shut down drive motors
{
StopRobot();
}
}
void WaitForTouch() {
int pressed = !GetDigitalInput(button);
while(pressed == 1 && IsAutoActive()) {
SetDrive(0,0);
}
PrintToScreen ("Pressed!\n");
}
bool Pathname::FullToRelative(const Pathname &relativeto)
{
if (relativeto.IsNull() || IsNull()) return false;
bool h1= HasDrive();
bool h2= relativeto.HasDrive();
if (h1 != h2) return false; //rozdilny zpusob adresace - nelze vytvorit relatvni cestu
if (h1== true && h2== true && toupper(GetDrive()) != toupper(relativeto.GetDrive()))
return false; //ruzne disky, nelze vytvorit relativni cestu
if (strncmp(_path,"\\\\",2) == 0) //sitova cesta
{
int slsh = 0; //citac lomitek
const char *a = _path;
const char *b = relativeto._path;
while (toupper(*a) == toupper(*b) && *a && slsh<3) //zacatek sitove cesty musi byt stejny
{
if (*a =='\\') slsh++;
a++;b++;
}
if (slsh != 3) return false; //pokud neni stejny, nelze vytvorit relativni cestu
}
int sublevel = 0;
const char *ps1= _path;
const char *ps2= relativeto._path;
if (h1)
{ps1 += 2;ps2 += 2;}
const char *sls = ps2;
while (toupper(*ps1) == toupper(*ps2) && *ps1)
{
if (*ps2=='\\') sls = ps2+1;
ps1++;ps2++;
}
ps1 -= ps2-sls;
if (sls)
{
while (sls = strchr(sls,'\\'))
{
sls++;
sublevel++;
}
}
char *buff = (char *)alloca((sublevel*3+strlen(ps1)+1)*sizeof(*buff));
char *pos = buff;
for (int i = 0;i<sublevel;i++)
{strcpy(pos,"..\\");pos += 3;}
strcpy(pos,ps1);
SetDrive(0);
SetDirectory(buff);
return true;
}
void Pathname::SetServerName(const char *server)
{
if (HasDrive()) SetDrive(0);
else
{
int np = IsNetworkPath();
if (np) strcpy(_path,_path+np); //str
}
char *buff = (char *)alloca((strlen(server)+strlen(_path)+5)*sizeof(*buff));
if (_path[0]!='\\')
sprintf(buff,"\\\\%s\\%s",server,_path);
else
sprintf(buff,"\\\\%s%s",server,_path);
SetDirectory(buff);
}
//-----------------------------------------------------------------------------
// Kills the player.
//-----------------------------------------------------------------------------
void PlayerObject::Kill()
{
// Indicate that the player is dying.
m_dying = true;
// Clear the player's movment.
SetDrive( 0.0f );
SetStrafe( 0.0f );
SetFire( false );
Stop();
// Clear the player's weapons.
ClearWeapons();
// Stop the player from changing weapons.
m_changingWeapon = 0.0f;
m_weaponChanging = false;
// Play the death animation.
PlayAnimation( ANIM_DEATH, 0.0f, false );
}
bool Pathname::RelativeToFull(const Pathname &ref)
{
if (ref.IsNull() || IsNull()) return false;
const char *beg;
if (HasDrive())
if (toupper(GetDrive()) != toupper(ref.GetDrive())) return false;
else beg = _path+2;
else beg = _path;
const char *end = strchr(ref._path,0);
if (beg[0] =='\\')
{
int np;
if (ref.HasDrive()) end = ref._path+2;
else if (np = ref.IsNetworkPath()) end = ref._path+np;
else end = ref._path;
}
else while (strncmp(beg,"..\\",3) == 0 || strncmp(beg,".\\",2) == 0)
{
if (beg[1] =='.')
{
if (end>ref._path)
{
end--;
while (end>ref._path && end[-1]!='\\') end--;
}
beg += 3;
}
else
beg += 2;
}
int partln = end-ref._path;
char *buff = (char *)alloca((partln+strlen(beg)+1)*sizeof(*buff));
memcpy(buff,ref._path,partln);
strcpy(buff+partln,beg);
SetDrive(0);
SetDirectory(buff);
return true;
}
// =======================================================================
// 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();
}
}
//=========================================
// 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();
}
//=========================================
// 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;
}
}
CDAudioDevice::CDAudioDevice()
{
_FindDrives("/dev/disk");
if (CountDrives() > 0)
SetDrive(0);
}
