2015_robocon_bteam
/
2015robot_main_zenkoku
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Diff: autoMode.h
- Revision:
- 0:b613dc16f27d
- Child:
- 2:738b28f6a04b
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/autoMode.h Wed Oct 28 09:03:19 2015 +0000
@@ -0,0 +1,359 @@
+#ifndef AUTOMODE_H
+#define AUTOMODE_H
+
+/***PID Controller***/
+//PID velocity_controller(36.0,5274.0 ,0.0,RATE);
+//PID direction_controller(36.0,3.0,0.0,RATE);
+PID velocity_controller(9.0,5274.0,0.0,RATE);
+PID direction_controller(36.0,3.0,0.0,RATE);
+
+Timeout OpStart;
+
+void OpponentsStart(){
+ spcount = 0.0;
+ step = 15;
+ CStep = 15;
+ flaga = 1;
+ flagf = 1;
+}
+
+#ifdef IM920
+/***IM920 correspondence***/
+void autoIM920() {
+ if(b==7){ /*mode change*/
+ if(edge7) {
+ edge7=0;
+ autoflag=0;
+ Indicator4=0;
+ IndicatorAuto=1;
+ flaga=0;
+ Move_l(0.0);
+ Move_r(0.0);
+ }
+ }
+ else if((b==6)&&(!flaga)){ /*start*/
+ if(edge6){
+ edge6=0;
+ resetState();
+ flagf=1;
+ spcount=0.0;
+// targ_velocity=speed;
+#ifdef BLUE
+// sendData(5,69); //right
+// sendData(5,70); //right
+ sendData(5,71); //right
+ wait(0.05);
+ sendData(4,69); //left
+ stateR = 69;
+ stateL = 69;
+#else
+ sendData(5,69); //right
+ wait(0.05);
+ sendData(4,69); //left
+ stateR = 69;
+ stateL = 69;
+#endif
+ wait(0.05);
+ step = 0;
+ CStep = 1;
+ flaga = 1;
+ }
+ }
+ else if((b==5)&&(!flaga)){ /*middle start*/
+ if(edge5){
+ edge5=0;
+ resetState();
+ flagf = 1;
+ spcount=0.0;
+// targ_velocity=speed;
+#ifdef BLUE
+ sendData(5,60); //right
+ wait(0.05);
+ sendData(4,60); //left
+ stateR = 60;
+ stateL = 61;
+#else
+ sendData(5,59); //right
+ wait(0.05);
+ sendData(4,59); //left
+ stateR = 59;
+ stateL = 59;
+#endif
+ wait(0.05);
+ step = 5;
+ CStep = 7;
+ flaga = 1;
+ }
+ }
+ else if((b==8)&&(!flaga)){ /*opponents start*/
+ if(edge8){
+ edge8=0;
+ resetState();
+// targ_velocity = speed;
+#ifdef BLUE
+ sendData(5,69); //right
+ wait(0.05);
+ sendData(4,69); //left
+ stateR = 71;
+ stateL = 71;
+#else
+ /*sendData(5,66); //right
+ wait(0.05);
+ sendData(4,66); //left*/
+ /*sendData(4,71); //left
+ wait(0.05);
+ sendData(5,71); //right*/
+ sendData(4,73); //left
+ wait(0.05);
+ sendData(5,73); //right
+
+ stateR = 71;
+ stateL = 72;
+#endif
+ wait(0.05);
+ step=114;
+ CStep=114;
+ OpStart.attach(&OpponentsStart,2.2);
+// OpStart.attach(&OpponentsStart,2.0);
+ }
+ }
+ else if(b==1){ /*L down*/
+ if(edge1){
+ edge1=0;
+ if(stateL!=1) stateL--;
+ sendData(4,stateL);
+ }
+ }
+ else if(b==2){ /*L up*/
+ if(edge2){
+ edge2=0;
+ if(stateL!=MAX_VALUE) stateL++;
+ sendData(4,stateL);
+ }
+ }
+ else if(b==3){ /*R down*/
+ if(edge3){
+ edge3=0;
+ if(stateR!=1) stateR--;
+ sendData(5,stateR);
+ }
+ }
+ else if(b==4){ /*R up*/
+ if(edge4){
+ edge4=0;
+ if(stateR!=MAX_VALUE) stateR++;
+ sendData(5,stateR);
+ }
+ }
+ if(a2){
+ skip = 1;
+ }
+ else if(!a2){
+ skip = 0;
+ }
+ if(b!=1) edge1=1;
+ if(b!=2) edge2=1;
+ if(b!=3) edge3=1;
+ if(b!=4) edge4=1;
+ if(b!=5) edge5=1;
+ if(b!=6) edge6=1;
+ if(b!=7) edge7=1;
+ if(b!=8) edge8=1;
+ if(b!=9) edge9=1;
+}
+#else
+void autoPS3(){
+ if(circle){ /*mode change*/
+ if(edge_circle){
+ edge_circle=0;
+ autoflag=0;
+ Indicator4=0;
+ IndicatorAuto=1;
+ flaga=0;
+ Move_l(0.0);
+ Move_r(0.0);
+ }
+ }
+ else if(triangle){ /*start*/
+ if(edge_triangle){
+ edge_triangle=0;
+ resetState();
+ flagf=1;
+ spcount=0.0;
+// targ_velocity=speed;
+#ifdef BLUE
+// sendData(5,69); //right
+ sendData(5,71); //right
+ wait(0.05);
+ sendData(4,69); //left
+ stateR = 69;
+ stateL = 69;
+#else
+ sendData(5,69); //right
+ wait(0.05);
+ sendData(4,69); //left
+ stateR = 69;
+ stateL = 69;
+#endif
+ wait(0.05);
+ step = 0;
+ CStep = 1;
+ flaga = 1;
+ }
+ }
+ else if(square){ /*middle start*/
+ if(edge_square){
+ edge_square=0;
+ resetState();
+ flagf = 1;
+ spcount=0.0;
+// targ_velocity=speed;
+#ifdef BLUE
+ sendData(5,60); //right
+ wait(0.05);
+ sendData(4,60); //left
+ stateR = 60;
+ stateL = 61;
+#else
+ sendData(5,59); //right
+ wait(0.05);
+ sendData(4,59); //left
+ stateR = 61;
+ stateL = 60;
+#endif
+ wait(0.05);
+ step = 5;
+ CStep = 7;
+ flaga = 1;
+ }
+ }
+ else if(cross){ /*opponents start*/
+ if(edge_cross){
+ edge_cross=0;
+ resetState();
+// targ_velocity = speed;
+#ifdef BLUE
+ sendData(5,69); //right
+ wait(0.05);
+ sendData(4,69); //left
+ stateR = 71;
+ stateL = 71;
+#else
+ /*sendData(5,66); //right
+ wait(0.05);
+ sendData(4,66); //left*/
+ /*sendData(4,71); //left
+ wait(0.05);
+ sendData(5,71); //right*/
+ sendData(4,73); //left
+ wait(0.05);
+ sendData(5,73); //right
+
+ stateR = 71;
+ stateL = 72;
+#endif
+ wait(0.05);
+ step=114;
+ CStep=114;
+ OpStart.attach(&OpponentsStart,2.2);
+// OpStart.attach(&OpponentsStart,2.0);
+ }
+ }
+ else if(l1){ /*L up*/
+ if(edge_l1){
+ edge_l1=0;
+ if(stateL!=92) stateL++;
+ sendData(4,stateL);
+ }
+ }
+ else if(l2){ /*L down*/
+ if(edge_l2){
+ edge_l2=0;
+ if(stateL!=1) stateL--;
+ sendData(4,stateL);
+ }
+ }
+ else if(r1){ /*R up*/
+ if(edge_r1){
+ edge_r1=0;
+ if(stateR!=92) stateR++;
+ sendData(5,stateR);
+ }
+ }
+ else if(r2){ /*R down*/
+ if(edge_r2){
+ edge_r2=0;
+ if(stateR!=1) stateR--;
+ sendData(5,stateR);
+ }
+ }
+ if(up){
+ skip = 1;
+ }
+ else if(!up){
+ skip = 0;
+ }
+}
+#endif
+/***The function is PID controller initialize.***/
+inline void initializeControllers()
+{
+// velocity_controller.setInputLimits(-200000.0, 200000.0); //x1
+ velocity_controller.setInputLimits(-20000.0, 20000.0); //x1
+ direction_controller.setInputLimits(-10.0, 10.0); //x2
+
+ //Pwm output from MOVE_OUTPUT_LIMIT_BOTTOM to MOVE_OUTPUT_LIMIT_TOP
+// velocity_controller.setOutputLimits(-1.0, 1.0);
+ velocity_controller.setOutputLimits(0.0, 1.0);
+ direction_controller.setOutputLimits(-1.0, 1.0);
+
+ //set bias. 初期値
+ velocity_controller.setBias(0.0);
+ direction_controller.setBias(0.0);
+
+ //set mode.
+ velocity_controller.setMode(AUTO_MODE);
+ direction_controller.setMode(AUTO_MODE);
+}
+
+/***The function is following move speed.***/
+inline void velocity_following()
+{
+ velocity_controller.setSetPoint((float)targ_velocity);
+ velocity_controller.setProcessValue((float)velocity);
+ x1 = (double)velocity_controller.compute();
+}
+
+inline void sita_following()
+{
+ direction_controller.setSetPoint((float)targ_sita);
+ direction_controller.setProcessValue((float)sita);
+ //direction_controller.setSetPoint(0.0); /*目標値とのずれをなくす*/
+ //direction_controller.setProcessValue(-y);
+ x2 = (double)direction_controller.compute();
+}
+
+inline void move_following()
+{
+ velocity_following();
+ sita_following();
+
+ if(flagf==0){
+ Vr = ( 2.0*(-x1) + x2 ) / 3.0;
+ Vl = ( 2.0*(-x1) - x2 ) / 3.0;
+ }
+ else if(flagf==1){
+ if(x2>0){
+ Vr = x1;
+ Vl = x1 - x2;
+ }
+ else{
+ Vr = x1 + x2;
+ Vl = x1;
+ }
+ }
+ Move_r( ( float ) Vr );
+ Move_l( ( float ) Vl );
+}
+
+#endif /*autoMode.h*/