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

main_ps3.cpp@19:ed178d04e11d, 2015-11-05 (annotated)

Committer:
DeguNaoto
Date:
Thu Nov 05 08:21:23 2015 +0000
Revision:
19:ed178d04e11d
Parent:
17:726b6f53a457
Child:
21:bdf8ac5c200c
20151105 ?????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DeguNaoto 0:b613dc16f27d 1 /**
DeguNaoto 0:b613dc16f27d 2 * This program is written in main micro computer "mbed" for 2015 NHK Robot Contest (Bteam).
DeguNaoto 0:b613dc16f27d 3 */
DeguNaoto 0:b613dc16f27d 4
DeguNaoto 0:b613dc16f27d 5 /***コース選択***/
DeguNaoto 3:8d8c25c556ae 6 #define BLUE
DeguNaoto 3:8d8c25c556ae 7 //#define RED
DeguNaoto 0:b613dc16f27d 8
DeguNaoto 2:738b28f6a04b 9 /***マシン状態計測***/
DeguNaoto 0:b613dc16f27d 10 //#define MESURE
DeguNaoto 0:b613dc16f27d 11
DeguNaoto 0:b613dc16f27d 12 #if defined(BLUE) && defined(RED)
DeguNaoto 0:b613dc16f27d 13 #error Caution, You should define either BLUE or RED
DeguNaoto 0:b613dc16f27d 14 #endif
DeguNaoto 0:b613dc16f27d 15
DeguNaoto 0:b613dc16f27d 16 #include "machine_ps3.h"
DeguNaoto 0:b613dc16f27d 17
DeguNaoto 0:b613dc16f27d 18 #define deff 70.0
DeguNaoto 0:b613dc16f27d 19
DeguNaoto 0:b613dc16f27d 20 Serial pc(USBTX, USBRX);
DeguNaoto 0:b613dc16f27d 21 #ifdef MESURE
DeguNaoto 2:738b28f6a04b 22 short mesureflag=0;
DeguNaoto 0:b613dc16f27d 23 LocalFileSystem local("local");
DeguNaoto 0:b613dc16f27d 24 #endif
DeguNaoto 0:b613dc16f27d 25
DeguNaoto 0:b613dc16f27d 26 int main() {
DeguNaoto 2:738b28f6a04b 27 Com.attach(&Call,RATE);
DeguNaoto 0:b613dc16f27d 28 initializeMotors();
DeguNaoto 0:b613dc16f27d 29 initializeControllers();
DeguNaoto 0:b613dc16f27d 30 initializeRS485();
DeguNaoto 14:943e663694c3 31 initializeMbedSerial();
DeguNaoto 0:b613dc16f27d 32 #ifdef BLUE
DeguNaoto 0:b613dc16f27d 33 sita=PI/4.0,targ_sita=PI/4.0;
DeguNaoto 0:b613dc16f27d 34 IndicatorBLUE = 1;
DeguNaoto 0:b613dc16f27d 35 #else
DeguNaoto 0:b613dc16f27d 36 sita=-PI/4.0,targ_sita=-PI/4.0;
DeguNaoto 0:b613dc16f27d 37 IndicatorRED = 1;
DeguNaoto 0:b613dc16f27d 38 #endif
DeguNaoto 0:b613dc16f27d 39 Indicator4=1;
DeguNaoto 0:b613dc16f27d 40 Enable=1;
DeguNaoto 0:b613dc16f27d 41 #ifdef MESURE
DeguNaoto 0:b613dc16f27d 42 FILE *fp_r = fopen("/local/velocity.dat", "w");
DeguNaoto 0:b613dc16f27d 43 double time=0.0;
DeguNaoto 0:b613dc16f27d 44 #endif
DeguNaoto 0:b613dc16f27d 45 wait(0.3);
DeguNaoto 0:b613dc16f27d 46 sendData(7,0);
DeguNaoto 0:b613dc16f27d 47 while(1) {
DeguNaoto 0:b613dc16f27d 48 if(autoflag){
DeguNaoto 0:b613dc16f27d 49 autoIM920(); /*IM920 button*/
DeguNaoto 0:b613dc16f27d 50 #ifdef BLUE
DeguNaoto 0:b613dc16f27d 51 //Blue
DeguNaoto 0:b613dc16f27d 52 /********************************Nomal Mode*********************************/
DeguNaoto 17:726b6f53a457 53 if((step==0)&&((9000.0>x)&&(x>800.0))) {
DeguNaoto 17:726b6f53a457 54 // targ_sita=-0.01;
DeguNaoto 3:8d8c25c556ae 55 targ_sita=0.0;
DeguNaoto 0:b613dc16f27d 56 step=1;
DeguNaoto 0:b613dc16f27d 57 }
DeguNaoto 17:726b6f53a457 58 // if((step==1)&&(x>8600.0+deff)) {
DeguNaoto 17:726b6f53a457 59 if((step==1)&&(x>9000.0)) {
DeguNaoto 0:b613dc16f27d 60 targ_velocity=0.0;
DeguNaoto 2:738b28f6a04b 61 direction_controller.setBias(0.0);
DeguNaoto 2:738b28f6a04b 62 direction_controller.reset();
DeguNaoto 3:8d8c25c556ae 63 velocity_controller.setBias(0.0);
DeguNaoto 3:8d8c25c556ae 64 velocity_controller.reset();
DeguNaoto 0:b613dc16f27d 65 step=2;
DeguNaoto 0:b613dc16f27d 66 }
DeguNaoto 0:b613dc16f27d 67 if((step==2)&&((velocity<500.0)&&(velocity>-500.0))){
DeguNaoto 0:b613dc16f27d 68 step=3;
DeguNaoto 0:b613dc16f27d 69 spcount=0.0;
DeguNaoto 0:b613dc16f27d 70 flagf=0;
DeguNaoto 5:0e6dd1ff3ff2 71 // targ_sita=0.1;
DeguNaoto 5:0e6dd1ff3ff2 72 targ_sita=0.0;
DeguNaoto 0:b613dc16f27d 73 }
DeguNaoto 6:ca3a74a93ae2 74 // if((step==3)&&(x<2000.0)) {
DeguNaoto 17:726b6f53a457 75 if((step==3)&&(x<1400.0)) {
DeguNaoto 0:b613dc16f27d 76 targ_sita=PI/4;
DeguNaoto 0:b613dc16f27d 77 step=4;
DeguNaoto 0:b613dc16f27d 78 }
DeguNaoto 17:726b6f53a457 79 if((step==4)&&(x<600.0)) {
DeguNaoto 0:b613dc16f27d 80 targ_velocity=0.0;
DeguNaoto 0:b613dc16f27d 81 step=114;
DeguNaoto 0:b613dc16f27d 82 }
DeguNaoto 0:b613dc16f27d 83 /***Cylinder***/
DeguNaoto 6:ca3a74a93ae2 84 if((x>3130.0+deff)&&(CStep==1)) {
DeguNaoto 0:b613dc16f27d 85 if(!skip) sendData(1,1);
DeguNaoto 0:b613dc16f27d 86 CStep=2;
DeguNaoto 0:b613dc16f27d 87 }
DeguNaoto 0:b613dc16f27d 88 if((x>5900.0+deff)&&(CStep==2)) {
DeguNaoto 0:b613dc16f27d 89 if(!skip) sendData(1,3);
DeguNaoto 0:b613dc16f27d 90 CStep=3;
DeguNaoto 0:b613dc16f27d 91 }
DeguNaoto 0:b613dc16f27d 92 if((x>7680.0+deff)&&(CStep==3)) {
DeguNaoto 0:b613dc16f27d 93 if(!skip) sendData(1,2);
DeguNaoto 0:b613dc16f27d 94 CStep=4;
DeguNaoto 0:b613dc16f27d 95 }
DeguNaoto 0:b613dc16f27d 96 if((x<6580.0+deff)&&(CStep==4)) {
DeguNaoto 0:b613dc16f27d 97 if(!skip) sendData(1,5);
DeguNaoto 0:b613dc16f27d 98 CStep=5;
DeguNaoto 0:b613dc16f27d 99 }
DeguNaoto 0:b613dc16f27d 100 if((x<6100.0+deff)&&(CStep==5)) {
DeguNaoto 0:b613dc16f27d 101 if(!skip) sendData(1,4);
DeguNaoto 0:b613dc16f27d 102 CStep=6;
DeguNaoto 0:b613dc16f27d 103 }
DeguNaoto 0:b613dc16f27d 104 if((x<4000.0)&&(CStep==6)){
DeguNaoto 0:b613dc16f27d 105 sendData(7,0);
DeguNaoto 0:b613dc16f27d 106 CStep=114;
DeguNaoto 0:b613dc16f27d 107 }
DeguNaoto 0:b613dc16f27d 108 /////////////////////////////////////////////////////////////////////////
DeguNaoto 0:b613dc16f27d 109 /******************************Middle Mode******************************/
DeguNaoto 0:b613dc16f27d 110 if((step==5)&&((5700.0>x)&&(x>800.0))) {
DeguNaoto 3:8d8c25c556ae 111 // targ_sita=0.025;
DeguNaoto 3:8d8c25c556ae 112 targ_sita=0.0;
DeguNaoto 0:b613dc16f27d 113 step=6;
DeguNaoto 0:b613dc16f27d 114 }
DeguNaoto 0:b613dc16f27d 115 if((step==6)&&(x>7000.0)){
DeguNaoto 0:b613dc16f27d 116 targ_velocity=0.0;
DeguNaoto 2:738b28f6a04b 117 direction_controller.reset();
DeguNaoto 2:738b28f6a04b 118 direction_controller.setBias(0.0);
DeguNaoto 0:b613dc16f27d 119 step=7;
DeguNaoto 0:b613dc16f27d 120 }
DeguNaoto 0:b613dc16f27d 121 if((step==7)&&((velocity<500.0)&&(velocity>-500.0))){
DeguNaoto 0:b613dc16f27d 122 flagf=0;
DeguNaoto 0:b613dc16f27d 123 spcount=0.0;
DeguNaoto 6:ca3a74a93ae2 124 targ_sita=0.0;
DeguNaoto 0:b613dc16f27d 125 step=8;
DeguNaoto 0:b613dc16f27d 126 }
DeguNaoto 0:b613dc16f27d 127 if((step==8)&&(x<1900.0)) {
DeguNaoto 0:b613dc16f27d 128 targ_sita=PI/4;
DeguNaoto 0:b613dc16f27d 129 step=9;
DeguNaoto 0:b613dc16f27d 130 }
DeguNaoto 0:b613dc16f27d 131 if((step==9)&&(x<-10.0)) {
DeguNaoto 0:b613dc16f27d 132 targ_velocity=0.0;
DeguNaoto 0:b613dc16f27d 133 step=114;
DeguNaoto 0:b613dc16f27d 134 }
DeguNaoto 0:b613dc16f27d 135
DeguNaoto 0:b613dc16f27d 136 if((x>4620.0+deff)&&(CStep==7)){
DeguNaoto 0:b613dc16f27d 137 sendData(1,5);
DeguNaoto 0:b613dc16f27d 138 CStep=8;
DeguNaoto 0:b613dc16f27d 139 }
DeguNaoto 0:b613dc16f27d 140 if((x>5720.0+deff)&&(CStep==8)){
DeguNaoto 0:b613dc16f27d 141 sendData(1,4);
DeguNaoto 0:b613dc16f27d 142 CStep=9;
DeguNaoto 0:b613dc16f27d 143 }
DeguNaoto 0:b613dc16f27d 144 if((x<4000.0)&&(CStep==9)){
DeguNaoto 0:b613dc16f27d 145 sendData(7,0);
DeguNaoto 0:b613dc16f27d 146 CStep=114;
DeguNaoto 0:b613dc16f27d 147 }
DeguNaoto 0:b613dc16f27d 148 /////////////////////////////////////////////////////////////////////////
DeguNaoto 0:b613dc16f27d 149 /*****************************Opponents Mode****************************/
DeguNaoto 0:b613dc16f27d 150 if((step==15)&&((5700.0>x)&&(x>50.0))) {
DeguNaoto 6:ca3a74a93ae2 151 targ_sita=0.0;
DeguNaoto 0:b613dc16f27d 152 step=16;
DeguNaoto 0:b613dc16f27d 153 }
DeguNaoto 0:b613dc16f27d 154 if((step==16)&&(x>2850.0+deff)){
DeguNaoto 0:b613dc16f27d 155 targ_velocity=0.0;
DeguNaoto 0:b613dc16f27d 156 step=17;
DeguNaoto 0:b613dc16f27d 157 }
DeguNaoto 0:b613dc16f27d 158 if((step==17)&&((velocity<5000.0)&&(velocity>-5000.0))){
DeguNaoto 0:b613dc16f27d 159 flagf=0;
DeguNaoto 0:b613dc16f27d 160 spcount=0.0;
DeguNaoto 6:ca3a74a93ae2 161 targ_sita=0.0;
DeguNaoto 0:b613dc16f27d 162 step=18;
DeguNaoto 2:738b28f6a04b 163 direction_controller.reset();
DeguNaoto 2:738b28f6a04b 164 direction_controller.setBias(0.0);
DeguNaoto 0:b613dc16f27d 165 }
DeguNaoto 0:b613dc16f27d 166 if((step==18)&&(x<-100.0)){
DeguNaoto 0:b613dc16f27d 167 targ_velocity=0.0;
DeguNaoto 0:b613dc16f27d 168 targ_sita=PI/4.0;
DeguNaoto 0:b613dc16f27d 169 step=114;
DeguNaoto 0:b613dc16f27d 170 }
DeguNaoto 0:b613dc16f27d 171
DeguNaoto 0:b613dc16f27d 172 if((x>1050.0+deff)&&(CStep==15)){
DeguNaoto 0:b613dc16f27d 173 sendData(1,5);
DeguNaoto 0:b613dc16f27d 174 CStep=16;
DeguNaoto 0:b613dc16f27d 175 }
DeguNaoto 0:b613dc16f27d 176 if((x>2505.0+deff)&&(CStep==16)){
DeguNaoto 0:b613dc16f27d 177 sendData(1,4);
DeguNaoto 0:b613dc16f27d 178 CStep=17;
DeguNaoto 0:b613dc16f27d 179 }
DeguNaoto 0:b613dc16f27d 180 if((x<2400.0+deff)&&(CStep==17)){
DeguNaoto 0:b613dc16f27d 181 sendData(7,0);
DeguNaoto 0:b613dc16f27d 182 CStep=114;
DeguNaoto 0:b613dc16f27d 183 }
DeguNaoto 0:b613dc16f27d 184 #else
DeguNaoto 0:b613dc16f27d 185 //Red
DeguNaoto 0:b613dc16f27d 186 /********************************Nomal Mode*********************************/
DeguNaoto 0:b613dc16f27d 187 if((step==0)&&((8650.0>x)&&(x>800.0))) {
DeguNaoto 6:ca3a74a93ae2 188 targ_sita=0.0;
DeguNaoto 0:b613dc16f27d 189 step=1;
DeguNaoto 0:b613dc16f27d 190 }
DeguNaoto 0:b613dc16f27d 191 if((step==1)&&(x>8600.0+deff)) {
DeguNaoto 0:b613dc16f27d 192 targ_velocity=0.0;
DeguNaoto 6:ca3a74a93ae2 193 direction_controller.setBias(0.0);
DeguNaoto 2:738b28f6a04b 194 direction_controller.reset();
DeguNaoto 6:ca3a74a93ae2 195 velocity_controller.setBias(0.0);
DeguNaoto 6:ca3a74a93ae2 196 velocity_controller.reset();
DeguNaoto 0:b613dc16f27d 197 step=2;
DeguNaoto 0:b613dc16f27d 198 }
DeguNaoto 0:b613dc16f27d 199 if((step==2)&&((velocity<500.0)&&(velocity>-500.0))){
DeguNaoto 0:b613dc16f27d 200 step=3;
DeguNaoto 0:b613dc16f27d 201 spcount=0.0;
DeguNaoto 0:b613dc16f27d 202 flagf=0;
DeguNaoto 6:ca3a74a93ae2 203 targ_sita=0.0;
DeguNaoto 0:b613dc16f27d 204 }
DeguNaoto 6:ca3a74a93ae2 205 if((step==3)&&(x<1850.0)) {
DeguNaoto 0:b613dc16f27d 206 targ_sita=-PI/4;
DeguNaoto 0:b613dc16f27d 207 step=4;
DeguNaoto 0:b613dc16f27d 208 }
DeguNaoto 6:ca3a74a93ae2 209 if((step==4)&&(x<100.0)) {
DeguNaoto 0:b613dc16f27d 210 targ_velocity=0.0;
DeguNaoto 0:b613dc16f27d 211 step=114;
DeguNaoto 0:b613dc16f27d 212 }
DeguNaoto 0:b613dc16f27d 213 /***Cylinder***/
DeguNaoto 0:b613dc16f27d 214 if((x>3100.0+deff)&&(CStep==1)) {
DeguNaoto 0:b613dc16f27d 215 if(!skip) sendData(1,1);
DeguNaoto 0:b613dc16f27d 216 CStep=2;
DeguNaoto 0:b613dc16f27d 217 }
DeguNaoto 0:b613dc16f27d 218 // if((x>5940.0+deff)&&(CStep==2)) {
DeguNaoto 0:b613dc16f27d 219 if((x>6000.0+deff)&&(CStep==2)) {
DeguNaoto 0:b613dc16f27d 220 if(!skip) sendData(1,2);
DeguNaoto 0:b613dc16f27d 221 CStep=3;
DeguNaoto 0:b613dc16f27d 222 }
DeguNaoto 0:b613dc16f27d 223 if((x>7680.0+deff)&&(CStep==3)) {
DeguNaoto 0:b613dc16f27d 224 if(!skip) sendData(1,3);
DeguNaoto 0:b613dc16f27d 225 CStep=4;
DeguNaoto 0:b613dc16f27d 226 }
DeguNaoto 0:b613dc16f27d 227 if((x<6730.0+deff)&&(CStep==4)) {
DeguNaoto 0:b613dc16f27d 228 if(!skip) sendData(1,4);
DeguNaoto 0:b613dc16f27d 229 CStep=5;
DeguNaoto 0:b613dc16f27d 230 }
DeguNaoto 0:b613dc16f27d 231 if((x<6170.0+deff)&&(CStep==5)) {
DeguNaoto 0:b613dc16f27d 232 if(!skip) sendData(1,5);
DeguNaoto 0:b613dc16f27d 233 CStep=6;
DeguNaoto 0:b613dc16f27d 234 }
DeguNaoto 0:b613dc16f27d 235 if((x<4000.0)&&(CStep==6)){
DeguNaoto 0:b613dc16f27d 236 sendData(7,0);
DeguNaoto 0:b613dc16f27d 237 CStep=114;
DeguNaoto 0:b613dc16f27d 238 }
DeguNaoto 0:b613dc16f27d 239 /////////////////////////////////////////////////////////////////////////
DeguNaoto 0:b613dc16f27d 240 /******************************Middle Mode******************************/
DeguNaoto 0:b613dc16f27d 241 if((step==5)&&((5700.0>x)&&(x>800.0))) {
DeguNaoto 0:b613dc16f27d 242 targ_sita=-0.025;
DeguNaoto 0:b613dc16f27d 243 step=6;
DeguNaoto 0:b613dc16f27d 244 }
DeguNaoto 0:b613dc16f27d 245 if((step==6)&&(x>7000.0)){
DeguNaoto 0:b613dc16f27d 246 targ_velocity=0.0;
DeguNaoto 2:738b28f6a04b 247 direction_controller.reset();
DeguNaoto 2:738b28f6a04b 248 direction_controller.setBias(0.0);
DeguNaoto 0:b613dc16f27d 249 step=7;
DeguNaoto 0:b613dc16f27d 250 }
DeguNaoto 0:b613dc16f27d 251 if((step==7)&&((velocity<500.0)&&(velocity>-500.0))){
DeguNaoto 0:b613dc16f27d 252 flagf=0;
DeguNaoto 0:b613dc16f27d 253 spcount=0.0;
DeguNaoto 0:b613dc16f27d 254 targ_sita=-0.035;
DeguNaoto 0:b613dc16f27d 255 step=8;
DeguNaoto 0:b613dc16f27d 256 }
DeguNaoto 0:b613dc16f27d 257 if((step==8)&&(x<1820.0)) {
DeguNaoto 0:b613dc16f27d 258 targ_sita=-PI/4;
DeguNaoto 0:b613dc16f27d 259 step=9;
DeguNaoto 0:b613dc16f27d 260 }
DeguNaoto 0:b613dc16f27d 261 if((step==9)&&(x<-10.0)) {
DeguNaoto 0:b613dc16f27d 262 targ_velocity=0.0;
DeguNaoto 0:b613dc16f27d 263 step=114;
DeguNaoto 0:b613dc16f27d 264 }
DeguNaoto 0:b613dc16f27d 265
DeguNaoto 0:b613dc16f27d 266 if((x>4690.0+deff)&&(CStep==7)){
DeguNaoto 0:b613dc16f27d 267 sendData(1,4);
DeguNaoto 0:b613dc16f27d 268 CStep=8;
DeguNaoto 0:b613dc16f27d 269 }
DeguNaoto 0:b613dc16f27d 270 if((x>5790.0+deff)&&(CStep==8)){
DeguNaoto 0:b613dc16f27d 271 sendData(1,5);
DeguNaoto 0:b613dc16f27d 272 CStep=9;
DeguNaoto 0:b613dc16f27d 273 }
DeguNaoto 0:b613dc16f27d 274 if((x<4000.0)&&(CStep==9)){
DeguNaoto 0:b613dc16f27d 275 sendData(7,0);
DeguNaoto 0:b613dc16f27d 276 CStep=114;
DeguNaoto 0:b613dc16f27d 277 }
DeguNaoto 0:b613dc16f27d 278 /////////////////////////////////////////////////////////////////////////
DeguNaoto 0:b613dc16f27d 279 /*****************************Opponents Mode****************************/
DeguNaoto 0:b613dc16f27d 280 if((step==15)&&((5700.0>x)&&(x>50.0))) {
DeguNaoto 0:b613dc16f27d 281 // targ_sita=-0.035;
DeguNaoto 0:b613dc16f27d 282 targ_sita=-0.025;
DeguNaoto 0:b613dc16f27d 283 step=16;
DeguNaoto 0:b613dc16f27d 284 }
DeguNaoto 0:b613dc16f27d 285 // if((step==16)&&(x>2800.0+deff)){
DeguNaoto 0:b613dc16f27d 286 if((step==16)&&(x>2850.0+deff)){
DeguNaoto 0:b613dc16f27d 287 targ_velocity=0.0;
DeguNaoto 0:b613dc16f27d 288 step=17;
DeguNaoto 0:b613dc16f27d 289 }
DeguNaoto 0:b613dc16f27d 290 if((step==17)&&((velocity<5000.0)&&(velocity>-5000.0))){
DeguNaoto 0:b613dc16f27d 291 flagf=0;
DeguNaoto 0:b613dc16f27d 292 spcount=0.0;
DeguNaoto 0:b613dc16f27d 293 targ_sita=-0.035;
DeguNaoto 0:b613dc16f27d 294 step=18;
DeguNaoto 2:738b28f6a04b 295 direction_controller.reset();
DeguNaoto 2:738b28f6a04b 296 direction_controller.setBias(0.0);
DeguNaoto 0:b613dc16f27d 297 }
DeguNaoto 0:b613dc16f27d 298 if((step==18)&&(x<-100.0)){
DeguNaoto 0:b613dc16f27d 299 targ_velocity=0.0;
DeguNaoto 0:b613dc16f27d 300 targ_sita=-PI/4.0;
DeguNaoto 0:b613dc16f27d 301 step=114;
DeguNaoto 0:b613dc16f27d 302 }
DeguNaoto 0:b613dc16f27d 303
DeguNaoto 0:b613dc16f27d 304 // if((x>1350.0+deff)&&(CStep==15)){
DeguNaoto 0:b613dc16f27d 305 if((x>1400.0+deff)&&(CStep==15)){
DeguNaoto 0:b613dc16f27d 306 sendData(1,4);
DeguNaoto 0:b613dc16f27d 307 CStep=16;
DeguNaoto 0:b613dc16f27d 308 }
DeguNaoto 0:b613dc16f27d 309 if((x>2700.0+deff)&&(CStep==16)){
DeguNaoto 0:b613dc16f27d 310 // if((x>2650.0+deff)&&(CStep==16)){
DeguNaoto 0:b613dc16f27d 311 // if((x>2850.0+deff)&&(CStep==16)){
DeguNaoto 0:b613dc16f27d 312 sendData(1,5);
DeguNaoto 0:b613dc16f27d 313 CStep=17;
DeguNaoto 0:b613dc16f27d 314 }
DeguNaoto 0:b613dc16f27d 315 if((x<2400.0+deff)&&(CStep==17)){
DeguNaoto 0:b613dc16f27d 316 sendData(7,0);
DeguNaoto 0:b613dc16f27d 317 CStep=114;
DeguNaoto 0:b613dc16f27d 318
DeguNaoto 0:b613dc16f27d 319 }
DeguNaoto 0:b613dc16f27d 320 #endif
DeguNaoto 2:738b28f6a04b 321 #ifdef MESURE
DeguNaoto 2:738b28f6a04b 322 if(down) fclose(fp_r);
DeguNaoto 2:738b28f6a04b 323 if(b==9) fclose(fp_r);
DeguNaoto 2:738b28f6a04b 324 #endif
DeguNaoto 0:b613dc16f27d 325 }
DeguNaoto 0:b613dc16f27d 326 else if(!autoflag) {
DeguNaoto 0:b613dc16f27d 327 flaga=0;
DeguNaoto 3:8d8c25c556ae 328 mesureSwing();
DeguNaoto 0:b613dc16f27d 329 manualMoveIM920(); /*analogStick*/
DeguNaoto 0:b613dc16f27d 330 manualIM920(); /*IM920 button*/
DeguNaoto 0:b613dc16f27d 331 }
DeguNaoto 0:b613dc16f27d 332 /***update state***/
DeguNaoto 19:ed178d04e11d 333 // pc.printf("x:%f ,y:%f ,sita:%f ,r:%f\r\n",x,y,sita,Pulses_move_r);
DeguNaoto 19:ed178d04e11d 334 pc.printf("a2:%d ,b:%d ,X:%d ,Y:%d\r\n",a2,b,X,Y);
DeguNaoto 0:b613dc16f27d 335 // wait(RATE);
DeguNaoto 0:b613dc16f27d 336 }
DeguNaoto 4:09f684eac572 337 }