2016_05_19ver Auto mode 10sec forward, 2sec stop, 2sec right turn Please change test_mode's right turn ppm
Dependencies: VNH5019 AigamozuControlPackets_2016
Dependents: Aigamozu_Robot_2016_ver1 GPSLOG_program AigamozuControlPackets_2016
Fork of AigamozuControlPackets by
Diff: AigamozuControlPackets.cpp
- Revision:
- 46:246051b4f24d
- Parent:
- 45:c57a98331225
- Child:
- 47:4da5b1b048aa
- Child:
- 49:c69fcf98e6b7
diff -r c57a98331225 -r 246051b4f24d AigamozuControlPackets.cpp --- a/AigamozuControlPackets.cpp Mon Jun 06 07:23:51 2016 +0000 +++ b/AigamozuControlPackets.cpp Mon Jun 06 09:52:28 2016 +0000 @@ -235,29 +235,29 @@ if(out_flag == true){ if(Automove_Timer.read_ms() <= 5000){ - _agzSheild.changeSpeed(1,16,1,64);//turn left + _agzSheild.changeSpeed(1,32,1,124);//turn left }else if(Automove_Timer.read_ms() <= 6000){ - _agzSheild.changeSpeed(0,64,0,64); + _agzSheild.changeSpeed(0,124,0,124); }else if(Automove_Timer.read_ms() <= 10000){ - _agzSheild.changeSpeed(1,64,1,64);//straight + _agzSheild.changeSpeed(1,124,1,124);//straight }else if(Automove_Timer.read_ms() > 10000){ Move_Timer.reset(); _agzSheild.changeSpeed(0,64,0,64); out_count_flag=false; out_flag=false; - printf("reset timer¥n"); + printf("reset timer\n"); } }else{ //if robot is inner // _agzSheild.changeSpeed(1,64,1,64);//straight if(Move_Timer.read_ms() < straight){ - _agzSheild.changeSpeed(1, 64, 1, 64); //straight + _agzSheild.changeSpeed(1, 124, 1, 124); //straight } else if(Move_Timer.read_ms() < wait){ _agzSheild.changeSpeed(0, 64, 0, 64); } else if(Move_Timer.read_ms() < turning){ - _agzSheild.changeSpeed(1, 64, 1, 16); //Turn Right + _agzSheild.changeSpeed(1, 124, 1, 32); //Turn Right } else if(Move_Timer.read_ms() > turning){ _agzSheild.changeSpeed(0, 64, 0, 64); @@ -326,7 +326,7 @@ // vector2D AP = AigamozuControlPackets::sub_vector(P, A); double c1 = AB.x * BP.y - AB.y * BP.x; - double c2 = BC.x * BP.y - BC.y * BP.x; + double c2 = BP.x * BC.y - BP.y * BC.x; if( ( c1 > 0 && c2 > 0) || ( c1 < 0 && c2 < 0) ) { return true;