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@23:26f9483439fe, 2015-11-07 (annotated)

Committer:
DeguNaoto
Date:
Sat Nov 07 01:03:46 2015 +0000
Revision:
23:26f9483439fe
Parent:
21:bdf8ac5c200c
Child:
24:6d2573d6f2b6
20151107 ???

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 21:bdf8ac5c200c 16 #ifdef MESURE
DeguNaoto 21:bdf8ac5c200c 17 #include "mbed.h"
DeguNaoto 21:bdf8ac5c200c 18 short mesureflag=0;
DeguNaoto 21:bdf8ac5c200c 19 LocalFileSystem local("local")
DeguNaoto 21:bdf8ac5c200c 20 short testflag=0;
DeguNaoto 21:bdf8ac5c200c 21 FILE *fp_r = fopen("/local/velocity.dat", "w");
DeguNaoto 21:bdf8ac5c200c 22 #endif
DeguNaoto 23:26f9483439fe 23 /*#include "mbed.h"
DeguNaoto 21:bdf8ac5c200c 24 LocalFileSystem local("local");
DeguNaoto 23:26f9483439fe 25 FILE *fp_r = fopen("/local/velocity.dat", "w");*/
DeguNaoto 21:bdf8ac5c200c 26
DeguNaoto 0:b613dc16f27d 27 #include "machine_ps3.h"
DeguNaoto 0:b613dc16f27d 28
DeguNaoto 0:b613dc16f27d 29 #define deff 70.0
DeguNaoto 0:b613dc16f27d 30
DeguNaoto 0:b613dc16f27d 31 Serial pc(USBTX, USBRX);
DeguNaoto 21:bdf8ac5c200c 32
DeguNaoto 21:bdf8ac5c200c 33
DeguNaoto 0:b613dc16f27d 34
DeguNaoto 0:b613dc16f27d 35 int main() {
DeguNaoto 2:738b28f6a04b 36 Com.attach(&Call,RATE);
DeguNaoto 0:b613dc16f27d 37 initializeMotors();
DeguNaoto 0:b613dc16f27d 38 initializeControllers();
DeguNaoto 0:b613dc16f27d 39 initializeRS485();
DeguNaoto 14:943e663694c3 40 initializeMbedSerial();
DeguNaoto 0:b613dc16f27d 41 #ifdef BLUE
DeguNaoto 0:b613dc16f27d 42 sita=PI/4.0,targ_sita=PI/4.0;
DeguNaoto 0:b613dc16f27d 43 IndicatorBLUE = 1;
DeguNaoto 0:b613dc16f27d 44 #else
DeguNaoto 0:b613dc16f27d 45 sita=-PI/4.0,targ_sita=-PI/4.0;
DeguNaoto 0:b613dc16f27d 46 IndicatorRED = 1;
DeguNaoto 0:b613dc16f27d 47 #endif
DeguNaoto 0:b613dc16f27d 48 Indicator4=1;
DeguNaoto 0:b613dc16f27d 49 Enable=1;
DeguNaoto 0:b613dc16f27d 50 wait(0.3);
DeguNaoto 0:b613dc16f27d 51 sendData(7,0);
DeguNaoto 0:b613dc16f27d 52 while(1) {
DeguNaoto 0:b613dc16f27d 53 if(autoflag){
DeguNaoto 0:b613dc16f27d 54 autoIM920(); /*IM920 button*/
DeguNaoto 0:b613dc16f27d 55 /********************************Nomal Mode*********************************/
DeguNaoto 23:26f9483439fe 56 if((step==0)&&((8600.0>x)&&(x>800.0))) {
DeguNaoto 23:26f9483439fe 57 targ_sita=-0.02;
DeguNaoto 21:bdf8ac5c200c 58 // targ_sita=0.0;
DeguNaoto 0:b613dc16f27d 59 step=1;
DeguNaoto 0:b613dc16f27d 60 }
DeguNaoto 17:726b6f53a457 61 // if((step==1)&&(x>8600.0+deff)) {
DeguNaoto 23:26f9483439fe 62 if((step==1)&&(x>8600.0)) {
DeguNaoto 0:b613dc16f27d 63 targ_velocity=0.0;
DeguNaoto 2:738b28f6a04b 64 direction_controller.setBias(0.0);
DeguNaoto 2:738b28f6a04b 65 direction_controller.reset();
DeguNaoto 3:8d8c25c556ae 66 velocity_controller.setBias(0.0);
DeguNaoto 3:8d8c25c556ae 67 velocity_controller.reset();
DeguNaoto 0:b613dc16f27d 68 step=2;
DeguNaoto 0:b613dc16f27d 69 }
DeguNaoto 0:b613dc16f27d 70 if((step==2)&&((velocity<500.0)&&(velocity>-500.0))){
DeguNaoto 0:b613dc16f27d 71 step=3;
DeguNaoto 0:b613dc16f27d 72 spcount=0.0;
DeguNaoto 0:b613dc16f27d 73 flagf=0;
DeguNaoto 5:0e6dd1ff3ff2 74 // targ_sita=0.1;
DeguNaoto 5:0e6dd1ff3ff2 75 targ_sita=0.0;
DeguNaoto 0:b613dc16f27d 76 }
DeguNaoto 6:ca3a74a93ae2 77 // if((step==3)&&(x<2000.0)) {
DeguNaoto 17:726b6f53a457 78 if((step==3)&&(x<1400.0)) {
DeguNaoto 0:b613dc16f27d 79 targ_sita=PI/4;
DeguNaoto 0:b613dc16f27d 80 step=4;
DeguNaoto 0:b613dc16f27d 81 }
DeguNaoto 17:726b6f53a457 82 if((step==4)&&(x<600.0)) {
DeguNaoto 0:b613dc16f27d 83 targ_velocity=0.0;
DeguNaoto 0:b613dc16f27d 84 step=114;
DeguNaoto 0:b613dc16f27d 85 }
DeguNaoto 0:b613dc16f27d 86 /***Cylinder***/
DeguNaoto 6:ca3a74a93ae2 87 if((x>3130.0+deff)&&(CStep==1)) {
DeguNaoto 21:bdf8ac5c200c 88 // if((x>2030.0+deff)&&(CStep==1)) {
DeguNaoto 23:26f9483439fe 89 if(!skip) sendData(1,1);
DeguNaoto 0:b613dc16f27d 90 CStep=2;
DeguNaoto 0:b613dc16f27d 91 }
DeguNaoto 0:b613dc16f27d 92 if((x>5900.0+deff)&&(CStep==2)) {
DeguNaoto 0:b613dc16f27d 93 if(!skip) sendData(1,3);
DeguNaoto 0:b613dc16f27d 94 CStep=3;
DeguNaoto 0:b613dc16f27d 95 }
DeguNaoto 0:b613dc16f27d 96 if((x>7680.0+deff)&&(CStep==3)) {
DeguNaoto 0:b613dc16f27d 97 if(!skip) sendData(1,2);
DeguNaoto 0:b613dc16f27d 98 CStep=4;
DeguNaoto 0:b613dc16f27d 99 }
DeguNaoto 0:b613dc16f27d 100 if((x<6580.0+deff)&&(CStep==4)) {
DeguNaoto 0:b613dc16f27d 101 if(!skip) sendData(1,5);
DeguNaoto 0:b613dc16f27d 102 CStep=5;
DeguNaoto 0:b613dc16f27d 103 }
DeguNaoto 0:b613dc16f27d 104 if((x<6100.0+deff)&&(CStep==5)) {
DeguNaoto 0:b613dc16f27d 105 if(!skip) sendData(1,4);
DeguNaoto 0:b613dc16f27d 106 CStep=6;
DeguNaoto 0:b613dc16f27d 107 }
DeguNaoto 0:b613dc16f27d 108 if((x<4000.0)&&(CStep==6)){
DeguNaoto 0:b613dc16f27d 109 sendData(7,0);
DeguNaoto 0:b613dc16f27d 110 CStep=114;
DeguNaoto 0:b613dc16f27d 111 }
DeguNaoto 0:b613dc16f27d 112 }
DeguNaoto 0:b613dc16f27d 113 else if(!autoflag) {
DeguNaoto 0:b613dc16f27d 114 flaga=0;
DeguNaoto 3:8d8c25c556ae 115 mesureSwing();
DeguNaoto 0:b613dc16f27d 116 manualMoveIM920(); /*analogStick*/
DeguNaoto 0:b613dc16f27d 117 manualIM920(); /*IM920 button*/
DeguNaoto 0:b613dc16f27d 118 }
DeguNaoto 0:b613dc16f27d 119 /***update state***/
DeguNaoto 21:bdf8ac5c200c 120 pc.printf("x:%f ,y:%f ,sita:%f ,r:%f\r\n",x,y,sita,Pulses_move_r);
DeguNaoto 21:bdf8ac5c200c 121 // pc.printf("a2:%d ,b:%d ,X:%d ,Y:%d\r\n",a2,b,X,Y);
DeguNaoto 0:b613dc16f27d 122 // wait(RATE);
DeguNaoto 0:b613dc16f27d 123 }
DeguNaoto 4:09f684eac572 124 }