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@78:abb760e0a935, 2015-10-10 (annotated)

Committer:
unicore32
Date:
Sat Oct 10 09:53:52 2015 +0000
Revision:
78:abb760e0a935
Parent:
68:2b2b88ecdcce
Parent:
77:7dd154b5ace3
Child:
79:7f86e18f40ef
????????

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 76:f9bcb7ac438f 8 //射出をスキップする機能を追加
DeguNaoto 71:4ebc1c0fcb4e 9 //スタートゾーンにマシンを戻した後に暴走しないようにする
DeguNaoto 73:86b7b82ba997 10 //手動時の操作性の改善
DeguNaoto 73:86b7b82ba997 11 //一度手動モードになったらスタートゾーンに戻るまで再び自動モードにならないようにする
DeguNaoto 34:f9ef622f4376 12
DeguNaoto 57:3fbd487e055e 13 /***コース選択***/
DeguNaoto 71:4ebc1c0fcb4e 14 //#define BLUE
DeguNaoto 71:4ebc1c0fcb4e 15 #define RED
DeguNaoto 43:f9a75ecbe44e 16
DeguNaoto 57:3fbd487e055e 17 /***コントローラ選択***/
DeguNaoto 57:3fbd487e055e 18 //#define IM920
DeguNaoto 57:3fbd487e055e 19 #define PS3
DeguNaoto 57:3fbd487e055e 20
DeguNaoto 57:3fbd487e055e 21 #if defined(IM920) && defined(PS3)
DeguNaoto 57:3fbd487e055e 22 #error Caution, You should define either IM920 or PS3
DeguNaoto 57:3fbd487e055e 23 #endif
DeguNaoto 57:3fbd487e055e 24
DeguNaoto 45:e11ec4f6d37e 25 #if defined(BLUE) && defined(RED)
DeguNaoto 45:e11ec4f6d37e 26 #error Caution, You should define either BLUE or RED
DeguNaoto 45:e11ec4f6d37e 27 #endif
DeguNaoto 45:e11ec4f6d37e 28
DeguNaoto 0:bd4719e15f7e 29 #include "machine_ps3.h"
DeguNaoto 0:bd4719e15f7e 30
DeguNaoto 0:bd4719e15f7e 31 Serial pc(USBTX, USBRX);
DeguNaoto 50:8ea4714316ce 32 //LocalFileSystem local("local");
DeguNaoto 77:7dd154b5ace3 33 Timeout MStop;
DeguNaoto 77:7dd154b5ace3 34
DeguNaoto 77:7dd154b5ace3 35 void Restart(){
DeguNaoto 77:7dd154b5ace3 36 if(step==10) step=11;
DeguNaoto 77:7dd154b5ace3 37 }
DeguNaoto 0:bd4719e15f7e 38
DeguNaoto 61:e018207ae860 39 int main() {
DeguNaoto 57:3fbd487e055e 40 #ifdef IM920
DeguNaoto 51:cb430192b28b 41 initializeIM920();
DeguNaoto 57:3fbd487e055e 42 #else
DeguNaoto 57:3fbd487e055e 43 initializeSBDBT();
DeguNaoto 57:3fbd487e055e 44 #endif
DeguNaoto 0:bd4719e15f7e 45 initializeMotors();
DeguNaoto 0:bd4719e15f7e 46 initializeControllers();
DeguNaoto 48:64d005c70df2 47 initializeSwing();
DeguNaoto 59:9d66edf3e734 48 initializeRS485();
DeguNaoto 43:f9a75ecbe44e 49 #ifdef BLUE
DeguNaoto 34:f9ef622f4376 50 sita=PI/4.0,targ_sita=PI/4.0;
DeguNaoto 55:db1797ac6cb1 51 IndicatorBLUE = 1;
DeguNaoto 43:f9a75ecbe44e 52 #else
DeguNaoto 43:f9a75ecbe44e 53 sita=-PI/4.0,targ_sita=-PI/4.0;
DeguNaoto 55:db1797ac6cb1 54 IndicatorRED = 1;
DeguNaoto 43:f9a75ecbe44e 55 #endif
DeguNaoto 21:79b94cb922f0 56 Indicator4=1;
DeguNaoto 26:8e6c736b6791 57 Enable=1;
DeguNaoto 50:8ea4714316ce 58 // FILE *fp_r = fopen("/local/velocity.dat", "w");
DeguNaoto 62:02a44f5bc51e 59 wait(0.3);
unicore32 68:2b2b88ecdcce 60 sendData(7,255);
DeguNaoto 50:8ea4714316ce 61 // double time=0.0;
DeguNaoto 0:bd4719e15f7e 62 while(1) {
DeguNaoto 21:79b94cb922f0 63 if(autoflag){
DeguNaoto 57:3fbd487e055e 64 #ifdef IM920
DeguNaoto 52:d9e1629da852 65 autoIM920(); /*IM920 button*/
DeguNaoto 57:3fbd487e055e 66 #else
DeguNaoto 57:3fbd487e055e 67 autoPS3(); /*PS3 button*/
DeguNaoto 57:3fbd487e055e 68 #endif
DeguNaoto 43:f9a75ecbe44e 69 #ifdef BLUE
DeguNaoto 60:4a75f3f3a934 70 //Blue
DeguNaoto 76:f9bcb7ac438f 71 /********************************Nomal Mode*********************************/
DeguNaoto 70:5b8ed76a31c7 72 if((step==0)&&((8650.0>x)&&(x>900.0))) targ_sita=0.03,step=1; /*nomal*/
DeguNaoto 64:a98fe602c26d 73 if((step==1)&&(x>8650.0)) targ_velocity=-speed,step=2,flagf=0;
DeguNaoto 70:5b8ed76a31c7 74 if((step==2)&&(x<1700.0)) {
DeguNaoto 66:14df82661dfa 75 targ_sita=PI/4;
DeguNaoto 66:14df82661dfa 76 step=3;
unicore32 68:2b2b88ecdcce 77 sendData(7,255);
DeguNaoto 66:14df82661dfa 78 }
DeguNaoto 73:86b7b82ba997 79 if((step==3)&&(x<-100.0)) {
DeguNaoto 73:86b7b82ba997 80 targ_velocity=0.0;
DeguNaoto 77:7dd154b5ace3 81 step=114;-
DeguNaoto 73:86b7b82ba997 82 autoflag = 0;
DeguNaoto 70:5b8ed76a31c7 83 }
DeguNaoto 76:f9bcb7ac438f 84 /***Cylinder***/
DeguNaoto 72:021a14ee970f 85 if((x>2900.0)&&(CStep==1)) { /*nomal*/
DeguNaoto 72:021a14ee970f 86 if(!skip) sendData(1,1);
DeguNaoto 77:7dd154b5ace3 87 CStep=2;
DeguNaoto 72:021a14ee970f 88 }
DeguNaoto 72:021a14ee970f 89 if((x>5850.0)&&(CStep==2)) {
DeguNaoto 72:021a14ee970f 90 if(!skip) sendData(1,2);
DeguNaoto 72:021a14ee970f 91 CStep=3;
DeguNaoto 72:021a14ee970f 92 }
DeguNaoto 72:021a14ee970f 93 if((x>7600.0)&&(CStep==3)) {
DeguNaoto 72:021a14ee970f 94 if(!skip) sendData(1,3);
DeguNaoto 72:021a14ee970f 95 CStep=4;
DeguNaoto 72:021a14ee970f 96 }
DeguNaoto 72:021a14ee970f 97 if((x<6600.0)&&(CStep==4)) {
DeguNaoto 72:021a14ee970f 98 if(!skip) sendData(1,5);
DeguNaoto 72:021a14ee970f 99 CStep=5;
DeguNaoto 72:021a14ee970f 100 }
DeguNaoto 72:021a14ee970f 101 if((x<6100.0)&&(CStep==5)) {
DeguNaoto 72:021a14ee970f 102 if(!skip) sendData(1,4);
DeguNaoto 72:021a14ee970f 103 CStep=6;
DeguNaoto 72:021a14ee970f 104 }
DeguNaoto 76:f9bcb7ac438f 105 /////////////////////////////////////////////////////////////////////////
DeguNaoto 76:f9bcb7ac438f 106 /******************************Middle Mode******************************/
DeguNaoto 77:7dd154b5ace3 107 if((step==4)&&((5700.0>x)&&(x>600.0))) {/*middle*/
DeguNaoto 77:7dd154b5ace3 108 targ_sita=0.03;
DeguNaoto 77:7dd154b5ace3 109 step=5;
DeguNaoto 77:7dd154b5ace3 110 }
DeguNaoto 76:f9bcb7ac438f 111 if((step==5)&&(x>5000.0)) {
DeguNaoto 76:f9bcb7ac438f 112 targ_velocity=-speed;
DeguNaoto 76:f9bcb7ac438f 113 step=6;
DeguNaoto 76:f9bcb7ac438f 114 sendData(1,5);
DeguNaoto 76:f9bcb7ac438f 115 flagf=0;
DeguNaoto 76:f9bcb7ac438f 116 }
DeguNaoto 76:f9bcb7ac438f 117 if((step==6)&&(x<1600.0)) {
DeguNaoto 76:f9bcb7ac438f 118 targ_sita=PI/4;
DeguNaoto 76:f9bcb7ac438f 119 step=7;
unicore32 68:2b2b88ecdcce 120 sendData(7,255);
DeguNaoto 76:f9bcb7ac438f 121 }
DeguNaoto 76:f9bcb7ac438f 122 if((step==7)&&(x<-100.0)) {
DeguNaoto 76:f9bcb7ac438f 123 targ_velocity=0.0;
DeguNaoto 76:f9bcb7ac438f 124 step=114;
DeguNaoto 76:f9bcb7ac438f 125 autoflag = 0;
DeguNaoto 76:f9bcb7ac438f 126 }
DeguNaoto 76:f9bcb7ac438f 127 /////////////////////////////////////////////////////////////////////////
DeguNaoto 77:7dd154b5ace3 128 /*****************************Opponents Mode****************************/
DeguNaoto 77:7dd154b5ace3 129 if((step==8)&&(x>1500.0)) {
DeguNaoto 77:7dd154b5ace3 130 targ_sita=-PI/2.0;
DeguNaoto 77:7dd154b5ace3 131 step=9;
DeguNaoto 77:7dd154b5ace3 132 }
DeguNaoto 77:7dd154b5ace3 133 if((step==9)&&(y<-1000.0)) {
DeguNaoto 77:7dd154b5ace3 134 targ_velocity = 0.0;
DeguNaoto 77:7dd154b5ace3 135 targ_sita = 4.0 * ( PI / 9.0 );
DeguNaoto 77:7dd154b5ace3 136 step=10;
DeguNaoto 77:7dd154b5ace3 137 }
DeguNaoto 77:7dd154b5ace3 138 if((step==10)&&(abs(sita-targ_sita)<0.05)) {
DeguNaoto 77:7dd154b5ace3 139 sendData(1,4);
DeguNaoto 77:7dd154b5ace3 140 MStop.attach(Restart,0.5);
DeguNaoto 77:7dd154b5ace3 141 }
DeguNaoto 77:7dd154b5ace3 142 if(step==11){
DeguNaoto 77:7dd154b5ace3 143 targ_sita = 0.0;
DeguNaoto 77:7dd154b5ace3 144 step = 12;
DeguNaoto 77:7dd154b5ace3 145 }
DeguNaoto 77:7dd154b5ace3 146 if((step==12)&&(abs(sita-targ_sita)<0.05)){
DeguNaoto 77:7dd154b5ace3 147 targ_velocity = -speed;
DeguNaoto 77:7dd154b5ace3 148 flagf = 0;
DeguNaoto 77:7dd154b5ace3 149 step = 13;
DeguNaoto 77:7dd154b5ace3 150 }
DeguNaoto 77:7dd154b5ace3 151 if((step==13)&&(y>0.0)){
DeguNaoto 77:7dd154b5ace3 152 targ_sita = 0.0;
DeguNaoto 77:7dd154b5ace3 153 step = 14;
DeguNaoto 77:7dd154b5ace3 154 }
DeguNaoto 77:7dd154b5ace3 155 if((step==14)&&(x<-100.0)){
DeguNaoto 77:7dd154b5ace3 156 targ_velocity = 0.0;
DeguNaoto 77:7dd154b5ace3 157 targ_sita = PI/4.0;
DeguNaoto 77:7dd154b5ace3 158 step = 114;
DeguNaoto 77:7dd154b5ace3 159 }
DeguNaoto 77:7dd154b5ace3 160 /////////////////////////////////////////////////////////////////////////
DeguNaoto 71:4ebc1c0fcb4e 161 #else
DeguNaoto 43:f9a75ecbe44e 162 //Red
DeguNaoto 76:f9bcb7ac438f 163 /********************************Nomal Mode*********************************/
DeguNaoto 76:f9bcb7ac438f 164 if((step==0)&&((8650.0>x)&&(x>900.0))) targ_sita=-0.03,step=1; /*nomal*/
DeguNaoto 71:4ebc1c0fcb4e 165 if((step==1)&&(x>8650.0)) targ_velocity=-speed,step=2,flagf=0;
DeguNaoto 71:4ebc1c0fcb4e 166 if((step==2)&&(x<1700.0)) {
DeguNaoto 71:4ebc1c0fcb4e 167 targ_sita=-PI/4;
DeguNaoto 71:4ebc1c0fcb4e 168 step=3;
DeguNaoto 71:4ebc1c0fcb4e 169 sendData(7,31);
DeguNaoto 71:4ebc1c0fcb4e 170 }
DeguNaoto 73:86b7b82ba997 171 if((step==3)&&(x<-100.0)) {
DeguNaoto 73:86b7b82ba997 172 targ_velocity=0.0;
DeguNaoto 73:86b7b82ba997 173 step=8;
DeguNaoto 73:86b7b82ba997 174 autoflag=0;
DeguNaoto 73:86b7b82ba997 175 }
DeguNaoto 76:f9bcb7ac438f 176 /***Cylinder***/
DeguNaoto 76:f9bcb7ac438f 177 if((x>2900.0)&&(CStep==1)) { /*nomal*/
DeguNaoto 75:4cdcd50ee666 178 if(!skip) sendData(1,1);
DeguNaoto 75:4cdcd50ee666 179 CStep=2;
DeguNaoto 75:4cdcd50ee666 180 }
DeguNaoto 75:4cdcd50ee666 181 if((x>5850.0)&&(CStep==2)) {
DeguNaoto 75:4cdcd50ee666 182 if(!skip) sendData(1,3);
DeguNaoto 75:4cdcd50ee666 183 CStep=3;
DeguNaoto 75:4cdcd50ee666 184 }
DeguNaoto 75:4cdcd50ee666 185 if((x>7600.0)&&(CStep==3)) {
DeguNaoto 75:4cdcd50ee666 186 if(!skip) sendData(1,2);
DeguNaoto 75:4cdcd50ee666 187 CStep=6;
DeguNaoto 75:4cdcd50ee666 188 }
DeguNaoto 75:4cdcd50ee666 189 if((x<6600.0)&&(CStep==4)) {
DeguNaoto 75:4cdcd50ee666 190 if(!skip) sendData(1,4);
DeguNaoto 75:4cdcd50ee666 191 CStep=5;
DeguNaoto 75:4cdcd50ee666 192 }
DeguNaoto 75:4cdcd50ee666 193 if((x<6100.0)&&(CStep==5)) {
DeguNaoto 75:4cdcd50ee666 194 if(!skip) sendData(1,5);
DeguNaoto 75:4cdcd50ee666 195 CStep=6;
DeguNaoto 75:4cdcd50ee666 196 }
DeguNaoto 76:f9bcb7ac438f 197 /////////////////////////////////////////////////////////////////////////
DeguNaoto 76:f9bcb7ac438f 198 /******************************Middle Mode******************************/
DeguNaoto 77:7dd154b5ace3 199 if((step==4)&&((5700.0>x)&&(x>600.0))) { /*middle*/
DeguNaoto 77:7dd154b5ace3 200 targ_sita=-0.03;
DeguNaoto 77:7dd154b5ace3 201 step=5;
DeguNaoto 77:7dd154b5ace3 202 medge=1;
DeguNaoto 77:7dd154b5ace3 203 }
DeguNaoto 76:f9bcb7ac438f 204 if((step==5)&&(x>5000.0)) {
DeguNaoto 76:f9bcb7ac438f 205 targ_velocity=-speed;
DeguNaoto 76:f9bcb7ac438f 206 step=6;
DeguNaoto 76:f9bcb7ac438f 207 sendData(1,4);
DeguNaoto 76:f9bcb7ac438f 208 flagf=0;
DeguNaoto 76:f9bcb7ac438f 209 }
DeguNaoto 76:f9bcb7ac438f 210 if((step==6)&&(x<2000.0)) {
DeguNaoto 76:f9bcb7ac438f 211 targ_sita=-PI/4;
DeguNaoto 76:f9bcb7ac438f 212 step=7;
unicore32 78:abb760e0a935 213 sendData(7,255);
DeguNaoto 76:f9bcb7ac438f 214 }
DeguNaoto 76:f9bcb7ac438f 215 if((step==7)&&(x<-100.0)) {
DeguNaoto 76:f9bcb7ac438f 216 targ_velocity=0.0;
DeguNaoto 76:f9bcb7ac438f 217 step=8;
DeguNaoto 76:f9bcb7ac438f 218 autoflag=0;
DeguNaoto 76:f9bcb7ac438f 219 }
DeguNaoto 76:f9bcb7ac438f 220 /////////////////////////////////////////////////////////////////////////
DeguNaoto 66:14df82661dfa 221 #endif
DeguNaoto 73:86b7b82ba997 222 mesure_state(); /*位置測定*/
DeguNaoto 76:f9bcb7ac438f 223 move_following(); /*移動制御*/
DeguNaoto 4:51d87d2b698c 224 }
DeguNaoto 21:79b94cb922f0 225 else if(!autoflag) {
DeguNaoto 73:86b7b82ba997 226 resetState();
DeguNaoto 57:3fbd487e055e 227 #ifdef IM920
DeguNaoto 52:d9e1629da852 228 manualMoveIM920(); /*analogStick*/
DeguNaoto 52:d9e1629da852 229 manualIM920(); /*IM920 button*/
DeguNaoto 57:3fbd487e055e 230 #else
DeguNaoto 57:3fbd487e055e 231 manualMovePS3(); /*analogStick*/
DeguNaoto 57:3fbd487e055e 232 manualPS3(); /*PS3 button*/
DeguNaoto 57:3fbd487e055e 233 #endif
DeguNaoto 48:64d005c70df2 234 //Swing
DeguNaoto 66:14df82661dfa 235 swingFollowing();
DeguNaoto 50:8ea4714316ce 236 /*if(square){
DeguNaoto 50:8ea4714316ce 237 IndicatorAuto=0;
DeguNaoto 50:8ea4714316ce 238 fclose(fp_r);
DeguNaoto 50:8ea4714316ce 239 }
DeguNaoto 50:8ea4714316ce 240 fprintf(fp_r, "time:%1.3f, %f[rad/s], pwm:%f\r\n",time,swingRadVelocity,cont);
DeguNaoto 50:8ea4714316ce 241 time+=0.01;*/
DeguNaoto 4:51d87d2b698c 242 }
DeguNaoto 55:db1797ac6cb1 243 /***update state***/
DeguNaoto 57:3fbd487e055e 244 #ifdef IM920
DeguNaoto 51:cb430192b28b 245 readIM920();
DeguNaoto 57:3fbd487e055e 246 #endif
DeguNaoto 48:64d005c70df2 247 mesureSwing();
DeguNaoto 0:bd4719e15f7e 248 wait(RATE);
DeguNaoto 55:db1797ac6cb1 249
DeguNaoto 56:ac01d6b46291 250 // 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 251 // pc.printf("A2 = %d, X = %d, Y = %d, B = %d, dead = %d\r\n", a2, X, Y, b, deadflag);
DeguNaoto 57:3fbd487e055e 252 // pc.printf("%f %f\r\n",cont,swingRadVelocity);
DeguNaoto 0:bd4719e15f7e 253 }
DeguNaoto 0:bd4719e15f7e 254 }