task main()
{
//waitForStart();
LineUp();
/*Tests*/
GoToIR();
bool LightGoDown = false;
if(SensorValue[Sensei] == 1)
{
LightGoDown = true;
}
MoveBackwards(0.1,50);
while(LightGoDown)
{
LineDown();
FollowLine();
if(AtObject())
{
break;
}
}
LineUp();
//testMovement();
}
/*----------------------------------------------------------------------------*/
void BlueAuto2(void)
{
///4 PRELOADS 3 PILE
/// STARTING TYLE IS FURTHERST TYLE
int YouAreGoodToCount = 0;
Transmission(1);
BallCounter = 4;
ShootingMode = 1;
IntakeShoot(6000);//SHOOT 4 PRELOADS // * FAILSAFE POINT
ShootingMode = 4;
DistanceCalculatorTurn(1);
while(DistanceCalculatorTurn(0) < 15) //*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE)
{
Move(-1, 50);
}
Break2(100,-10);
delay(200);
stage = 1;
SetRPM(2200);
DistanceCalculator(1);
while(DistanceCalculator(0) < 12) //*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE)
{
Move(2, 127);
}
clearTimer(T3);
while(SensorValue[IR1] == 1 ) //*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE)
{
IntakeOnAuto();
Move(2, 20);
if(time1[T3] > 6000)
{
FailSafeEngaged = 1;
break;
}
}
if(FailSafeEngaged == 1)Blue1Fail1();/////////////////////RED1FAIL1
IntakePower(0);
Break(100,10);
while(MoveDist(-1, 20,0) == 1) // DO WE NEED FAILSAFE HERE?
{
IntakeOnAuto();
}
while(MoveDist(35, 40,3000) == 1)//17 // DO WE NEED FAILSAFE HERE?
{
IntakeOnAuto();
if(FailSafeEngaged == 1)Blue1FailInfinite();/////////////////////RED1FAIL1
}
IntakePower(0);
while(LineStatus() == 0)
{
Move(-2, 100);
}
Break(100,-10);
while(MoveDist(3, 30,0) == 1);
delay(200);
SetRPM(2200);
while(LineStatus() == 0) //TURN UNTIL YOU SEE THE LINE
{
Move(1, 20);
}
DistanceCalculator(1);
while(LineStatus() == 1) //*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE)
{
FollowLine(40);
if(LineStatusOffset() == 1)
{
DistanceCalculator(1);
YouAreGoodToCount = 1;
}
if(DistanceCalculator(0) > 18 && YouAreGoodToCount == 1)
{
DistanceCalculator(1);
break;
}
}
if(DistanceCalculator(0) < 50 && YouAreGoodToCount == 0)Blue1FailInfinite();//**FAIL GOOD
Break(100,20);
delay(500);
IntakePower(127);///SHOT FIRST PILE
delay(3000);
ShootingMode = 3;
StopDrive();
stage = 1;
DistanceCalculator(1);
while(SensorValue[IR1] == 1 && LineStatus() == 1) //*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE)
{
FollowLine(35);
IntakeOnAuto();
}
Break(100,20);
StopDrive();
clearTimer(T3);
while(stage == 1) // WHAT IF TWO BALLS ARE STUCK FOREVER? TIMER?
{
if(time1[T3] > 10000)
{
break;
}
IntakeOnAuto();
}
clearTimer(T3);
while(LineStatus() == 1 && DistanceCalculator(0) < 37 )//*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE)
{
FollowLine(35);
IntakeOnAuto();
if(time1[T3] > 10000)
{
break;
}
}
MoveTime(500,127);
StopDrive();
delay(500);
IntakePower(127);///SHOT BAR SHOTS
delay(3500);
IntakePower(0);
SetRPM(2200);
stage = 1;
//////////////////////////////////////////////////////SHOT 8 COMPLETE
DistanceCalculator(1);
while(DistanceCalculator(0) < 50) //*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE)
{
Move(-2, 127);
}
Break(200,-15);
delay(200);
DistanceCalculatorTurn(1);
clearTimer(T3);
while(DistanceCalculatorTurn(0) < 62) //*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE)
{
Move(-1, 100);
if(time1[T3] > 5000)
{
FailSafeEngaged = 1;
break;
}
}
if(FailSafeEngaged == 1)Blue1FailInfinite();
Break2(100,10);
delay(300);
DistanceCalculator(1);
clearTimer(T3);
while(DistanceCalculator(0) < 24) //*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE)
{
Move(2, 127);
if(time1[T3] > 5000)
{
FailSafeEngaged = 1;
break;
}
}
if(FailSafeEngaged == 1)Blue1FailInfinite();
clearTimer(T3);
while(SensorValue[IR1] == 1 )//PILE BY THE WALL
{
IntakeOnAuto();
Move(2, 20);
if(time1[T3] > 5000)
{
FailSafeEngaged = 1;
break;
}
}
if(FailSafeEngaged == 1)Blue1FailInfinite();
IntakePower(0);
Break(100,10);
while(MoveDist(-1, 20,0) == 1) // DO WE NEED FAILSAFE HERE?
{
IntakeOnAuto();
}
while(MoveDist(19, 35,2000) == 1) // DO WE NEED FAILSAFE HERE?
{
IntakeOnAuto();
}
IntakePower(0);
///////////////////////////////////////////////////////////////////////////////////PICK UP 3RD
SetRPM(2200);
clearTimer(T3);
while(LineStatus() == 0 )
{
Move(-2, 127);
if(time1[T3] > 5000)
{
FailSafeEngaged = 1;
break;
}
}
if(FailSafeEngaged == 1)Blue1FailInfinite(); //**FAIL GOOD
Break(200,-15);
while(MoveDist(3, 40,0) == 1);
clearTimer(T3);
while(LineStatus() == 0) // TURN UNTILL IT SEES LINE (WHAT IF WE NEVER SEE LINE)
{
Move(1,22);
if(time1[T3] > 5000)
{
FailSafeEngaged = 1;
break;
}
}
if(FailSafeEngaged == 1)Blue1FailInfinite(); //**FAIL GOOD
DistanceCalculator(1);
clearTimer(T3);
while(LineStatus() == 1 && DistanceCalculator(0) < 12)//*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE)
{
FollowLine(35);
if(time1[T3] > 5000)
{
FailSafeEngaged = 1;
break;
}
}
if(FailSafeEngaged == 1)Blue1FailInfinite(); //**FAIL GOOD
Break(100,20);
StopDrive();
delay(500);
IntakePower(127);///SHOT LAST BALLS BAR SHOTS
delay(3000);
TurnDegree(-45,100,2000);
while(MoveDist(24, 40,0) == 1);
ShootingMode = 4;
KillALL();
while(1);
}
int main(void)
{
Initialize();
timer_start(TIMER_TEMP, TIMER_TEMP_DELAY);
timer_start(TIMER_LCD, TIMER_LCD_DELAY);
timer_start(TIMER_TURN90, TIMER_TURN90_DELAY);
timer_start(TIMER_RECULE, TIMER_RECULE_DELAY);
/*
u08 tempmic;
while (!GetMicValLuca(LEVELMIC, &tempmic)) {
snprintf(str_l1, LCD_SIZE, "%02x - %02x", tempmic, LEVELMIC);
}*/
int nb_s = 0;
while (1) {
timer_handle();
//Check switches
if (TactSWModuleCtrl(BUTTON_SLOT, BUTTON_STARTSTOP)) {
while (TactSWModuleCtrl(BUTTON_SLOT, BUTTON_STARTSTOP));
if (!enable) {
enable = 1;
} else {
enable = 0;
}
}
if (enable) {
if (TactSWModuleCtrl(BUTTON_SLOT, BUTTON_MODESEL)) {
while (TactSWModuleCtrl(BUTTON_SLOT, BUTTON_MODESEL));
mode++;
mode %= 3;
}
}
if (TactSWModuleCtrl(BUTTON_SLOT, BUTTON_VAL)) {
while (TactSWModuleCtrl(BUTTON_SLOT, BUTTON_VAL));
val_buttons++;
val_buttons %= MAX_SPEED;
}
if (enable) {
switch (mode) {
case MODE_LINE:
FollowLine();
break;
case MODE_SUMO:
StayInside();
break;
case MODE_CARRE:
DoCarre();
break;
}
} else {
//Stop
AVANCE(0);
}
//Test the timer
if (timer_complete(TIMER_TEMP)) {
timer_start(TIMER_TEMP, 500);
nb_s ++;
//snprintf(str_l1, LCD_SIZE, "%d", nb_s);
}
if (timer_complete(TIMER_LCD)) {
if (!enable) {
snprintf(str_l1, LCD_SIZE, "off");
} else {
char mode_str[20];
switch (mode) {
case MODE_LINE: snprintf(mode_str, LCD_SIZE, "line"); break;
case MODE_SUMO: snprintf(mode_str, LCD_SIZE, "sumo"); break;
case MODE_CARRE: snprintf(mode_str, LCD_SIZE, "carr"); break;
default: snprintf(mode_str, LCD_SIZE, "????"); break;
}
snprintf(str_l1, LCD_SIZE, "%s %02d ", mode_str, val_buttons);
}
timer_start(TIMER_LCD, TIMER_LCD_DELAY);
lcd_str_out(1,1, str_l1);
lcd_str_out(2,1, str_l2);
}
}
return 0;
}
void RedAuto1(void)
{
int Launch = 0;
Transmission(1);
BallCounter = 4;
ShootingMode = 1;
IntakeShoot(6000);//SHOOT 4 PRELOADS // * FAILSAFE POINT
ShootingMode = 4;
stage = 1;
TurnDegree(-10,40,0); // DO WE NEED FAILSAFE HERE?
delay(200);
Move(2,127);
delay(500);
clearTimer(T3);
while(SensorValue[IR1] == 1) //*FAILSAFE ?WHAT IF WE DONT SEE ANY BALLS
{
IntakeOnAuto();
Move(2,20);
if(time1[T3] > 8000)
{
FailSafeEngaged = 1;
break;
}
}
if(FailSafeEngaged == 1)Red1Fail1();/////////////////////RED1FAIL1
IntakePower(0);
Break(100,10);
while(MoveDist(-1, 20,0) == 1) // DO WE NEED FAILSAFE HERE?
{
IntakeOnAuto();
}
while(MoveDist(40, 50,0) == 1) //*FAILSAFE ?WHAT IF WE ARE BLOCKED (ROBOT FINISHED ABOUT MID COURT)
{
if(FailSafeEngaged == 1)Red1Fail1();/////////////////////RED1FAIL1
IntakeOnAuto();
}
IntakePower(0);
TurnDegree(-45,127,2000); //*FAILSAFE ?WHAT IF WE ARE BLOCKED
if(FailSafeEngaged == 1)Red1FailInfinite();
delay(200);
SetRPM(2250);//LAUNCHER ON (NEXT SHOOTING RPM)
clearTimer(T3);
while(LineStatus() == 0) //*FAILSAFE AT WHAT POINT DO WE STOP MOVING? WHAT IF WE ARE OF COURSE?
{
Move(-2,30);
if(time1[T3] > 3500)
{
FailSafeEngaged = 1;
break;
}
}
if(FailSafeEngaged == 1)Red1FailInfinite();
Break(100,-20);
delay(200);
while(MoveDist(7, 30,0) == 1){} // DO WE NEED FAILSAFE HERE?
clearTimer(T3);
while(LineStatus() == 0) // TURN UNTILL IT SEES LINE (WHAT IF WE NEVER SEE LINE)
{
Move(1,22);
if(time1[T3] > 3000)
{
FailSafeEngaged = 1;
break;
}
}
if(FailSafeEngaged == 1)Red1FailInfinite();
while(LineStatus() == 1 && LineStatusOffset() == 0) //*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE)
{
FollowLine(40);
if(LineStatus() == 1 && LineStatusOffset() == 1) Launch = 1;//WE REACHED THE CROSS(MIDDLE OF FIELD SET FLAG
}
if(LineStatus() == 1 && LineStatusOffset() == 1) Launch = 1;
Break(100,20);
StopDrive();
delay(300);
if(Launch) //IF LAUNCH FLAG WAS SATISFIED EARLIER THEN WE CAN LAUNCH BALLS
{
IntakePower(127); // SHOOT MID COURT
delay(3000);
}
stage = 1;
ShootingMode = 3;
DistanceCalculator(1);
while(SensorValue[IR1] == 1 && LineStatus() == 1 && DistanceCalculator(0) < 55) //*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE)
{
FollowLine(35);
IntakeOnAuto();
}
Break(100,20);
StopDrive();
while(stage == 1) // WHAT IF TWO BALLS ARE STUCK FOREVER? TIMER?
{
IntakeOnAuto();
}
while(LineStatus() == 1 && DistanceCalculator(0) < 55)//*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE)
{
FollowLine(35);
IntakeOnAuto();
}
MoveTime(300,127);
StopDrive();
IntakePower(127);///SHOT LAST BALLS BAR SHOTS
delay(4000);
stage = 1;
///////////
KillALL();
while(1);
}
