2015_robocon_bteam / Mbed 2 deprecated 2015robot_main_zenkoku

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

autoMode.h@3:8d8c25c556ae, 2015-10-31 (annotated)

Committer:
DeguNaoto
Date:
Sat Oct 31 00:30:52 2015 +0000
Revision:
3:8d8c25c556ae
Parent:
2:738b28f6a04b
Child:
6:ca3a74a93ae2
20151031 ?????????????????(???)

Who changed what in which revision?

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