2015_robocon_bteam
/
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.
Diff: main_ps3.cpp
- Revision:
- 0:b613dc16f27d
- Child:
- 1:3ac2087996f3
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main_ps3.cpp Wed Oct 28 09:03:19 2015 +0000
@@ -0,0 +1,386 @@
+/**
+ * This program is written in main micro computer "mbed" for 2015 NHK Robot Contest (Bteam).
+ */
+
+//回転機構wait PI/4 追加で待つ
+//射出をスキップする機能を追加
+//スタートゾーンにマシンを戻した後に暴走しないようにする
+//手動時の操作性の改善
+//一度手動モードになったらスタートゾーンに戻るまで再び自動モードにならないようにする
+//flagfで前進・後退切り替え
+//大和田射角
+//午後に一定回転制御の実験
+//モード切替時に時間を待つ
+//赤チーム側を青チーム側のプログラムに合わせて実装
+//横幅調整済み
+//相手妨害モードいらない new!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
+//赤チームモード、相手チーム側の射出位置調整
+
+/***コース選択***/
+//#define BLUE
+#define RED
+
+/***コントローラ選択***/
+#define IM920
+//#define PS3
+
+/***回転機構測定***/
+//#define MESURE
+
+#if defined(IM920) && defined(PS3)
+#error Caution, You should define either IM920 or PS3
+#endif
+
+#if defined(BLUE) && defined(RED)
+#error Caution, You should define either BLUE or RED
+#endif
+
+#include "machine_ps3.h"
+
+#define deff 70.0
+
+Serial pc(USBTX, USBRX);
+#ifdef MESURE
+LocalFileSystem local("local");
+#endif
+
+short cal=0;
+Ticker Com;
+void Call(){
+ mesure_state();
+ if(autoflag) cal=1;
+}
+
+int main() {
+ Com.attach(&Call,0.01);
+#ifdef IM920
+ initializeIM920();
+#else
+ initializeSBDBT();
+#endif
+ initializeMotors();
+ initializeControllers();
+ initializeRS485();
+#ifdef BLUE
+ sita=PI/4.0,targ_sita=PI/4.0;
+ IndicatorBLUE = 1;
+#else
+ sita=-PI/4.0,targ_sita=-PI/4.0;
+ IndicatorRED = 1;
+#endif
+ Indicator4=1;
+ Enable=1;
+#ifdef MESURE
+ FILE *fp_r = fopen("/local/velocity.dat", "w");
+ double time=0.0;
+#endif
+ wait(0.3);
+ sendData(7,0);
+ while(1) {
+ if(autoflag){
+#ifdef IM920
+ autoIM920(); /*IM920 button*/
+#else
+ autoPS3(); /*PS3 button*/
+#endif
+#ifdef BLUE
+ //Blue
+ if(spcount<speed){
+ spcount+=speed/100.0;
+ targ_velocity=spcount;
+ }
+ /********************************Nomal Mode*********************************/
+ if((step==0)&&((8650.0>x)&&(x>800.0))) {
+ targ_sita=0.025;
+ step=1;
+ }
+ if((step==1)&&(x>8600.0+deff)) {
+ targ_velocity=0.0;
+ step=2;
+ }
+ if((step==2)&&((velocity<500.0)&&(velocity>-500.0))){
+ step=3;
+ spcount=0.0;
+ flagf=0;
+ targ_sita=0.035;
+ }
+ if((step==3)&&(x<2000.0)) {
+ targ_sita=PI/4;
+ step=4;
+ }
+ if((step==4)&&(x<0.0)) {
+ targ_velocity=0.0;
+ step=114;
+ }
+ /***Cylinder***/
+ if((x>3030.0+deff)&&(CStep==1)) {
+ if(!skip) sendData(1,1);
+ CStep=2;
+ }
+ if((x>5900.0+deff)&&(CStep==2)) {
+ if(!skip) sendData(1,3);
+ CStep=3;
+ }
+ if((x>7680.0+deff)&&(CStep==3)) {
+ if(!skip) sendData(1,2);
+ CStep=4;
+ }
+ if((x<6580.0+deff)&&(CStep==4)) {
+ if(!skip) sendData(1,5);
+ CStep=5;
+ }
+ if((x<6100.0+deff)&&(CStep==5)) {
+ if(!skip) sendData(1,4);
+ CStep=6;
+ }
+ if((x<4000.0)&&(CStep==6)){
+ sendData(7,0);
+ CStep=114;
+ }
+ /////////////////////////////////////////////////////////////////////////
+ /******************************Middle Mode******************************/
+ if((step==5)&&((5700.0>x)&&(x>800.0))) {
+ targ_sita=0.025;
+ step=6;
+ }
+ if((step==6)&&(x>7000.0)){
+ targ_velocity=0.0;
+ step=7;
+ }
+ if((step==7)&&((velocity<500.0)&&(velocity>-500.0))){
+ flagf=0;
+ spcount=0.0;
+ targ_sita=0.035;
+ step=8;
+ }
+ if((step==8)&&(x<1900.0)) {
+ targ_sita=PI/4;
+ step=9;
+ }
+ if((step==9)&&(x<-10.0)) {
+ targ_velocity=0.0;
+ step=114;
+ }
+
+ if((x>4620.0+deff)&&(CStep==7)){
+ sendData(1,5);
+ CStep=8;
+ }
+ if((x>5720.0+deff)&&(CStep==8)){
+ sendData(1,4);
+ CStep=9;
+ }
+ if((x<4000.0)&&(CStep==9)){
+ sendData(7,0);
+ CStep=114;
+ }
+ /////////////////////////////////////////////////////////////////////////
+ /*****************************Opponents Mode****************************/
+ if((step==15)&&((5700.0>x)&&(x>50.0))) {
+ targ_sita=0.025;
+ step=16;
+ }
+ if((step==16)&&(x>2850.0+deff)){
+ targ_velocity=0.0;
+ step=17;
+ }
+ if((step==17)&&((velocity<5000.0)&&(velocity>-5000.0))){
+ flagf=0;
+ spcount=0.0;
+ targ_sita=0.035;
+ step=18;
+ }
+ if((step==18)&&(x<-100.0)){
+ targ_velocity=0.0;
+ targ_sita=PI/4.0;
+ step=114;
+ }
+
+// if((x>1200.0+deff)&&(CStep==15)){
+// if((x>1000.0+deff)&&(CStep==15)){
+ if((x>1050.0+deff)&&(CStep==15)){
+ sendData(1,5);
+ CStep=16;
+ }
+ if((x>2505.0+deff)&&(CStep==16)){
+// if((x>2455.0+deff)&&(CStep==16)){
+// if((x>2630.0)&&(CStep==16)){
+ sendData(1,4);
+ CStep=17;
+ }
+ if((x<2400.0+deff)&&(CStep==17)){
+ sendData(7,0);
+ CStep=114;
+ }
+#else
+ //Red
+ if(spcount<speed){
+ spcount+=speed/100.0;
+ targ_velocity=spcount;
+ }
+ /********************************Nomal Mode*********************************/
+ if((step==0)&&((8650.0>x)&&(x>800.0))) {
+ targ_sita=-0.025;
+ step=1;
+ }
+ if((step==1)&&(x>8600.0+deff)) {
+ targ_velocity=0.0;
+ step=2;
+ }
+ if((step==2)&&((velocity<500.0)&&(velocity>-500.0))){
+ step=3;
+ spcount=0.0;
+ flagf=0;
+ targ_sita=-0.035;
+ }
+ if((step==3)&&(x<2000.0)) {
+ targ_sita=-PI/4;
+ step=4;
+ }
+ if((step==4)&&(x<0.0)) {
+ targ_velocity=0.0;
+ step=114;
+ }
+ /***Cylinder***/
+// if((x>3030.0+deff)&&(CStep==1)) {
+ if((x>3100.0+deff)&&(CStep==1)) {
+ if(!skip) sendData(1,1);
+ CStep=2;
+ }
+// if((x>5940.0+deff)&&(CStep==2)) {
+ if((x>6000.0+deff)&&(CStep==2)) {
+ if(!skip) sendData(1,2);
+ CStep=3;
+ }
+ if((x>7680.0+deff)&&(CStep==3)) {
+ if(!skip) sendData(1,3);
+ CStep=4;
+ }
+ if((x<6730.0+deff)&&(CStep==4)) {
+ if(!skip) sendData(1,4);
+ CStep=5;
+ }
+ if((x<6170.0+deff)&&(CStep==5)) {
+ if(!skip) sendData(1,5);
+ CStep=6;
+ }
+ if((x<4000.0)&&(CStep==6)){
+ sendData(7,0);
+ CStep=114;
+ }
+ /////////////////////////////////////////////////////////////////////////
+ /******************************Middle Mode******************************/
+ if((step==5)&&((5700.0>x)&&(x>800.0))) {
+ targ_sita=-0.025;
+ step=6;
+ }
+ if((step==6)&&(x>7000.0)){
+ targ_velocity=0.0;
+ step=7;
+ }
+ if((step==7)&&((velocity<500.0)&&(velocity>-500.0))){
+ flagf=0;
+ spcount=0.0;
+ targ_sita=-0.035;
+ step=8;
+ }
+ if((step==8)&&(x<1820.0)) {
+ targ_sita=-PI/4;
+ step=9;
+ }
+ if((step==9)&&(x<-10.0)) {
+ targ_velocity=0.0;
+ step=114;
+ }
+
+ if((x>4690.0+deff)&&(CStep==7)){
+ sendData(1,4);
+ CStep=8;
+ }
+ if((x>5790.0+deff)&&(CStep==8)){
+ sendData(1,5);
+ CStep=9;
+ }
+ if((x<4000.0)&&(CStep==9)){
+ sendData(7,0);
+ CStep=114;
+ }
+ /////////////////////////////////////////////////////////////////////////
+ /*****************************Opponents Mode****************************/
+ if((step==15)&&((5700.0>x)&&(x>50.0))) {
+// targ_sita=-0.035;
+ targ_sita=-0.025;
+ step=16;
+ }
+// if((step==16)&&(x>2800.0+deff)){
+ if((step==16)&&(x>2850.0+deff)){
+ targ_velocity=0.0;
+ step=17;
+ }
+ if((step==17)&&((velocity<5000.0)&&(velocity>-5000.0))){
+ flagf=0;
+ spcount=0.0;
+ targ_sita=-0.035;
+ step=18;
+ }
+ if((step==18)&&(x<-100.0)){
+ targ_velocity=0.0;
+ targ_sita=-PI/4.0;
+ step=114;
+ }
+
+// if((x>1350.0+deff)&&(CStep==15)){
+ if((x>1400.0+deff)&&(CStep==15)){
+ sendData(1,4);
+ CStep=16;
+ }
+ if((x>2700.0+deff)&&(CStep==16)){
+// if((x>2650.0+deff)&&(CStep==16)){
+// if((x>2850.0+deff)&&(CStep==16)){
+ sendData(1,5);
+ CStep=17;
+ }
+ if((x<2400.0+deff)&&(CStep==17)){
+ sendData(7,0);
+ CStep=114;
+
+ }
+#endif
+// mesure_state(); /*位置測定*/
+// move_following(); /*移動制御*/
+ if(cal){
+ cal=0;
+ move_following();
+ }
+ }
+ else if(!autoflag) {
+ flaga=0;
+#ifdef IM920
+ manualMoveIM920(); /*analogStick*/
+ manualIM920(); /*IM920 button*/
+#else
+ manualMovePS3(); /*analogStick*/
+ manualPS3(); /*PS3 button*/
+#endif
+#ifdef MESURE
+ if(down){
+ IndicatorAuto=0;
+ fclose(fp_r);
+ }
+ time+=0.01;
+#endif
+ }
+ /***update state***/
+#ifdef IM920
+ readIM920();
+#endif
+// mesureSwing();
+ pc.printf("x:%f,y:%f\r\n",x,y);
+// wait(RATE);
+
+// pc.printf("%f\r\n",(float)((((2.0 * PI) / swingRadVelocity) / 4.0)));
+// pc.printf("sita:%f, x:%f, y:%f ,x1:%f, x2:%f ,velocity:%f\r\n",sita,x,y,x1,x2,velocity);
+// pc.printf("A2 = %d, X = %d, Y = %d, B = %d, dead = %d\r\n", a2, X, Y, b, deadflag);
+// pc.printf("%f %f %f\r\n",cont,swingRadVelocity, (double)SwingSens.getPulses());
+ }
+}