task main(){
nMotorEncoder[port2] = 0;
nMotorEncoder[port3] = 0;
//startTask(killswitch);
TurnRobot(90,100,dia,trc,fix);
MoveDist(100,100,dia);
TurnRobot(90,100,dia,trc,fix);
MoveDist(100,100,dia);
TurnRobot(90,100,dia,trc,fix);
MoveDist(100,100,dia);
TurnRobot(90,100,dia,trc,fix);
MoveDist(100,100,dia);
}
task main(){
MoveDist(105,80,dia);
motor[clawMotor] = -100;
wait1Msec(300);
motor[clawMotor] = -10;
motor[armMotor] = 100;
wait1Msec(1000);
motor[armMotor] = 10;
MoveDist(-20,80,dia);
TurnRobot(90,100,dia,trc,leftMotor,rightMotor);
MoveDist(68,50,dia);
TurnRobot(85,130,dia,trc,leftMotor,rightMotor);
MoveDist(100,50,dia);
motor[armMotor] = -50;
wait1Msec(700);
motor[armMotor] = 10;
motor[clawMotor] = 100;
wait1Msec(500);
MoveDist(-75,80,dia);
TurnRobot(-108,90,dia,trc,leftMotor,rightMotor);
MoveDist(45,80,dia);
motor[clawMotor] = -100;
wait1Msec(500);
motor[clawMotor] = -20;
motor[armMotor] = 100;
wait1Msec(700);
motor[armMotor] = 10;
MoveDist(-50,80,dia);
TurnRobot(90,100,dia,trc,leftMotor,rightMotor);
MoveDist(74,100,dia);
motor[armMotor] = -50;
wait1Msec(700);
motor[armMotor] = 10;
motor[clawMotor] = 100;
wait1Msec(500);
motor[clawMotor] = 0;
MoveDist(-20,100,dia);
}
/*----------------------------------------------------------------------------*/
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);
}
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);
}
//メソッド：void 競技走行する（）
void ExtraStageLookUp::ExtraRun()
{
/* 停止部分 */
int stopTime = m_pDeviceInterface->m_pCClock->now();
DV dv_now;
while(1){
dv_now = m_pDeviceValueGet->DeviceValueGetter();
if(m_pDeviceInterface->m_pCClock->now() - stopTime < 1500){
dv_now = m_pRunningCalculation->RunningCalculate(dv_now, 0);
m_pMotorDrive->TailMotorDrive(3);
ev3_lcd_draw_string("EXrun section0start", 0, 70);
}
else if(m_pDeviceInterface->m_pCClock->now() - stopTime < 2000){
dv_now.GYRO_OFFSET = -10;
dv_now = m_pRunningCalculation->RunningCalculate(dv_now, 0);
m_pMotorDrive->TailMotorDrive(80);
ev3_lcd_draw_string("EXrun section0 < 3500", 0, 80);
}
else{
break;
}
m_pMotorDrive->LRMotorDrive(dv_now.Lmotor_pwm, dv_now.Rmotor_pwm);
m_pDeviceInterface->m_pCClock->sleep(3); /* 4msec周期起動 */
}
ev3_lcd_draw_string("EXrun fin", 0, 90);
//
float fTailAngle = m_fTailAngleStand; // しっぽ立ち上がり
int nLmotor = 20;
int nRmotor = 20;
// int16_t nDistance = 9999;
m_uStartTime = m_pDeviceInterface->m_pCClock->now(); // 開始時間
//ログ出力
char* cLogBuff = m_pUIGet->GetBlueT()->pcLogBuff;
sprintf(cLogBuff,"ExtraStageLookUp::ExtraRun go 1: %lu,%d,%d,sizeof(int)=%d\n", GetElapsedTime(),
nLmotor,
nRmotor,
sizeof(int)
);
m_pUIGet->WriteLog(cLogBuff);
//BBBBBBBBBBB
// テストデータ取得
// Spin( 'N', 3600, fTailAngle );
//
// PauseEt( 3000, fTailAngle); // ポーズ
//
// // 5000mm 進む
// MoveDist( 'N', 5000, fTailAngle, nLmotor, nRmotor );
// PauseEt(3000, fTailAngle); // ポーズ
//BBBBBBBBBBB
// PauseEt(500, fTailAngle); // ポーズ
// // ０ しっぽを出しながら止まる
// MoveDist( 'N', 100, fTailAngle, 5 );
// ① →
//ゲートまで近づく
// GoGate('R', fTailAngle);
GoGate('N', fTailAngle);
PauseEt(500, fTailAngle); // ポーズ
// ② →
//走行体をリンボー
fTailAngle = Limbo( 'N', fTailAngle);
PauseEt(500, fTailAngle); // ポーズ
// ゲートを潜る為、230mm 進む
MoveDist( 'N', 230, fTailAngle, 10 );
PauseEt(500, fTailAngle); // ポーズ
// ③ Ｕ
// Ｕターン 180度回転
Spin(185, fTailAngle);
PauseEt(500, fTailAngle); // ポーズ
// 戻り
// ③ ←
// ゲートを潜る、330mm 戻る
MoveDist( 'N', 330, fTailAngle, 10 );
PauseEt(500, fTailAngle); // ポーズ
// ④ Ｕ
// 再度Ｕターン -180度回転
Spin(-185, fTailAngle);
PauseEt(500, fTailAngle); // ポーズ
// ④ →
// ゲートを潜る、360mm 進む
MoveDist( 'N', 360, fTailAngle, 20 );
// ⑤ →
// トレースを可能とする為に上体を起こす
fTailAngle = StandUpTail(fTailAngle); // しっぽを立てる
PauseEt(2222, fTailAngle); // ポーズ
PauseEt(2222, fTailAngle); // ポーズ
//
// // トレースを確実に行う為に故意に右側にはみ出す
// Spin(-10, fTailAngle); // -10度回転
// PauseEt(500, fTailAngle); // ポーズ
// MoveDist( 'N', 40, fTailAngle, 10 ); // 3cm進む
// PauseEt(500, fTailAngle); // ポーズ
// ⑥ →
// ガレージまで走る！！
// 距離 m_nFinalRunDist // ■■＜最終調整が必要＞
// トレース走行
// MoveDist( 'R', m_nFinalRunDist, fTailAngle, 10 );
MoveDist( 'N', m_nFinalRunDist, fTailAngle, 10 );
MoveDist( 'N', 50, fTailAngle, 5 ); // 急停止すると倒れる場合があるので減速走行
// ゲートイン
fTailAngle = StandUpTailFine(fTailAngle); // しっぽを思い切り立てる
PauseEt(4000, fTailAngle); // ポーズ
// ゲートに届かなかった場合を考慮して、微速前進とポーズを繰り返す
// ※ゲートイン判定が出た後は、ゲートにぶつかってもお構いなしなはず
for( int i=0 ; i< 5 ; i++ ){
MoveDist( 'N', 30, fTailAngle, 5 ); // ゲートまで再度トライ
PauseEt(4000, fTailAngle); // ポーズ
}
MoveDist( 'N', 30, fTailAngle, 5 ); // ゲートまで再度トライ
// Ending();
PauseEt(5000, fTailAngle); // ポーズ
}
