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@26:760f1bce8214, 2015-11-08 (annotated)

Committer:
DeguNaoto
Date:
Sun Nov 08 10:56:27 2015 +0000
Revision:
26:760f1bce8214
Parent:
24:6d2573d6f2b6
Child:
27:88863fab46c0
220151108 ???????????

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 24:6d2573d6f2b6 43 #ifdef BLUE
DeguNaoto 0:b613dc16f27d 44 autoIM920(); /*IM920 button*/
DeguNaoto 0:b613dc16f27d 45 /********************************Nomal Mode*********************************/
DeguNaoto 26:760f1bce8214 46 if((step==0)&&((9000.0>x)&&(x>800.0))) {
DeguNaoto 26:760f1bce8214 47 targ_sita=-0.07;
DeguNaoto 21:bdf8ac5c200c 48 // targ_sita=0.0;
DeguNaoto 0:b613dc16f27d 49 step=1;
DeguNaoto 0:b613dc16f27d 50 }
DeguNaoto 26:760f1bce8214 51 if((step==1)&&(x>9000.0)) {
DeguNaoto 0:b613dc16f27d 52 targ_velocity=0.0;
DeguNaoto 26:760f1bce8214 53 /*direction_controller.setBias(0.0);
DeguNaoto 26:760f1bce8214 54 direction_controller.reset();*/
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 26:760f1bce8214 65 targ_sita=-0.07;
DeguNaoto 0:b613dc16f27d 66 }
DeguNaoto 17:726b6f53a457 67 if((step==3)&&(x<1400.0)) {
DeguNaoto 0:b613dc16f27d 68 targ_sita=PI/4;
DeguNaoto 0:b613dc16f27d 69 step=4;
DeguNaoto 0:b613dc16f27d 70 }
DeguNaoto 26:760f1bce8214 71 if((step==4)&&(x<600.0)) {
DeguNaoto 24:6d2573d6f2b6 72 dpcount=speed;
DeguNaoto 0:b613dc16f27d 73 step=114;
DeguNaoto 0:b613dc16f27d 74 }
DeguNaoto 26:760f1bce8214 75
DeguNaoto 0:b613dc16f27d 76 /***Cylinder***/
DeguNaoto 26:760f1bce8214 77 if((x>3100.0)&&(CStep==1)) {
DeguNaoto 23:26f9483439fe 78 if(!skip) sendData(1,1);
DeguNaoto 0:b613dc16f27d 79 CStep=2;
DeguNaoto 0:b613dc16f27d 80 }
DeguNaoto 26:760f1bce8214 81 if((x>6000.0)&&(CStep==2)) {
DeguNaoto 0:b613dc16f27d 82 if(!skip) sendData(1,3);
DeguNaoto 0:b613dc16f27d 83 CStep=3;
DeguNaoto 0:b613dc16f27d 84 }
DeguNaoto 26:760f1bce8214 85 if((x>7700.0)&&(CStep==3)) {
DeguNaoto 0:b613dc16f27d 86 if(!skip) sendData(1,2);
DeguNaoto 0:b613dc16f27d 87 CStep=4;
DeguNaoto 0:b613dc16f27d 88 }
DeguNaoto 26:760f1bce8214 89 if((x<8500.0)&&(CStep==4)) {
DeguNaoto 0:b613dc16f27d 90 if(!skip) sendData(1,5);
DeguNaoto 0:b613dc16f27d 91 CStep=5;
DeguNaoto 0:b613dc16f27d 92 }
DeguNaoto 24:6d2573d6f2b6 93 if((x<6800.0)&&(CStep==5)) {
DeguNaoto 24:6d2573d6f2b6 94 if(!skip) sendData(1,6);
DeguNaoto 0:b613dc16f27d 95 CStep=6;
DeguNaoto 0:b613dc16f27d 96 }
DeguNaoto 26:760f1bce8214 97 if((x<4900.0)&&(CStep==6)) {
DeguNaoto 24:6d2573d6f2b6 98 if(!skip) sendData(1,4);
DeguNaoto 24:6d2573d6f2b6 99 CStep=7;
DeguNaoto 24:6d2573d6f2b6 100 }
DeguNaoto 26:760f1bce8214 101 if((x<1000.0)&&(CStep==7)){
DeguNaoto 0:b613dc16f27d 102 sendData(7,0);
DeguNaoto 0:b613dc16f27d 103 CStep=114;
DeguNaoto 0:b613dc16f27d 104 }
DeguNaoto 24:6d2573d6f2b6 105 #else
DeguNaoto 24:6d2573d6f2b6 106 #endif
DeguNaoto 0:b613dc16f27d 107 }
DeguNaoto 0:b613dc16f27d 108 else if(!autoflag) {
DeguNaoto 0:b613dc16f27d 109 flaga=0;
DeguNaoto 0:b613dc16f27d 110 manualIM920(); /*IM920 button*/
DeguNaoto 24:6d2573d6f2b6 111 // mesureSwing();
DeguNaoto 24:6d2573d6f2b6 112 // swingFollowing();
DeguNaoto 24:6d2573d6f2b6 113 // wait(RATE);
DeguNaoto 24:6d2573d6f2b6 114 // fprintf(fp_r, "%f\r\n",swingRadVelocity);
DeguNaoto 24:6d2573d6f2b6 115 // if(b==11) fclose(fp_r);
DeguNaoto 24:6d2573d6f2b6 116 pc.printf("Swing:%f\r\n",swingRadVelocity);
DeguNaoto 0:b613dc16f27d 117 }
DeguNaoto 0:b613dc16f27d 118 /***update state***/
DeguNaoto 24:6d2573d6f2b6 119 // pc.printf("Swing:%f\r\n",SwingSens.getPulses());
DeguNaoto 24:6d2573d6f2b6 120 // pc.printf("x:%f ,y:%f ,sita:%f ,r:%f\r\n",x,y,sita,Pulses_move_r);
DeguNaoto 0:b613dc16f27d 121 }
DeguNaoto 4:09f684eac572 122 }