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@32:b8c8ad2eeca7, 2015-11-11 (annotated)

Committer:
DeguNaoto
Date:
Wed Nov 11 06:40:39 2015 +0000
Revision:
32:b8c8ad2eeca7
Parent:
31:897fbd97d264
Child:
33:a4323c20494b
20151110 ???

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 0:b613dc16f27d 42 if(autoflag){
DeguNaoto 27:88863fab46c0 43 autoIM920(); /*IM920 button*/
DeguNaoto 24:6d2573d6f2b6 44 #ifdef BLUE
DeguNaoto 30:cd344beb415d 45 /********************************Own & Middle Mode*********************************/
DeguNaoto 32:b8c8ad2eeca7 46 if((step==0)&&((10000.0>x)&&(x>900.0))) {
DeguNaoto 27:88863fab46c0 47 targ_sita=-0.06;
DeguNaoto 21:bdf8ac5c200c 48 // targ_sita=0.0;
DeguNaoto 0:b613dc16f27d 49 step=1;
DeguNaoto 0:b613dc16f27d 50 }
DeguNaoto 30:cd344beb415d 51 if((step==1)&&(x>9000.0)) {
DeguNaoto 0:b613dc16f27d 52 targ_velocity=0.0;
DeguNaoto 3:8d8c25c556ae 53 velocity_controller.setBias(0.0);
DeguNaoto 3:8d8c25c556ae 54 velocity_controller.reset();
DeguNaoto 26:760f1bce8214 55 // dpcount=speed;
DeguNaoto 0:b613dc16f27d 56 step=2;
DeguNaoto 0:b613dc16f27d 57 }
DeguNaoto 0:b613dc16f27d 58 if((step==2)&&((velocity<500.0)&&(velocity>-500.0))){
DeguNaoto 0:b613dc16f27d 59 step=3;
DeguNaoto 0:b613dc16f27d 60 spcount=0.0;
DeguNaoto 0:b613dc16f27d 61 flagf=0;
DeguNaoto 24:6d2573d6f2b6 62 // targ_sita=0.0;
DeguNaoto 27:88863fab46c0 63 targ_sita=-0.06;
DeguNaoto 0:b613dc16f27d 64 }
DeguNaoto 17:726b6f53a457 65 if((step==3)&&(x<1400.0)) {
DeguNaoto 0:b613dc16f27d 66 targ_sita=PI/4;
DeguNaoto 0:b613dc16f27d 67 step=4;
DeguNaoto 0:b613dc16f27d 68 }
DeguNaoto 30:cd344beb415d 69 if((step==4)&&(x<500.0)) {
DeguNaoto 24:6d2573d6f2b6 70 dpcount=speed;
DeguNaoto 0:b613dc16f27d 71 step=114;
DeguNaoto 0:b613dc16f27d 72 }
DeguNaoto 26:760f1bce8214 73
DeguNaoto 30:cd344beb415d 74 //Cylinder
DeguNaoto 30:cd344beb415d 75 if((x>3100.0)&&(CStep==0)) {
DeguNaoto 23:26f9483439fe 76 if(!skip) sendData(1,1);
DeguNaoto 30:cd344beb415d 77 CStep=1;
DeguNaoto 0:b613dc16f27d 78 }
DeguNaoto 30:cd344beb415d 79 if((x>6000.0)&&(CStep==1)) {
DeguNaoto 30:cd344beb415d 80 if(!skip) sendData(1,2);
DeguNaoto 30:cd344beb415d 81 CStep=2;
DeguNaoto 30:cd344beb415d 82 }
DeguNaoto 30:cd344beb415d 83 if((x>7700.0)&&(CStep==2)) {
DeguNaoto 0:b613dc16f27d 84 if(!skip) sendData(1,3);
DeguNaoto 0:b613dc16f27d 85 CStep=3;
DeguNaoto 0:b613dc16f27d 86 }
DeguNaoto 32:b8c8ad2eeca7 87 if((x<7000.0)&&(CStep==3)) {
DeguNaoto 32:b8c8ad2eeca7 88 if(!skip) sendData(1,4); //over
DeguNaoto 0:b613dc16f27d 89 CStep=4;
DeguNaoto 0:b613dc16f27d 90 }
DeguNaoto 32:b8c8ad2eeca7 91 if((x<6920.0)&&(CStep==4)) {
DeguNaoto 32:b8c8ad2eeca7 92 if(!skip) sendData(1,5); //middle
DeguNaoto 32:b8c8ad2eeca7 93 wait(0.001);
DeguNaoto 32:b8c8ad2eeca7 94 if(!skip) sendData(1,6); //front
DeguNaoto 0:b613dc16f27d 95 CStep=5;
DeguNaoto 0:b613dc16f27d 96 }
DeguNaoto 32:b8c8ad2eeca7 97 if((x<6850.0)&&(CStep==5)) {
DeguNaoto 32:b8c8ad2eeca7 98 // if(!skip) sendData(1,6); //front
DeguNaoto 0:b613dc16f27d 99 CStep=6;
DeguNaoto 0:b613dc16f27d 100 }
DeguNaoto 30:cd344beb415d 101 if((x<3000.0)&&(CStep==6)){
DeguNaoto 30:cd344beb415d 102 sendData(7,0);
DeguNaoto 30:cd344beb415d 103 CStep=114;
DeguNaoto 27:88863fab46c0 104 }
DeguNaoto 27:88863fab46c0 105
DeguNaoto 30:cd344beb415d 106 /********************************Own & Opponent Mode*********************************/
DeguNaoto 30:cd344beb415d 107 if((step==10)&&((10000.0>x)&&(x>800.0))) {
DeguNaoto 30:cd344beb415d 108 targ_sita=-0.06;
DeguNaoto 30:cd344beb415d 109 // targ_sita=0.0;
DeguNaoto 30:cd344beb415d 110 step=11;
DeguNaoto 30:cd344beb415d 111 }
DeguNaoto 30:cd344beb415d 112 if((step==11)&&(x>10000.0)) {
DeguNaoto 30:cd344beb415d 113 targ_velocity=0.0;
DeguNaoto 30:cd344beb415d 114 velocity_controller.setBias(0.0);
DeguNaoto 30:cd344beb415d 115 velocity_controller.reset();
DeguNaoto 30:cd344beb415d 116 // dpcount=speed;
DeguNaoto 30:cd344beb415d 117 step=12;
DeguNaoto 30:cd344beb415d 118 }
DeguNaoto 30:cd344beb415d 119 if((step==12)&&((velocity<500.0)&&(velocity>-500.0))){
DeguNaoto 30:cd344beb415d 120 step=13;
DeguNaoto 30:cd344beb415d 121 spcount=0.0;
DeguNaoto 30:cd344beb415d 122 flagf=0;
DeguNaoto 30:cd344beb415d 123 // targ_sita=0.0;
DeguNaoto 30:cd344beb415d 124 targ_sita=-0.06;
DeguNaoto 30:cd344beb415d 125 }
DeguNaoto 30:cd344beb415d 126 if((step==13)&&(x<1400.0)) {
DeguNaoto 30:cd344beb415d 127 targ_sita=PI/4;
DeguNaoto 30:cd344beb415d 128 step=14;
DeguNaoto 30:cd344beb415d 129 }
DeguNaoto 30:cd344beb415d 130 if((step==14)&&(x<600.0)) {
DeguNaoto 30:cd344beb415d 131 dpcount=speed;
DeguNaoto 30:cd344beb415d 132 step=114;
DeguNaoto 30:cd344beb415d 133 }
DeguNaoto 30:cd344beb415d 134
DeguNaoto 30:cd344beb415d 135 //Cylinder
DeguNaoto 30:cd344beb415d 136 if((x>3100.0)&&(CStep==10)) {
DeguNaoto 30:cd344beb415d 137 if(!skip) sendData(1,1);
DeguNaoto 30:cd344beb415d 138 CStep=11;
DeguNaoto 30:cd344beb415d 139 }
DeguNaoto 30:cd344beb415d 140 if((x>6000.0)&&(CStep==11)) {
DeguNaoto 30:cd344beb415d 141 if(!skip) sendData(1,2);
DeguNaoto 30:cd344beb415d 142 CStep=12;
DeguNaoto 30:cd344beb415d 143 }
DeguNaoto 30:cd344beb415d 144 if((x>7700.0)&&(CStep==12)) {
DeguNaoto 30:cd344beb415d 145 if(!skip) sendData(1,3);
DeguNaoto 30:cd344beb415d 146 CStep=13;
DeguNaoto 30:cd344beb415d 147 }
DeguNaoto 30:cd344beb415d 148 if((x>10000.0)&&(CStep==13)) {
DeguNaoto 30:cd344beb415d 149 CStep=14;
DeguNaoto 30:cd344beb415d 150 }
DeguNaoto 30:cd344beb415d 151 if((x<9300.0)&&(CStep==14)) {
DeguNaoto 30:cd344beb415d 152 if(!skip) sendData(1,4);
DeguNaoto 30:cd344beb415d 153 CStep=15;
DeguNaoto 30:cd344beb415d 154 }
DeguNaoto 30:cd344beb415d 155 if((x<7450.0)&&(CStep==15)) {
DeguNaoto 30:cd344beb415d 156 if(!skip) sendData(1,5);
DeguNaoto 30:cd344beb415d 157 CStep=16;
DeguNaoto 30:cd344beb415d 158 }
DeguNaoto 30:cd344beb415d 159 if((x<5350.0)&&(CStep==16)) {
DeguNaoto 30:cd344beb415d 160 if(!skip) sendData(1,6);
DeguNaoto 30:cd344beb415d 161 CStep=17;
DeguNaoto 30:cd344beb415d 162 }
DeguNaoto 30:cd344beb415d 163 if((x<1000.0)&&(CStep==17)){
DeguNaoto 0:b613dc16f27d 164 sendData(7,0);
DeguNaoto 0:b613dc16f27d 165 CStep=114;
DeguNaoto 0:b613dc16f27d 166 }
DeguNaoto 24:6d2573d6f2b6 167 #else
DeguNaoto 24:6d2573d6f2b6 168 #endif
DeguNaoto 0:b613dc16f27d 169 }
DeguNaoto 0:b613dc16f27d 170 else if(!autoflag) {
DeguNaoto 0:b613dc16f27d 171 manualIM920(); /*IM920 button*/
DeguNaoto 30:cd344beb415d 172 /********************************Swing Mode*********************************/
DeguNaoto 32:b8c8ad2eeca7 173 if((mstep==0)&&((10000.0>x)&&(x>1000.0))) {
DeguNaoto 30:cd344beb415d 174 dpcount=speed;
DeguNaoto 30:cd344beb415d 175 mstep=114;
DeguNaoto 30:cd344beb415d 176 }
DeguNaoto 30:cd344beb415d 177
DeguNaoto 31:897fbd97d264 178 // pc.printf("Swing:%f\r\n",swingRadVelocity);
DeguNaoto 0:b613dc16f27d 179 }
DeguNaoto 31:897fbd97d264 180 pc.printf("b:%d\r\n",swingRadVelocity);
DeguNaoto 24:6d2573d6f2b6 181 // pc.printf("Swing:%f\r\n",SwingSens.getPulses());
DeguNaoto 24:6d2573d6f2b6 182 // pc.printf("x:%f ,y:%f ,sita:%f ,r:%f\r\n",x,y,sita,Pulses_move_r);
DeguNaoto 0:b613dc16f27d 183 }
DeguNaoto 4:09f684eac572 184 }