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@114:325e4c158141, 2015-10-27 (annotated)

Committer:
DeguNaoto
Date:
Tue Oct 27 07:49:43 2015 +0000
Revision:
114:325e4c158141
Parent:
112:717acc7c99f9 
??????????; ????ver

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 108:7eb434cfcbd7 41 #ifdef BLUE
DeguNaoto 114:325e4c158141 42 // sendData(5,69); //right
DeguNaoto 114:325e4c158141 43 // sendData(5,70); //right
DeguNaoto 114:325e4c158141 44 sendData(5,71); //right
DeguNaoto 89:048a72382b0a 45 wait(0.05);
DeguNaoto 103:ffd3ca4a7a71 46 sendData(4,69); //left
DeguNaoto 85:dd18a2d79956 47 stateR = 69;
DeguNaoto 85:dd18a2d79956 48 stateL = 69;
DeguNaoto 108:7eb434cfcbd7 49 #else
DeguNaoto 108:7eb434cfcbd7 50 sendData(5,69); //right
DeguNaoto 108:7eb434cfcbd7 51 wait(0.05);
DeguNaoto 108:7eb434cfcbd7 52 sendData(4,69); //left
DeguNaoto 108:7eb434cfcbd7 53 stateR = 69;
DeguNaoto 108:7eb434cfcbd7 54 stateL = 69;
DeguNaoto 108:7eb434cfcbd7 55 #endif
DeguNaoto 108:7eb434cfcbd7 56 wait(0.05);
DeguNaoto 83:e1638c58e1f1 57 step = 0;
DeguNaoto 83:e1638c58e1f1 58 CStep = 1;
DeguNaoto 86:5f0b065cb4d3 59 flaga = 1;
DeguNaoto 83:e1638c58e1f1 60 }
DeguNaoto 52:d9e1629da852 61 }
DeguNaoto 89:048a72382b0a 62 else if((b==5)&&(!flaga)){ /*middle start*/
DeguNaoto 83:e1638c58e1f1 63 if(edge5){
DeguNaoto 83:e1638c58e1f1 64 edge5=0;
DeguNaoto 83:e1638c58e1f1 65 resetState();
DeguNaoto 83:e1638c58e1f1 66 flagf = 1;
DeguNaoto 89:048a72382b0a 67 spcount=0.0;
DeguNaoto 89:048a72382b0a 68 // targ_velocity=speed;
DeguNaoto 108:7eb434cfcbd7 69 #ifdef BLUE
DeguNaoto 103:ffd3ca4a7a71 70 sendData(5,60); //right
DeguNaoto 89:048a72382b0a 71 wait(0.05);
DeguNaoto 108:7eb434cfcbd7 72 sendData(4,60); //left
DeguNaoto 108:7eb434cfcbd7 73 stateR = 60;
DeguNaoto 108:7eb434cfcbd7 74 stateL = 61;
DeguNaoto 108:7eb434cfcbd7 75 #else
DeguNaoto 111:846ea36dc1dc 76 sendData(5,59); //right
DeguNaoto 89:048a72382b0a 77 wait(0.05);
DeguNaoto 111:846ea36dc1dc 78 sendData(4,59); //left
DeguNaoto 114:325e4c158141 79 stateR = 59;
DeguNaoto 114:325e4c158141 80 stateL = 59;
DeguNaoto 108:7eb434cfcbd7 81 #endif
DeguNaoto 108:7eb434cfcbd7 82 wait(0.05);
DeguNaoto 83:e1638c58e1f1 83 step = 5;
DeguNaoto 101:b67d33e56b66 84 CStep = 7;
DeguNaoto 87:6b6b6611d33f 85 flaga = 1;
DeguNaoto 83:e1638c58e1f1 86 }
DeguNaoto 61:e018207ae860 87 }
DeguNaoto 89:048a72382b0a 88 else if((b==8)&&(!flaga)){ /*opponents start*/
DeguNaoto 83:e1638c58e1f1 89 if(edge8){
DeguNaoto 83:e1638c58e1f1 90 edge8=0;
DeguNaoto 83:e1638c58e1f1 91 resetState();
DeguNaoto 89:048a72382b0a 92 // targ_velocity = speed;
DeguNaoto 105:0be5d5d64782 93 #ifdef BLUE
DeguNaoto 108:7eb434cfcbd7 94 sendData(5,69); //right
DeguNaoto 89:048a72382b0a 95 wait(0.05);
DeguNaoto 108:7eb434cfcbd7 96 sendData(4,69); //left
DeguNaoto 108:7eb434cfcbd7 97 stateR = 71;
DeguNaoto 108:7eb434cfcbd7 98 stateL = 71;
DeguNaoto 105:0be5d5d64782 99 #else
DeguNaoto 112:717acc7c99f9 100 /*sendData(5,66); //right
DeguNaoto 112:717acc7c99f9 101 wait(0.05);
DeguNaoto 112:717acc7c99f9 102 sendData(4,66); //left*/
DeguNaoto 112:717acc7c99f9 103 /*sendData(4,71); //left
DeguNaoto 109:e009296125db 104 wait(0.05);
DeguNaoto 112:717acc7c99f9 105 sendData(5,71); //right*/
DeguNaoto 112:717acc7c99f9 106 sendData(4,73); //left
DeguNaoto 105:0be5d5d64782 107 wait(0.05);
DeguNaoto 112:717acc7c99f9 108 sendData(5,73); //right
DeguNaoto 112:717acc7c99f9 109
DeguNaoto 108:7eb434cfcbd7 110 stateR = 71;
DeguNaoto 108:7eb434cfcbd7 111 stateL = 72;
DeguNaoto 105:0be5d5d64782 112 #endif
DeguNaoto 89:048a72382b0a 113 wait(0.05);
DeguNaoto 97:5e6f513b26f2 114 step=114;
DeguNaoto 97:5e6f513b26f2 115 CStep=114;
DeguNaoto 105:0be5d5d64782 116 OpStart.attach(&OpponentsStart,2.2);
DeguNaoto 105:0be5d5d64782 117 // OpStart.attach(&OpponentsStart,2.0);
DeguNaoto 83:e1638c58e1f1 118 }
DeguNaoto 52:d9e1629da852 119 }
DeguNaoto 52:d9e1629da852 120 else if(b==1){ /*L down*/
DeguNaoto 52:d9e1629da852 121 if(edge1){
DeguNaoto 52:d9e1629da852 122 edge1=0;
DeguNaoto 52:d9e1629da852 123 if(stateL!=1) stateL--;
DeguNaoto 57:3fbd487e055e 124 sendData(4,stateL);
DeguNaoto 52:d9e1629da852 125 }
DeguNaoto 52:d9e1629da852 126 }
DeguNaoto 52:d9e1629da852 127 else if(b==2){ /*L up*/
DeguNaoto 52:d9e1629da852 128 if(edge2){
DeguNaoto 52:d9e1629da852 129 edge2=0;
unicore32 69:bf094811b4a9 130 if(stateL!=MAX_VALUE) stateL++;
DeguNaoto 57:3fbd487e055e 131 sendData(4,stateL);
DeguNaoto 22:3996c3f41922 132 }
DeguNaoto 52:d9e1629da852 133 }
DeguNaoto 52:d9e1629da852 134 else if(b==3){ /*R down*/
DeguNaoto 52:d9e1629da852 135 if(edge3){
DeguNaoto 52:d9e1629da852 136 edge3=0;
DeguNaoto 52:d9e1629da852 137 if(stateR!=1) stateR--;
DeguNaoto 57:3fbd487e055e 138 sendData(5,stateR);
DeguNaoto 52:d9e1629da852 139 }
DeguNaoto 52:d9e1629da852 140 }
DeguNaoto 52:d9e1629da852 141 else if(b==4){ /*R up*/
DeguNaoto 52:d9e1629da852 142 if(edge4){
DeguNaoto 52:d9e1629da852 143 edge4=0;
unicore32 69:bf094811b4a9 144 if(stateR!=MAX_VALUE) stateR++;
DeguNaoto 57:3fbd487e055e 145 sendData(5,stateR);
DeguNaoto 52:d9e1629da852 146 }
DeguNaoto 52:d9e1629da852 147 }
DeguNaoto 75:4cdcd50ee666 148 if(a2){
DeguNaoto 75:4cdcd50ee666 149 skip = 1;
DeguNaoto 75:4cdcd50ee666 150 }
DeguNaoto 75:4cdcd50ee666 151 else if(!a2){
DeguNaoto 75:4cdcd50ee666 152 skip = 0;
DeguNaoto 75:4cdcd50ee666 153 }
DeguNaoto 52:d9e1629da852 154 if(b!=1) edge1=1;
DeguNaoto 52:d9e1629da852 155 if(b!=2) edge2=1;
DeguNaoto 52:d9e1629da852 156 if(b!=3) edge3=1;
DeguNaoto 52:d9e1629da852 157 if(b!=4) edge4=1;
DeguNaoto 84:919a335ac81e 158 if(b!=5) edge5=1;
DeguNaoto 83:e1638c58e1f1 159 if(b!=6) edge6=1;
DeguNaoto 84:919a335ac81e 160 if(b!=7) edge7=1;
DeguNaoto 83:e1638c58e1f1 161 if(b!=8) edge8=1;
DeguNaoto 108:7eb434cfcbd7 162 if(b!=9) edge9=1;
DeguNaoto 22:3996c3f41922 163 }
DeguNaoto 57:3fbd487e055e 164 #else
DeguNaoto 57:3fbd487e055e 165 void autoPS3(){
DeguNaoto 57:3fbd487e055e 166 if(circle){ /*mode change*/
DeguNaoto 57:3fbd487e055e 167 if(edge_circle){
DeguNaoto 57:3fbd487e055e 168 edge_circle=0;
DeguNaoto 57:3fbd487e055e 169 autoflag=0;
DeguNaoto 57:3fbd487e055e 170 Indicator4=0;
DeguNaoto 57:3fbd487e055e 171 IndicatorAuto=1;
DeguNaoto 114:325e4c158141 172 flaga=0;
DeguNaoto 114:325e4c158141 173 Move_l(0.0);
DeguNaoto 114:325e4c158141 174 Move_r(0.0);
DeguNaoto 57:3fbd487e055e 175 }
DeguNaoto 57:3fbd487e055e 176 }
DeguNaoto 114:325e4c158141 177 else if(triangle){ /*start*/
DeguNaoto 114:325e4c158141 178 if(edge_triangle){
DeguNaoto 114:325e4c158141 179 edge_triangle=0;
DeguNaoto 84:919a335ac81e 180 resetState();
DeguNaoto 84:919a335ac81e 181 flagf=1;
DeguNaoto 114:325e4c158141 182 spcount=0.0;
DeguNaoto 114:325e4c158141 183 // targ_velocity=speed;
DeguNaoto 114:325e4c158141 184 #ifdef BLUE
DeguNaoto 114:325e4c158141 185 // sendData(5,69); //right
DeguNaoto 114:325e4c158141 186 sendData(5,71); //right
DeguNaoto 114:325e4c158141 187 wait(0.05);
DeguNaoto 114:325e4c158141 188 sendData(4,69); //left
DeguNaoto 85:dd18a2d79956 189 stateR = 69;
DeguNaoto 85:dd18a2d79956 190 stateL = 69;
DeguNaoto 114:325e4c158141 191 #else
DeguNaoto 114:325e4c158141 192 sendData(5,69); //right
DeguNaoto 114:325e4c158141 193 wait(0.05);
DeguNaoto 114:325e4c158141 194 sendData(4,69); //left
DeguNaoto 114:325e4c158141 195 stateR = 69;
DeguNaoto 114:325e4c158141 196 stateL = 69;
DeguNaoto 114:325e4c158141 197 #endif
DeguNaoto 114:325e4c158141 198 wait(0.05);
DeguNaoto 84:919a335ac81e 199 step = 0;
DeguNaoto 84:919a335ac81e 200 CStep = 1;
DeguNaoto 87:6b6b6611d33f 201 flaga = 1;
DeguNaoto 84:919a335ac81e 202 }
DeguNaoto 61:e018207ae860 203 }
DeguNaoto 114:325e4c158141 204 else if(square){ /*middle start*/
DeguNaoto 114:325e4c158141 205 if(edge_square){
DeguNaoto 114:325e4c158141 206 edge_square=0;
DeguNaoto 84:919a335ac81e 207 resetState();
DeguNaoto 84:919a335ac81e 208 flagf = 1;
DeguNaoto 114:325e4c158141 209 spcount=0.0;
DeguNaoto 114:325e4c158141 210 // targ_velocity=speed;
DeguNaoto 114:325e4c158141 211 #ifdef BLUE
DeguNaoto 114:325e4c158141 212 sendData(5,60); //right
DeguNaoto 114:325e4c158141 213 wait(0.05);
DeguNaoto 114:325e4c158141 214 sendData(4,60); //left
DeguNaoto 114:325e4c158141 215 stateR = 60;
DeguNaoto 114:325e4c158141 216 stateL = 61;
DeguNaoto 114:325e4c158141 217 #else
DeguNaoto 114:325e4c158141 218 sendData(5,59); //right
DeguNaoto 114:325e4c158141 219 wait(0.05);
DeguNaoto 114:325e4c158141 220 sendData(4,59); //left
DeguNaoto 84:919a335ac81e 221 stateR = 61;
DeguNaoto 114:325e4c158141 222 stateL = 60;
DeguNaoto 114:325e4c158141 223 #endif
DeguNaoto 114:325e4c158141 224 wait(0.05);
DeguNaoto 84:919a335ac81e 225 step = 5;
DeguNaoto 114:325e4c158141 226 CStep = 7;
DeguNaoto 87:6b6b6611d33f 227 flaga = 1;
DeguNaoto 84:919a335ac81e 228 }
DeguNaoto 77:7dd154b5ace3 229 }
DeguNaoto 114:325e4c158141 230 else if(cross){ /*opponents start*/
DeguNaoto 114:325e4c158141 231 if(edge_cross){
DeguNaoto 114:325e4c158141 232 edge_cross=0;
DeguNaoto 84:919a335ac81e 233 resetState();
DeguNaoto 114:325e4c158141 234 // targ_velocity = speed;
DeguNaoto 114:325e4c158141 235 #ifdef BLUE
DeguNaoto 114:325e4c158141 236 sendData(5,69); //right
DeguNaoto 114:325e4c158141 237 wait(0.05);
DeguNaoto 114:325e4c158141 238 sendData(4,69); //left
DeguNaoto 114:325e4c158141 239 stateR = 71;
DeguNaoto 114:325e4c158141 240 stateL = 71;
DeguNaoto 114:325e4c158141 241 #else
DeguNaoto 114:325e4c158141 242 /*sendData(5,66); //right
DeguNaoto 114:325e4c158141 243 wait(0.05);
DeguNaoto 114:325e4c158141 244 sendData(4,66); //left*/
DeguNaoto 114:325e4c158141 245 /*sendData(4,71); //left
DeguNaoto 114:325e4c158141 246 wait(0.05);
DeguNaoto 114:325e4c158141 247 sendData(5,71); //right*/
DeguNaoto 114:325e4c158141 248 sendData(4,73); //left
DeguNaoto 114:325e4c158141 249 wait(0.05);
DeguNaoto 114:325e4c158141 250 sendData(5,73); //right
DeguNaoto 114:325e4c158141 251
DeguNaoto 114:325e4c158141 252 stateR = 71;
DeguNaoto 114:325e4c158141 253 stateL = 72;
DeguNaoto 114:325e4c158141 254 #endif
DeguNaoto 114:325e4c158141 255 wait(0.05);
DeguNaoto 114:325e4c158141 256 step=114;
DeguNaoto 114:325e4c158141 257 CStep=114;
DeguNaoto 114:325e4c158141 258 OpStart.attach(&OpponentsStart,2.2);
DeguNaoto 114:325e4c158141 259 // OpStart.attach(&OpponentsStart,2.0);
DeguNaoto 84:919a335ac81e 260 }
DeguNaoto 57:3fbd487e055e 261 }
DeguNaoto 57:3fbd487e055e 262 else if(l1){ /*L up*/
DeguNaoto 57:3fbd487e055e 263 if(edge_l1){
DeguNaoto 57:3fbd487e055e 264 edge_l1=0;
DeguNaoto 114:325e4c158141 265 if(stateL!=92) stateL++;
DeguNaoto 57:3fbd487e055e 266 sendData(4,stateL);
DeguNaoto 57:3fbd487e055e 267 }
DeguNaoto 57:3fbd487e055e 268 }
DeguNaoto 57:3fbd487e055e 269 else if(l2){ /*L down*/
DeguNaoto 57:3fbd487e055e 270 if(edge_l2){
DeguNaoto 57:3fbd487e055e 271 edge_l2=0;
DeguNaoto 114:325e4c158141 272 if(stateL!=1) stateL--;
DeguNaoto 57:3fbd487e055e 273 sendData(4,stateL);
DeguNaoto 57:3fbd487e055e 274 }
DeguNaoto 57:3fbd487e055e 275 }
DeguNaoto 57:3fbd487e055e 276 else if(r1){ /*R up*/
DeguNaoto 57:3fbd487e055e 277 if(edge_r1){
DeguNaoto 57:3fbd487e055e 278 edge_r1=0;
DeguNaoto 114:325e4c158141 279 if(stateR!=92) stateR++;
DeguNaoto 57:3fbd487e055e 280 sendData(5,stateR);
DeguNaoto 57:3fbd487e055e 281 }
DeguNaoto 57:3fbd487e055e 282 }
DeguNaoto 57:3fbd487e055e 283 else if(r2){ /*R down*/
DeguNaoto 57:3fbd487e055e 284 if(edge_r2){
DeguNaoto 57:3fbd487e055e 285 edge_r2=0;
DeguNaoto 114:325e4c158141 286 if(stateR!=1) stateR--;
DeguNaoto 57:3fbd487e055e 287 sendData(5,stateR);
DeguNaoto 57:3fbd487e055e 288 }
DeguNaoto 57:3fbd487e055e 289 }
DeguNaoto 114:325e4c158141 290 if(up){
DeguNaoto 75:4cdcd50ee666 291 skip = 1;
DeguNaoto 75:4cdcd50ee666 292 }
DeguNaoto 114:325e4c158141 293 else if(!up){
DeguNaoto 75:4cdcd50ee666 294 skip = 0;
DeguNaoto 75:4cdcd50ee666 295 }
DeguNaoto 57:3fbd487e055e 296 }
DeguNaoto 57:3fbd487e055e 297 #endif
DeguNaoto 21:79b94cb922f0 298 /***The function is PID controller initialize.***/
DeguNaoto 21:79b94cb922f0 299 inline void initializeControllers()
DeguNaoto 21:79b94cb922f0 300 {
DeguNaoto 66:14df82661dfa 301 // velocity_controller.setInputLimits(-200000.0, 200000.0); //x1
DeguNaoto 66:14df82661dfa 302 velocity_controller.setInputLimits(-20000.0, 20000.0); //x1
DeguNaoto 21:79b94cb922f0 303 direction_controller.setInputLimits(-10.0, 10.0); //x2
DeguNaoto 21:79b94cb922f0 304
DeguNaoto 21:79b94cb922f0 305 //Pwm output from MOVE_OUTPUT_LIMIT_BOTTOM to MOVE_OUTPUT_LIMIT_TOP
DeguNaoto 79:7f86e18f40ef 306 // velocity_controller.setOutputLimits(-1.0, 1.0);
DeguNaoto 79:7f86e18f40ef 307 velocity_controller.setOutputLimits(0.0, 1.0);
DeguNaoto 21:79b94cb922f0 308 direction_controller.setOutputLimits(-1.0, 1.0);
DeguNaoto 31:74e77ef0831c 309
DeguNaoto 21:79b94cb922f0 310 //set bias. 初期値
DeguNaoto 21:79b94cb922f0 311 velocity_controller.setBias(0.0);
DeguNaoto 21:79b94cb922f0 312 direction_controller.setBias(0.0);
DeguNaoto 21:79b94cb922f0 313
DeguNaoto 21:79b94cb922f0 314 //set mode.
DeguNaoto 21:79b94cb922f0 315 velocity_controller.setMode(AUTO_MODE);
DeguNaoto 21:79b94cb922f0 316 direction_controller.setMode(AUTO_MODE);
DeguNaoto 21:79b94cb922f0 317 }
DeguNaoto 21:79b94cb922f0 318
DeguNaoto 21:79b94cb922f0 319 /***The function is following move speed.***/
DeguNaoto 21:79b94cb922f0 320 inline void velocity_following()
DeguNaoto 21:79b94cb922f0 321 {
DeguNaoto 21:79b94cb922f0 322 velocity_controller.setSetPoint((float)targ_velocity);
DeguNaoto 21:79b94cb922f0 323 velocity_controller.setProcessValue((float)velocity);
DeguNaoto 21:79b94cb922f0 324 x1 = (double)velocity_controller.compute();
DeguNaoto 21:79b94cb922f0 325 }
DeguNaoto 21:79b94cb922f0 326
DeguNaoto 21:79b94cb922f0 327 inline void sita_following()
DeguNaoto 21:79b94cb922f0 328 {
DeguNaoto 21:79b94cb922f0 329 direction_controller.setSetPoint((float)targ_sita);
DeguNaoto 21:79b94cb922f0 330 direction_controller.setProcessValue((float)sita);
DeguNaoto 55:db1797ac6cb1 331 //direction_controller.setSetPoint(0.0); /*目標値とのずれをなくす*/
DeguNaoto 21:79b94cb922f0 332 //direction_controller.setProcessValue(-y);
DeguNaoto 21:79b94cb922f0 333 x2 = (double)direction_controller.compute();
DeguNaoto 21:79b94cb922f0 334 }
DeguNaoto 21:79b94cb922f0 335
DeguNaoto 21:79b94cb922f0 336 inline void move_following()
DeguNaoto 21:79b94cb922f0 337 {
DeguNaoto 21:79b94cb922f0 338 velocity_following();
DeguNaoto 21:79b94cb922f0 339 sita_following();
DeguNaoto 64:a98fe602c26d 340
DeguNaoto 64:a98fe602c26d 341 if(flagf==0){
DeguNaoto 79:7f86e18f40ef 342 Vr = ( 2.0*(-x1) + x2 ) / 3.0;
DeguNaoto 79:7f86e18f40ef 343 Vl = ( 2.0*(-x1) - x2 ) / 3.0;
DeguNaoto 63:dad3f4e5e99c 344 }
DeguNaoto 64:a98fe602c26d 345 else if(flagf==1){
DeguNaoto 64:a98fe602c26d 346 if(x2>0){
DeguNaoto 64:a98fe602c26d 347 Vr = x1;
DeguNaoto 64:a98fe602c26d 348 Vl = x1 - x2;
DeguNaoto 64:a98fe602c26d 349 }
DeguNaoto 64:a98fe602c26d 350 else{
DeguNaoto 64:a98fe602c26d 351 Vr = x1 + x2;
DeguNaoto 64:a98fe602c26d 352 Vl = x1;
DeguNaoto 64:a98fe602c26d 353 }
DeguNaoto 63:dad3f4e5e99c 354 }
DeguNaoto 63:dad3f4e5e99c 355
DeguNaoto 114:325e4c158141 356 // if( abs(Vr) < 0.2 ) Vr = 0.0;
DeguNaoto 114:325e4c158141 357 if((Vr<0.2)&&(Vr>-0.2)) Vr = 0.0;
DeguNaoto 114:325e4c158141 358 // if( abs(Vl) < 0.2 ) Vl = 0.0;
DeguNaoto 114:325e4c158141 359 if((Vl<0.2)&&(Vl>-0.2)) Vl = 0.0;
DeguNaoto 21:79b94cb922f0 360 Move_r( ( float ) Vr );
DeguNaoto 21:79b94cb922f0 361 Move_l( ( float ) Vl );
DeguNaoto 21:79b94cb922f0 362 }
DeguNaoto 21:79b94cb922f0 363
DeguNaoto 114:325e4c158141 364 #endif /*autoMode.h*/