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@41:cd916d59832d, 2015-11-17 (annotated)

Committer:
DeguNaoto
Date:
Tue Nov 17 08:17:19 2015 +0000
Revision:
41:cd916d59832d
Parent:
40:aed70f4dcd06
Child:
42:c420857a3a6f
20151117 ?????????????

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 39:5ba406327aa3 6 #define BLUE
DeguNaoto 39:5ba406327aa3 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 38:f4e9893641ac 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 39:5ba406327aa3 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 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 40:aed70f4dcd06 81 if((x>6070.0)&&(CStep==1)) {
DeguNaoto 30:cd344beb415d 82 if(!skip) sendData(1,2);
DeguNaoto 30:cd344beb415d 83 CStep=2;
DeguNaoto 30:cd344beb415d 84 }
DeguNaoto 40:aed70f4dcd06 85 if((x>7720.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 39:5ba406327aa3 108 if((x<7800.0)&&(CStep==5)) {
DeguNaoto 35:7b6786193aa2 109 if(!skip) sendData(1,6); //front
DeguNaoto 0:b613dc16f27d 110 CStep=6;
DeguNaoto 0:b613dc16f27d 111 }
DeguNaoto 40:aed70f4dcd06 112 if((x<6670.0)&&(CStep==6)) {
DeguNaoto 37:75fcd28f48c7 113 if(!skip) sendData(1,5); //middle
DeguNaoto 36:b8954b13a6d5 114 CStep=7;
DeguNaoto 36:b8954b13a6d5 115 }
DeguNaoto 40:aed70f4dcd06 116 if((x<5470.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 39:5ba406327aa3 150 if((step==14)&&(x<700.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 39:5ba406327aa3 173 if((x<9500.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 39:5ba406327aa3 177 if((x<7800.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 39:5ba406327aa3 182 if((x<5400.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 38:f4e9893641ac 194 // if((step==0)&&((10000.0>x)&&(x>1000.0))) {
DeguNaoto 38:f4e9893641ac 195 if((step==0)&&((10000.0>x)&&(x>850.0))) {
DeguNaoto 38:f4e9893641ac 196 targ_sita=0.02;
DeguNaoto 34:aa2a5c888a27 197 // targ_sita=0.0;
DeguNaoto 34:aa2a5c888a27 198 step=1;
DeguNaoto 34:aa2a5c888a27 199 }
DeguNaoto 34:aa2a5c888a27 200 if((step==1)&&(x>9000.0)) {
DeguNaoto 34:aa2a5c888a27 201 targ_velocity=0.0;
DeguNaoto 34:aa2a5c888a27 202 velocity_controller.setBias(0.0);
DeguNaoto 34:aa2a5c888a27 203 velocity_controller.reset();
DeguNaoto 34:aa2a5c888a27 204 // dpcount=speed;
DeguNaoto 34:aa2a5c888a27 205 step=2;
DeguNaoto 34:aa2a5c888a27 206 }
DeguNaoto 34:aa2a5c888a27 207 if((step==2)&&((velocity<500.0)&&(velocity>-500.0))){
DeguNaoto 34:aa2a5c888a27 208 step=3;
DeguNaoto 34:aa2a5c888a27 209 spcount=0.0;
DeguNaoto 34:aa2a5c888a27 210 flagf=0;
DeguNaoto 34:aa2a5c888a27 211 // targ_sita=0.0;
DeguNaoto 38:f4e9893641ac 212 targ_sita=0.025;
DeguNaoto 34:aa2a5c888a27 213 }
DeguNaoto 38:f4e9893641ac 214 if((step==3)&&(x<2000.0)) {
DeguNaoto 34:aa2a5c888a27 215 targ_sita=-PI/4;
DeguNaoto 34:aa2a5c888a27 216 step=4;
DeguNaoto 34:aa2a5c888a27 217 }
DeguNaoto 34:aa2a5c888a27 218 if((step==4)&&(x<800.0)) {
DeguNaoto 34:aa2a5c888a27 219 dpcount=speed;
DeguNaoto 34:aa2a5c888a27 220 step=114;
DeguNaoto 34:aa2a5c888a27 221 }
DeguNaoto 34:aa2a5c888a27 222
DeguNaoto 34:aa2a5c888a27 223 //Cylinder
DeguNaoto 38:f4e9893641ac 224 if((x>3250.0)&&(CStep==0)) {
DeguNaoto 34:aa2a5c888a27 225 if(!skip) sendData(1,1);
DeguNaoto 34:aa2a5c888a27 226 CStep=1;
DeguNaoto 34:aa2a5c888a27 227 }
DeguNaoto 38:f4e9893641ac 228 if((x>6100.0)&&(CStep==1)) {
DeguNaoto 34:aa2a5c888a27 229 if(!skip) sendData(1,3);
DeguNaoto 34:aa2a5c888a27 230 CStep=2;
DeguNaoto 34:aa2a5c888a27 231 }
DeguNaoto 38:f4e9893641ac 232 if((x>7750.0)&&(CStep==2)) {
DeguNaoto 34:aa2a5c888a27 233 if(!skip) sendData(1,2);
DeguNaoto 34:aa2a5c888a27 234 CStep=3;
DeguNaoto 34:aa2a5c888a27 235 }
DeguNaoto 36:b8954b13a6d5 236 if((x>8000.0)&&(CStep==3)) {
DeguNaoto 36:b8954b13a6d5 237 if(!skip) sendData(1,8);
DeguNaoto 34:aa2a5c888a27 238 CStep=4;
DeguNaoto 34:aa2a5c888a27 239 }
DeguNaoto 38:f4e9893641ac 240 if((x>8500.0)&&(CStep==4)) CStep=5;
DeguNaoto 38:f4e9893641ac 241 /////////////////////////////////////////
DeguNaoto 38:f4e9893641ac 242 /*if((x<7200.0)&&(CStep==4)) {
DeguNaoto 38:f4e9893641ac 243 if(!skip) sendData(1,4); //over
DeguNaoto 34:aa2a5c888a27 244 CStep=5;
DeguNaoto 34:aa2a5c888a27 245 }
DeguNaoto 36:b8954b13a6d5 246 if((x<6950.0)&&(CStep==5)) {
DeguNaoto 38:f4e9893641ac 247 if(!skip) sendData(1,6); //fornt
DeguNaoto 34:aa2a5c888a27 248 CStep=6;
DeguNaoto 34:aa2a5c888a27 249 }
DeguNaoto 36:b8954b13a6d5 250 if((x<6700.0)&&(CStep==6)) {
DeguNaoto 36:b8954b13a6d5 251 if(!skip) sendData(1,5); //middle
DeguNaoto 36:b8954b13a6d5 252 CStep=7;
DeguNaoto 38:f4e9893641ac 253 }*/
DeguNaoto 38:f4e9893641ac 254 /////////////////////////////////////////
DeguNaoto 38:f4e9893641ac 255 if((x<7970.0)&&(CStep==5)) {
DeguNaoto 38:f4e9893641ac 256 if(!skip) sendData(1,4); //front
DeguNaoto 38:f4e9893641ac 257 CStep=6;
DeguNaoto 36:b8954b13a6d5 258 }
DeguNaoto 38:f4e9893641ac 259 if((x<6830.0)&&(CStep==6)) {
DeguNaoto 38:f4e9893641ac 260 if(!skip) sendData(1,5); //middle
DeguNaoto 38:f4e9893641ac 261 CStep=7;
DeguNaoto 38:f4e9893641ac 262 }
DeguNaoto 38:f4e9893641ac 263 if((x<5580.0)&&(CStep==7)) {
DeguNaoto 38:f4e9893641ac 264 if(!skip) sendData(1,6); //over
DeguNaoto 38:f4e9893641ac 265 CStep=8;
DeguNaoto 38:f4e9893641ac 266 }
DeguNaoto 38:f4e9893641ac 267 /////////////////////////////////////////
DeguNaoto 38:f4e9893641ac 268 if((x<3000.0)&&(CStep==8)){
DeguNaoto 36:b8954b13a6d5 269 sendData(7,0);
DeguNaoto 36:b8954b13a6d5 270 CStep=114;
DeguNaoto 36:b8954b13a6d5 271 }
DeguNaoto 36:b8954b13a6d5 272
DeguNaoto 36:b8954b13a6d5 273 /********************************Own & Opponent Mode*********************************/
DeguNaoto 38:f4e9893641ac 274 if((step==10)&&((10000.0>x)&&(x>630.0))) {
DeguNaoto 38:f4e9893641ac 275 targ_sita=0.025;
DeguNaoto 36:b8954b13a6d5 276 // targ_sita=0.0;
DeguNaoto 36:b8954b13a6d5 277 step=11;
DeguNaoto 36:b8954b13a6d5 278 }
DeguNaoto 36:b8954b13a6d5 279 if((step==11)&&(x>10000.0)) {
DeguNaoto 36:b8954b13a6d5 280 targ_velocity=0.0;
DeguNaoto 36:b8954b13a6d5 281 velocity_controller.setBias(0.0);
DeguNaoto 36:b8954b13a6d5 282 velocity_controller.reset();
DeguNaoto 36:b8954b13a6d5 283 step=12;
DeguNaoto 36:b8954b13a6d5 284 }
DeguNaoto 36:b8954b13a6d5 285 if((step==12)&&((velocity<500.0)&&(velocity>-500.0))){
DeguNaoto 36:b8954b13a6d5 286 step=13;
DeguNaoto 36:b8954b13a6d5 287 spcount=0.0;
DeguNaoto 36:b8954b13a6d5 288 flagf=0;
DeguNaoto 36:b8954b13a6d5 289 // flagf=3;
DeguNaoto 36:b8954b13a6d5 290 // targ_sita=0.0;
DeguNaoto 38:f4e9893641ac 291 targ_sita=0.02;
DeguNaoto 36:b8954b13a6d5 292 }
DeguNaoto 38:f4e9893641ac 293 if((step==13)&&(x<1450.0)) {
DeguNaoto 38:f4e9893641ac 294 targ_sita=-PI/4;
DeguNaoto 36:b8954b13a6d5 295 step=14;
DeguNaoto 36:b8954b13a6d5 296 }
DeguNaoto 38:f4e9893641ac 297 if((step==14)&&(x<700.0)) {
DeguNaoto 36:b8954b13a6d5 298 dpcount=speed;
DeguNaoto 36:b8954b13a6d5 299 step=114;
DeguNaoto 36:b8954b13a6d5 300 }
DeguNaoto 36:b8954b13a6d5 301
DeguNaoto 36:b8954b13a6d5 302 //Cylinderd
DeguNaoto 38:f4e9893641ac 303 if((x>3100.0)&&(CStep==10)) {
DeguNaoto 36:b8954b13a6d5 304 if(!skip) sendData(1,1);
DeguNaoto 36:b8954b13a6d5 305 CStep=11;
DeguNaoto 36:b8954b13a6d5 306 }
DeguNaoto 38:f4e9893641ac 307 if((x>6000.0)&&(CStep==11)) {
DeguNaoto 36:b8954b13a6d5 308 if(!skip) sendData(1,3);
DeguNaoto 36:b8954b13a6d5 309 CStep=12;
DeguNaoto 36:b8954b13a6d5 310 }
DeguNaoto 38:f4e9893641ac 311 if((x>7650.0)&&(CStep==12)) {
DeguNaoto 36:b8954b13a6d5 312 if(!skip) sendData(1,2);
DeguNaoto 36:b8954b13a6d5 313 CStep=13;
DeguNaoto 36:b8954b13a6d5 314 }
DeguNaoto 38:f4e9893641ac 315 if((x>9800.0)&&(CStep==13)) {
DeguNaoto 38:f4e9893641ac 316 sendData(1,8);
DeguNaoto 36:b8954b13a6d5 317 CStep=14;
DeguNaoto 36:b8954b13a6d5 318 }
DeguNaoto 38:f4e9893641ac 319 if((x>9900.0)&&(CStep==14)) CStep=15;
DeguNaoto 38:f4e9893641ac 320 if((x<9750.0)&&(CStep==15)) { //7.8
DeguNaoto 38:f4e9893641ac 321 if(!skip) sendData(1,6);
DeguNaoto 36:b8954b13a6d5 322 CStep=16;
DeguNaoto 36:b8954b13a6d5 323 }
DeguNaoto 38:f4e9893641ac 324 if((x<7900.0)&&(CStep==16)) { //7.8
DeguNaoto 38:f4e9893641ac 325 // if((x<7950.0)&&(CStep==16)) { //7.8
DeguNaoto 38:f4e9893641ac 326 if(!skip) sendData(1,5);
DeguNaoto 36:b8954b13a6d5 327 CStep=17;
DeguNaoto 36:b8954b13a6d5 328 }
DeguNaoto 38:f4e9893641ac 329 if((x<5670.0)&&(CStep==17)) { //7.8
DeguNaoto 38:f4e9893641ac 330 // if((x<5750.0)&&(CStep==17)) { //7.8
DeguNaoto 38:f4e9893641ac 331 if(!skip) sendData(1,4);
DeguNaoto 38:f4e9893641ac 332 CStep=18;
DeguNaoto 38:f4e9893641ac 333 }
DeguNaoto 38:f4e9893641ac 334 if((x<1000.0)&&(CStep==18)){
DeguNaoto 34:aa2a5c888a27 335 sendData(7,0);
DeguNaoto 34:aa2a5c888a27 336 CStep=114;
DeguNaoto 34:aa2a5c888a27 337 }
DeguNaoto 24:6d2573d6f2b6 338 #endif
DeguNaoto 0:b613dc16f27d 339 }
DeguNaoto 33:a4323c20494b 340 // else if(!modeflag) {
DeguNaoto 33:a4323c20494b 341 else if(!autoflag){
DeguNaoto 0:b613dc16f27d 342 manualIM920(); /*IM920 button*/
DeguNaoto 34:aa2a5c888a27 343 #ifdef BLUE
DeguNaoto 36:b8954b13a6d5 344 /********************************Swing Mode(middle)*********************************/
DeguNaoto 40:aed70f4dcd06 345 if((mstep==0)&&((10000.0>x)&&(x>1350.0))) {
DeguNaoto 30:cd344beb415d 346 dpcount=speed;
DeguNaoto 33:a4323c20494b 347 mstep=1;
DeguNaoto 33:a4323c20494b 348 }
DeguNaoto 33:a4323c20494b 349 if((mstep==1)&&((velocity<5.0)&&(velocity>-5.0))){
DeguNaoto 40:aed70f4dcd06 350 targ_sita=PI/4.0+PI/30.0;
DeguNaoto 37:75fcd28f48c7 351 swingmoved=2;
DeguNaoto 37:75fcd28f48c7 352 mstep=251;
DeguNaoto 37:75fcd28f48c7 353 }
DeguNaoto 37:75fcd28f48c7 354 if((mstep==251)&&(((targ_sita-sita)<0.01)&&((targ_sita-sita)>-0.01))){
DeguNaoto 37:75fcd28f48c7 355 flagf=2;
DeguNaoto 36:b8954b13a6d5 356 }
DeguNaoto 36:b8954b13a6d5 357 if(mstep==2){
DeguNaoto 36:b8954b13a6d5 358 mstep=3;
DeguNaoto 37:75fcd28f48c7 359 targ_sita=3.0*PI/4.0-PI/8.0;
DeguNaoto 37:75fcd28f48c7 360 sendData(7,0);
DeguNaoto 37:75fcd28f48c7 361 flagf=0;
DeguNaoto 36:b8954b13a6d5 362 }
DeguNaoto 36:b8954b13a6d5 363 if((mstep==3)&&(((targ_sita-sita)<0.05)&&((targ_sita-sita)>-0.05))){
DeguNaoto 36:b8954b13a6d5 364 flagf=1;
DeguNaoto 36:b8954b13a6d5 365 targ_velocity=speed;
DeguNaoto 36:b8954b13a6d5 366 mstep=4;
DeguNaoto 36:b8954b13a6d5 367 }
DeguNaoto 37:75fcd28f48c7 368 if((mstep==4)&&(x<1300.0)){
DeguNaoto 37:75fcd28f48c7 369 targ_sita=5*PI/6.0;
DeguNaoto 36:b8954b13a6d5 370 mstep=5;
DeguNaoto 36:b8954b13a6d5 371 }
DeguNaoto 37:75fcd28f48c7 372 if((mstep==5)&&(x<-10.0)){
DeguNaoto 36:b8954b13a6d5 373 targ_velocity=0.0;
DeguNaoto 37:75fcd28f48c7 374 targ_sita=3.0*PI/4.0;
DeguNaoto 30:cd344beb415d 375 mstep=114;
DeguNaoto 30:cd344beb415d 376 }
DeguNaoto 30:cd344beb415d 377
DeguNaoto 36:b8954b13a6d5 378
DeguNaoto 36:b8954b13a6d5 379 /********************************Swing Mode(over)*********************************/
DeguNaoto 37:75fcd28f48c7 380 // if((mstep==10)&&((10000.0>x)&&(x>1500.0))) {
DeguNaoto 37:75fcd28f48c7 381 if((mstep==10)&&((10000.0>x)&&(x>1550.0))) {
DeguNaoto 36:b8954b13a6d5 382 dpcount=speed;
DeguNaoto 33:a4323c20494b 383 mstep=11;
DeguNaoto 33:a4323c20494b 384 }
DeguNaoto 36:b8954b13a6d5 385 if((mstep==11)&&((velocity<5.0)&&(velocity>-5.0))){
DeguNaoto 37:75fcd28f48c7 386 targ_sita=PI/4.0+PI/16.0+PI/45.0;
DeguNaoto 36:b8954b13a6d5 387 swingmoved=2;
DeguNaoto 37:75fcd28f48c7 388 mstep=252;
DeguNaoto 37:75fcd28f48c7 389 }
DeguNaoto 37:75fcd28f48c7 390 if((mstep==252)&&(((targ_sita-sita)<0.01)&&((targ_sita-sita)>-0.01))){
DeguNaoto 37:75fcd28f48c7 391 flagf=2;
DeguNaoto 36:b8954b13a6d5 392 }
DeguNaoto 36:b8954b13a6d5 393 if(mstep==12){
DeguNaoto 33:a4323c20494b 394 mstep=13;
DeguNaoto 37:75fcd28f48c7 395 targ_sita=3.0*PI/4.0-PI/8.0;
DeguNaoto 37:75fcd28f48c7 396 sendData(7,0);
DeguNaoto 37:75fcd28f48c7 397 flagf=0;
DeguNaoto 33:a4323c20494b 398 }
DeguNaoto 36:b8954b13a6d5 399 if((mstep==13)&&(((targ_sita-sita)<0.05)&&((targ_sita-sita)>-0.05))){
DeguNaoto 36:b8954b13a6d5 400 flagf=1;
DeguNaoto 36:b8954b13a6d5 401 targ_velocity=speed;
DeguNaoto 33:a4323c20494b 402 mstep=14;
DeguNaoto 33:a4323c20494b 403 }
DeguNaoto 37:75fcd28f48c7 404 if((mstep==14)&&(x<1300.0)){
DeguNaoto 37:75fcd28f48c7 405 targ_sita=5*PI/6.0;
DeguNaoto 36:b8954b13a6d5 406 mstep=15;
DeguNaoto 36:b8954b13a6d5 407 }
DeguNaoto 37:75fcd28f48c7 408 if((mstep==15)&&(x<-10.0)){
DeguNaoto 36:b8954b13a6d5 409 targ_velocity=0.0;
DeguNaoto 37:75fcd28f48c7 410 targ_sita=3.0*PI/4.0;
DeguNaoto 36:b8954b13a6d5 411 mstep=114;
DeguNaoto 36:b8954b13a6d5 412 }
DeguNaoto 36:b8954b13a6d5 413
DeguNaoto 37:75fcd28f48c7 414 /********************************Swing Mode(front)*********************************/
DeguNaoto 38:f4e9893641ac 415 if((mstep==20)&&((10000.0>x)&&(x>1550.0))) {
DeguNaoto 36:b8954b13a6d5 416 dpcount=speed;
DeguNaoto 36:b8954b13a6d5 417 mstep=21;
DeguNaoto 36:b8954b13a6d5 418 }
DeguNaoto 36:b8954b13a6d5 419 if((mstep==21)&&((velocity<5.0)&&(velocity>-5.0))){
DeguNaoto 41:cd916d59832d 420 targ_sita=PI/4.0-PI/8.0+PI/60.0;
DeguNaoto 38:f4e9893641ac 421 swingmoved=2;
DeguNaoto 38:f4e9893641ac 422 mstep=253;
DeguNaoto 38:f4e9893641ac 423 }
DeguNaoto 38:f4e9893641ac 424 if((mstep==252)&&(((targ_sita-sita)<0.01)&&((targ_sita-sita)>-0.01))){
DeguNaoto 38:f4e9893641ac 425 flagf=2;
DeguNaoto 36:b8954b13a6d5 426 }
DeguNaoto 36:b8954b13a6d5 427 if(mstep==22){
DeguNaoto 36:b8954b13a6d5 428 mstep=23;
DeguNaoto 38:f4e9893641ac 429 targ_sita=3.0*PI/4.0-PI/8.0;
DeguNaoto 37:75fcd28f48c7 430 sendData(7,0);
DeguNaoto 38:f4e9893641ac 431 flagf=0;
DeguNaoto 36:b8954b13a6d5 432 }
DeguNaoto 36:b8954b13a6d5 433 if((mstep==23)&&(((targ_sita-sita)<0.05)&&((targ_sita-sita)>-0.05))){
DeguNaoto 36:b8954b13a6d5 434 flagf=1;
DeguNaoto 36:b8954b13a6d5 435 targ_velocity=speed;
DeguNaoto 36:b8954b13a6d5 436 mstep=24;
DeguNaoto 36:b8954b13a6d5 437 }
DeguNaoto 37:75fcd28f48c7 438 if((mstep==24)&&(x<1300.0)){
DeguNaoto 38:f4e9893641ac 439 targ_sita=5*PI/6.0;
DeguNaoto 36:b8954b13a6d5 440 mstep=25;
DeguNaoto 36:b8954b13a6d5 441 }
DeguNaoto 37:75fcd28f48c7 442 if((mstep==25)&&(x<-10.0)){
DeguNaoto 36:b8954b13a6d5 443 targ_velocity=0.0;
DeguNaoto 38:f4e9893641ac 444 targ_sita=3.0*PI/4.0;
DeguNaoto 36:b8954b13a6d5 445 mstep=114;
DeguNaoto 36:b8954b13a6d5 446 }
DeguNaoto 36:b8954b13a6d5 447
DeguNaoto 34:aa2a5c888a27 448 #else
DeguNaoto 38:f4e9893641ac 449 /********************************Swing Mode(middle)*********************************/
DeguNaoto 38:f4e9893641ac 450 if((mstep==0)&&((10000.0>x)&&(x>1550.0))) {
DeguNaoto 38:f4e9893641ac 451 dpcount=speed;
DeguNaoto 38:f4e9893641ac 452 mstep=1;
DeguNaoto 38:f4e9893641ac 453 }
DeguNaoto 38:f4e9893641ac 454 if((mstep==1)&&((velocity<5.0)&&(velocity>-5.0))){
DeguNaoto 40:aed70f4dcd06 455 targ_sita=-PI/8.0-PI/30.0+PI/40.0;
DeguNaoto 38:f4e9893641ac 456 swingmoved=2;
DeguNaoto 38:f4e9893641ac 457 mstep=251;
DeguNaoto 38:f4e9893641ac 458 }
DeguNaoto 38:f4e9893641ac 459 if((mstep==251)&&(((targ_sita-sita)<0.01)&&((targ_sita-sita)>-0.01))){
DeguNaoto 38:f4e9893641ac 460 flagf=2;
DeguNaoto 38:f4e9893641ac 461 }
DeguNaoto 38:f4e9893641ac 462 if(mstep==2){
DeguNaoto 38:f4e9893641ac 463 mstep=3;
DeguNaoto 38:f4e9893641ac 464 targ_sita=-PI/18.0;
DeguNaoto 38:f4e9893641ac 465 sendData(7,0);
DeguNaoto 38:f4e9893641ac 466 flagf=0;
DeguNaoto 38:f4e9893641ac 467 }
DeguNaoto 38:f4e9893641ac 468 if((mstep==3)&&(((targ_sita-sita)<0.05)&&((targ_sita-sita)>-0.05))){
DeguNaoto 38:f4e9893641ac 469 flagf=1;
DeguNaoto 38:f4e9893641ac 470 targ_velocity=speed;
DeguNaoto 38:f4e9893641ac 471 mstep=4;
DeguNaoto 38:f4e9893641ac 472 }
DeguNaoto 38:f4e9893641ac 473 if((mstep==4)&&(x<1600.0)){
DeguNaoto 38:f4e9893641ac 474 targ_sita=-5.0*PI/6.0;
DeguNaoto 38:f4e9893641ac 475 mstep=5;
DeguNaoto 38:f4e9893641ac 476 }
DeguNaoto 38:f4e9893641ac 477 if((mstep==5)&&(x<-10.0)){
DeguNaoto 38:f4e9893641ac 478 targ_velocity=0.0;
DeguNaoto 38:f4e9893641ac 479 targ_sita=-3.0*PI/4.0;
DeguNaoto 38:f4e9893641ac 480 mstep=114;
DeguNaoto 38:f4e9893641ac 481 }
DeguNaoto 38:f4e9893641ac 482
DeguNaoto 38:f4e9893641ac 483
DeguNaoto 38:f4e9893641ac 484 /********************************Swing Mode(over)*********************************/
DeguNaoto 38:f4e9893641ac 485 // if((mstep==10)&&((10000.0>x)&&(x>1500.0))) {
DeguNaoto 38:f4e9893641ac 486 if((mstep==10)&&((10000.0>x)&&(x>1550.0))) {
DeguNaoto 38:f4e9893641ac 487 dpcount=speed;
DeguNaoto 38:f4e9893641ac 488 mstep=11;
DeguNaoto 38:f4e9893641ac 489 }
DeguNaoto 38:f4e9893641ac 490 if((mstep==11)&&((velocity<5.0)&&(velocity>-5.0))){
DeguNaoto 39:5ba406327aa3 491 targ_sita=-PI/3.0-PI/10.0+PI/4.0;
DeguNaoto 38:f4e9893641ac 492 swingmoved=2;
DeguNaoto 38:f4e9893641ac 493 mstep=252;
DeguNaoto 38:f4e9893641ac 494 }
DeguNaoto 38:f4e9893641ac 495 if((mstep==252)&&(((targ_sita-sita)<0.01)&&((targ_sita-sita)>-0.01))){
DeguNaoto 38:f4e9893641ac 496 flagf=2;
DeguNaoto 38:f4e9893641ac 497 }
DeguNaoto 38:f4e9893641ac 498 if(mstep==12){
DeguNaoto 38:f4e9893641ac 499 mstep=13;
DeguNaoto 38:f4e9893641ac 500 targ_sita=-3.0*PI/4.0+PI/8.0;
DeguNaoto 38:f4e9893641ac 501 sendData(7,0);
DeguNaoto 38:f4e9893641ac 502 flagf=0;
DeguNaoto 38:f4e9893641ac 503 }
DeguNaoto 38:f4e9893641ac 504 if((mstep==13)&&(((targ_sita-sita)<0.05)&&((targ_sita-sita)>-0.05))){
DeguNaoto 38:f4e9893641ac 505 flagf=1;
DeguNaoto 38:f4e9893641ac 506 targ_velocity=speed;
DeguNaoto 38:f4e9893641ac 507 mstep=14;
DeguNaoto 38:f4e9893641ac 508 }
DeguNaoto 38:f4e9893641ac 509 if((mstep==14)&&(x<1600.0)){
DeguNaoto 38:f4e9893641ac 510 targ_sita=-5.0*PI/6.0;
DeguNaoto 38:f4e9893641ac 511 mstep=15;
DeguNaoto 38:f4e9893641ac 512 }
DeguNaoto 38:f4e9893641ac 513 if((mstep==15)&&(x<-10.0)){
DeguNaoto 38:f4e9893641ac 514 targ_velocity=0.0;
DeguNaoto 38:f4e9893641ac 515 targ_sita=-3.0*PI/4.0;
DeguNaoto 38:f4e9893641ac 516 mstep=114;
DeguNaoto 38:f4e9893641ac 517 }
DeguNaoto 38:f4e9893641ac 518
DeguNaoto 38:f4e9893641ac 519 /********************************Swing Mode(front)*********************************/
DeguNaoto 38:f4e9893641ac 520 if((mstep==20)&&((10000.0>x)&&(x>1550.0))) {
DeguNaoto 38:f4e9893641ac 521 dpcount=speed;
DeguNaoto 38:f4e9893641ac 522 mstep=21;
DeguNaoto 38:f4e9893641ac 523 }
DeguNaoto 38:f4e9893641ac 524 if((mstep==21)&&((velocity<5.0)&&(velocity>-5.0))){
DeguNaoto 38:f4e9893641ac 525 targ_sita=0.0;
DeguNaoto 38:f4e9893641ac 526 swingmoved=2;
DeguNaoto 38:f4e9893641ac 527 mstep=253;
DeguNaoto 38:f4e9893641ac 528 }
DeguNaoto 38:f4e9893641ac 529 if((mstep==252)&&(((targ_sita-sita)<0.01)&&((targ_sita-sita)>-0.01))){
DeguNaoto 38:f4e9893641ac 530 flagf=2;
DeguNaoto 38:f4e9893641ac 531 }
DeguNaoto 38:f4e9893641ac 532 if(mstep==22){
DeguNaoto 38:f4e9893641ac 533 mstep=23;
DeguNaoto 38:f4e9893641ac 534 targ_sita=PI/6.0;
DeguNaoto 38:f4e9893641ac 535 sendData(7,0);
DeguNaoto 38:f4e9893641ac 536 flagf=0;
DeguNaoto 38:f4e9893641ac 537 }
DeguNaoto 38:f4e9893641ac 538 if((mstep==23)&&(((targ_sita-sita)<0.05)&&((targ_sita-sita)>-0.05))){
DeguNaoto 38:f4e9893641ac 539 flagf=1;
DeguNaoto 38:f4e9893641ac 540 targ_velocity=speed;
DeguNaoto 38:f4e9893641ac 541 mstep=24;
DeguNaoto 38:f4e9893641ac 542 }
DeguNaoto 38:f4e9893641ac 543 if((mstep==24)&&(x<1600.0)){
DeguNaoto 38:f4e9893641ac 544 targ_sita=-5.0*PI/6.0;
DeguNaoto 38:f4e9893641ac 545 mstep=25;
DeguNaoto 38:f4e9893641ac 546 }
DeguNaoto 38:f4e9893641ac 547 if((mstep==25)&&(x<-10.0)){
DeguNaoto 38:f4e9893641ac 548 targ_velocity=0.0;
DeguNaoto 38:f4e9893641ac 549 targ_sita=-3.0*PI/4.0;
DeguNaoto 38:f4e9893641ac 550 mstep=114;
DeguNaoto 38:f4e9893641ac 551 }
DeguNaoto 34:aa2a5c888a27 552 #endif
DeguNaoto 0:b613dc16f27d 553 }
DeguNaoto 34:aa2a5c888a27 554 // pc.printf("b:%d\r\n",swingRadVelocity);
DeguNaoto 24:6d2573d6f2b6 555 // pc.printf("Swing:%f\r\n",SwingSens.getPulses());
DeguNaoto 38:f4e9893641ac 556 // pc.printf("x:%f ,y:%f ,sita:%f ,r:%f\r\n",x,y,sita,Pulses_move_r);
DeguNaoto 0:b613dc16f27d 557 }
DeguNaoto 4:09f684eac572 558 }