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@110:2ecb2f8aff81, 2015-10-20 (annotated)

Committer:
DeguNaoto
Date:
Tue Oct 20 08:05:07 2015 +0000
Revision:
110:2ecb2f8aff81
Parent:
109:e009296125db
Child:
111:846ea36dc1dc
20151020 ???

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