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@35:7b6786193aa2, 2015-11-14 (annotated)

Committer:
DeguNaoto
Date:
Sat Nov 14 04:05:13 2015 +0000
Revision:
35:7b6786193aa2
Parent:
34:aa2a5c888a27
Child:
36:b8954b13a6d5
20151113 ??????????????

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(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 33:a4323c20494b 47 if((step==0)&&((10000.0>x)&&(x>1000.0))) {
DeguNaoto 33:a4323c20494b 48 targ_sita=-0.03;
DeguNaoto 21:bdf8ac5c200c 49 // targ_sita=0.0;
DeguNaoto 0:b613dc16f27d 50 step=1;
DeguNaoto 0:b613dc16f27d 51 }
DeguNaoto 30:cd344beb415d 52 if((step==1)&&(x>9000.0)) {
DeguNaoto 0:b613dc16f27d 53 targ_velocity=0.0;
DeguNaoto 3:8d8c25c556ae 54 velocity_controller.setBias(0.0);
DeguNaoto 3:8d8c25c556ae 55 velocity_controller.reset();
DeguNaoto 26:760f1bce8214 56 // dpcount=speed;
DeguNaoto 0:b613dc16f27d 57 step=2;
DeguNaoto 0:b613dc16f27d 58 }
DeguNaoto 0:b613dc16f27d 59 if((step==2)&&((velocity<500.0)&&(velocity>-500.0))){
DeguNaoto 0:b613dc16f27d 60 step=3;
DeguNaoto 0:b613dc16f27d 61 spcount=0.0;
DeguNaoto 0:b613dc16f27d 62 flagf=0;
DeguNaoto 24:6d2573d6f2b6 63 // targ_sita=0.0;
DeguNaoto 33:a4323c20494b 64 targ_sita=-0.03;
DeguNaoto 0:b613dc16f27d 65 }
DeguNaoto 33:a4323c20494b 66 if((step==3)&&(x<1700.0)) {
DeguNaoto 0:b613dc16f27d 67 targ_sita=PI/4;
DeguNaoto 0:b613dc16f27d 68 step=4;
DeguNaoto 0:b613dc16f27d 69 }
DeguNaoto 33:a4323c20494b 70 if((step==4)&&(x<800.0)) {
DeguNaoto 24:6d2573d6f2b6 71 dpcount=speed;
DeguNaoto 0:b613dc16f27d 72 step=114;
DeguNaoto 0:b613dc16f27d 73 }
DeguNaoto 26:760f1bce8214 74
DeguNaoto 30:cd344beb415d 75 //Cylinder
DeguNaoto 34:aa2a5c888a27 76 if((x>3200.0)&&(CStep==0)) {
DeguNaoto 23:26f9483439fe 77 if(!skip) sendData(1,1);
DeguNaoto 30:cd344beb415d 78 CStep=1;
DeguNaoto 0:b613dc16f27d 79 }
DeguNaoto 35:7b6786193aa2 80 if((x>6100.0)&&(CStep==1)) {
DeguNaoto 30:cd344beb415d 81 if(!skip) sendData(1,2);
DeguNaoto 30:cd344beb415d 82 CStep=2;
DeguNaoto 30:cd344beb415d 83 }
DeguNaoto 34:aa2a5c888a27 84 if((x>7750.0)&&(CStep==2)) {
DeguNaoto 0:b613dc16f27d 85 if(!skip) sendData(1,3);
DeguNaoto 0:b613dc16f27d 86 CStep=3;
DeguNaoto 0:b613dc16f27d 87 }
DeguNaoto 35:7b6786193aa2 88 if((x<7200.0)&&(CStep==3)) {
DeguNaoto 32:b8c8ad2eeca7 89 if(!skip) sendData(1,4); //over
DeguNaoto 0:b613dc16f27d 90 CStep=4;
DeguNaoto 0:b613dc16f27d 91 }
DeguNaoto 34:aa2a5c888a27 92 if((x<6950.0)&&(CStep==4)) {
DeguNaoto 35:7b6786193aa2 93 if(!skip) sendData(1,6); //front
DeguNaoto 0:b613dc16f27d 94 CStep=5;
DeguNaoto 0:b613dc16f27d 95 }
DeguNaoto 35:7b6786193aa2 96 if((x<6700.0)&&(CStep==5)) {
DeguNaoto 35:7b6786193aa2 97 if(!skip) sendData(1,5); //middle
DeguNaoto 0:b613dc16f27d 98 CStep=6;
DeguNaoto 0:b613dc16f27d 99 }
DeguNaoto 30:cd344beb415d 100 if((x<3000.0)&&(CStep==6)){
DeguNaoto 30:cd344beb415d 101 sendData(7,0);
DeguNaoto 30:cd344beb415d 102 CStep=114;
DeguNaoto 27:88863fab46c0 103 }
DeguNaoto 27:88863fab46c0 104
DeguNaoto 30:cd344beb415d 105 /********************************Own & Opponent Mode*********************************/
DeguNaoto 35:7b6786193aa2 106 if((step==10)&&((9800.0>x)&&(x>700.0))) {
DeguNaoto 33:a4323c20494b 107 targ_sita=-0.03;
DeguNaoto 30:cd344beb415d 108 // targ_sita=0.0;
DeguNaoto 30:cd344beb415d 109 step=11;
DeguNaoto 30:cd344beb415d 110 }
DeguNaoto 30:cd344beb415d 111 if((step==11)&&(x>10000.0)) {
DeguNaoto 30:cd344beb415d 112 targ_velocity=0.0;
DeguNaoto 30:cd344beb415d 113 velocity_controller.setBias(0.0);
DeguNaoto 30:cd344beb415d 114 velocity_controller.reset();
DeguNaoto 30:cd344beb415d 115 step=12;
DeguNaoto 30:cd344beb415d 116 }
DeguNaoto 30:cd344beb415d 117 if((step==12)&&((velocity<500.0)&&(velocity>-500.0))){
DeguNaoto 30:cd344beb415d 118 step=13;
DeguNaoto 30:cd344beb415d 119 spcount=0.0;
DeguNaoto 30:cd344beb415d 120 flagf=0;
DeguNaoto 35:7b6786193aa2 121 // flagf=3;
DeguNaoto 30:cd344beb415d 122 // targ_sita=0.0;
DeguNaoto 33:a4323c20494b 123 targ_sita=-0.03;
DeguNaoto 30:cd344beb415d 124 }
DeguNaoto 30:cd344beb415d 125 if((step==13)&&(x<1400.0)) {
DeguNaoto 30:cd344beb415d 126 targ_sita=PI/4;
DeguNaoto 30:cd344beb415d 127 step=14;
DeguNaoto 30:cd344beb415d 128 }
DeguNaoto 30:cd344beb415d 129 if((step==14)&&(x<600.0)) {
DeguNaoto 30:cd344beb415d 130 dpcount=speed;
DeguNaoto 30:cd344beb415d 131 step=114;
DeguNaoto 30:cd344beb415d 132 }
DeguNaoto 30:cd344beb415d 133
DeguNaoto 33:a4323c20494b 134 //Cylinderd
DeguNaoto 35:7b6786193aa2 135 if((x>3200.0)&&(CStep==10)) {
DeguNaoto 35:7b6786193aa2 136 if(!skip) sendData(1,1);
DeguNaoto 35:7b6786193aa2 137 CStep=11;
DeguNaoto 35:7b6786193aa2 138 }
DeguNaoto 35:7b6786193aa2 139 if((x>6050.0)&&(CStep==11)) {
DeguNaoto 35:7b6786193aa2 140 if(!skip) sendData(1,2);
DeguNaoto 30:cd344beb415d 141 CStep=12;
DeguNaoto 30:cd344beb415d 142 }
DeguNaoto 35:7b6786193aa2 143 if((x>7700.0)&&(CStep==12)) {
DeguNaoto 35:7b6786193aa2 144 if(!skip) sendData(1,3);
DeguNaoto 30:cd344beb415d 145 CStep=13;
DeguNaoto 30:cd344beb415d 146 }
DeguNaoto 35:7b6786193aa2 147 if((x>9500.0)&&(CStep==13)) CStep=14;
DeguNaoto 35:7b6786193aa2 148 if((x<9400.0)&&(CStep==14)) {
DeguNaoto 35:7b6786193aa2 149 if(!skip) sendData(1,4);
DeguNaoto 35:7b6786193aa2 150 CStep=15;
DeguNaoto 35:7b6786193aa2 151 }
DeguNaoto 35:7b6786193aa2 152 if((x<7250.0)&&(CStep==15)) {
DeguNaoto 35:7b6786193aa2 153 if(!skip) sendData(1,5);
DeguNaoto 35:7b6786193aa2 154 CStep=16;
DeguNaoto 35:7b6786193aa2 155 }
DeguNaoto 35:7b6786193aa2 156 if((x<4850.0)&&(CStep==16)) {
DeguNaoto 35:7b6786193aa2 157 // if((x<5450.0)&&(CStep==16)) {
DeguNaoto 33:a4323c20494b 158 if(!skip) sendData(1,6);
DeguNaoto 35:7b6786193aa2 159 CStep=17;
DeguNaoto 30:cd344beb415d 160 }
DeguNaoto 35:7b6786193aa2 161 if((x<1000.0)&&(CStep==17)){
DeguNaoto 0:b613dc16f27d 162 sendData(7,0);
DeguNaoto 0:b613dc16f27d 163 CStep=114;
DeguNaoto 0:b613dc16f27d 164 }
DeguNaoto 24:6d2573d6f2b6 165 #else
DeguNaoto 34:aa2a5c888a27 166 /********************************Own & Middle Mode*********************************/
DeguNaoto 34:aa2a5c888a27 167 if((step==0)&&((10000.0>x)&&(x>1000.0))) {
DeguNaoto 34:aa2a5c888a27 168 targ_sita=0.03;
DeguNaoto 34:aa2a5c888a27 169 // targ_sita=0.0;
DeguNaoto 34:aa2a5c888a27 170 step=1;
DeguNaoto 34:aa2a5c888a27 171 }
DeguNaoto 34:aa2a5c888a27 172 if((step==1)&&(x>9000.0)) {
DeguNaoto 34:aa2a5c888a27 173 targ_velocity=0.0;
DeguNaoto 34:aa2a5c888a27 174 velocity_controller.setBias(0.0);
DeguNaoto 34:aa2a5c888a27 175 velocity_controller.reset();
DeguNaoto 34:aa2a5c888a27 176 // dpcount=speed;
DeguNaoto 34:aa2a5c888a27 177 step=2;
DeguNaoto 34:aa2a5c888a27 178 }
DeguNaoto 34:aa2a5c888a27 179 if((step==2)&&((velocity<500.0)&&(velocity>-500.0))){
DeguNaoto 34:aa2a5c888a27 180 step=3;
DeguNaoto 34:aa2a5c888a27 181 spcount=0.0;
DeguNaoto 34:aa2a5c888a27 182 flagf=0;
DeguNaoto 34:aa2a5c888a27 183 // targ_sita=0.0;
DeguNaoto 34:aa2a5c888a27 184 targ_sita=-0.03;
DeguNaoto 34:aa2a5c888a27 185 }
DeguNaoto 34:aa2a5c888a27 186 if((step==3)&&(x<1700.0)) {
DeguNaoto 34:aa2a5c888a27 187 targ_sita=-PI/4;
DeguNaoto 34:aa2a5c888a27 188 step=4;
DeguNaoto 34:aa2a5c888a27 189 }
DeguNaoto 34:aa2a5c888a27 190 if((step==4)&&(x<800.0)) {
DeguNaoto 34:aa2a5c888a27 191 dpcount=speed;
DeguNaoto 34:aa2a5c888a27 192 step=114;
DeguNaoto 34:aa2a5c888a27 193 }
DeguNaoto 34:aa2a5c888a27 194
DeguNaoto 34:aa2a5c888a27 195 //Cylinder
DeguNaoto 34:aa2a5c888a27 196 if((x>3250.0)&&(CStep==0)) {
DeguNaoto 34:aa2a5c888a27 197 if(!skip) sendData(1,1);
DeguNaoto 34:aa2a5c888a27 198 CStep=1;
DeguNaoto 34:aa2a5c888a27 199 }
DeguNaoto 34:aa2a5c888a27 200 if((x>6200.0)&&(CStep==1)) {
DeguNaoto 34:aa2a5c888a27 201 if(!skip) sendData(1,3);
DeguNaoto 34:aa2a5c888a27 202 CStep=2;
DeguNaoto 34:aa2a5c888a27 203 }
DeguNaoto 34:aa2a5c888a27 204 if((x>7800.0)&&(CStep==2)) {
DeguNaoto 34:aa2a5c888a27 205 if(!skip) sendData(1,2);
DeguNaoto 34:aa2a5c888a27 206 CStep=3;
DeguNaoto 34:aa2a5c888a27 207 }
DeguNaoto 34:aa2a5c888a27 208 if((x<7000.0)&&(CStep==3)) {
DeguNaoto 34:aa2a5c888a27 209 if(!skip) sendData(1,6); //over
DeguNaoto 34:aa2a5c888a27 210 CStep=4;
DeguNaoto 34:aa2a5c888a27 211 }
DeguNaoto 34:aa2a5c888a27 212 if((x<6920.0)&&(CStep==4)) {
DeguNaoto 34:aa2a5c888a27 213 if(!skip) sendData(1,5); //middle
DeguNaoto 34:aa2a5c888a27 214 CStep=5;
DeguNaoto 34:aa2a5c888a27 215 }
DeguNaoto 34:aa2a5c888a27 216 if((x<6900.0)&&(CStep==5)) {
DeguNaoto 34:aa2a5c888a27 217 if(!skip) sendData(1,4); //front
DeguNaoto 34:aa2a5c888a27 218 CStep=6;
DeguNaoto 34:aa2a5c888a27 219 }
DeguNaoto 34:aa2a5c888a27 220 if((x<3000.0)&&(CStep==6)){
DeguNaoto 34:aa2a5c888a27 221 sendData(7,0);
DeguNaoto 34:aa2a5c888a27 222 CStep=114;
DeguNaoto 34:aa2a5c888a27 223 }
DeguNaoto 24:6d2573d6f2b6 224 #endif
DeguNaoto 0:b613dc16f27d 225 }
DeguNaoto 33:a4323c20494b 226 // else if(!modeflag) {
DeguNaoto 33:a4323c20494b 227 else if(!autoflag){
DeguNaoto 0:b613dc16f27d 228 manualIM920(); /*IM920 button*/
DeguNaoto 34:aa2a5c888a27 229 #ifdef BLUE
DeguNaoto 30:cd344beb415d 230 /********************************Swing Mode*********************************/
DeguNaoto 33:a4323c20494b 231 if((mstep==0)&&((10000.0>x)&&(x>1700.0))) {
DeguNaoto 30:cd344beb415d 232 dpcount=speed;
DeguNaoto 33:a4323c20494b 233 mstep=1;
DeguNaoto 33:a4323c20494b 234 }
DeguNaoto 33:a4323c20494b 235 if((mstep==1)&&((velocity<5.0)&&(velocity>-5.0))){
DeguNaoto 34:aa2a5c888a27 236 targ_sita=PI/4.0;
DeguNaoto 30:cd344beb415d 237 mstep=114;
DeguNaoto 30:cd344beb415d 238 }
DeguNaoto 30:cd344beb415d 239
DeguNaoto 33:a4323c20494b 240 /********************************Opponent Struct Mode*********************************/
DeguNaoto 33:a4323c20494b 241 if((mstep==10)&&((10000.0>x)&&(x>1150.0))) {
DeguNaoto 33:a4323c20494b 242 targ_sita=-0.03;
DeguNaoto 33:a4323c20494b 243 // targ_sita=0.0;
DeguNaoto 33:a4323c20494b 244 mstep=11;
DeguNaoto 33:a4323c20494b 245 }
DeguNaoto 33:a4323c20494b 246 /*if((mstep==11)&&(x>8000.0)) {
DeguNaoto 33:a4323c20494b 247 targ_velocity=0.0;
DeguNaoto 33:a4323c20494b 248 mstep=13;
DeguNaoto 33:a4323c20494b 249 }*/
DeguNaoto 33:a4323c20494b 250 if((mstep==11)&&(x>8000.0)) {
DeguNaoto 33:a4323c20494b 251 targ_sita=-PI/4.0;
DeguNaoto 33:a4323c20494b 252 mstep=12;
DeguNaoto 33:a4323c20494b 253 }
DeguNaoto 33:a4323c20494b 254 if((mstep==12)&&(x>8700.0)) {
DeguNaoto 33:a4323c20494b 255 targ_velocity=0.0;
DeguNaoto 33:a4323c20494b 256 mstep=13;
DeguNaoto 33:a4323c20494b 257 }
DeguNaoto 33:a4323c20494b 258 if((mstep==13)&&((velocity<5.0)&&(velocity>-5.0))){
DeguNaoto 33:a4323c20494b 259 targ_sita=0.0;
DeguNaoto 33:a4323c20494b 260 mstep=14;
DeguNaoto 33:a4323c20494b 261 }
DeguNaoto 33:a4323c20494b 262 /*if((mstep==14)&&((sita<0.01)&&(sita>-0.01))){
DeguNaoto 33:a4323c20494b 263 sendData(1,6);
DeguNaoto 33:a4323c20494b 264 mstep=114;
DeguNaoto 33:a4323c20494b 265 }*/
DeguNaoto 33:a4323c20494b 266 /*if((mCStep==0)&&(x>7100.0)){
DeguNaoto 33:a4323c20494b 267 sendData(1,4);
DeguNaoto 33:a4323c20494b 268 mCStep=114;
DeguNaoto 33:a4323c20494b 269 }*/
DeguNaoto 34:aa2a5c888a27 270 #else
DeguNaoto 34:aa2a5c888a27 271 #endif
DeguNaoto 0:b613dc16f27d 272 }
DeguNaoto 34:aa2a5c888a27 273 // pc.printf("b:%d\r\n",swingRadVelocity);
DeguNaoto 24:6d2573d6f2b6 274 // pc.printf("Swing:%f\r\n",SwingSens.getPulses());
DeguNaoto 34:aa2a5c888a27 275 pc.printf("x:%f ,y:%f ,sita:%f ,r:%f\r\n",x,y,sita,Pulses_move_r);
DeguNaoto 0:b613dc16f27d 276 }
DeguNaoto 4:09f684eac572 277 }