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@37:75fcd28f48c7, 2015-11-15 (annotated)

Committer:
DeguNaoto
Date:
Sun Nov 15 11:45:29 2015 +0000
Revision:
37:75fcd28f48c7
Parent:
36:b8954b13a6d5
Child:
38:f4e9893641ac
20151115???????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DeguNaoto 0:b613dc16f27d 1 /**
DeguNaoto 36:b8954b13a6d5 2 * This program is written 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 Serial pc(USBTX, USBRX);
DeguNaoto 21:bdf8ac5c200c 19
DeguNaoto 24:6d2573d6f2b6 20 //LocalFileSystem local("local");
DeguNaoto 0:b613dc16f27d 21
DeguNaoto 0:b613dc16f27d 22 int main() {
DeguNaoto 24:6d2573d6f2b6 23 // FILE *fp_r = fopen("/local/velocity.dat", "w");
DeguNaoto 2:738b28f6a04b 24 Com.attach(&Call,RATE);
DeguNaoto 0:b613dc16f27d 25 initializeMotors();
DeguNaoto 0:b613dc16f27d 26 initializeControllers();
DeguNaoto 0:b613dc16f27d 27 initializeRS485();
DeguNaoto 14:943e663694c3 28 initializeMbedSerial();
DeguNaoto 24:6d2573d6f2b6 29 initializeSwing();
DeguNaoto 0:b613dc16f27d 30 #ifdef BLUE
DeguNaoto 0:b613dc16f27d 31 sita=PI/4.0,targ_sita=PI/4.0;
DeguNaoto 0:b613dc16f27d 32 IndicatorBLUE = 1;
DeguNaoto 0:b613dc16f27d 33 #else
DeguNaoto 0:b613dc16f27d 34 sita=-PI/4.0,targ_sita=-PI/4.0;
DeguNaoto 0:b613dc16f27d 35 IndicatorRED = 1;
DeguNaoto 0:b613dc16f27d 36 #endif
DeguNaoto 0:b613dc16f27d 37 Indicator4=1;
DeguNaoto 0:b613dc16f27d 38 Enable=1;
DeguNaoto 0:b613dc16f27d 39 wait(0.3);
DeguNaoto 0:b613dc16f27d 40 sendData(7,0);
DeguNaoto 0:b613dc16f27d 41 while(1) {
DeguNaoto 33:a4323c20494b 42 // if(modeflag){
DeguNaoto 0:b613dc16f27d 43 if(autoflag){
DeguNaoto 27:88863fab46c0 44 autoIM920(); /*IM920 button*/
DeguNaoto 24:6d2573d6f2b6 45 #ifdef BLUE
DeguNaoto 30:cd344beb415d 46 /********************************Own & Middle Mode*********************************/
DeguNaoto 36:b8954b13a6d5 47 // if((step==0)&&((10000.0>x)&&(x>1000.0))) {
DeguNaoto 36:b8954b13a6d5 48 if((step==0)&&((10000.0>x)&&(x>940.0))) {
DeguNaoto 33:a4323c20494b 49 targ_sita=-0.03;
DeguNaoto 21:bdf8ac5c200c 50 // targ_sita=0.0;
DeguNaoto 0:b613dc16f27d 51 step=1;
DeguNaoto 0:b613dc16f27d 52 }
DeguNaoto 30:cd344beb415d 53 if((step==1)&&(x>9000.0)) {
DeguNaoto 0:b613dc16f27d 54 targ_velocity=0.0;
DeguNaoto 3:8d8c25c556ae 55 velocity_controller.setBias(0.0);
DeguNaoto 3:8d8c25c556ae 56 velocity_controller.reset();
DeguNaoto 26:760f1bce8214 57 // dpcount=speed;
DeguNaoto 0:b613dc16f27d 58 step=2;
DeguNaoto 0:b613dc16f27d 59 }
DeguNaoto 0:b613dc16f27d 60 if((step==2)&&((velocity<500.0)&&(velocity>-500.0))){
DeguNaoto 0:b613dc16f27d 61 step=3;
DeguNaoto 0:b613dc16f27d 62 spcount=0.0;
DeguNaoto 0:b613dc16f27d 63 flagf=0;
DeguNaoto 24:6d2573d6f2b6 64 // targ_sita=0.0;
DeguNaoto 33:a4323c20494b 65 targ_sita=-0.03;
DeguNaoto 0:b613dc16f27d 66 }
DeguNaoto 37:75fcd28f48c7 67 if((step==3)&&(x<1800.0)) {
DeguNaoto 0:b613dc16f27d 68 targ_sita=PI/4;
DeguNaoto 0:b613dc16f27d 69 step=4;
DeguNaoto 0:b613dc16f27d 70 }
DeguNaoto 37:75fcd28f48c7 71 if((step==4)&&(x<750.0)) {
DeguNaoto 24:6d2573d6f2b6 72 dpcount=speed;
DeguNaoto 0:b613dc16f27d 73 step=114;
DeguNaoto 0:b613dc16f27d 74 }
DeguNaoto 26:760f1bce8214 75
DeguNaoto 30:cd344beb415d 76 //Cylinder
DeguNaoto 36:b8954b13a6d5 77 if((x>3250.0)&&(CStep==0)) {
DeguNaoto 23:26f9483439fe 78 if(!skip) sendData(1,1);
DeguNaoto 30:cd344beb415d 79 CStep=1;
DeguNaoto 0:b613dc16f27d 80 }
DeguNaoto 35:7b6786193aa2 81 if((x>6100.0)&&(CStep==1)) {
DeguNaoto 30:cd344beb415d 82 if(!skip) sendData(1,2);
DeguNaoto 30:cd344beb415d 83 CStep=2;
DeguNaoto 30:cd344beb415d 84 }
DeguNaoto 34:aa2a5c888a27 85 if((x>7750.0)&&(CStep==2)) {
DeguNaoto 0:b613dc16f27d 86 if(!skip) sendData(1,3);
DeguNaoto 0:b613dc16f27d 87 CStep=3;
DeguNaoto 0:b613dc16f27d 88 }
DeguNaoto 36:b8954b13a6d5 89 if((x>8000.0)&&(CStep==3)) {
DeguNaoto 36:b8954b13a6d5 90 if(!skip) sendData(1,8);
DeguNaoto 0:b613dc16f27d 91 CStep=4;
DeguNaoto 0:b613dc16f27d 92 }
DeguNaoto 37:75fcd28f48c7 93 if((x>8500.0)&&(CStep==4)) CStep=5;
DeguNaoto 37:75fcd28f48c7 94 /////////////////////////////////////////
DeguNaoto 36:b8954b13a6d5 95 /*if((x<7200.0)&&(CStep==4)) {
DeguNaoto 36:b8954b13a6d5 96 if(!skip) sendData(1,4); //over
DeguNaoto 36:b8954b13a6d5 97 CStep=5;
DeguNaoto 36:b8954b13a6d5 98 }
DeguNaoto 36:b8954b13a6d5 99 if((x<6950.0)&&(CStep==5)) {
DeguNaoto 36:b8954b13a6d5 100 if(!skip) sendData(1,6); //fornt
DeguNaoto 36:b8954b13a6d5 101 CStep=6;
DeguNaoto 36:b8954b13a6d5 102 }
DeguNaoto 36:b8954b13a6d5 103 if((x<6700.0)&&(CStep==6)) {
DeguNaoto 36:b8954b13a6d5 104 if(!skip) sendData(1,5); //middle
DeguNaoto 36:b8954b13a6d5 105 CStep=7;
DeguNaoto 36:b8954b13a6d5 106 }*/
DeguNaoto 36:b8954b13a6d5 107 /////////////////////////////////////////
DeguNaoto 37:75fcd28f48c7 108 if((x<7870.0)&&(CStep==5)) {
DeguNaoto 35:7b6786193aa2 109 if(!skip) sendData(1,6); //front
DeguNaoto 0:b613dc16f27d 110 CStep=6;
DeguNaoto 0:b613dc16f27d 111 }
DeguNaoto 37:75fcd28f48c7 112 if((x<6800.0)&&(CStep==6)) {
DeguNaoto 37:75fcd28f48c7 113 if(!skip) sendData(1,5); //middle
DeguNaoto 36:b8954b13a6d5 114 CStep=7;
DeguNaoto 36:b8954b13a6d5 115 }
DeguNaoto 37:75fcd28f48c7 116 if((x<5550.0)&&(CStep==7)) {
DeguNaoto 37:75fcd28f48c7 117 if(!skip) sendData(1,4); //over
DeguNaoto 37:75fcd28f48c7 118 CStep=8;
DeguNaoto 37:75fcd28f48c7 119 }
DeguNaoto 36:b8954b13a6d5 120 /////////////////////////////////////////
DeguNaoto 37:75fcd28f48c7 121 if((x<3000.0)&&(CStep==8)){
DeguNaoto 30:cd344beb415d 122 sendData(7,0);
DeguNaoto 30:cd344beb415d 123 CStep=114;
DeguNaoto 27:88863fab46c0 124 }
DeguNaoto 27:88863fab46c0 125
DeguNaoto 30:cd344beb415d 126 /********************************Own & Opponent Mode*********************************/
DeguNaoto 37:75fcd28f48c7 127 if((step==10)&&((10000.0>x)&&(x>700.0))) {
DeguNaoto 33:a4323c20494b 128 targ_sita=-0.03;
DeguNaoto 30:cd344beb415d 129 // targ_sita=0.0;
DeguNaoto 30:cd344beb415d 130 step=11;
DeguNaoto 30:cd344beb415d 131 }
DeguNaoto 30:cd344beb415d 132 if((step==11)&&(x>10000.0)) {
DeguNaoto 30:cd344beb415d 133 targ_velocity=0.0;
DeguNaoto 30:cd344beb415d 134 velocity_controller.setBias(0.0);
DeguNaoto 30:cd344beb415d 135 velocity_controller.reset();
DeguNaoto 30:cd344beb415d 136 step=12;
DeguNaoto 30:cd344beb415d 137 }
DeguNaoto 30:cd344beb415d 138 if((step==12)&&((velocity<500.0)&&(velocity>-500.0))){
DeguNaoto 30:cd344beb415d 139 step=13;
DeguNaoto 30:cd344beb415d 140 spcount=0.0;
DeguNaoto 30:cd344beb415d 141 flagf=0;
DeguNaoto 35:7b6786193aa2 142 // flagf=3;
DeguNaoto 30:cd344beb415d 143 // targ_sita=0.0;
DeguNaoto 33:a4323c20494b 144 targ_sita=-0.03;
DeguNaoto 30:cd344beb415d 145 }
DeguNaoto 37:75fcd28f48c7 146 if((step==13)&&(x<1450.0)) {
DeguNaoto 30:cd344beb415d 147 targ_sita=PI/4;
DeguNaoto 30:cd344beb415d 148 step=14;
DeguNaoto 30:cd344beb415d 149 }
DeguNaoto 30:cd344beb415d 150 if((step==14)&&(x<600.0)) {
DeguNaoto 30:cd344beb415d 151 dpcount=speed;
DeguNaoto 30:cd344beb415d 152 step=114;
DeguNaoto 30:cd344beb415d 153 }
DeguNaoto 30:cd344beb415d 154
DeguNaoto 33:a4323c20494b 155 //Cylinderd
DeguNaoto 35:7b6786193aa2 156 if((x>3200.0)&&(CStep==10)) {
DeguNaoto 35:7b6786193aa2 157 if(!skip) sendData(1,1);
DeguNaoto 35:7b6786193aa2 158 CStep=11;
DeguNaoto 35:7b6786193aa2 159 }
DeguNaoto 35:7b6786193aa2 160 if((x>6050.0)&&(CStep==11)) {
DeguNaoto 35:7b6786193aa2 161 if(!skip) sendData(1,2);
DeguNaoto 30:cd344beb415d 162 CStep=12;
DeguNaoto 30:cd344beb415d 163 }
DeguNaoto 35:7b6786193aa2 164 if((x>7700.0)&&(CStep==12)) {
DeguNaoto 35:7b6786193aa2 165 if(!skip) sendData(1,3);
DeguNaoto 30:cd344beb415d 166 CStep=13;
DeguNaoto 30:cd344beb415d 167 }
DeguNaoto 37:75fcd28f48c7 168 if((x>9800.0)&&(CStep==13)) {
DeguNaoto 37:75fcd28f48c7 169 sendData(1,8);
DeguNaoto 36:b8954b13a6d5 170 CStep=14;
DeguNaoto 36:b8954b13a6d5 171 }
DeguNaoto 37:75fcd28f48c7 172 if((x>9900.0)&&(CStep==14)) CStep=15;
DeguNaoto 37:75fcd28f48c7 173 if((x<9680.0)&&(CStep==15)) { //7.8
DeguNaoto 35:7b6786193aa2 174 if(!skip) sendData(1,4);
DeguNaoto 35:7b6786193aa2 175 CStep=16;
DeguNaoto 35:7b6786193aa2 176 }
DeguNaoto 37:75fcd28f48c7 177 if((x<7900.0)&&(CStep==16)) { //7.8
DeguNaoto 37:75fcd28f48c7 178 // if((x<7950.0)&&(CStep==16)) { //7.8
DeguNaoto 37:75fcd28f48c7 179 if(!skip) sendData(1,5);
DeguNaoto 35:7b6786193aa2 180 CStep=17;
DeguNaoto 30:cd344beb415d 181 }
DeguNaoto 37:75fcd28f48c7 182 if((x<5600.0)&&(CStep==17)) { //7.8
DeguNaoto 37:75fcd28f48c7 183 // if((x<5750.0)&&(CStep==17)) { //7.8
DeguNaoto 37:75fcd28f48c7 184 if(!skip) sendData(1,6);
DeguNaoto 37:75fcd28f48c7 185 CStep=18;
DeguNaoto 37:75fcd28f48c7 186 }
DeguNaoto 37:75fcd28f48c7 187 if((x<1000.0)&&(CStep==18)){
DeguNaoto 0:b613dc16f27d 188 sendData(7,0);
DeguNaoto 0:b613dc16f27d 189 CStep=114;
DeguNaoto 0:b613dc16f27d 190 }
DeguNaoto 37:75fcd28f48c7 191
DeguNaoto 24:6d2573d6f2b6 192 #else
DeguNaoto 34:aa2a5c888a27 193 /********************************Own & Middle Mode*********************************/
DeguNaoto 36:b8954b13a6d5 194 if((step==0)&&((10000.0>x)&&(x>800.0))) {
DeguNaoto 34:aa2a5c888a27 195 targ_sita=0.03;
DeguNaoto 34:aa2a5c888a27 196 // targ_sita=0.0;
DeguNaoto 34:aa2a5c888a27 197 step=1;
DeguNaoto 34:aa2a5c888a27 198 }
DeguNaoto 34:aa2a5c888a27 199 if((step==1)&&(x>9000.0)) {
DeguNaoto 34:aa2a5c888a27 200 targ_velocity=0.0;
DeguNaoto 34:aa2a5c888a27 201 velocity_controller.setBias(0.0);
DeguNaoto 34:aa2a5c888a27 202 velocity_controller.reset();
DeguNaoto 34:aa2a5c888a27 203 // dpcount=speed;
DeguNaoto 34:aa2a5c888a27 204 step=2;
DeguNaoto 34:aa2a5c888a27 205 }
DeguNaoto 34:aa2a5c888a27 206 if((step==2)&&((velocity<500.0)&&(velocity>-500.0))){
DeguNaoto 34:aa2a5c888a27 207 step=3;
DeguNaoto 34:aa2a5c888a27 208 spcount=0.0;
DeguNaoto 34:aa2a5c888a27 209 flagf=0;
DeguNaoto 34:aa2a5c888a27 210 // targ_sita=0.0;
DeguNaoto 36:b8954b13a6d5 211 targ_sita=0.03;
DeguNaoto 34:aa2a5c888a27 212 }
DeguNaoto 34:aa2a5c888a27 213 if((step==3)&&(x<1700.0)) {
DeguNaoto 34:aa2a5c888a27 214 targ_sita=-PI/4;
DeguNaoto 34:aa2a5c888a27 215 step=4;
DeguNaoto 34:aa2a5c888a27 216 }
DeguNaoto 34:aa2a5c888a27 217 if((step==4)&&(x<800.0)) {
DeguNaoto 34:aa2a5c888a27 218 dpcount=speed;
DeguNaoto 34:aa2a5c888a27 219 step=114;
DeguNaoto 34:aa2a5c888a27 220 }
DeguNaoto 34:aa2a5c888a27 221
DeguNaoto 34:aa2a5c888a27 222 //Cylinder
DeguNaoto 36:b8954b13a6d5 223 if((x>3100.0)&&(CStep==0)) {
DeguNaoto 34:aa2a5c888a27 224 if(!skip) sendData(1,1);
DeguNaoto 34:aa2a5c888a27 225 CStep=1;
DeguNaoto 34:aa2a5c888a27 226 }
DeguNaoto 36:b8954b13a6d5 227 if((x>6050.0)&&(CStep==1)) {
DeguNaoto 34:aa2a5c888a27 228 if(!skip) sendData(1,3);
DeguNaoto 34:aa2a5c888a27 229 CStep=2;
DeguNaoto 34:aa2a5c888a27 230 }
DeguNaoto 36:b8954b13a6d5 231 if((x>7650.0)&&(CStep==2)) {
DeguNaoto 34:aa2a5c888a27 232 if(!skip) sendData(1,2);
DeguNaoto 34:aa2a5c888a27 233 CStep=3;
DeguNaoto 34:aa2a5c888a27 234 }
DeguNaoto 36:b8954b13a6d5 235 if((x>8000.0)&&(CStep==3)) {
DeguNaoto 36:b8954b13a6d5 236 if(!skip) sendData(1,8);
DeguNaoto 34:aa2a5c888a27 237 CStep=4;
DeguNaoto 34:aa2a5c888a27 238 }
DeguNaoto 36:b8954b13a6d5 239 if((x<7200.0)&&(CStep==4)) {
DeguNaoto 36:b8954b13a6d5 240 if(!skip) sendData(1,6); //over
DeguNaoto 34:aa2a5c888a27 241 CStep=5;
DeguNaoto 34:aa2a5c888a27 242 }
DeguNaoto 36:b8954b13a6d5 243 if((x<6950.0)&&(CStep==5)) {
DeguNaoto 34:aa2a5c888a27 244 if(!skip) sendData(1,4); //front
DeguNaoto 34:aa2a5c888a27 245 CStep=6;
DeguNaoto 34:aa2a5c888a27 246 }
DeguNaoto 36:b8954b13a6d5 247 if((x<6700.0)&&(CStep==6)) {
DeguNaoto 36:b8954b13a6d5 248 if(!skip) sendData(1,5); //middle
DeguNaoto 36:b8954b13a6d5 249 CStep=7;
DeguNaoto 36:b8954b13a6d5 250 }
DeguNaoto 36:b8954b13a6d5 251 if((x<3000.0)&&(CStep==7)){
DeguNaoto 36:b8954b13a6d5 252 sendData(7,0);
DeguNaoto 36:b8954b13a6d5 253 CStep=114;
DeguNaoto 36:b8954b13a6d5 254 }
DeguNaoto 36:b8954b13a6d5 255
DeguNaoto 36:b8954b13a6d5 256 /********************************Own & Opponent Mode*********************************/
DeguNaoto 36:b8954b13a6d5 257 if((step==10)&&((9800.0>x)&&(x>700.0))) {
DeguNaoto 36:b8954b13a6d5 258 targ_sita=-0.03;
DeguNaoto 36:b8954b13a6d5 259 // targ_sita=0.0;
DeguNaoto 36:b8954b13a6d5 260 step=11;
DeguNaoto 36:b8954b13a6d5 261 }
DeguNaoto 36:b8954b13a6d5 262 if((step==11)&&(x>10000.0)) {
DeguNaoto 36:b8954b13a6d5 263 targ_velocity=0.0;
DeguNaoto 36:b8954b13a6d5 264 velocity_controller.setBias(0.0);
DeguNaoto 36:b8954b13a6d5 265 velocity_controller.reset();
DeguNaoto 36:b8954b13a6d5 266 step=12;
DeguNaoto 36:b8954b13a6d5 267 }
DeguNaoto 36:b8954b13a6d5 268 if((step==12)&&((velocity<500.0)&&(velocity>-500.0))){
DeguNaoto 36:b8954b13a6d5 269 step=13;
DeguNaoto 36:b8954b13a6d5 270 spcount=0.0;
DeguNaoto 36:b8954b13a6d5 271 flagf=0;
DeguNaoto 36:b8954b13a6d5 272 // flagf=3;
DeguNaoto 36:b8954b13a6d5 273 // targ_sita=0.0;
DeguNaoto 36:b8954b13a6d5 274 targ_sita=-0.03;
DeguNaoto 36:b8954b13a6d5 275 }
DeguNaoto 36:b8954b13a6d5 276 if((step==13)&&(x<1400.0)) {
DeguNaoto 36:b8954b13a6d5 277 targ_sita=PI/4;
DeguNaoto 36:b8954b13a6d5 278 step=14;
DeguNaoto 36:b8954b13a6d5 279 }
DeguNaoto 36:b8954b13a6d5 280 if((step==14)&&(x<600.0)) {
DeguNaoto 36:b8954b13a6d5 281 dpcount=speed;
DeguNaoto 36:b8954b13a6d5 282 step=114;
DeguNaoto 36:b8954b13a6d5 283 }
DeguNaoto 36:b8954b13a6d5 284
DeguNaoto 36:b8954b13a6d5 285 //Cylinderd
DeguNaoto 36:b8954b13a6d5 286 if((x>3200.0)&&(CStep==10)) {
DeguNaoto 36:b8954b13a6d5 287 if(!skip) sendData(1,1);
DeguNaoto 36:b8954b13a6d5 288 CStep=11;
DeguNaoto 36:b8954b13a6d5 289 }
DeguNaoto 36:b8954b13a6d5 290 if((x>6050.0)&&(CStep==11)) {
DeguNaoto 36:b8954b13a6d5 291 if(!skip) sendData(1,3);
DeguNaoto 36:b8954b13a6d5 292 CStep=12;
DeguNaoto 36:b8954b13a6d5 293 }
DeguNaoto 36:b8954b13a6d5 294 if((x>7700.0)&&(CStep==12)) {
DeguNaoto 36:b8954b13a6d5 295 if(!skip) sendData(1,2);
DeguNaoto 36:b8954b13a6d5 296 CStep=13;
DeguNaoto 36:b8954b13a6d5 297 }
DeguNaoto 36:b8954b13a6d5 298 if((x>9500.0)&&(CStep==13)) {
DeguNaoto 36:b8954b13a6d5 299 if(!skip) sendData(1,8);
DeguNaoto 36:b8954b13a6d5 300 CStep=14;
DeguNaoto 36:b8954b13a6d5 301 }
DeguNaoto 36:b8954b13a6d5 302 if((x<9400.0)&&(CStep==14)) {
DeguNaoto 36:b8954b13a6d5 303 if(!skip) sendData(1,6); //over
DeguNaoto 36:b8954b13a6d5 304 CStep=15;
DeguNaoto 36:b8954b13a6d5 305 }
DeguNaoto 37:75fcd28f48c7 306 if((x<6600.0)&&(CStep==15)) {
DeguNaoto 36:b8954b13a6d5 307 if(!skip) sendData(1,5); //middle
DeguNaoto 36:b8954b13a6d5 308 CStep=16;
DeguNaoto 36:b8954b13a6d5 309 }
DeguNaoto 37:75fcd28f48c7 310 if((x<4820.0)&&(CStep==16)) {
DeguNaoto 37:75fcd28f48c7 311 // if((x<4850.0)&&(CStep==16)) {
DeguNaoto 36:b8954b13a6d5 312 if(!skip) sendData(1,4); //front
DeguNaoto 36:b8954b13a6d5 313 CStep=17;
DeguNaoto 36:b8954b13a6d5 314 }
DeguNaoto 36:b8954b13a6d5 315 if((x<1000.0)&&(CStep==17)){
DeguNaoto 34:aa2a5c888a27 316 sendData(7,0);
DeguNaoto 34:aa2a5c888a27 317 CStep=114;
DeguNaoto 34:aa2a5c888a27 318 }
DeguNaoto 24:6d2573d6f2b6 319 #endif
DeguNaoto 0:b613dc16f27d 320 }
DeguNaoto 33:a4323c20494b 321 // else if(!modeflag) {
DeguNaoto 33:a4323c20494b 322 else if(!autoflag){
DeguNaoto 0:b613dc16f27d 323 manualIM920(); /*IM920 button*/
DeguNaoto 34:aa2a5c888a27 324 #ifdef BLUE
DeguNaoto 36:b8954b13a6d5 325 /********************************Swing Mode(middle)*********************************/
DeguNaoto 37:75fcd28f48c7 326 if((mstep==0)&&((10000.0>x)&&(x>1550.0))) {
DeguNaoto 30:cd344beb415d 327 dpcount=speed;
DeguNaoto 33:a4323c20494b 328 mstep=1;
DeguNaoto 33:a4323c20494b 329 }
DeguNaoto 33:a4323c20494b 330 if((mstep==1)&&((velocity<5.0)&&(velocity>-5.0))){
DeguNaoto 34:aa2a5c888a27 331 targ_sita=PI/4.0;
DeguNaoto 37:75fcd28f48c7 332 swingmoved=2;
DeguNaoto 37:75fcd28f48c7 333 mstep=251;
DeguNaoto 37:75fcd28f48c7 334 }
DeguNaoto 37:75fcd28f48c7 335 if((mstep==251)&&(((targ_sita-sita)<0.01)&&((targ_sita-sita)>-0.01))){
DeguNaoto 37:75fcd28f48c7 336 flagf=2;
DeguNaoto 36:b8954b13a6d5 337 }
DeguNaoto 36:b8954b13a6d5 338 if(mstep==2){
DeguNaoto 36:b8954b13a6d5 339 mstep=3;
DeguNaoto 37:75fcd28f48c7 340 targ_sita=3.0*PI/4.0-PI/8.0;
DeguNaoto 37:75fcd28f48c7 341 sendData(7,0);
DeguNaoto 37:75fcd28f48c7 342 flagf=0;
DeguNaoto 36:b8954b13a6d5 343 }
DeguNaoto 36:b8954b13a6d5 344 if((mstep==3)&&(((targ_sita-sita)<0.05)&&((targ_sita-sita)>-0.05))){
DeguNaoto 36:b8954b13a6d5 345 flagf=1;
DeguNaoto 36:b8954b13a6d5 346 targ_velocity=speed;
DeguNaoto 36:b8954b13a6d5 347 mstep=4;
DeguNaoto 36:b8954b13a6d5 348 }
DeguNaoto 37:75fcd28f48c7 349 if((mstep==4)&&(x<1300.0)){
DeguNaoto 37:75fcd28f48c7 350 targ_sita=5*PI/6.0;
DeguNaoto 36:b8954b13a6d5 351 mstep=5;
DeguNaoto 36:b8954b13a6d5 352 }
DeguNaoto 37:75fcd28f48c7 353 if((mstep==5)&&(x<-10.0)){
DeguNaoto 36:b8954b13a6d5 354 targ_velocity=0.0;
DeguNaoto 37:75fcd28f48c7 355 targ_sita=3.0*PI/4.0;
DeguNaoto 30:cd344beb415d 356 mstep=114;
DeguNaoto 30:cd344beb415d 357 }
DeguNaoto 30:cd344beb415d 358
DeguNaoto 36:b8954b13a6d5 359
DeguNaoto 36:b8954b13a6d5 360 /********************************Swing Mode(over)*********************************/
DeguNaoto 37:75fcd28f48c7 361 // if((mstep==10)&&((10000.0>x)&&(x>1500.0))) {
DeguNaoto 37:75fcd28f48c7 362 if((mstep==10)&&((10000.0>x)&&(x>1550.0))) {
DeguNaoto 36:b8954b13a6d5 363 dpcount=speed;
DeguNaoto 33:a4323c20494b 364 mstep=11;
DeguNaoto 33:a4323c20494b 365 }
DeguNaoto 36:b8954b13a6d5 366 if((mstep==11)&&((velocity<5.0)&&(velocity>-5.0))){
DeguNaoto 37:75fcd28f48c7 367 targ_sita=PI/4.0+PI/16.0+PI/45.0;
DeguNaoto 36:b8954b13a6d5 368 swingmoved=2;
DeguNaoto 37:75fcd28f48c7 369 mstep=252;
DeguNaoto 37:75fcd28f48c7 370 }
DeguNaoto 37:75fcd28f48c7 371 if((mstep==252)&&(((targ_sita-sita)<0.01)&&((targ_sita-sita)>-0.01))){
DeguNaoto 37:75fcd28f48c7 372 flagf=2;
DeguNaoto 36:b8954b13a6d5 373 }
DeguNaoto 36:b8954b13a6d5 374 if(mstep==12){
DeguNaoto 33:a4323c20494b 375 mstep=13;
DeguNaoto 37:75fcd28f48c7 376 targ_sita=3.0*PI/4.0-PI/8.0;
DeguNaoto 37:75fcd28f48c7 377 sendData(7,0);
DeguNaoto 37:75fcd28f48c7 378 flagf=0;
DeguNaoto 33:a4323c20494b 379 }
DeguNaoto 36:b8954b13a6d5 380 if((mstep==13)&&(((targ_sita-sita)<0.05)&&((targ_sita-sita)>-0.05))){
DeguNaoto 36:b8954b13a6d5 381 flagf=1;
DeguNaoto 36:b8954b13a6d5 382 targ_velocity=speed;
DeguNaoto 33:a4323c20494b 383 mstep=14;
DeguNaoto 33:a4323c20494b 384 }
DeguNaoto 37:75fcd28f48c7 385 if((mstep==14)&&(x<1300.0)){
DeguNaoto 37:75fcd28f48c7 386 targ_sita=5*PI/6.0;
DeguNaoto 36:b8954b13a6d5 387 mstep=15;
DeguNaoto 36:b8954b13a6d5 388 }
DeguNaoto 37:75fcd28f48c7 389 if((mstep==15)&&(x<-10.0)){
DeguNaoto 36:b8954b13a6d5 390 targ_velocity=0.0;
DeguNaoto 37:75fcd28f48c7 391 targ_sita=3.0*PI/4.0;
DeguNaoto 36:b8954b13a6d5 392 mstep=114;
DeguNaoto 36:b8954b13a6d5 393 }
DeguNaoto 36:b8954b13a6d5 394
DeguNaoto 37:75fcd28f48c7 395 /********************************Swing Mode(front)*********************************/
DeguNaoto 36:b8954b13a6d5 396 if((mstep==20)&&((10000.0>x)&&(x>1500.0))) {
DeguNaoto 36:b8954b13a6d5 397 dpcount=speed;
DeguNaoto 36:b8954b13a6d5 398 mstep=21;
DeguNaoto 36:b8954b13a6d5 399 }
DeguNaoto 36:b8954b13a6d5 400 if((mstep==21)&&((velocity<5.0)&&(velocity>-5.0))){
DeguNaoto 36:b8954b13a6d5 401 targ_sita=PI/4.0+PI/16.0;
DeguNaoto 36:b8954b13a6d5 402 swingmoved=3;
DeguNaoto 33:a4323c20494b 403 mstep=114;
DeguNaoto 36:b8954b13a6d5 404 }
DeguNaoto 36:b8954b13a6d5 405 if(mstep==22){
DeguNaoto 36:b8954b13a6d5 406 mstep=23;
DeguNaoto 36:b8954b13a6d5 407 targ_sita=3.0*PI/4.0;
DeguNaoto 37:75fcd28f48c7 408 sendData(7,0);
DeguNaoto 36:b8954b13a6d5 409 }
DeguNaoto 36:b8954b13a6d5 410 if((mstep==23)&&(((targ_sita-sita)<0.05)&&((targ_sita-sita)>-0.05))){
DeguNaoto 36:b8954b13a6d5 411 flagf=1;
DeguNaoto 36:b8954b13a6d5 412 targ_velocity=speed;
DeguNaoto 36:b8954b13a6d5 413 mstep=24;
DeguNaoto 36:b8954b13a6d5 414 }
DeguNaoto 37:75fcd28f48c7 415 if((mstep==24)&&(x<1300.0)){
DeguNaoto 36:b8954b13a6d5 416 targ_sita=PI;
DeguNaoto 36:b8954b13a6d5 417 mstep=25;
DeguNaoto 36:b8954b13a6d5 418 }
DeguNaoto 37:75fcd28f48c7 419 if((mstep==25)&&(x<-10.0)){
DeguNaoto 36:b8954b13a6d5 420 targ_velocity=0.0;
DeguNaoto 36:b8954b13a6d5 421 mstep=114;
DeguNaoto 36:b8954b13a6d5 422 }
DeguNaoto 36:b8954b13a6d5 423
DeguNaoto 34:aa2a5c888a27 424 #else
DeguNaoto 34:aa2a5c888a27 425 #endif
DeguNaoto 0:b613dc16f27d 426 }
DeguNaoto 34:aa2a5c888a27 427 // pc.printf("b:%d\r\n",swingRadVelocity);
DeguNaoto 24:6d2573d6f2b6 428 // pc.printf("Swing:%f\r\n",SwingSens.getPulses());
DeguNaoto 34:aa2a5c888a27 429 pc.printf("x:%f ,y:%f ,sita:%f ,r:%f\r\n",x,y,sita,Pulses_move_r);
DeguNaoto 0:b613dc16f27d 430 }
DeguNaoto 4:09f684eac572 431 }