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@83:e1638c58e1f1, 2015-10-12 (annotated)

Committer:
DeguNaoto
Date:
Mon Oct 12 06:15:45 2015 +0000
Revision:
83:e1638c58e1f1
Parent:
80:7e24b99b9b61
Child:
84:919a335ac81e
20151012 ??????; ????????; ????????

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