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@36:b8954b13a6d5, 2015-11-15 (annotated)

Committer:
DeguNaoto
Date:
Sun Nov 15 00:30:44 2015 +0000
Revision:
36:b8954b13a6d5
Parent:
35:7b6786193aa2
Child:
37:75fcd28f48c7
20151115 Own&middle??????

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 33:a4323c20494b 67 if((step==3)&&(x<1700.0)) {
DeguNaoto 0:b613dc16f27d 68 targ_sita=PI/4;
DeguNaoto 0:b613dc16f27d 69 step=4;
DeguNaoto 0:b613dc16f27d 70 }
DeguNaoto 33:a4323c20494b 71 if((step==4)&&(x<800.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 36:b8954b13a6d5 93 /*if((x<7200.0)&&(CStep==4)) {
DeguNaoto 36:b8954b13a6d5 94 if(!skip) sendData(1,4); //over
DeguNaoto 36:b8954b13a6d5 95 CStep=5;
DeguNaoto 36:b8954b13a6d5 96 }
DeguNaoto 36:b8954b13a6d5 97 if((x<6950.0)&&(CStep==5)) {
DeguNaoto 36:b8954b13a6d5 98 if(!skip) sendData(1,6); //fornt
DeguNaoto 36:b8954b13a6d5 99 CStep=6;
DeguNaoto 36:b8954b13a6d5 100 }
DeguNaoto 36:b8954b13a6d5 101 if((x<6700.0)&&(CStep==6)) {
DeguNaoto 36:b8954b13a6d5 102 if(!skip) sendData(1,5); //middle
DeguNaoto 36:b8954b13a6d5 103 CStep=7;
DeguNaoto 36:b8954b13a6d5 104 }*/
DeguNaoto 36:b8954b13a6d5 105 /////////////////////////////////////////
DeguNaoto 36:b8954b13a6d5 106 if((x<7850.0)&&(CStep==4)) {
DeguNaoto 35:7b6786193aa2 107 if(!skip) sendData(1,6); //front
DeguNaoto 0:b613dc16f27d 108 CStep=5;
DeguNaoto 0:b613dc16f27d 109 }
DeguNaoto 35:7b6786193aa2 110 if((x<6700.0)&&(CStep==5)) {
DeguNaoto 35:7b6786193aa2 111 if(!skip) sendData(1,5); //middle
DeguNaoto 0:b613dc16f27d 112 CStep=6;
DeguNaoto 0:b613dc16f27d 113 }
DeguNaoto 36:b8954b13a6d5 114 if((x<5600.0)&&(CStep==6)) {
DeguNaoto 36:b8954b13a6d5 115 if(!skip) sendData(1,4); //over
DeguNaoto 36:b8954b13a6d5 116 CStep=7;
DeguNaoto 36:b8954b13a6d5 117 }
DeguNaoto 36:b8954b13a6d5 118 /////////////////////////////////////////
DeguNaoto 36:b8954b13a6d5 119 if((x<3000.0)&&(CStep==7)){
DeguNaoto 30:cd344beb415d 120 sendData(7,0);
DeguNaoto 30:cd344beb415d 121 CStep=114;
DeguNaoto 27:88863fab46c0 122 }
DeguNaoto 27:88863fab46c0 123
DeguNaoto 30:cd344beb415d 124 /********************************Own & Opponent Mode*********************************/
DeguNaoto 35:7b6786193aa2 125 if((step==10)&&((9800.0>x)&&(x>700.0))) {
DeguNaoto 33:a4323c20494b 126 targ_sita=-0.03;
DeguNaoto 30:cd344beb415d 127 // targ_sita=0.0;
DeguNaoto 30:cd344beb415d 128 step=11;
DeguNaoto 30:cd344beb415d 129 }
DeguNaoto 30:cd344beb415d 130 if((step==11)&&(x>10000.0)) {
DeguNaoto 30:cd344beb415d 131 targ_velocity=0.0;
DeguNaoto 30:cd344beb415d 132 velocity_controller.setBias(0.0);
DeguNaoto 30:cd344beb415d 133 velocity_controller.reset();
DeguNaoto 30:cd344beb415d 134 step=12;
DeguNaoto 30:cd344beb415d 135 }
DeguNaoto 30:cd344beb415d 136 if((step==12)&&((velocity<500.0)&&(velocity>-500.0))){
DeguNaoto 30:cd344beb415d 137 step=13;
DeguNaoto 30:cd344beb415d 138 spcount=0.0;
DeguNaoto 30:cd344beb415d 139 flagf=0;
DeguNaoto 35:7b6786193aa2 140 // flagf=3;
DeguNaoto 30:cd344beb415d 141 // targ_sita=0.0;
DeguNaoto 33:a4323c20494b 142 targ_sita=-0.03;
DeguNaoto 30:cd344beb415d 143 }
DeguNaoto 30:cd344beb415d 144 if((step==13)&&(x<1400.0)) {
DeguNaoto 30:cd344beb415d 145 targ_sita=PI/4;
DeguNaoto 30:cd344beb415d 146 step=14;
DeguNaoto 30:cd344beb415d 147 }
DeguNaoto 30:cd344beb415d 148 if((step==14)&&(x<600.0)) {
DeguNaoto 30:cd344beb415d 149 dpcount=speed;
DeguNaoto 30:cd344beb415d 150 step=114;
DeguNaoto 30:cd344beb415d 151 }
DeguNaoto 30:cd344beb415d 152
DeguNaoto 33:a4323c20494b 153 //Cylinderd
DeguNaoto 35:7b6786193aa2 154 if((x>3200.0)&&(CStep==10)) {
DeguNaoto 35:7b6786193aa2 155 if(!skip) sendData(1,1);
DeguNaoto 35:7b6786193aa2 156 CStep=11;
DeguNaoto 35:7b6786193aa2 157 }
DeguNaoto 35:7b6786193aa2 158 if((x>6050.0)&&(CStep==11)) {
DeguNaoto 35:7b6786193aa2 159 if(!skip) sendData(1,2);
DeguNaoto 30:cd344beb415d 160 CStep=12;
DeguNaoto 30:cd344beb415d 161 }
DeguNaoto 35:7b6786193aa2 162 if((x>7700.0)&&(CStep==12)) {
DeguNaoto 35:7b6786193aa2 163 if(!skip) sendData(1,3);
DeguNaoto 30:cd344beb415d 164 CStep=13;
DeguNaoto 30:cd344beb415d 165 }
DeguNaoto 36:b8954b13a6d5 166 if((x>9500.0)&&(CStep==13)) {
DeguNaoto 36:b8954b13a6d5 167 if(!skip) sendData(1,8);
DeguNaoto 36:b8954b13a6d5 168 CStep=14;
DeguNaoto 36:b8954b13a6d5 169 }
DeguNaoto 35:7b6786193aa2 170 if((x<9400.0)&&(CStep==14)) {
DeguNaoto 35:7b6786193aa2 171 if(!skip) sendData(1,4);
DeguNaoto 35:7b6786193aa2 172 CStep=15;
DeguNaoto 35:7b6786193aa2 173 }
DeguNaoto 35:7b6786193aa2 174 if((x<7250.0)&&(CStep==15)) {
DeguNaoto 35:7b6786193aa2 175 if(!skip) sendData(1,5);
DeguNaoto 35:7b6786193aa2 176 CStep=16;
DeguNaoto 35:7b6786193aa2 177 }
DeguNaoto 35:7b6786193aa2 178 if((x<4850.0)&&(CStep==16)) {
DeguNaoto 35:7b6786193aa2 179 // if((x<5450.0)&&(CStep==16)) {
DeguNaoto 33:a4323c20494b 180 if(!skip) sendData(1,6);
DeguNaoto 35:7b6786193aa2 181 CStep=17;
DeguNaoto 30:cd344beb415d 182 }
DeguNaoto 35:7b6786193aa2 183 if((x<1000.0)&&(CStep==17)){
DeguNaoto 0:b613dc16f27d 184 sendData(7,0);
DeguNaoto 0:b613dc16f27d 185 CStep=114;
DeguNaoto 0:b613dc16f27d 186 }
DeguNaoto 24:6d2573d6f2b6 187 #else
DeguNaoto 34:aa2a5c888a27 188 /********************************Own & Middle Mode*********************************/
DeguNaoto 36:b8954b13a6d5 189 if((step==0)&&((10000.0>x)&&(x>800.0))) {
DeguNaoto 34:aa2a5c888a27 190 targ_sita=0.03;
DeguNaoto 34:aa2a5c888a27 191 // targ_sita=0.0;
DeguNaoto 34:aa2a5c888a27 192 step=1;
DeguNaoto 34:aa2a5c888a27 193 }
DeguNaoto 34:aa2a5c888a27 194 if((step==1)&&(x>9000.0)) {
DeguNaoto 34:aa2a5c888a27 195 targ_velocity=0.0;
DeguNaoto 34:aa2a5c888a27 196 velocity_controller.setBias(0.0);
DeguNaoto 34:aa2a5c888a27 197 velocity_controller.reset();
DeguNaoto 34:aa2a5c888a27 198 // dpcount=speed;
DeguNaoto 34:aa2a5c888a27 199 step=2;
DeguNaoto 34:aa2a5c888a27 200 }
DeguNaoto 34:aa2a5c888a27 201 if((step==2)&&((velocity<500.0)&&(velocity>-500.0))){
DeguNaoto 34:aa2a5c888a27 202 step=3;
DeguNaoto 34:aa2a5c888a27 203 spcount=0.0;
DeguNaoto 34:aa2a5c888a27 204 flagf=0;
DeguNaoto 34:aa2a5c888a27 205 // targ_sita=0.0;
DeguNaoto 36:b8954b13a6d5 206 targ_sita=0.03;
DeguNaoto 34:aa2a5c888a27 207 }
DeguNaoto 34:aa2a5c888a27 208 if((step==3)&&(x<1700.0)) {
DeguNaoto 34:aa2a5c888a27 209 targ_sita=-PI/4;
DeguNaoto 34:aa2a5c888a27 210 step=4;
DeguNaoto 34:aa2a5c888a27 211 }
DeguNaoto 34:aa2a5c888a27 212 if((step==4)&&(x<800.0)) {
DeguNaoto 34:aa2a5c888a27 213 dpcount=speed;
DeguNaoto 34:aa2a5c888a27 214 step=114;
DeguNaoto 34:aa2a5c888a27 215 }
DeguNaoto 34:aa2a5c888a27 216
DeguNaoto 34:aa2a5c888a27 217 //Cylinder
DeguNaoto 36:b8954b13a6d5 218 if((x>3100.0)&&(CStep==0)) {
DeguNaoto 34:aa2a5c888a27 219 if(!skip) sendData(1,1);
DeguNaoto 34:aa2a5c888a27 220 CStep=1;
DeguNaoto 34:aa2a5c888a27 221 }
DeguNaoto 36:b8954b13a6d5 222 if((x>6050.0)&&(CStep==1)) {
DeguNaoto 34:aa2a5c888a27 223 if(!skip) sendData(1,3);
DeguNaoto 34:aa2a5c888a27 224 CStep=2;
DeguNaoto 34:aa2a5c888a27 225 }
DeguNaoto 36:b8954b13a6d5 226 if((x>7650.0)&&(CStep==2)) {
DeguNaoto 34:aa2a5c888a27 227 if(!skip) sendData(1,2);
DeguNaoto 34:aa2a5c888a27 228 CStep=3;
DeguNaoto 34:aa2a5c888a27 229 }
DeguNaoto 36:b8954b13a6d5 230 if((x>8000.0)&&(CStep==3)) {
DeguNaoto 36:b8954b13a6d5 231 if(!skip) sendData(1,8);
DeguNaoto 34:aa2a5c888a27 232 CStep=4;
DeguNaoto 34:aa2a5c888a27 233 }
DeguNaoto 36:b8954b13a6d5 234 if((x<7200.0)&&(CStep==4)) {
DeguNaoto 36:b8954b13a6d5 235 if(!skip) sendData(1,6); //over
DeguNaoto 34:aa2a5c888a27 236 CStep=5;
DeguNaoto 34:aa2a5c888a27 237 }
DeguNaoto 36:b8954b13a6d5 238 if((x<6950.0)&&(CStep==5)) {
DeguNaoto 34:aa2a5c888a27 239 if(!skip) sendData(1,4); //front
DeguNaoto 34:aa2a5c888a27 240 CStep=6;
DeguNaoto 34:aa2a5c888a27 241 }
DeguNaoto 36:b8954b13a6d5 242 if((x<6700.0)&&(CStep==6)) {
DeguNaoto 36:b8954b13a6d5 243 if(!skip) sendData(1,5); //middle
DeguNaoto 36:b8954b13a6d5 244 CStep=7;
DeguNaoto 36:b8954b13a6d5 245 }
DeguNaoto 36:b8954b13a6d5 246 if((x<3000.0)&&(CStep==7)){
DeguNaoto 36:b8954b13a6d5 247 sendData(7,0);
DeguNaoto 36:b8954b13a6d5 248 CStep=114;
DeguNaoto 36:b8954b13a6d5 249 }
DeguNaoto 36:b8954b13a6d5 250
DeguNaoto 36:b8954b13a6d5 251 /********************************Own & Opponent Mode*********************************/
DeguNaoto 36:b8954b13a6d5 252 if((step==10)&&((9800.0>x)&&(x>700.0))) {
DeguNaoto 36:b8954b13a6d5 253 targ_sita=-0.03;
DeguNaoto 36:b8954b13a6d5 254 // targ_sita=0.0;
DeguNaoto 36:b8954b13a6d5 255 step=11;
DeguNaoto 36:b8954b13a6d5 256 }
DeguNaoto 36:b8954b13a6d5 257 if((step==11)&&(x>10000.0)) {
DeguNaoto 36:b8954b13a6d5 258 targ_velocity=0.0;
DeguNaoto 36:b8954b13a6d5 259 velocity_controller.setBias(0.0);
DeguNaoto 36:b8954b13a6d5 260 velocity_controller.reset();
DeguNaoto 36:b8954b13a6d5 261 step=12;
DeguNaoto 36:b8954b13a6d5 262 }
DeguNaoto 36:b8954b13a6d5 263 if((step==12)&&((velocity<500.0)&&(velocity>-500.0))){
DeguNaoto 36:b8954b13a6d5 264 step=13;
DeguNaoto 36:b8954b13a6d5 265 spcount=0.0;
DeguNaoto 36:b8954b13a6d5 266 flagf=0;
DeguNaoto 36:b8954b13a6d5 267 // flagf=3;
DeguNaoto 36:b8954b13a6d5 268 // targ_sita=0.0;
DeguNaoto 36:b8954b13a6d5 269 targ_sita=-0.03;
DeguNaoto 36:b8954b13a6d5 270 }
DeguNaoto 36:b8954b13a6d5 271 if((step==13)&&(x<1400.0)) {
DeguNaoto 36:b8954b13a6d5 272 targ_sita=PI/4;
DeguNaoto 36:b8954b13a6d5 273 step=14;
DeguNaoto 36:b8954b13a6d5 274 }
DeguNaoto 36:b8954b13a6d5 275 if((step==14)&&(x<600.0)) {
DeguNaoto 36:b8954b13a6d5 276 dpcount=speed;
DeguNaoto 36:b8954b13a6d5 277 step=114;
DeguNaoto 36:b8954b13a6d5 278 }
DeguNaoto 36:b8954b13a6d5 279
DeguNaoto 36:b8954b13a6d5 280 //Cylinderd
DeguNaoto 36:b8954b13a6d5 281 if((x>3200.0)&&(CStep==10)) {
DeguNaoto 36:b8954b13a6d5 282 if(!skip) sendData(1,1);
DeguNaoto 36:b8954b13a6d5 283 CStep=11;
DeguNaoto 36:b8954b13a6d5 284 }
DeguNaoto 36:b8954b13a6d5 285 if((x>6050.0)&&(CStep==11)) {
DeguNaoto 36:b8954b13a6d5 286 if(!skip) sendData(1,3);
DeguNaoto 36:b8954b13a6d5 287 CStep=12;
DeguNaoto 36:b8954b13a6d5 288 }
DeguNaoto 36:b8954b13a6d5 289 if((x>7700.0)&&(CStep==12)) {
DeguNaoto 36:b8954b13a6d5 290 if(!skip) sendData(1,2);
DeguNaoto 36:b8954b13a6d5 291 CStep=13;
DeguNaoto 36:b8954b13a6d5 292 }
DeguNaoto 36:b8954b13a6d5 293 if((x>9500.0)&&(CStep==13)) {
DeguNaoto 36:b8954b13a6d5 294 if(!skip) sendData(1,8);
DeguNaoto 36:b8954b13a6d5 295 CStep=14;
DeguNaoto 36:b8954b13a6d5 296 }
DeguNaoto 36:b8954b13a6d5 297 if((x<9400.0)&&(CStep==14)) {
DeguNaoto 36:b8954b13a6d5 298 if(!skip) sendData(1,6); //over
DeguNaoto 36:b8954b13a6d5 299 CStep=15;
DeguNaoto 36:b8954b13a6d5 300 }
DeguNaoto 36:b8954b13a6d5 301 if((x<7250.0)&&(CStep==15)) {
DeguNaoto 36:b8954b13a6d5 302 if(!skip) sendData(1,5); //middle
DeguNaoto 36:b8954b13a6d5 303 CStep=16;
DeguNaoto 36:b8954b13a6d5 304 }
DeguNaoto 36:b8954b13a6d5 305 if((x<4850.0)&&(CStep==16)) {
DeguNaoto 36:b8954b13a6d5 306 // if((x<5450.0)&&(CStep==16)) {
DeguNaoto 36:b8954b13a6d5 307 if(!skip) sendData(1,4); //front
DeguNaoto 36:b8954b13a6d5 308 CStep=17;
DeguNaoto 36:b8954b13a6d5 309 }
DeguNaoto 36:b8954b13a6d5 310 if((x<1000.0)&&(CStep==17)){
DeguNaoto 34:aa2a5c888a27 311 sendData(7,0);
DeguNaoto 34:aa2a5c888a27 312 CStep=114;
DeguNaoto 34:aa2a5c888a27 313 }
DeguNaoto 24:6d2573d6f2b6 314 #endif
DeguNaoto 0:b613dc16f27d 315 }
DeguNaoto 33:a4323c20494b 316 // else if(!modeflag) {
DeguNaoto 33:a4323c20494b 317 else if(!autoflag){
DeguNaoto 0:b613dc16f27d 318 manualIM920(); /*IM920 button*/
DeguNaoto 34:aa2a5c888a27 319 #ifdef BLUE
DeguNaoto 36:b8954b13a6d5 320 /********************************Swing Mode(middle)*********************************/
DeguNaoto 36:b8954b13a6d5 321 if((mstep==0)&&((10000.0>x)&&(x>1500.0))) {
DeguNaoto 30:cd344beb415d 322 dpcount=speed;
DeguNaoto 33:a4323c20494b 323 mstep=1;
DeguNaoto 33:a4323c20494b 324 }
DeguNaoto 33:a4323c20494b 325 if((mstep==1)&&((velocity<5.0)&&(velocity>-5.0))){
DeguNaoto 34:aa2a5c888a27 326 targ_sita=PI/4.0;
DeguNaoto 36:b8954b13a6d5 327 swingmoved=1;
DeguNaoto 36:b8954b13a6d5 328 mstep=114;
DeguNaoto 36:b8954b13a6d5 329 }
DeguNaoto 36:b8954b13a6d5 330 if(mstep==2){
DeguNaoto 36:b8954b13a6d5 331 mstep=3;
DeguNaoto 36:b8954b13a6d5 332 targ_sita=3.0*PI/4.0;
DeguNaoto 36:b8954b13a6d5 333 }
DeguNaoto 36:b8954b13a6d5 334 if((mstep==3)&&(((targ_sita-sita)<0.05)&&((targ_sita-sita)>-0.05))){
DeguNaoto 36:b8954b13a6d5 335 flagf=1;
DeguNaoto 36:b8954b13a6d5 336 targ_velocity=speed;
DeguNaoto 36:b8954b13a6d5 337 mstep=4;
DeguNaoto 36:b8954b13a6d5 338 }
DeguNaoto 36:b8954b13a6d5 339 if((mstep==4)&&(x<400.0)){
DeguNaoto 36:b8954b13a6d5 340 targ_sita=PI;
DeguNaoto 36:b8954b13a6d5 341 mstep=5;
DeguNaoto 36:b8954b13a6d5 342 }
DeguNaoto 36:b8954b13a6d5 343 if((mstep==5)&&(x<0.0)){
DeguNaoto 36:b8954b13a6d5 344 targ_velocity=0.0;
DeguNaoto 30:cd344beb415d 345 mstep=114;
DeguNaoto 30:cd344beb415d 346 }
DeguNaoto 30:cd344beb415d 347
DeguNaoto 36:b8954b13a6d5 348
DeguNaoto 36:b8954b13a6d5 349 /********************************Swing Mode(over)*********************************/
DeguNaoto 36:b8954b13a6d5 350 if((mstep==10)&&((10000.0>x)&&(x>1500.0))) {
DeguNaoto 36:b8954b13a6d5 351 dpcount=speed;
DeguNaoto 33:a4323c20494b 352 mstep=11;
DeguNaoto 33:a4323c20494b 353 }
DeguNaoto 36:b8954b13a6d5 354 if((mstep==11)&&((velocity<5.0)&&(velocity>-5.0))){
DeguNaoto 36:b8954b13a6d5 355 targ_sita=PI/4.0+PI/16.0;
DeguNaoto 36:b8954b13a6d5 356 swingmoved=2;
DeguNaoto 36:b8954b13a6d5 357 mstep=114;
DeguNaoto 36:b8954b13a6d5 358 }
DeguNaoto 36:b8954b13a6d5 359 if(mstep==12){
DeguNaoto 33:a4323c20494b 360 mstep=13;
DeguNaoto 36:b8954b13a6d5 361 targ_sita=3.0*PI/4.0;
DeguNaoto 33:a4323c20494b 362 }
DeguNaoto 36:b8954b13a6d5 363 if((mstep==13)&&(((targ_sita-sita)<0.05)&&((targ_sita-sita)>-0.05))){
DeguNaoto 36:b8954b13a6d5 364 flagf=1;
DeguNaoto 36:b8954b13a6d5 365 targ_velocity=speed;
DeguNaoto 33:a4323c20494b 366 mstep=14;
DeguNaoto 33:a4323c20494b 367 }
DeguNaoto 36:b8954b13a6d5 368 if((mstep==14)&&(x<400.0)){
DeguNaoto 36:b8954b13a6d5 369 targ_sita=PI;
DeguNaoto 36:b8954b13a6d5 370 mstep=15;
DeguNaoto 36:b8954b13a6d5 371 }
DeguNaoto 36:b8954b13a6d5 372 if((mstep==15)&&(x<0.0)){
DeguNaoto 36:b8954b13a6d5 373 targ_velocity=0.0;
DeguNaoto 36:b8954b13a6d5 374 mstep=114;
DeguNaoto 36:b8954b13a6d5 375 }
DeguNaoto 36:b8954b13a6d5 376
DeguNaoto 36:b8954b13a6d5 377 /********************************Swing Mode(over)*********************************/
DeguNaoto 36:b8954b13a6d5 378 if((mstep==20)&&((10000.0>x)&&(x>1500.0))) {
DeguNaoto 36:b8954b13a6d5 379 dpcount=speed;
DeguNaoto 36:b8954b13a6d5 380 mstep=21;
DeguNaoto 36:b8954b13a6d5 381 }
DeguNaoto 36:b8954b13a6d5 382 if((mstep==21)&&((velocity<5.0)&&(velocity>-5.0))){
DeguNaoto 36:b8954b13a6d5 383 targ_sita=PI/4.0+PI/16.0;
DeguNaoto 36:b8954b13a6d5 384 swingmoved=3;
DeguNaoto 33:a4323c20494b 385 mstep=114;
DeguNaoto 36:b8954b13a6d5 386 }
DeguNaoto 36:b8954b13a6d5 387 if(mstep==22){
DeguNaoto 36:b8954b13a6d5 388 mstep=23;
DeguNaoto 36:b8954b13a6d5 389 targ_sita=3.0*PI/4.0;
DeguNaoto 36:b8954b13a6d5 390 }
DeguNaoto 36:b8954b13a6d5 391 if((mstep==23)&&(((targ_sita-sita)<0.05)&&((targ_sita-sita)>-0.05))){
DeguNaoto 36:b8954b13a6d5 392 flagf=1;
DeguNaoto 36:b8954b13a6d5 393 targ_velocity=speed;
DeguNaoto 36:b8954b13a6d5 394 mstep=24;
DeguNaoto 36:b8954b13a6d5 395 }
DeguNaoto 36:b8954b13a6d5 396 if((mstep==24)&&(x<400.0)){
DeguNaoto 36:b8954b13a6d5 397 targ_sita=PI;
DeguNaoto 36:b8954b13a6d5 398 mstep=25;
DeguNaoto 36:b8954b13a6d5 399 }
DeguNaoto 36:b8954b13a6d5 400 if((mstep==25)&&(x<0.0)){
DeguNaoto 36:b8954b13a6d5 401 targ_velocity=0.0;
DeguNaoto 36:b8954b13a6d5 402 mstep=114;
DeguNaoto 36:b8954b13a6d5 403 }
DeguNaoto 36:b8954b13a6d5 404
DeguNaoto 34:aa2a5c888a27 405 #else
DeguNaoto 34:aa2a5c888a27 406 #endif
DeguNaoto 0:b613dc16f27d 407 }
DeguNaoto 34:aa2a5c888a27 408 // pc.printf("b:%d\r\n",swingRadVelocity);
DeguNaoto 24:6d2573d6f2b6 409 // pc.printf("Swing:%f\r\n",SwingSens.getPulses());
DeguNaoto 34:aa2a5c888a27 410 pc.printf("x:%f ,y:%f ,sita:%f ,r:%f\r\n",x,y,sita,Pulses_move_r);
DeguNaoto 0:b613dc16f27d 411 }
DeguNaoto 4:09f684eac572 412 }