void lineFollow(){
if (DBG_LEVEL == 1)
_DBG_("CALIB LINE");
calibrateSensors();
if (DBG_LEVEL == 1)
_DBG_("START LINE FOLLOWING");
uint8_t sequence[] = {35, 45 , 35, 35 ,45};
cmdPIDstart(sequence);
}
Simon_Says_lib::buttons Simon_Says_lib::start(SimonActuator* startAct)
{
unsigned long timer;
calibrateSensors();
startAct->extend();
buttons val = readNextLight();
startAct->retract();
timer = millis();
while (millis() < (timer + 2 * (RETRACT_TIME))){
// Retract for RETRACT_TIME milliseconds
}
startAct->stop();
return val;
}
bool Simon_Says_lib::playSimon(unsigned long timeOut){
for (int i = 0; i < NUM_BUTTONS; i++)
{
actuators[i]->stop();
}
this->homeTop(5000);
this->homeLow(5000);
calibrateSensors();
Simon_Says_lib::buttons sequence[NUM_ROUNDS];
Simon_Says_lib::buttons light;
int nRound = 0;
long complete = millis();
while (millis() < (complete + timeOut)){
if (nRound == 0)
light = this->start(actuators[START]);
else
light = this->readNextLight();
if (light != Simon_Says_lib::NONE){
sequence[nRound] = light; // Add the new light to the sequence.
this->playSequence(sequence, nRound); // Plays the game sequence.
this->oldSequence(sequence, nRound); // Reads the sequence up to the current round as the game plays it back.
}
else{
this->setPauseTime(0);
this->playSequence(sequence, nRound-1); // If we miss the light, play the previous sequence then quit (Max time)
this->homeTop(5000);
return 0;
}
nRound++;
}
this->homeTop(5000);
return 1;
}
//------------[AUTO:RAMP START]------------//
//Position: Centered on the ramp //
//Orientation: Intake forward, sensor forward//
task main()
{
initializeRobot();
waitForStart();
initializeSensors();
calibrateSensors();
startTask(display);
startTask(sensorPoll);
startTask(DCControl);
startTask(servoControl);
//1. Go down Ramp//
rampMove();
//2. Move to IR checking Position//
spinDeg(-85.0);
moveStraight(70.0, 25.0);
//3. IR Beacon Position Search//
for(int i = 0; i < 20; i++)
{
/*writeDebugStreamLine("irSeeker.acValues[0]: %d", irSeeker.acValues[0]);
wait1Msec(100);
writeDebugStreamLine("irSeeker.acValues[1]: %d", irSeeker.acValues[1]);
wait1Msec(100);
writeDebugStreamLine("irSeeker.acValues[2]: %d", irSeeker.acValues[2]);
wait1Msec(100);
writeDebugStreamLine("irSeeker.acValues[3]: %d", irSeeker.acValues[3]);
wait1Msec(100);
writeDebugStreamLine("irSeeker.acValues[4]: %d", irSeeker.acValues[4]);
wait1Msec(100);*/
readSensor(&irSeeker);
if(irSeeker.acValues[2] >= 40)
{
irCounter1++;
}
else if(irSeeker.acValues[2] >= 28 || irSeeker.acValues[3] > 20)
{
irCounter2++;
}
else if(irSeeker.acValues[2] < 28)
{
irCounter3++;
}
}
if(irCounter1 > irCounter3 && irCounter1 > irCounter2)
{
pos = 3;
}
else if(irCounter2 > irCounter3 && irCounter2 > irCounter1)
{
pos = 2;
}
else if(irCounter3 > irCounter1 && irCounter3 > irCounter2)
{
pos = 1;
}
/*writeDebugStreamLine("irCounter1 : %d", irCounter1);
writeDebugStreamLine("irCounter2 : %d", irCounter2);
writeDebugStreamLine("irCounter3 : %d", irCounter3);
writeDebugStreamLine("pos: %d", pos);*/
switch(pos)
{
//4a. Knock kickstand Down//
case 1:
spinDeg(-5.0);
moveStraight(70.0, 60.0);
spinDeg(-87.0);
moveStraight(70.0, 20.0);
moveStraight(-70.0, 10.0);
break;
//4b. Knock kickstand Down//
case 2:
moveStraight(70.0, 45.0);
spinDeg(-45.0);
moveStraight(70.0, 40.0);
moveStraight(-70.0, 20.0);
break;
//4c. Knock kickstand Down//
case 3:
spinDeg(-45.0);
moveStraight(70.0, 73.0);
spinDeg(45.0);
moveStraight(70.0, 45.0);
moveStraight(-70.0, 20.0);
break;
//4d. Go to goal//
default:
moveStraight(70.0, 61.0);
spinDeg(90.0);
moveStraight(70.0, 61.0);
spinDeg(45.0);
break;
}
//5. Tubes Go Up//
//plateOpen = true;
//intakeDown = true;
//tubesUp = true;
//Edit this sleep variable with the tubesUp timing//
//sleep(6000);
//6. Head Goes Up//
//headUp = true;
while(true){}
}
