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@34:aa2a5c888a27, 2015-11-13 (annotated)

Committer:
DeguNaoto
Date:
Fri Nov 13 08:51:09 2015 +0000
Revision:
34:aa2a5c888a27
Parent:
33:a4323c20494b
Child:
35:7b6786193aa2
20151112 ???

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 34:aa2a5c888a27 80 if((x>6150.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 34:aa2a5c888a27 88 if((x<7050.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 32:b8c8ad2eeca7 93 if(!skip) sendData(1,5); //middle
DeguNaoto 0:b613dc16f27d 94 CStep=5;
DeguNaoto 0:b613dc16f27d 95 }
DeguNaoto 34:aa2a5c888a27 96 if((x<6950.0)&&(CStep==5)) {
DeguNaoto 33:a4323c20494b 97 if(!skip) sendData(1,6); //front
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 33:a4323c20494b 106 if((step==10)&&((10000.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 30:cd344beb415d 121 // targ_sita=0.0;
DeguNaoto 33:a4323c20494b 122 targ_sita=-0.03;
DeguNaoto 30:cd344beb415d 123 }
DeguNaoto 30:cd344beb415d 124 if((step==13)&&(x<1400.0)) {
DeguNaoto 30:cd344beb415d 125 targ_sita=PI/4;
DeguNaoto 30:cd344beb415d 126 step=14;
DeguNaoto 30:cd344beb415d 127 }
DeguNaoto 30:cd344beb415d 128 if((step==14)&&(x<600.0)) {
DeguNaoto 30:cd344beb415d 129 dpcount=speed;
DeguNaoto 30:cd344beb415d 130 step=114;
DeguNaoto 30:cd344beb415d 131 }
DeguNaoto 30:cd344beb415d 132
DeguNaoto 33:a4323c20494b 133 //Cylinderd
DeguNaoto 33:a4323c20494b 134 if((x>9500.0)&&(CStep==10)) CStep=11;
DeguNaoto 33:a4323c20494b 135 if((x<9400.0)&&(CStep==11)) {
DeguNaoto 33:a4323c20494b 136 if(!skip) sendData(1,4);
DeguNaoto 30:cd344beb415d 137 CStep=12;
DeguNaoto 30:cd344beb415d 138 }
DeguNaoto 33:a4323c20494b 139 if((x<6900.0)&&(CStep==12)) {
DeguNaoto 33:a4323c20494b 140 if(!skip) sendData(1,5);
DeguNaoto 30:cd344beb415d 141 CStep=13;
DeguNaoto 30:cd344beb415d 142 }
DeguNaoto 33:a4323c20494b 143 if((x<5350.0)&&(CStep==13)) {
DeguNaoto 33:a4323c20494b 144 if(!skip) sendData(1,6);
DeguNaoto 30:cd344beb415d 145 CStep=14;
DeguNaoto 30:cd344beb415d 146 }
DeguNaoto 33:a4323c20494b 147 if((x<1000.0)&&(CStep==14)){
DeguNaoto 0:b613dc16f27d 148 sendData(7,0);
DeguNaoto 0:b613dc16f27d 149 CStep=114;
DeguNaoto 0:b613dc16f27d 150 }
DeguNaoto 24:6d2573d6f2b6 151 #else
DeguNaoto 34:aa2a5c888a27 152 /********************************Own & Middle Mode*********************************/
DeguNaoto 34:aa2a5c888a27 153 if((step==0)&&((10000.0>x)&&(x>1000.0))) {
DeguNaoto 34:aa2a5c888a27 154 targ_sita=0.03;
DeguNaoto 34:aa2a5c888a27 155 // targ_sita=0.0;
DeguNaoto 34:aa2a5c888a27 156 step=1;
DeguNaoto 34:aa2a5c888a27 157 }
DeguNaoto 34:aa2a5c888a27 158 if((step==1)&&(x>9000.0)) {
DeguNaoto 34:aa2a5c888a27 159 targ_velocity=0.0;
DeguNaoto 34:aa2a5c888a27 160 velocity_controller.setBias(0.0);
DeguNaoto 34:aa2a5c888a27 161 velocity_controller.reset();
DeguNaoto 34:aa2a5c888a27 162 // dpcount=speed;
DeguNaoto 34:aa2a5c888a27 163 step=2;
DeguNaoto 34:aa2a5c888a27 164 }
DeguNaoto 34:aa2a5c888a27 165 if((step==2)&&((velocity<500.0)&&(velocity>-500.0))){
DeguNaoto 34:aa2a5c888a27 166 step=3;
DeguNaoto 34:aa2a5c888a27 167 spcount=0.0;
DeguNaoto 34:aa2a5c888a27 168 flagf=0;
DeguNaoto 34:aa2a5c888a27 169 // targ_sita=0.0;
DeguNaoto 34:aa2a5c888a27 170 targ_sita=-0.03;
DeguNaoto 34:aa2a5c888a27 171 }
DeguNaoto 34:aa2a5c888a27 172 if((step==3)&&(x<1700.0)) {
DeguNaoto 34:aa2a5c888a27 173 targ_sita=-PI/4;
DeguNaoto 34:aa2a5c888a27 174 step=4;
DeguNaoto 34:aa2a5c888a27 175 }
DeguNaoto 34:aa2a5c888a27 176 if((step==4)&&(x<800.0)) {
DeguNaoto 34:aa2a5c888a27 177 dpcount=speed;
DeguNaoto 34:aa2a5c888a27 178 step=114;
DeguNaoto 34:aa2a5c888a27 179 }
DeguNaoto 34:aa2a5c888a27 180
DeguNaoto 34:aa2a5c888a27 181 //Cylinder
DeguNaoto 34:aa2a5c888a27 182 if((x>3250.0)&&(CStep==0)) {
DeguNaoto 34:aa2a5c888a27 183 if(!skip) sendData(1,1);
DeguNaoto 34:aa2a5c888a27 184 CStep=1;
DeguNaoto 34:aa2a5c888a27 185 }
DeguNaoto 34:aa2a5c888a27 186 if((x>6200.0)&&(CStep==1)) {
DeguNaoto 34:aa2a5c888a27 187 if(!skip) sendData(1,3);
DeguNaoto 34:aa2a5c888a27 188 CStep=2;
DeguNaoto 34:aa2a5c888a27 189 }
DeguNaoto 34:aa2a5c888a27 190 if((x>7800.0)&&(CStep==2)) {
DeguNaoto 34:aa2a5c888a27 191 if(!skip) sendData(1,2);
DeguNaoto 34:aa2a5c888a27 192 CStep=3;
DeguNaoto 34:aa2a5c888a27 193 }
DeguNaoto 34:aa2a5c888a27 194 if((x<7000.0)&&(CStep==3)) {
DeguNaoto 34:aa2a5c888a27 195 if(!skip) sendData(1,6); //over
DeguNaoto 34:aa2a5c888a27 196 CStep=4;
DeguNaoto 34:aa2a5c888a27 197 }
DeguNaoto 34:aa2a5c888a27 198 if((x<6920.0)&&(CStep==4)) {
DeguNaoto 34:aa2a5c888a27 199 if(!skip) sendData(1,5); //middle
DeguNaoto 34:aa2a5c888a27 200 CStep=5;
DeguNaoto 34:aa2a5c888a27 201 }
DeguNaoto 34:aa2a5c888a27 202 if((x<6900.0)&&(CStep==5)) {
DeguNaoto 34:aa2a5c888a27 203 if(!skip) sendData(1,4); //front
DeguNaoto 34:aa2a5c888a27 204 CStep=6;
DeguNaoto 34:aa2a5c888a27 205 }
DeguNaoto 34:aa2a5c888a27 206 if((x<3000.0)&&(CStep==6)){
DeguNaoto 34:aa2a5c888a27 207 sendData(7,0);
DeguNaoto 34:aa2a5c888a27 208 CStep=114;
DeguNaoto 34:aa2a5c888a27 209 }
DeguNaoto 24:6d2573d6f2b6 210 #endif
DeguNaoto 0:b613dc16f27d 211 }
DeguNaoto 33:a4323c20494b 212 // else if(!modeflag) {
DeguNaoto 33:a4323c20494b 213 else if(!autoflag){
DeguNaoto 0:b613dc16f27d 214 manualIM920(); /*IM920 button*/
DeguNaoto 34:aa2a5c888a27 215 #ifdef BLUE
DeguNaoto 30:cd344beb415d 216 /********************************Swing Mode*********************************/
DeguNaoto 33:a4323c20494b 217 if((mstep==0)&&((10000.0>x)&&(x>1700.0))) {
DeguNaoto 30:cd344beb415d 218 dpcount=speed;
DeguNaoto 33:a4323c20494b 219 mstep=1;
DeguNaoto 33:a4323c20494b 220 }
DeguNaoto 33:a4323c20494b 221 if((mstep==1)&&((velocity<5.0)&&(velocity>-5.0))){
DeguNaoto 34:aa2a5c888a27 222 targ_sita=PI/4.0;
DeguNaoto 30:cd344beb415d 223 mstep=114;
DeguNaoto 30:cd344beb415d 224 }
DeguNaoto 30:cd344beb415d 225
DeguNaoto 33:a4323c20494b 226 /********************************Opponent Struct Mode*********************************/
DeguNaoto 33:a4323c20494b 227 if((mstep==10)&&((10000.0>x)&&(x>1150.0))) {
DeguNaoto 33:a4323c20494b 228 targ_sita=-0.03;
DeguNaoto 33:a4323c20494b 229 // targ_sita=0.0;
DeguNaoto 33:a4323c20494b 230 mstep=11;
DeguNaoto 33:a4323c20494b 231 }
DeguNaoto 33:a4323c20494b 232 /*if((mstep==11)&&(x>8000.0)) {
DeguNaoto 33:a4323c20494b 233 targ_velocity=0.0;
DeguNaoto 33:a4323c20494b 234 mstep=13;
DeguNaoto 33:a4323c20494b 235 }*/
DeguNaoto 33:a4323c20494b 236 if((mstep==11)&&(x>8000.0)) {
DeguNaoto 33:a4323c20494b 237 targ_sita=-PI/4.0;
DeguNaoto 33:a4323c20494b 238 mstep=12;
DeguNaoto 33:a4323c20494b 239 }
DeguNaoto 33:a4323c20494b 240 if((mstep==12)&&(x>8700.0)) {
DeguNaoto 33:a4323c20494b 241 targ_velocity=0.0;
DeguNaoto 33:a4323c20494b 242 mstep=13;
DeguNaoto 33:a4323c20494b 243 }
DeguNaoto 33:a4323c20494b 244 if((mstep==13)&&((velocity<5.0)&&(velocity>-5.0))){
DeguNaoto 33:a4323c20494b 245 targ_sita=0.0;
DeguNaoto 33:a4323c20494b 246 mstep=14;
DeguNaoto 33:a4323c20494b 247 }
DeguNaoto 33:a4323c20494b 248 /*if((mstep==14)&&((sita<0.01)&&(sita>-0.01))){
DeguNaoto 33:a4323c20494b 249 sendData(1,6);
DeguNaoto 33:a4323c20494b 250 mstep=114;
DeguNaoto 33:a4323c20494b 251 }*/
DeguNaoto 33:a4323c20494b 252 /*if((mCStep==0)&&(x>7100.0)){
DeguNaoto 33:a4323c20494b 253 sendData(1,4);
DeguNaoto 33:a4323c20494b 254 mCStep=114;
DeguNaoto 33:a4323c20494b 255 }*/
DeguNaoto 34:aa2a5c888a27 256 #else
DeguNaoto 34:aa2a5c888a27 257 #endif
DeguNaoto 0:b613dc16f27d 258 }
DeguNaoto 34:aa2a5c888a27 259 // pc.printf("b:%d\r\n",swingRadVelocity);
DeguNaoto 24:6d2573d6f2b6 260 // pc.printf("Swing:%f\r\n",SwingSens.getPulses());
DeguNaoto 34:aa2a5c888a27 261 pc.printf("x:%f ,y:%f ,sita:%f ,r:%f\r\n",x,y,sita,Pulses_move_r);
DeguNaoto 0:b613dc16f27d 262 }
DeguNaoto 4:09f684eac572 263 }