2015_robocon_bteam / Mbed 2 deprecated 2015robot_main

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

Fork of 2015robot_main by Naoto Deguchi

main_ps3.cpp@68:2b2b88ecdcce, 2015-10-08 (annotated)

Committer:
unicore32
Date:
Thu Oct 08 09:04:48 2015 +0000
Revision:
68:2b2b88ecdcce
Parent:
66:14df82661dfa
Child:
78:abb760e0a935
8bit???

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DeguNaoto 0:bd4719e15f7e 1 /**
DeguNaoto 0:bd4719e15f7e 2 * This program is written in main micro computer "mbed" for 2015 NHK Robot Contest (Bteam).
DeguNaoto 0:bd4719e15f7e 3 */
DeguNaoto 2:cf8ca6742db9 4
DeguNaoto 34:f9ef622f4376 5 //速度コントローラと向きコントローラはそのまま
DeguNaoto 62:02a44f5bc51e 6 //大和田にPIC1のシリンダの信号をタイマーで書かせる
DeguNaoto 66:14df82661dfa 7 //回転機構wait PI/4 追加で待つ
DeguNaoto 34:f9ef622f4376 8
DeguNaoto 57:3fbd487e055e 9 /***コース選択***/
DeguNaoto 52:d9e1629da852 10 #define BLUE
DeguNaoto 52:d9e1629da852 11 //#define RED
DeguNaoto 43:f9a75ecbe44e 12
DeguNaoto 57:3fbd487e055e 13 /***コントローラ選択***/
DeguNaoto 57:3fbd487e055e 14 //#define IM920
DeguNaoto 57:3fbd487e055e 15 #define PS3
DeguNaoto 57:3fbd487e055e 16
DeguNaoto 57:3fbd487e055e 17 #if defined(IM920) && defined(PS3)
DeguNaoto 57:3fbd487e055e 18 #error Caution, You should define either IM920 or PS3
DeguNaoto 57:3fbd487e055e 19 #endif
DeguNaoto 57:3fbd487e055e 20
DeguNaoto 45:e11ec4f6d37e 21 #if defined(BLUE) && defined(RED)
DeguNaoto 45:e11ec4f6d37e 22 #error Caution, You should define either BLUE or RED
DeguNaoto 45:e11ec4f6d37e 23 #endif
DeguNaoto 45:e11ec4f6d37e 24
DeguNaoto 0:bd4719e15f7e 25 #include "machine_ps3.h"
DeguNaoto 0:bd4719e15f7e 26
DeguNaoto 0:bd4719e15f7e 27 Serial pc(USBTX, USBRX);
DeguNaoto 50:8ea4714316ce 28 //LocalFileSystem local("local");
DeguNaoto 0:bd4719e15f7e 29
DeguNaoto 61:e018207ae860 30 int main() {
DeguNaoto 57:3fbd487e055e 31 #ifdef IM920
DeguNaoto 51:cb430192b28b 32 initializeIM920();
DeguNaoto 57:3fbd487e055e 33 #else
DeguNaoto 57:3fbd487e055e 34 initializeSBDBT();
DeguNaoto 57:3fbd487e055e 35 #endif
DeguNaoto 0:bd4719e15f7e 36 initializeMotors();
DeguNaoto 0:bd4719e15f7e 37 initializeControllers();
DeguNaoto 48:64d005c70df2 38 initializeSwing();
DeguNaoto 59:9d66edf3e734 39 initializeRS485();
DeguNaoto 43:f9a75ecbe44e 40 #ifdef BLUE
DeguNaoto 34:f9ef622f4376 41 sita=PI/4.0,targ_sita=PI/4.0;
DeguNaoto 55:db1797ac6cb1 42 IndicatorBLUE = 1;
DeguNaoto 43:f9a75ecbe44e 43 #else
DeguNaoto 43:f9a75ecbe44e 44 sita=-PI/4.0,targ_sita=-PI/4.0;
DeguNaoto 55:db1797ac6cb1 45 IndicatorRED = 1;
DeguNaoto 43:f9a75ecbe44e 46 #endif
DeguNaoto 21:79b94cb922f0 47 Indicator4=1;
DeguNaoto 26:8e6c736b6791 48 Enable=1;
DeguNaoto 50:8ea4714316ce 49 // FILE *fp_r = fopen("/local/velocity.dat", "w");
DeguNaoto 62:02a44f5bc51e 50 wait(0.3);
unicore32 68:2b2b88ecdcce 51 sendData(7,255);
DeguNaoto 50:8ea4714316ce 52 // double time=0.0;
DeguNaoto 0:bd4719e15f7e 53 while(1) {
DeguNaoto 21:79b94cb922f0 54 if(autoflag){
DeguNaoto 57:3fbd487e055e 55 #ifdef IM920
DeguNaoto 52:d9e1629da852 56 autoIM920(); /*IM920 button*/
DeguNaoto 57:3fbd487e055e 57 #else
DeguNaoto 57:3fbd487e055e 58 autoPS3(); /*PS3 button*/
DeguNaoto 57:3fbd487e055e 59 #endif
DeguNaoto 43:f9a75ecbe44e 60 #ifdef BLUE
DeguNaoto 60:4a75f3f3a934 61 //Blue
DeguNaoto 60:4a75f3f3a934 62 // if((step==0)&&((8650.0>x)&&(x>1400.0))) targ_sita=0.0,step=1;
DeguNaoto 64:a98fe602c26d 63 // if((step==0)&&((8650.0>x)&&(x>1400.0))) targ_sita=0.015,step=1; /*nomal*/
DeguNaoto 66:14df82661dfa 64 if((step==0)&&((8650.0>x)&&(x>900.0))) targ_sita=0.015,step=1; /*nomal*/
DeguNaoto 64:a98fe602c26d 65 if((step==1)&&(x>8650.0)) targ_velocity=-speed,step=2,flagf=0;
DeguNaoto 66:14df82661dfa 66 if((step==2)&&(x<1950.0)) {
DeguNaoto 66:14df82661dfa 67 targ_sita=PI/4;
DeguNaoto 66:14df82661dfa 68 step=3;
unicore32 68:2b2b88ecdcce 69 sendData(7,255);
DeguNaoto 66:14df82661dfa 70 }
DeguNaoto 66:14df82661dfa 71 if((step==3)&&(x<70.0)) targ_velocity=0.0,step=8;
DeguNaoto 35:2db63dec2a67 72
DeguNaoto 66:14df82661dfa 73 if((step==4)&&((5700.0>x)&&(x>900.0))) targ_sita=0.015,step=5; /*middle*/
DeguNaoto 66:14df82661dfa 74 if((step==5)&&(x>5000.0)) { /*middle shoot*/
DeguNaoto 61:e018207ae860 75 targ_velocity=-speed;
DeguNaoto 61:e018207ae860 76 step=6;
DeguNaoto 64:a98fe602c26d 77 sendData(1,5);
DeguNaoto 64:a98fe602c26d 78 flagf=0;
DeguNaoto 61:e018207ae860 79 }
DeguNaoto 66:14df82661dfa 80 if((step==6)&&(x<1750.0)) {
DeguNaoto 66:14df82661dfa 81 targ_sita=PI/4;
DeguNaoto 66:14df82661dfa 82 step=7;
unicore32 68:2b2b88ecdcce 83 sendData(7,255);
DeguNaoto 66:14df82661dfa 84 }
DeguNaoto 66:14df82661dfa 85 if((step==7)&&(x<70.0)) targ_velocity=0.0,step=8;
DeguNaoto 61:e018207ae860 86
DeguNaoto 66:14df82661dfa 87 if((x>2900.0)&&(CStep==1)) CStep=2,sendData(1,1); /*nomal*/
DeguNaoto 66:14df82661dfa 88 if((x>5850.0)&&(CStep==2)) CStep=3,sendData(1,2);
DeguNaoto 66:14df82661dfa 89 if((x>7600.0)&&(CStep==3)) CStep=4,sendData(1,3);
DeguNaoto 66:14df82661dfa 90 if((x<6500.0)&&(CStep==4)) CStep=5,sendData(1,5);
DeguNaoto 64:a98fe602c26d 91 if((x<6100.0)&&(CStep==5)) CStep=6,sendData(1,4);
DeguNaoto 61:e018207ae860 92
DeguNaoto 64:a98fe602c26d 93 /*if((x>3344.0)&&(CStep==1)) CStep=2,sendData(1,1);
DeguNaoto 48:64d005c70df2 94 if((x>6234.0)&&(CStep==2)) CStep=3,sendData(1,2);
DeguNaoto 36:fd04196b4b34 95 if((x>7885.0)&&(CStep==3)) CStep=4,sendData(1,3);
DeguNaoto 61:e018207ae860 96 if((x<6750.0)&&(CStep==4)) CStep=5,sendData(1,5);
DeguNaoto 64:a98fe602c26d 97 if((x<6300.0)&&(CStep==5)) CStep=6,sendData(1,4);*/
DeguNaoto 61:e018207ae860 98
DeguNaoto 43:f9a75ecbe44e 99 #else
DeguNaoto 43:f9a75ecbe44e 100 //Red
DeguNaoto 62:02a44f5bc51e 101 if((step==0)&&((8650.0>x)&&(x>1400.0))) targ_sita=-0.015,step=1;
DeguNaoto 43:f9a75ecbe44e 102 if((step==1)&&(x>8650.0)) targ_velocity=-speed,step=2;
DeguNaoto 43:f9a75ecbe44e 103 if((step==2)&&(x<2000.0)) targ_sita=-PI/4,step=3;
DeguNaoto 62:02a44f5bc51e 104 if((step==3)&&(x<10.0)) targ_velocity=0.0,step=8;
DeguNaoto 62:02a44f5bc51e 105
DeguNaoto 62:02a44f5bc51e 106 if((step==4)&&((5700.0>x)&&(x>950.0))) targ_sita=-0.015,step=5; /*middle*/
DeguNaoto 62:02a44f5bc51e 107 if((step==5)&&(x>5300.0)) { /*middle shoot*/
DeguNaoto 62:02a44f5bc51e 108 targ_velocity=-speed;
DeguNaoto 62:02a44f5bc51e 109 step=6;
DeguNaoto 62:02a44f5bc51e 110 sendData(1,4);
DeguNaoto 62:02a44f5bc51e 111 }
DeguNaoto 62:02a44f5bc51e 112 if((step==6)&&(x<2000.0)) targ_sita=-PI/4,step=7;
DeguNaoto 62:02a44f5bc51e 113 if((step==7)&&(x<10.0)) targ_velocity=0.0,step=8;
DeguNaoto 43:f9a75ecbe44e 114
DeguNaoto 48:64d005c70df2 115 if((x>3344.0)&&(CStep==1)) CStep=2,sendData(1,1);
DeguNaoto 48:64d005c70df2 116 if((x>6234.0)&&(CStep==2)) CStep=3,sendData(1,3);
DeguNaoto 48:64d005c70df2 117 if((x>7885.0)&&(CStep==3)) CStep=4,sendData(1,2);
DeguNaoto 52:d9e1629da852 118 if((x<6700.0)&&(CStep==4)) CStep=5,sendData(1,4);
DeguNaoto 52:d9e1629da852 119 if((x<6300.0)&&(CStep==5)) CStep=6,sendData(1,5);
DeguNaoto 66:14df82661dfa 120 #endif
DeguNaoto 21:79b94cb922f0 121 move_following();
DeguNaoto 4:51d87d2b698c 122 }
DeguNaoto 21:79b94cb922f0 123 else if(!autoflag) {
DeguNaoto 57:3fbd487e055e 124 #ifdef IM920
DeguNaoto 52:d9e1629da852 125 manualMoveIM920(); /*analogStick*/
DeguNaoto 52:d9e1629da852 126 manualIM920(); /*IM920 button*/
DeguNaoto 57:3fbd487e055e 127 #else
DeguNaoto 57:3fbd487e055e 128 manualMovePS3(); /*analogStick*/
DeguNaoto 57:3fbd487e055e 129 manualPS3(); /*PS3 button*/
DeguNaoto 57:3fbd487e055e 130 #endif
DeguNaoto 48:64d005c70df2 131 //Swing
DeguNaoto 66:14df82661dfa 132 swingFollowing();
DeguNaoto 50:8ea4714316ce 133 /*if(square){
DeguNaoto 50:8ea4714316ce 134 IndicatorAuto=0;
DeguNaoto 50:8ea4714316ce 135 fclose(fp_r);
DeguNaoto 50:8ea4714316ce 136 }
DeguNaoto 50:8ea4714316ce 137 fprintf(fp_r, "time:%1.3f, %f[rad/s], pwm:%f\r\n",time,swingRadVelocity,cont);
DeguNaoto 50:8ea4714316ce 138 time+=0.01;*/
DeguNaoto 4:51d87d2b698c 139 }
DeguNaoto 55:db1797ac6cb1 140 /***update state***/
DeguNaoto 57:3fbd487e055e 141 #ifdef IM920
DeguNaoto 51:cb430192b28b 142 readIM920();
DeguNaoto 57:3fbd487e055e 143 #endif
DeguNaoto 21:79b94cb922f0 144 mesure_state();
DeguNaoto 48:64d005c70df2 145 mesureSwing();
DeguNaoto 0:bd4719e15f7e 146 wait(RATE);
DeguNaoto 55:db1797ac6cb1 147
DeguNaoto 56:ac01d6b46291 148 // pc.printf("sita:%f, x:%f, y:%f ,x1:%f, x2:%f ,velocity:%f\r\n",sita,x,y,x1,x2,velocity);
DeguNaoto 52:d9e1629da852 149 // pc.printf("A2 = %d, X = %d, Y = %d, B = %d, dead = %d\r\n", a2, X, Y, b, deadflag);
DeguNaoto 57:3fbd487e055e 150 // pc.printf("%f %f\r\n",cont,swingRadVelocity);
DeguNaoto 0:bd4719e15f7e 151 }
DeguNaoto 0:bd4719e15f7e 152 }