void readColor(){
int reading = analogRead(A0);
Serial.println(reading);
if(reading >= blackMin && reading <= blackMax){
driveInInches(-10);
turnInDegrees(-90);
}
else if(reading >= blueMin && reading <= blueMax){
turnInDegrees(-45);
}
else if(reading >= greenMin && reading <= greenMax){
turnInDegrees(45);
}
else if(reading >= greyMin && reading <= greyMax){
driveInInches(8);
turnInDegrees(720);
}
else if(reading >= orangeMin && reading <= orangeMax){
leftServo.write(94);
rightServo.write(0);
delay(4800);
stopServos();
}
else if(reading >= whiteMin && reading <= whiteMax){
finishLine = true;
}
}
void servoTimerEvent()
{
//mPORTDToggleBits(BIT_3);
//StartCritical();
stopServos();
int j;
switch(servoStateMachineCurrentState){
case LOW:
updateAllEncoders();
if(getRunPidIsr()){
RunPIDControl();
//Print_Level l = getPrintLevel();
interpolateZXY();
//setPrintLevelNoPrint();
}
//runLinearInterpolationServo(start,stop);
runSort();
for (j=0;j<NUM_SERVO;j++){
pinOn(j);
}
lastValue = 0;
sortedIndex=0;
//1ms delay for all servos
setTimerPreTime();
return;
case PRETIME:
if(setUpNextServo())
return;
case TIME:
stopCurrentServo();
if(servoStateMachineCurrentState == TIME){
if(setUpNextServo() == false) {
//fast stop for channels with the same value
servoTimerEvent();
}
}
//If there are still more channels to be turned off after the recoursion, break
if(servoStateMachineCurrentState != FINISH)
return;
case FINISH:
setTimerLowTime();
return;
}
//EndCritical();
}
void driveStraight(int velocity, int time) {
leftServo.write(velocity);
rightServo.write(180 - velocity);
delay(time);
stopServos();
}
void turnLeftInDegrees(float degree) {
leftServo.write(10);
rightServo.write(10);
delay(8 * degree);
stopServos();
}
void turnRightInDegrees(float degree) {
leftServo.write(170);
rightServo.write(170);
delay(8.84 * degree);
stopServos();
}
void orangeCommand(){
leftServo.write(94);
rightServo.write(0);
delay(4800);
stopServos();
}
void turnRight() {
leftServo.write(170);
rightServo.write(170);
delay(720);
stopServos();
}
