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@82:1bbe138c0b76, 2015-10-11 (annotated)

Committer:
DeguNaoto
Date:
Sun Oct 11 08:51:11 2015 +0000
Revision:
82:1bbe138c0b76
Parent:
80:7e24b99b9b61 
20151011 IM920??????

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