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@13:57d8e360e9aa, 2015-11-03 (annotated)

Committer:
DeguNaoto
Date:
Tue Nov 03 00:45:17 2015 +0000
Revision:
13:57d8e360e9aa
Parent:
10:8a2255451513
Child:
14:943e663694c3
????????????

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