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@81:fee3a0a05d51, 2015-10-11 (annotated)

Committer:
DeguNaoto
Date:
Sun Oct 11 07:44:05 2015 +0000
Revision:
81:fee3a0a05d51
Parent:
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 79:7f86e18f40ef 14 #define BLUE
DeguNaoto 79:7f86e18f40ef 15 //#define RED
DeguNaoto 43:f9a75ecbe44e 16
DeguNaoto 57:3fbd487e055e 17 /***コントローラ選択***/
DeguNaoto 81:fee3a0a05d51 18 //#define IM920
DeguNaoto 81:fee3a0a05d51 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 79:7f86e18f40ef 73 if((step==1)&&(x>8650.0)) {
DeguNaoto 79:7f86e18f40ef 74 step=2;
DeguNaoto 79:7f86e18f40ef 75 flagf=0;
DeguNaoto 79:7f86e18f40ef 76 targ_sita=0.05;
DeguNaoto 79:7f86e18f40ef 77 }
DeguNaoto 79:7f86e18f40ef 78 if((step==2)&&(x<1950.0)) {
DeguNaoto 66:14df82661dfa 79 targ_sita=PI/4;
DeguNaoto 66:14df82661dfa 80 step=3;
unicore32 68:2b2b88ecdcce 81 sendData(7,255);
DeguNaoto 66:14df82661dfa 82 }
DeguNaoto 73:86b7b82ba997 83 if((step==3)&&(x<-100.0)) {
DeguNaoto 73:86b7b82ba997 84 targ_velocity=0.0;
DeguNaoto 79:7f86e18f40ef 85 step=114;
DeguNaoto 70:5b8ed76a31c7 86 }
DeguNaoto 76:f9bcb7ac438f 87 /***Cylinder***/
DeguNaoto 72:021a14ee970f 88 if((x>2900.0)&&(CStep==1)) { /*nomal*/
DeguNaoto 72:021a14ee970f 89 if(!skip) sendData(1,1);
DeguNaoto 77:7dd154b5ace3 90 CStep=2;
DeguNaoto 72:021a14ee970f 91 }
DeguNaoto 72:021a14ee970f 92 if((x>5850.0)&&(CStep==2)) {
DeguNaoto 72:021a14ee970f 93 if(!skip) sendData(1,2);
DeguNaoto 72:021a14ee970f 94 CStep=3;
DeguNaoto 72:021a14ee970f 95 }
DeguNaoto 72:021a14ee970f 96 if((x>7600.0)&&(CStep==3)) {
DeguNaoto 72:021a14ee970f 97 if(!skip) sendData(1,3);
DeguNaoto 72:021a14ee970f 98 CStep=4;
DeguNaoto 72:021a14ee970f 99 }
DeguNaoto 72:021a14ee970f 100 if((x<6600.0)&&(CStep==4)) {
DeguNaoto 72:021a14ee970f 101 if(!skip) sendData(1,5);
DeguNaoto 72:021a14ee970f 102 CStep=5;
DeguNaoto 72:021a14ee970f 103 }
DeguNaoto 72:021a14ee970f 104 if((x<6100.0)&&(CStep==5)) {
DeguNaoto 72:021a14ee970f 105 if(!skip) sendData(1,4);
DeguNaoto 72:021a14ee970f 106 CStep=6;
DeguNaoto 72:021a14ee970f 107 }
DeguNaoto 76:f9bcb7ac438f 108 /////////////////////////////////////////////////////////////////////////
DeguNaoto 76:f9bcb7ac438f 109 /******************************Middle Mode******************************/
DeguNaoto 77:7dd154b5ace3 110 if((step==4)&&((5700.0>x)&&(x>600.0))) {/*middle*/
DeguNaoto 77:7dd154b5ace3 111 targ_sita=0.03;
DeguNaoto 77:7dd154b5ace3 112 step=5;
DeguNaoto 77:7dd154b5ace3 113 }
DeguNaoto 81:fee3a0a05d51 114 if((step==5)&&(x>4950.0)) {
DeguNaoto 76:f9bcb7ac438f 115 step=6;
DeguNaoto 76:f9bcb7ac438f 116 sendData(1,5);
DeguNaoto 76:f9bcb7ac438f 117 flagf=0;
DeguNaoto 79:7f86e18f40ef 118 targ_sita=0.05;
DeguNaoto 76:f9bcb7ac438f 119 }
DeguNaoto 76:f9bcb7ac438f 120 if((step==6)&&(x<1600.0)) {
DeguNaoto 76:f9bcb7ac438f 121 targ_sita=PI/4;
DeguNaoto 76:f9bcb7ac438f 122 step=7;
unicore32 68:2b2b88ecdcce 123 sendData(7,255);
DeguNaoto 76:f9bcb7ac438f 124 }
DeguNaoto 76:f9bcb7ac438f 125 if((step==7)&&(x<-100.0)) {
DeguNaoto 76:f9bcb7ac438f 126 targ_velocity=0.0;
DeguNaoto 76:f9bcb7ac438f 127 step=114;
DeguNaoto 76:f9bcb7ac438f 128 }
DeguNaoto 76:f9bcb7ac438f 129 /////////////////////////////////////////////////////////////////////////
DeguNaoto 77:7dd154b5ace3 130 /*****************************Opponents Mode****************************/
DeguNaoto 79:7f86e18f40ef 131 if((step==8)&&(y<-1700.0)) {
DeguNaoto 79:7f86e18f40ef 132 targ_velocity=0.0;
DeguNaoto 79:7f86e18f40ef 133 targ_sita=0.0;
DeguNaoto 79:7f86e18f40ef 134 step=114;
DeguNaoto 77:7dd154b5ace3 135 }
DeguNaoto 77:7dd154b5ace3 136 /////////////////////////////////////////////////////////////////////////
DeguNaoto 71:4ebc1c0fcb4e 137 #else
DeguNaoto 43:f9a75ecbe44e 138 //Red
DeguNaoto 76:f9bcb7ac438f 139 /********************************Nomal Mode*********************************/
DeguNaoto 79:7f86e18f40ef 140 if((step==0)&&((8650.0>x)&&(x>800.0))) targ_sita=-0.03,step=1; /*nomal*/
DeguNaoto 79:7f86e18f40ef 141 if((step==1)&&(x>8650.0)) {
DeguNaoto 79:7f86e18f40ef 142 step=2;
DeguNaoto 79:7f86e18f40ef 143 flagf=0;
DeguNaoto 79:7f86e18f40ef 144 targ_sita=-0.05;
DeguNaoto 79:7f86e18f40ef 145 }
DeguNaoto 79:7f86e18f40ef 146 if((step==2)&&(x<1900.0)) {
DeguNaoto 71:4ebc1c0fcb4e 147 targ_sita=-PI/4;
DeguNaoto 71:4ebc1c0fcb4e 148 step=3;
DeguNaoto 71:4ebc1c0fcb4e 149 sendData(7,31);
DeguNaoto 71:4ebc1c0fcb4e 150 }
DeguNaoto 73:86b7b82ba997 151 if((step==3)&&(x<-100.0)) {
DeguNaoto 73:86b7b82ba997 152 targ_velocity=0.0;
DeguNaoto 73:86b7b82ba997 153 step=8;
DeguNaoto 73:86b7b82ba997 154 }
DeguNaoto 76:f9bcb7ac438f 155 /***Cylinder***/
DeguNaoto 76:f9bcb7ac438f 156 if((x>2900.0)&&(CStep==1)) { /*nomal*/
DeguNaoto 75:4cdcd50ee666 157 if(!skip) sendData(1,1);
DeguNaoto 75:4cdcd50ee666 158 CStep=2;
DeguNaoto 75:4cdcd50ee666 159 }
DeguNaoto 75:4cdcd50ee666 160 if((x>5850.0)&&(CStep==2)) {
DeguNaoto 75:4cdcd50ee666 161 if(!skip) sendData(1,3);
DeguNaoto 75:4cdcd50ee666 162 CStep=3;
DeguNaoto 75:4cdcd50ee666 163 }
DeguNaoto 75:4cdcd50ee666 164 if((x>7600.0)&&(CStep==3)) {
DeguNaoto 75:4cdcd50ee666 165 if(!skip) sendData(1,2);
DeguNaoto 75:4cdcd50ee666 166 CStep=6;
DeguNaoto 75:4cdcd50ee666 167 }
DeguNaoto 75:4cdcd50ee666 168 if((x<6600.0)&&(CStep==4)) {
DeguNaoto 75:4cdcd50ee666 169 if(!skip) sendData(1,4);
DeguNaoto 75:4cdcd50ee666 170 CStep=5;
DeguNaoto 75:4cdcd50ee666 171 }
DeguNaoto 75:4cdcd50ee666 172 if((x<6100.0)&&(CStep==5)) {
DeguNaoto 75:4cdcd50ee666 173 if(!skip) sendData(1,5);
DeguNaoto 75:4cdcd50ee666 174 CStep=6;
DeguNaoto 75:4cdcd50ee666 175 }
DeguNaoto 76:f9bcb7ac438f 176 /////////////////////////////////////////////////////////////////////////
DeguNaoto 76:f9bcb7ac438f 177 /******************************Middle Mode******************************/
DeguNaoto 79:7f86e18f40ef 178 if((step==4)&&((5700.0>x)&&(x>800.0))) { /*middle*/
DeguNaoto 77:7dd154b5ace3 179 targ_sita=-0.03;
DeguNaoto 77:7dd154b5ace3 180 step=5;
DeguNaoto 77:7dd154b5ace3 181 medge=1;
DeguNaoto 77:7dd154b5ace3 182 }
DeguNaoto 76:f9bcb7ac438f 183 if((step==5)&&(x>5000.0)) {
DeguNaoto 76:f9bcb7ac438f 184 step=6;
DeguNaoto 76:f9bcb7ac438f 185 sendData(1,4);
DeguNaoto 76:f9bcb7ac438f 186 flagf=0;
DeguNaoto 79:7f86e18f40ef 187 targ_sita=-0.05;
DeguNaoto 76:f9bcb7ac438f 188 }
DeguNaoto 76:f9bcb7ac438f 189 if((step==6)&&(x<2000.0)) {
DeguNaoto 76:f9bcb7ac438f 190 targ_sita=-PI/4;
DeguNaoto 76:f9bcb7ac438f 191 step=7;
unicore32 78:abb760e0a935 192 sendData(7,255);
DeguNaoto 76:f9bcb7ac438f 193 }
DeguNaoto 76:f9bcb7ac438f 194 if((step==7)&&(x<-100.0)) {
DeguNaoto 76:f9bcb7ac438f 195 targ_velocity=0.0;
DeguNaoto 76:f9bcb7ac438f 196 step=8;
DeguNaoto 76:f9bcb7ac438f 197 autoflag=0;
DeguNaoto 76:f9bcb7ac438f 198 }
DeguNaoto 76:f9bcb7ac438f 199 /////////////////////////////////////////////////////////////////////////
DeguNaoto 66:14df82661dfa 200 #endif
DeguNaoto 73:86b7b82ba997 201 mesure_state(); /*位置測定*/
DeguNaoto 76:f9bcb7ac438f 202 move_following(); /*移動制御*/
DeguNaoto 4:51d87d2b698c 203 }
DeguNaoto 21:79b94cb922f0 204 else if(!autoflag) {
DeguNaoto 73:86b7b82ba997 205 resetState();
DeguNaoto 57:3fbd487e055e 206 #ifdef IM920
DeguNaoto 52:d9e1629da852 207 manualMoveIM920(); /*analogStick*/
DeguNaoto 52:d9e1629da852 208 manualIM920(); /*IM920 button*/
DeguNaoto 57:3fbd487e055e 209 #else
DeguNaoto 57:3fbd487e055e 210 manualMovePS3(); /*analogStick*/
DeguNaoto 57:3fbd487e055e 211 manualPS3(); /*PS3 button*/
DeguNaoto 57:3fbd487e055e 212 #endif
DeguNaoto 48:64d005c70df2 213 //Swing
DeguNaoto 66:14df82661dfa 214 swingFollowing();
DeguNaoto 50:8ea4714316ce 215 /*if(square){
DeguNaoto 50:8ea4714316ce 216 IndicatorAuto=0;
DeguNaoto 50:8ea4714316ce 217 fclose(fp_r);
DeguNaoto 50:8ea4714316ce 218 }
DeguNaoto 50:8ea4714316ce 219 fprintf(fp_r, "time:%1.3f, %f[rad/s], pwm:%f\r\n",time,swingRadVelocity,cont);
DeguNaoto 50:8ea4714316ce 220 time+=0.01;*/
DeguNaoto 4:51d87d2b698c 221 }
DeguNaoto 55:db1797ac6cb1 222 /***update state***/
DeguNaoto 57:3fbd487e055e 223 #ifdef IM920
DeguNaoto 51:cb430192b28b 224 readIM920();
DeguNaoto 57:3fbd487e055e 225 #endif
DeguNaoto 48:64d005c70df2 226 mesureSwing();
DeguNaoto 0:bd4719e15f7e 227 wait(RATE);
DeguNaoto 55:db1797ac6cb1 228
DeguNaoto 56:ac01d6b46291 229 // 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 230 // pc.printf("A2 = %d, X = %d, Y = %d, B = %d, dead = %d\r\n", a2, X, Y, b, deadflag);
DeguNaoto 57:3fbd487e055e 231 // pc.printf("%f %f\r\n",cont,swingRadVelocity);
DeguNaoto 0:bd4719e15f7e 232 }
DeguNaoto 0:bd4719e15f7e 233 }