2015_robocon_bteam / Mbed 2 deprecated 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.

Dependencies:   PID QEI mbed

autoMode.h@21:bdf8ac5c200c, 2015-11-06 (annotated)

Committer:
DeguNaoto
Date:
Fri Nov 06 11:35:39 2015 +0000
Revision:
21:bdf8ac5c200c
Parent:
17:726b6f53a457
Child:
23:26f9483439fe
20151106 ??

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DeguNaoto 0:b613dc16f27d 1 #ifndef AUTOMODE_H
DeguNaoto 0:b613dc16f27d 2 #define AUTOMODE_H
DeguNaoto 0:b613dc16f27d 3
DeguNaoto 0:b613dc16f27d 4 /***PID Controller***/
DeguNaoto 0:b613dc16f27d 5 //PID velocity_controller(36.0,5274.0 ,0.0,RATE);
DeguNaoto 0:b613dc16f27d 6 //PID direction_controller(36.0,3.0,0.0,RATE);
DeguNaoto 0:b613dc16f27d 7 PID velocity_controller(9.0,5274.0,0.0,RATE);
DeguNaoto 0:b613dc16f27d 8 PID direction_controller(36.0,3.0,0.0,RATE);
DeguNaoto 0:b613dc16f27d 9
DeguNaoto 0:b613dc16f27d 10 Timeout OpStart;
DeguNaoto 0:b613dc16f27d 11
DeguNaoto 0:b613dc16f27d 12 void OpponentsStart(){
DeguNaoto 0:b613dc16f27d 13 spcount = 0.0;
DeguNaoto 0:b613dc16f27d 14 step = 15;
DeguNaoto 0:b613dc16f27d 15 CStep = 15;
DeguNaoto 0:b613dc16f27d 16 flaga = 1;
DeguNaoto 0:b613dc16f27d 17 flagf = 1;
DeguNaoto 0:b613dc16f27d 18 }
DeguNaoto 0:b613dc16f27d 19
DeguNaoto 0:b613dc16f27d 20 /***IM920 correspondence***/
DeguNaoto 0:b613dc16f27d 21 void autoIM920() {
DeguNaoto 0:b613dc16f27d 22 if(b==7){ /*mode change*/
DeguNaoto 0:b613dc16f27d 23 if(edge7) {
DeguNaoto 3:8d8c25c556ae 24 Com.detach();
DeguNaoto 0:b613dc16f27d 25 edge7=0;
DeguNaoto 0:b613dc16f27d 26 autoflag=0;
DeguNaoto 0:b613dc16f27d 27 Indicator4=0;
DeguNaoto 0:b613dc16f27d 28 IndicatorAuto=1;
DeguNaoto 6:ca3a74a93ae2 29 direction_controller.setTunings(12.0,9.0,0.0);
DeguNaoto 0:b613dc16f27d 30 flaga=0;
DeguNaoto 0:b613dc16f27d 31 Move_l(0.0);
DeguNaoto 0:b613dc16f27d 32 Move_r(0.0);
DeguNaoto 17:726b6f53a457 33 wait(0.1);
DeguNaoto 0:b613dc16f27d 34 }
DeguNaoto 0:b613dc16f27d 35 }
DeguNaoto 0:b613dc16f27d 36 else if((b==6)&&(!flaga)){ /*start*/
DeguNaoto 0:b613dc16f27d 37 if(edge6){
DeguNaoto 0:b613dc16f27d 38 edge6=0;
DeguNaoto 0:b613dc16f27d 39 resetState();
DeguNaoto 0:b613dc16f27d 40 flagf=1;
DeguNaoto 0:b613dc16f27d 41 spcount=0.0;
DeguNaoto 2:738b28f6a04b 42 direction_controller.setBias(0.0);
DeguNaoto 0:b613dc16f27d 43 // targ_velocity=speed;
DeguNaoto 0:b613dc16f27d 44 #ifdef BLUE
DeguNaoto 17:726b6f53a457 45 sendData(5,69); //right
DeguNaoto 0:b613dc16f27d 46 wait(0.05);
DeguNaoto 0:b613dc16f27d 47 sendData(4,69); //left
DeguNaoto 17:726b6f53a457 48 wait(0.05);
DeguNaoto 17:726b6f53a457 49 sendData(6,59); //middle
DeguNaoto 0:b613dc16f27d 50 stateR = 69;
DeguNaoto 0:b613dc16f27d 51 stateL = 69;
DeguNaoto 0:b613dc16f27d 52 #else
DeguNaoto 0:b613dc16f27d 53 sendData(5,69); //right
DeguNaoto 0:b613dc16f27d 54 wait(0.05);
DeguNaoto 0:b613dc16f27d 55 sendData(4,69); //left
DeguNaoto 17:726b6f53a457 56 wait(0.05);
DeguNaoto 17:726b6f53a457 57 sendData(6,59); //middle
DeguNaoto 0:b613dc16f27d 58 stateR = 69;
DeguNaoto 0:b613dc16f27d 59 stateL = 69;
DeguNaoto 0:b613dc16f27d 60 #endif
DeguNaoto 0:b613dc16f27d 61 wait(0.05);
DeguNaoto 0:b613dc16f27d 62 step = 0;
DeguNaoto 0:b613dc16f27d 63 CStep = 1;
DeguNaoto 0:b613dc16f27d 64 flaga = 1;
DeguNaoto 0:b613dc16f27d 65 }
DeguNaoto 0:b613dc16f27d 66 }
DeguNaoto 0:b613dc16f27d 67 else if((b==5)&&(!flaga)){ /*middle start*/
DeguNaoto 0:b613dc16f27d 68 if(edge5){
DeguNaoto 0:b613dc16f27d 69 edge5=0;
DeguNaoto 0:b613dc16f27d 70 resetState();
DeguNaoto 0:b613dc16f27d 71 flagf = 1;
DeguNaoto 0:b613dc16f27d 72 spcount=0.0;
DeguNaoto 2:738b28f6a04b 73 direction_controller.setBias(0.0);
DeguNaoto 0:b613dc16f27d 74 // targ_velocity=speed;
DeguNaoto 0:b613dc16f27d 75 #ifdef BLUE
DeguNaoto 0:b613dc16f27d 76 sendData(5,60); //right
DeguNaoto 0:b613dc16f27d 77 wait(0.05);
DeguNaoto 0:b613dc16f27d 78 sendData(4,60); //left
DeguNaoto 17:726b6f53a457 79 wait(0.05);
DeguNaoto 17:726b6f53a457 80 sendData(6,49); //middle
DeguNaoto 0:b613dc16f27d 81 stateR = 60;
DeguNaoto 0:b613dc16f27d 82 stateL = 61;
DeguNaoto 0:b613dc16f27d 83 #else
DeguNaoto 0:b613dc16f27d 84 sendData(5,59); //right
DeguNaoto 0:b613dc16f27d 85 wait(0.05);
DeguNaoto 0:b613dc16f27d 86 sendData(4,59); //left
DeguNaoto 17:726b6f53a457 87 wait(0.05);
DeguNaoto 17:726b6f53a457 88 sendData(6,49); //middle
DeguNaoto 0:b613dc16f27d 89 stateR = 59;
DeguNaoto 0:b613dc16f27d 90 stateL = 59;
DeguNaoto 0:b613dc16f27d 91 #endif
DeguNaoto 0:b613dc16f27d 92 wait(0.05);
DeguNaoto 0:b613dc16f27d 93 step = 5;
DeguNaoto 0:b613dc16f27d 94 CStep = 7;
DeguNaoto 0:b613dc16f27d 95 flaga = 1;
DeguNaoto 0:b613dc16f27d 96 }
DeguNaoto 0:b613dc16f27d 97 }
DeguNaoto 0:b613dc16f27d 98 else if((b==8)&&(!flaga)){ /*opponents start*/
DeguNaoto 0:b613dc16f27d 99 if(edge8){
DeguNaoto 0:b613dc16f27d 100 edge8=0;
DeguNaoto 0:b613dc16f27d 101 resetState();
DeguNaoto 2:738b28f6a04b 102 direction_controller.setBias(0.0);
DeguNaoto 0:b613dc16f27d 103 // targ_velocity = speed;
DeguNaoto 0:b613dc16f27d 104 #ifdef BLUE
DeguNaoto 0:b613dc16f27d 105 sendData(5,69); //right
DeguNaoto 0:b613dc16f27d 106 wait(0.05);
DeguNaoto 0:b613dc16f27d 107 sendData(4,69); //left
DeguNaoto 17:726b6f53a457 108 wait(0.05);
DeguNaoto 17:726b6f53a457 109 sendData(6,69); //middle
DeguNaoto 0:b613dc16f27d 110 stateR = 71;
DeguNaoto 0:b613dc16f27d 111 stateL = 71;
DeguNaoto 0:b613dc16f27d 112 #else
DeguNaoto 0:b613dc16f27d 113 sendData(4,73); //left
DeguNaoto 0:b613dc16f27d 114 wait(0.05);
DeguNaoto 0:b613dc16f27d 115 sendData(5,73); //right
DeguNaoto 17:726b6f53a457 116 wait(0.05);
DeguNaoto 17:726b6f53a457 117 sendData(6,69); //middle
DeguNaoto 0:b613dc16f27d 118 stateR = 71;
DeguNaoto 0:b613dc16f27d 119 stateL = 72;
DeguNaoto 0:b613dc16f27d 120 #endif
DeguNaoto 0:b613dc16f27d 121 wait(0.05);
DeguNaoto 0:b613dc16f27d 122 step=114;
DeguNaoto 0:b613dc16f27d 123 CStep=114;
DeguNaoto 0:b613dc16f27d 124 OpStart.attach(&OpponentsStart,2.2);
DeguNaoto 0:b613dc16f27d 125 // OpStart.attach(&OpponentsStart,2.0);
DeguNaoto 0:b613dc16f27d 126 }
DeguNaoto 0:b613dc16f27d 127 }
DeguNaoto 0:b613dc16f27d 128 else if(b==1){ /*L down*/
DeguNaoto 0:b613dc16f27d 129 if(edge1){
DeguNaoto 0:b613dc16f27d 130 edge1=0;
DeguNaoto 0:b613dc16f27d 131 if(stateL!=1) stateL--;
DeguNaoto 0:b613dc16f27d 132 sendData(4,stateL);
DeguNaoto 0:b613dc16f27d 133 }
DeguNaoto 0:b613dc16f27d 134 }
DeguNaoto 0:b613dc16f27d 135 else if(b==2){ /*L up*/
DeguNaoto 0:b613dc16f27d 136 if(edge2){
DeguNaoto 0:b613dc16f27d 137 edge2=0;
DeguNaoto 0:b613dc16f27d 138 if(stateL!=MAX_VALUE) stateL++;
DeguNaoto 0:b613dc16f27d 139 sendData(4,stateL);
DeguNaoto 0:b613dc16f27d 140 }
DeguNaoto 0:b613dc16f27d 141 }
DeguNaoto 0:b613dc16f27d 142 else if(b==3){ /*R down*/
DeguNaoto 0:b613dc16f27d 143 if(edge3){
DeguNaoto 0:b613dc16f27d 144 edge3=0;
DeguNaoto 0:b613dc16f27d 145 if(stateR!=1) stateR--;
DeguNaoto 0:b613dc16f27d 146 sendData(5,stateR);
DeguNaoto 0:b613dc16f27d 147 }
DeguNaoto 0:b613dc16f27d 148 }
DeguNaoto 0:b613dc16f27d 149 else if(b==4){ /*R up*/
DeguNaoto 0:b613dc16f27d 150 if(edge4){
DeguNaoto 0:b613dc16f27d 151 edge4=0;
DeguNaoto 0:b613dc16f27d 152 if(stateR!=MAX_VALUE) stateR++;
DeguNaoto 0:b613dc16f27d 153 sendData(5,stateR);
DeguNaoto 0:b613dc16f27d 154 }
DeguNaoto 0:b613dc16f27d 155 }
DeguNaoto 21:bdf8ac5c200c 156 /*if(a2){
DeguNaoto 0:b613dc16f27d 157 skip = 1;
DeguNaoto 0:b613dc16f27d 158 }
DeguNaoto 0:b613dc16f27d 159 else if(!a2){
DeguNaoto 0:b613dc16f27d 160 skip = 0;
DeguNaoto 21:bdf8ac5c200c 161 }*/
DeguNaoto 21:bdf8ac5c200c 162 skip=0;
DeguNaoto 0:b613dc16f27d 163 if(b!=1) edge1=1;
DeguNaoto 0:b613dc16f27d 164 if(b!=2) edge2=1;
DeguNaoto 0:b613dc16f27d 165 if(b!=3) edge3=1;
DeguNaoto 0:b613dc16f27d 166 if(b!=4) edge4=1;
DeguNaoto 0:b613dc16f27d 167 if(b!=5) edge5=1;
DeguNaoto 0:b613dc16f27d 168 if(b!=6) edge6=1;
DeguNaoto 0:b613dc16f27d 169 if(b!=7) edge7=1;
DeguNaoto 0:b613dc16f27d 170 if(b!=8) edge8=1;
DeguNaoto 0:b613dc16f27d 171 if(b!=9) edge9=1;
DeguNaoto 0:b613dc16f27d 172 }
DeguNaoto 0:b613dc16f27d 173 /***The function is PID controller initialize.***/
DeguNaoto 0:b613dc16f27d 174 inline void initializeControllers()
DeguNaoto 0:b613dc16f27d 175 {
DeguNaoto 0:b613dc16f27d 176 velocity_controller.setInputLimits(-20000.0, 20000.0); //x1
DeguNaoto 6:ca3a74a93ae2 177 // velocity_controller.setInputLimits(-1500.0, 1500.0); //x1
DeguNaoto 2:738b28f6a04b 178 direction_controller.setInputLimits(-PI, PI); //x2
DeguNaoto 0:b613dc16f27d 179
DeguNaoto 0:b613dc16f27d 180 //Pwm output from MOVE_OUTPUT_LIMIT_BOTTOM to MOVE_OUTPUT_LIMIT_TOP
DeguNaoto 0:b613dc16f27d 181 velocity_controller.setOutputLimits(0.0, 1.0);
DeguNaoto 0:b613dc16f27d 182 direction_controller.setOutputLimits(-1.0, 1.0);
DeguNaoto 0:b613dc16f27d 183
DeguNaoto 0:b613dc16f27d 184 //set bias. 初期値
DeguNaoto 0:b613dc16f27d 185 velocity_controller.setBias(0.0);
DeguNaoto 0:b613dc16f27d 186 direction_controller.setBias(0.0);
DeguNaoto 0:b613dc16f27d 187
DeguNaoto 0:b613dc16f27d 188 //set mode.
DeguNaoto 0:b613dc16f27d 189 velocity_controller.setMode(AUTO_MODE);
DeguNaoto 0:b613dc16f27d 190 direction_controller.setMode(AUTO_MODE);
DeguNaoto 0:b613dc16f27d 191 }
DeguNaoto 0:b613dc16f27d 192
DeguNaoto 0:b613dc16f27d 193 /***The function is following move speed.***/
DeguNaoto 0:b613dc16f27d 194 inline void velocity_following()
DeguNaoto 0:b613dc16f27d 195 {
DeguNaoto 0:b613dc16f27d 196 velocity_controller.setSetPoint((float)targ_velocity);
DeguNaoto 0:b613dc16f27d 197 velocity_controller.setProcessValue((float)velocity);
DeguNaoto 0:b613dc16f27d 198 x1 = (double)velocity_controller.compute();
DeguNaoto 0:b613dc16f27d 199 }
DeguNaoto 0:b613dc16f27d 200
DeguNaoto 0:b613dc16f27d 201 inline void sita_following()
DeguNaoto 0:b613dc16f27d 202 {
DeguNaoto 0:b613dc16f27d 203 direction_controller.setSetPoint((float)targ_sita);
DeguNaoto 0:b613dc16f27d 204 direction_controller.setProcessValue((float)sita);
DeguNaoto 0:b613dc16f27d 205 x2 = (double)direction_controller.compute();
DeguNaoto 0:b613dc16f27d 206 }
DeguNaoto 0:b613dc16f27d 207
DeguNaoto 0:b613dc16f27d 208 inline void move_following()
DeguNaoto 0:b613dc16f27d 209 {
DeguNaoto 0:b613dc16f27d 210 velocity_following();
DeguNaoto 0:b613dc16f27d 211 sita_following();
DeguNaoto 0:b613dc16f27d 212
DeguNaoto 0:b613dc16f27d 213 if(flagf==0){
DeguNaoto 2:738b28f6a04b 214 // Vr = ( 2.0*(-x1) + x2 ) / 3.0;
DeguNaoto 2:738b28f6a04b 215 // Vl = ( 2.0*(-x1) - x2 ) / 3.0;
DeguNaoto 3:8d8c25c556ae 216 /*if(x2>0.0){
DeguNaoto 2:738b28f6a04b 217 Vr = -x1;
DeguNaoto 2:738b28f6a04b 218 Vl = -x1 - x2;
DeguNaoto 2:738b28f6a04b 219 }
DeguNaoto 2:738b28f6a04b 220 else{
DeguNaoto 2:738b28f6a04b 221 Vr = -x1 + x2;
DeguNaoto 2:738b28f6a04b 222 Vl = -x1;
DeguNaoto 3:8d8c25c556ae 223 }*/
DeguNaoto 3:8d8c25c556ae 224 if(x2>0.0){
DeguNaoto 3:8d8c25c556ae 225 Vr = -x1 + x2;
DeguNaoto 3:8d8c25c556ae 226 Vl = -x1;
DeguNaoto 3:8d8c25c556ae 227 }
DeguNaoto 3:8d8c25c556ae 228 else{
DeguNaoto 3:8d8c25c556ae 229 Vr = -x1;
DeguNaoto 3:8d8c25c556ae 230 Vl = -x1 - x2;
DeguNaoto 2:738b28f6a04b 231 }
DeguNaoto 0:b613dc16f27d 232 }
DeguNaoto 0:b613dc16f27d 233 else if(flagf==1){
DeguNaoto 2:738b28f6a04b 234 if(x2>0.0){
DeguNaoto 0:b613dc16f27d 235 Vr = x1;
DeguNaoto 0:b613dc16f27d 236 Vl = x1 - x2;
DeguNaoto 0:b613dc16f27d 237 }
DeguNaoto 0:b613dc16f27d 238 else{
DeguNaoto 0:b613dc16f27d 239 Vr = x1 + x2;
DeguNaoto 0:b613dc16f27d 240 Vl = x1;
DeguNaoto 0:b613dc16f27d 241 }
DeguNaoto 0:b613dc16f27d 242 }
DeguNaoto 0:b613dc16f27d 243 Move_r( ( float ) Vr );
DeguNaoto 0:b613dc16f27d 244 Move_l( ( float ) Vl );
DeguNaoto 0:b613dc16f27d 245 }
DeguNaoto 0:b613dc16f27d 246
DeguNaoto 0:b613dc16f27d 247 #endif /*autoMode.h*/