2015_robocon_bteam / Mbed 2 deprecated 2015robot_main

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

Fork of 2015robot_main by Naoto Deguchi

autoMode.h@106:20dab118bd5b, 2015-10-19 (annotated)

Committer:
DeguNaoto
Date:
Mon Oct 19 07:21:30 2015 +0000
Revision:
106:20dab118bd5b
Parent:
105:0be5d5d64782
Child:
108:7eb434cfcbd7
20151019 ?????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DeguNaoto 21:79b94cb922f0 1 #ifndef AUTOMODE_H
DeguNaoto 21:79b94cb922f0 2 #define AUTOMODE_H
DeguNaoto 21:79b94cb922f0 3
DeguNaoto 21:79b94cb922f0 4 /***PID Controller***/
DeguNaoto 66:14df82661dfa 5 //PID velocity_controller(36.0,5274.0 ,0.0,RATE);
DeguNaoto 66:14df82661dfa 6 //PID direction_controller(36.0,3.0,0.0,RATE);
DeguNaoto 83:e1638c58e1f1 7 PID velocity_controller(9.0,5274.0,0.0,RATE);
DeguNaoto 34:f9ef622f4376 8 PID direction_controller(36.0,3.0,0.0,RATE);
DeguNaoto 21:79b94cb922f0 9
DeguNaoto 96:a972ba5cf2d7 10 Timeout OpStart;
DeguNaoto 96:a972ba5cf2d7 11
DeguNaoto 96:a972ba5cf2d7 12 void OpponentsStart(){
DeguNaoto 101:b67d33e56b66 13 spcount = 0.0;
DeguNaoto 101:b67d33e56b66 14 step = 15;
DeguNaoto 101:b67d33e56b66 15 CStep = 15;
DeguNaoto 101:b67d33e56b66 16 flaga = 1;
DeguNaoto 101:b67d33e56b66 17 flagf = 1;
DeguNaoto 96:a972ba5cf2d7 18 }
DeguNaoto 96:a972ba5cf2d7 19
DeguNaoto 57:3fbd487e055e 20 #ifdef IM920
DeguNaoto 52:d9e1629da852 21 /***IM920 correspondence***/
DeguNaoto 52:d9e1629da852 22 void autoIM920() {
DeguNaoto 52:d9e1629da852 23 if(b==7){ /*mode change*/
unicore32 80:7e24b99b9b61 24 if(edge7) {
DeguNaoto 52:d9e1629da852 25 edge7=0;
DeguNaoto 52:d9e1629da852 26 autoflag=0;
DeguNaoto 52:d9e1629da852 27 Indicator4=0;
DeguNaoto 52:d9e1629da852 28 IndicatorAuto=1;
DeguNaoto 86:5f0b065cb4d3 29 flaga=0;
DeguNaoto 101:b67d33e56b66 30 Move_l(0.0);
DeguNaoto 101:b67d33e56b66 31 Move_r(0.0);
DeguNaoto 52:d9e1629da852 32 }
DeguNaoto 52:d9e1629da852 33 }
DeguNaoto 89:048a72382b0a 34 else if((b==6)&&(!flaga)){ /*start*/
DeguNaoto 83:e1638c58e1f1 35 if(edge6){
DeguNaoto 83:e1638c58e1f1 36 edge6=0;
DeguNaoto 83:e1638c58e1f1 37 resetState();
DeguNaoto 83:e1638c58e1f1 38 flagf=1;
DeguNaoto 89:048a72382b0a 39 spcount=0.0;
DeguNaoto 89:048a72382b0a 40 // targ_velocity=speed;
DeguNaoto 85:dd18a2d79956 41 // sendData(7,70);
DeguNaoto 103:ffd3ca4a7a71 42 sendData(5,69); //right
DeguNaoto 89:048a72382b0a 43 wait(0.05);
DeguNaoto 103:ffd3ca4a7a71 44 sendData(4,69); //left
DeguNaoto 89:048a72382b0a 45 wait(0.05);
DeguNaoto 85:dd18a2d79956 46 stateR = 69;
DeguNaoto 85:dd18a2d79956 47 stateL = 69;
DeguNaoto 83:e1638c58e1f1 48 step = 0;
DeguNaoto 83:e1638c58e1f1 49 CStep = 1;
DeguNaoto 86:5f0b065cb4d3 50 flaga = 1;
DeguNaoto 83:e1638c58e1f1 51 }
DeguNaoto 52:d9e1629da852 52 }
DeguNaoto 89:048a72382b0a 53 else if((b==5)&&(!flaga)){ /*middle start*/
DeguNaoto 83:e1638c58e1f1 54 if(edge5){
DeguNaoto 83:e1638c58e1f1 55 edge5=0;
DeguNaoto 83:e1638c58e1f1 56 resetState();
DeguNaoto 83:e1638c58e1f1 57 flagf = 1;
DeguNaoto 89:048a72382b0a 58 spcount=0.0;
DeguNaoto 89:048a72382b0a 59 // targ_velocity=speed;
DeguNaoto 89:048a72382b0a 60 // sendData(7,59);
DeguNaoto 103:ffd3ca4a7a71 61 sendData(5,60); //right
DeguNaoto 89:048a72382b0a 62 wait(0.05);
DeguNaoto 89:048a72382b0a 63 sendData(4,61); //left
DeguNaoto 89:048a72382b0a 64 wait(0.05);
DeguNaoto 88:cbed08732872 65 stateR = 59;
DeguNaoto 88:cbed08732872 66 stateL = 59;
DeguNaoto 83:e1638c58e1f1 67 step = 5;
DeguNaoto 101:b67d33e56b66 68 CStep = 7;
DeguNaoto 87:6b6b6611d33f 69 flaga = 1;
DeguNaoto 83:e1638c58e1f1 70 }
DeguNaoto 61:e018207ae860 71 }
DeguNaoto 89:048a72382b0a 72 else if((b==8)&&(!flaga)){ /*opponents start*/
DeguNaoto 83:e1638c58e1f1 73 if(edge8){
DeguNaoto 83:e1638c58e1f1 74 edge8=0;
DeguNaoto 83:e1638c58e1f1 75 resetState();
DeguNaoto 89:048a72382b0a 76 // targ_velocity = speed;
DeguNaoto 105:0be5d5d64782 77 #ifdef BLUE
DeguNaoto 103:ffd3ca4a7a71 78 sendData(5,71); //right
DeguNaoto 89:048a72382b0a 79 wait(0.05);
DeguNaoto 103:ffd3ca4a7a71 80 sendData(4,71); //left
DeguNaoto 105:0be5d5d64782 81 #else
DeguNaoto 106:20dab118bd5b 82 sendData(5,71); //right
DeguNaoto 105:0be5d5d64782 83 wait(0.05);
DeguNaoto 106:20dab118bd5b 84 sendData(4,72); //left
DeguNaoto 106:20dab118bd5b 85 // sendData(4,71); //left
DeguNaoto 105:0be5d5d64782 86 #endif
DeguNaoto 89:048a72382b0a 87 wait(0.05);
DeguNaoto 88:cbed08732872 88 stateR = 70;
DeguNaoto 88:cbed08732872 89 stateL = 70;
DeguNaoto 97:5e6f513b26f2 90 step=114;
DeguNaoto 97:5e6f513b26f2 91 CStep=114;
DeguNaoto 105:0be5d5d64782 92 OpStart.attach(&OpponentsStart,2.2);
DeguNaoto 105:0be5d5d64782 93 // OpStart.attach(&OpponentsStart,2.0);
DeguNaoto 96:a972ba5cf2d7 94 // spcount=0.0;
DeguNaoto 96:a972ba5cf2d7 95 // step = 15;
DeguNaoto 96:a972ba5cf2d7 96 // CStep = 15;
DeguNaoto 96:a972ba5cf2d7 97 // flaga = 1;
DeguNaoto 101:b67d33e56b66 98 // flagf = 1;
DeguNaoto 83:e1638c58e1f1 99 }
DeguNaoto 52:d9e1629da852 100 }
DeguNaoto 90:9fe39ebe721f 101 else if(b==9){ /******/
DeguNaoto 52:d9e1629da852 102 }
DeguNaoto 52:d9e1629da852 103 else if(b==1){ /*L down*/
DeguNaoto 52:d9e1629da852 104 if(edge1){
DeguNaoto 52:d9e1629da852 105 edge1=0;
DeguNaoto 52:d9e1629da852 106 if(stateL!=1) stateL--;
DeguNaoto 57:3fbd487e055e 107 sendData(4,stateL);
DeguNaoto 52:d9e1629da852 108 }
DeguNaoto 52:d9e1629da852 109 }
DeguNaoto 52:d9e1629da852 110 else if(b==2){ /*L up*/
DeguNaoto 52:d9e1629da852 111 if(edge2){
DeguNaoto 52:d9e1629da852 112 edge2=0;
unicore32 69:bf094811b4a9 113 if(stateL!=MAX_VALUE) stateL++;
DeguNaoto 57:3fbd487e055e 114 sendData(4,stateL);
DeguNaoto 22:3996c3f41922 115 }
DeguNaoto 52:d9e1629da852 116 }
DeguNaoto 52:d9e1629da852 117 else if(b==3){ /*R down*/
DeguNaoto 52:d9e1629da852 118 if(edge3){
DeguNaoto 52:d9e1629da852 119 edge3=0;
DeguNaoto 52:d9e1629da852 120 if(stateR!=1) stateR--;
DeguNaoto 57:3fbd487e055e 121 sendData(5,stateR);
DeguNaoto 52:d9e1629da852 122 }
DeguNaoto 52:d9e1629da852 123 }
DeguNaoto 52:d9e1629da852 124 else if(b==4){ /*R up*/
DeguNaoto 52:d9e1629da852 125 if(edge4){
DeguNaoto 52:d9e1629da852 126 edge4=0;
unicore32 69:bf094811b4a9 127 if(stateR!=MAX_VALUE) stateR++;
DeguNaoto 57:3fbd487e055e 128 sendData(5,stateR);
DeguNaoto 52:d9e1629da852 129 }
DeguNaoto 52:d9e1629da852 130 }
DeguNaoto 75:4cdcd50ee666 131 if(a2){
DeguNaoto 75:4cdcd50ee666 132 skip = 1;
DeguNaoto 75:4cdcd50ee666 133 }
DeguNaoto 75:4cdcd50ee666 134 else if(!a2){
DeguNaoto 75:4cdcd50ee666 135 skip = 0;
DeguNaoto 75:4cdcd50ee666 136 }
DeguNaoto 52:d9e1629da852 137 if(b!=1) edge1=1;
DeguNaoto 52:d9e1629da852 138 if(b!=2) edge2=1;
DeguNaoto 52:d9e1629da852 139 if(b!=3) edge3=1;
DeguNaoto 52:d9e1629da852 140 if(b!=4) edge4=1;
DeguNaoto 84:919a335ac81e 141 if(b!=5) edge5=1;
DeguNaoto 83:e1638c58e1f1 142 if(b!=6) edge6=1;
DeguNaoto 84:919a335ac81e 143 if(b!=7) edge7=1;
DeguNaoto 83:e1638c58e1f1 144 if(b!=8) edge8=1;
DeguNaoto 22:3996c3f41922 145 }
DeguNaoto 57:3fbd487e055e 146 #else
DeguNaoto 57:3fbd487e055e 147 void autoPS3(){
DeguNaoto 57:3fbd487e055e 148 if(circle){ /*mode change*/
DeguNaoto 57:3fbd487e055e 149 if(edge_circle){
DeguNaoto 57:3fbd487e055e 150 edge_circle=0;
DeguNaoto 57:3fbd487e055e 151 autoflag=0;
DeguNaoto 57:3fbd487e055e 152 Indicator4=0;
DeguNaoto 57:3fbd487e055e 153 IndicatorAuto=1;
DeguNaoto 87:6b6b6611d33f 154 flaga = 0;
DeguNaoto 57:3fbd487e055e 155 }
DeguNaoto 57:3fbd487e055e 156 }
DeguNaoto 57:3fbd487e055e 157 else if(up){ /*start*/
DeguNaoto 84:919a335ac81e 158 if(edge_up){
DeguNaoto 84:919a335ac81e 159 edge_up=0;
DeguNaoto 84:919a335ac81e 160 resetState();
DeguNaoto 84:919a335ac81e 161 flagf=1;
DeguNaoto 84:919a335ac81e 162 targ_velocity=speed;
DeguNaoto 85:dd18a2d79956 163 // sendData(7,70);
DeguNaoto 101:b67d33e56b66 164 sendData(5,68);
DeguNaoto 84:919a335ac81e 165 wait(0.1);
DeguNaoto 101:b67d33e56b66 166 sendData(4,68);
DeguNaoto 85:dd18a2d79956 167 stateR = 69;
DeguNaoto 85:dd18a2d79956 168 stateL = 69;
DeguNaoto 84:919a335ac81e 169 step = 0;
DeguNaoto 84:919a335ac81e 170 CStep = 1;
DeguNaoto 87:6b6b6611d33f 171 flaga = 1;
DeguNaoto 84:919a335ac81e 172 }
DeguNaoto 61:e018207ae860 173 }
DeguNaoto 61:e018207ae860 174 else if(right){ /*middle start*/
DeguNaoto 84:919a335ac81e 175 if(edge_right){
DeguNaoto 84:919a335ac81e 176 edge_right=0;
DeguNaoto 84:919a335ac81e 177 resetState();
DeguNaoto 84:919a335ac81e 178 flagf = 1;
DeguNaoto 84:919a335ac81e 179 targ_velocity=speed;
DeguNaoto 84:919a335ac81e 180 sendData(7,61);
DeguNaoto 85:dd18a2d79956 181 /*sendData(4,61);
DeguNaoto 85:dd18a2d79956 182 wait(0.1);
DeguNaoto 85:dd18a2d79956 183 sendData(5,58);*/
DeguNaoto 84:919a335ac81e 184 stateR = 61;
DeguNaoto 84:919a335ac81e 185 stateL = 61;
DeguNaoto 84:919a335ac81e 186 step = 5;
DeguNaoto 84:919a335ac81e 187 CStep = 6;
DeguNaoto 87:6b6b6611d33f 188 flaga = 1;
DeguNaoto 84:919a335ac81e 189 }
DeguNaoto 77:7dd154b5ace3 190 }
DeguNaoto 77:7dd154b5ace3 191 else if(down){ /*opponents start*/
DeguNaoto 84:919a335ac81e 192 if(edge_down){
DeguNaoto 84:919a335ac81e 193 edge_down=0;
DeguNaoto 84:919a335ac81e 194 resetState();
DeguNaoto 84:919a335ac81e 195 flagf = 1;
DeguNaoto 84:919a335ac81e 196 targ_velocity = speed;
DeguNaoto 85:dd18a2d79956 197 sendData(7,70);
DeguNaoto 85:dd18a2d79956 198 stateR = 70;
DeguNaoto 85:dd18a2d79956 199 stateL = 70;
DeguNaoto 84:919a335ac81e 200 step = 15;
DeguNaoto 84:919a335ac81e 201 CStep = 15;
DeguNaoto 87:6b6b6611d33f 202 flaga = 1;
DeguNaoto 84:919a335ac81e 203 }
DeguNaoto 57:3fbd487e055e 204 }
DeguNaoto 57:3fbd487e055e 205 else if(square){ /*reset*/
unicore32 68:2b2b88ecdcce 206 sendData(7,255);
DeguNaoto 57:3fbd487e055e 207 }
DeguNaoto 57:3fbd487e055e 208 else if(cross){ /*stop*/
DeguNaoto 57:3fbd487e055e 209 Motor_swing=0;
DeguNaoto 64:a98fe602c26d 210 // sita=PI/4.0,x=0.0,y=0.0;
DeguNaoto 57:3fbd487e055e 211 targ_velocity=0.0;
DeguNaoto 79:7f86e18f40ef 212 #ifdef BLUE
DeguNaoto 57:3fbd487e055e 213 targ_sita=PI/4.0;
DeguNaoto 79:7f86e18f40ef 214 #else
DeguNaoto 79:7f86e18f40ef 215 targ_sita=-PI/4.0;
DeguNaoto 79:7f86e18f40ef 216 #endif
DeguNaoto 57:3fbd487e055e 217 velocity_controller.reset();
DeguNaoto 57:3fbd487e055e 218 direction_controller.reset();
DeguNaoto 57:3fbd487e055e 219 }
DeguNaoto 57:3fbd487e055e 220 else if(l1){ /*L up*/
DeguNaoto 57:3fbd487e055e 221 if(edge_l1){
DeguNaoto 57:3fbd487e055e 222 edge_l1=0;
DeguNaoto 61:e018207ae860 223 if(stateL!=1) stateL--;
DeguNaoto 57:3fbd487e055e 224 sendData(4,stateL);
DeguNaoto 57:3fbd487e055e 225 }
DeguNaoto 57:3fbd487e055e 226 }
DeguNaoto 57:3fbd487e055e 227 else if(l2){ /*L down*/
DeguNaoto 57:3fbd487e055e 228 if(edge_l2){
DeguNaoto 57:3fbd487e055e 229 edge_l2=0;
unicore32 69:bf094811b4a9 230 if(stateL!=MAX_VALUE) stateL++;
DeguNaoto 57:3fbd487e055e 231 sendData(4,stateL);
DeguNaoto 57:3fbd487e055e 232 }
DeguNaoto 57:3fbd487e055e 233 }
DeguNaoto 57:3fbd487e055e 234 else if(r1){ /*R up*/
DeguNaoto 57:3fbd487e055e 235 if(edge_r1){
DeguNaoto 57:3fbd487e055e 236 edge_r1=0;
DeguNaoto 61:e018207ae860 237 if(stateR!=1) stateR--;
DeguNaoto 57:3fbd487e055e 238 sendData(5,stateR);
DeguNaoto 57:3fbd487e055e 239 }
DeguNaoto 57:3fbd487e055e 240 }
DeguNaoto 57:3fbd487e055e 241 else if(r2){ /*R down*/
DeguNaoto 57:3fbd487e055e 242 if(edge_r2){
DeguNaoto 57:3fbd487e055e 243 edge_r2=0;
unicore32 69:bf094811b4a9 244 if(stateR!=MAX_VALUE) stateR++;
DeguNaoto 57:3fbd487e055e 245 sendData(5,stateR);
DeguNaoto 57:3fbd487e055e 246 }
DeguNaoto 57:3fbd487e055e 247 }
DeguNaoto 75:4cdcd50ee666 248 if(triangle){
DeguNaoto 75:4cdcd50ee666 249 skip = 1;
DeguNaoto 75:4cdcd50ee666 250 }
DeguNaoto 75:4cdcd50ee666 251 else if(!triangle){
DeguNaoto 75:4cdcd50ee666 252 skip = 0;
DeguNaoto 75:4cdcd50ee666 253 }
DeguNaoto 57:3fbd487e055e 254 }
DeguNaoto 57:3fbd487e055e 255 #endif
DeguNaoto 21:79b94cb922f0 256 /***The function is PID controller initialize.***/
DeguNaoto 21:79b94cb922f0 257 inline void initializeControllers()
DeguNaoto 21:79b94cb922f0 258 {
DeguNaoto 66:14df82661dfa 259 // velocity_controller.setInputLimits(-200000.0, 200000.0); //x1
DeguNaoto 66:14df82661dfa 260 velocity_controller.setInputLimits(-20000.0, 20000.0); //x1
DeguNaoto 21:79b94cb922f0 261 direction_controller.setInputLimits(-10.0, 10.0); //x2
DeguNaoto 21:79b94cb922f0 262
DeguNaoto 21:79b94cb922f0 263 //Pwm output from MOVE_OUTPUT_LIMIT_BOTTOM to MOVE_OUTPUT_LIMIT_TOP
DeguNaoto 79:7f86e18f40ef 264 // velocity_controller.setOutputLimits(-1.0, 1.0);
DeguNaoto 79:7f86e18f40ef 265 velocity_controller.setOutputLimits(0.0, 1.0);
DeguNaoto 21:79b94cb922f0 266 direction_controller.setOutputLimits(-1.0, 1.0);
DeguNaoto 31:74e77ef0831c 267
DeguNaoto 21:79b94cb922f0 268 //set bias. 初期値
DeguNaoto 21:79b94cb922f0 269 velocity_controller.setBias(0.0);
DeguNaoto 21:79b94cb922f0 270 direction_controller.setBias(0.0);
DeguNaoto 21:79b94cb922f0 271
DeguNaoto 21:79b94cb922f0 272 //set mode.
DeguNaoto 21:79b94cb922f0 273 velocity_controller.setMode(AUTO_MODE);
DeguNaoto 21:79b94cb922f0 274 direction_controller.setMode(AUTO_MODE);
DeguNaoto 21:79b94cb922f0 275 }
DeguNaoto 21:79b94cb922f0 276
DeguNaoto 21:79b94cb922f0 277 /***The function is following move speed.***/
DeguNaoto 21:79b94cb922f0 278 inline void velocity_following()
DeguNaoto 21:79b94cb922f0 279 {
DeguNaoto 21:79b94cb922f0 280 velocity_controller.setSetPoint((float)targ_velocity);
DeguNaoto 21:79b94cb922f0 281 velocity_controller.setProcessValue((float)velocity);
DeguNaoto 21:79b94cb922f0 282 x1 = (double)velocity_controller.compute();
DeguNaoto 21:79b94cb922f0 283 }
DeguNaoto 21:79b94cb922f0 284
DeguNaoto 21:79b94cb922f0 285 inline void sita_following()
DeguNaoto 21:79b94cb922f0 286 {
DeguNaoto 21:79b94cb922f0 287 direction_controller.setSetPoint((float)targ_sita);
DeguNaoto 21:79b94cb922f0 288 direction_controller.setProcessValue((float)sita);
DeguNaoto 55:db1797ac6cb1 289 //direction_controller.setSetPoint(0.0); /*目標値とのずれをなくす*/
DeguNaoto 21:79b94cb922f0 290 //direction_controller.setProcessValue(-y);
DeguNaoto 21:79b94cb922f0 291 x2 = (double)direction_controller.compute();
DeguNaoto 21:79b94cb922f0 292 }
DeguNaoto 21:79b94cb922f0 293
DeguNaoto 21:79b94cb922f0 294 inline void move_following()
DeguNaoto 21:79b94cb922f0 295 {
DeguNaoto 21:79b94cb922f0 296 velocity_following();
DeguNaoto 21:79b94cb922f0 297 sita_following();
DeguNaoto 64:a98fe602c26d 298
DeguNaoto 64:a98fe602c26d 299 if(flagf==0){
DeguNaoto 79:7f86e18f40ef 300 Vr = ( 2.0*(-x1) + x2 ) / 3.0;
DeguNaoto 79:7f86e18f40ef 301 Vl = ( 2.0*(-x1) - x2 ) / 3.0;
DeguNaoto 63:dad3f4e5e99c 302 }
DeguNaoto 64:a98fe602c26d 303 else if(flagf==1){
DeguNaoto 64:a98fe602c26d 304 if(x2>0){
DeguNaoto 64:a98fe602c26d 305 Vr = x1;
DeguNaoto 64:a98fe602c26d 306 Vl = x1 - x2;
DeguNaoto 64:a98fe602c26d 307 }
DeguNaoto 64:a98fe602c26d 308 else{
DeguNaoto 64:a98fe602c26d 309 Vr = x1 + x2;
DeguNaoto 64:a98fe602c26d 310 Vl = x1;
DeguNaoto 64:a98fe602c26d 311 }
DeguNaoto 63:dad3f4e5e99c 312 }
DeguNaoto 63:dad3f4e5e99c 313
DeguNaoto 64:a98fe602c26d 314 if( abs(Vr) < 0.2 ) Vr = 0.0;
DeguNaoto 64:a98fe602c26d 315 if( abs(Vl) < 0.2 ) Vl = 0.0;
DeguNaoto 21:79b94cb922f0 316 Move_r( ( float ) Vr );
DeguNaoto 21:79b94cb922f0 317 Move_l( ( float ) Vl );
DeguNaoto 21:79b94cb922f0 318 }
DeguNaoto 21:79b94cb922f0 319
DeguNaoto 21:79b94cb922f0 320 #endif /*autoMode.h*/