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@22:4127fcf8c7d7, 2015-11-07 (annotated)

Committer:
DeguNaoto
Date:
Sat Nov 07 00:16:09 2015 +0000
Revision:
22:4127fcf8c7d7
Parent:
20:39a51962ed38 
20151107 ????????

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