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@88:cbed08732872, 2015-10-14 (annotated)

Committer:
DeguNaoto
Date:
Wed Oct 14 10:05:55 2015 +0000
Revision:
88:cbed08732872
Parent:
87:6b6b6611d33f
Child:
89:048a72382b0a
20151014????

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