2015_robocon_bteam
/
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.
Fork of
2015robot_main
by 
Naoto Deguchi
Diff: main_ps3.cpp
- Revision:
- 83:e1638c58e1f1
- Parent:
- 79:7f86e18f40ef
- Child:
- 84:919a335ac81e
--- a/main_ps3.cpp Sun Oct 11 07:42:18 2015 +0000
+++ b/main_ps3.cpp Mon Oct 12 06:15:45 2015 +0000
@@ -9,14 +9,20 @@
//スタートゾーンにマシンを戻した後に暴走しないようにする
//手動時の操作性の改善
//一度手動モードになったらスタートゾーンに戻るまで再び自動モードにならないようにする
+//flagfで前進・後退切り替え
+//大和田射角
+//午後に一定回転制御の実験
/***コース選択***/
#define BLUE
//#define RED
/***コントローラ選択***/
-#define IM920
-//#define PS3
+//#define IM920
+#define PS3
+
+/***回転機構測定***/
+//#define MESURE
#if defined(IM920) && defined(PS3)
#error Caution, You should define either IM920 or PS3
@@ -29,7 +35,9 @@
#include "machine_ps3.h"
Serial pc(USBTX, USBRX);
-//LocalFileSystem local("local");
+#ifdef MESURE
+LocalFileSystem local("local");
+#endif
Timeout MStop;
void Restart(){
@@ -55,10 +63,12 @@
#endif
Indicator4=1;
Enable=1;
-// FILE *fp_r = fopen("/local/velocity.dat", "w");
+#ifdef MESURE
+ FILE *fp_r = fopen("/local/velocity.dat", "w");
+ double time=0.0;
+#endif
wait(0.3);
sendData(7,255);
-// double time=0.0;
while(1) {
if(autoflag){
#ifdef IM920
@@ -69,27 +79,35 @@
#ifdef BLUE
//Blue
/********************************Nomal Mode*********************************/
- if((step==0)&&((8650.0>x)&&(x>900.0))) targ_sita=0.03,step=1; /*nomal*/
- if((step==1)&&(x>8650.0)) {
+ if((step==0)&&((8650.0>x)&&(x>850.0))) {
+ targ_sita=0.08;
+ step=1;
+ }
+ if((step==1)&&(x>8600.0)) {
+ targ_velocity=0.0;
step=2;
+ }
+ if((step==2)&&(abs(velocity)<1000.0)) {
+ step=3;
+ targ_velocity=speed;
flagf=0;
- targ_sita=0.05;
+ targ_sita=0.07;
}
- if((step==2)&&(x<1950.0)) {
+ if((step==3)&&(x<1950.0)) {
targ_sita=PI/4;
- step=3;
+ step=4;
sendData(7,255);
}
- if((step==3)&&(x<-100.0)) {
+ if((step==4)&&(x<-100.0)) {
targ_velocity=0.0;
step=114;
}
/***Cylinder***/
- if((x>2900.0)&&(CStep==1)) { /*nomal*/
+ if((x>2950.0)&&(CStep==1)) { /*nomal*/
if(!skip) sendData(1,1);
CStep=2;
}
- if((x>5850.0)&&(CStep==2)) {
+ if((x>5880.0)&&(CStep==2)) {
if(!skip) sendData(1,2);
CStep=3;
}
@@ -103,57 +121,81 @@
}
if((x<6100.0)&&(CStep==5)) {
if(!skip) sendData(1,4);
- CStep=6;
+ CStep=114;
}
/////////////////////////////////////////////////////////////////////////
/******************************Middle Mode******************************/
- if((step==4)&&((5700.0>x)&&(x>600.0))) {/*middle*/
- targ_sita=0.03;
- step=5;
- }
- if((step==5)&&(x>4900.0)) {
+ if((step==5)&&((5700.0>x)&&(x>800.0))) {
+ targ_sita=0.08;
step=6;
- sendData(1,5);
- flagf=0;
- targ_sita=0.05;
+ }
+ if((step==6)&&(x>7000.0)){
+ targ_velocity=0.0;
+ step=7;
}
- if((step==6)&&(x<1600.0)) {
+ if((step==7)&&(abs(velocity)<1000.0)) {
+ flagf=0;
+ targ_velocity=speed;
+ targ_sita=0.07;
+ step=8;
+ }
+ if((step==8)&&(x<1900.0)) {
targ_sita=PI/4;
- step=7;
+ step=9;
sendData(7,255);
}
- if((step==7)&&(x<-100.0)) {
+ if((step==9)&&(x<-100.0)) {
targ_velocity=0.0;
step=114;
}
+
+ if((x>4850.0)&&(CStep==6)){
+ sendData(1,5);
+ CStep=7;
+ }
+ if((x>5350.0)&&(CStep==7)){
+ sendData(1,4);
+ CStep=114;
+ }
/////////////////////////////////////////////////////////////////////////
/*****************************Opponents Mode****************************/
- if((step==8)&&(y<-1700.0)) {
+ if((step==15)&&((5700.0>x)&&(x>50.0))) {
+ targ_sita=0.03;
+ step=16;
+ }
+ if((step==16)&&(x>2800.0)){
targ_velocity=0.0;
- targ_sita=0.0;
step=114;
}
/////////////////////////////////////////////////////////////////////////
#else
//Red
/********************************Nomal Mode*********************************/
- if((step==0)&&((8650.0>x)&&(x>800.0))) targ_sita=-0.03,step=1; /*nomal*/
- if((step==1)&&(x>8650.0)) {
+ if((step==0)&&((8650.0>x)&&(x>800.0))) {
+ targ_sita=-0.08;
+ step=1;
+ }
+ if((step==1)&&(x>8600.0)) {
+ targ_velocity=0.0;
step=2;
- flagf=0;
- targ_sita=-0.05;
}
- if((step==2)&&(x<1900.0)) {
- targ_sita=-PI/4;
+ if((step==2)&&(abs(velocity)<1000.0)) {
step=3;
- sendData(7,31);
+ targ_velocity=speed;
+ flagf=0;
+ targ_sita=-0.07;
}
- if((step==3)&&(x<-100.0)) {
+ if((step==3)&&(x<1950.0)) {
+ targ_sita=-PI/4;
+ step=4;
+ sendData(7,255);
+ }
+ if((step==4)&&(x<-100.0)) {
targ_velocity=0.0;
- step=8;
+ step=114;
}
- /***Cylinder***/
- if((x>2900.0)&&(CStep==1)) { /*nomal*/
+
+ if((x>2950.0)&&(CStep==1)) {
if(!skip) sendData(1,1);
CStep=2;
}
@@ -165,30 +207,34 @@
if(!skip) sendData(1,2);
CStep=6;
}
- if((x<6600.0)&&(CStep==4)) {
+ if((x<6650.0)&&(CStep==4)) {
if(!skip) sendData(1,4);
CStep=5;
}
- if((x<6100.0)&&(CStep==5)) {
+ if((x<6130.0)&&(CStep==5)) {
if(!skip) sendData(1,5);
- CStep=6;
+ CStep=114;
}
+
/////////////////////////////////////////////////////////////////////////
/******************************Middle Mode******************************/
- if((step==4)&&((5700.0>x)&&(x>800.0))) { /*middle*/
- targ_sita=-0.03;
- step=5;
- medge=1;
+ if((step==5)&&((5700.0>x)&&(x>800.0))) {
+ targ_sita=-0.08;
+ step=6;
+ }
+ if((step==6)&&(x>7000.0)){
+ targ_velocity=0.0;
+ step=7;
}
- if((step==5)&&(x>5000.0)) {
- step=6;
- sendData(1,4);
+ if((step==7)&&(abs(velocity)<1000.0)) {
flagf=0;
- targ_sita=-0.05;
+ targ_velocity=speed;
+ targ_sita=-0.07;
+ step=8;
}
- if((step==6)&&(x<2000.0)) {
+ if((step==8)&&(x<1900.0)) {
targ_sita=-PI/4;
- step=7;
+ step=9;
sendData(7,255);
}
if((step==7)&&(x<-100.0)) {
@@ -196,6 +242,15 @@
step=8;
autoflag=0;
}
+
+ if((x>4850.0)&&(CStep==6)){
+ sendData(1,4);
+ CStep=7;
+ }
+ if((step==9)&&(x<-100.0)) {
+ targ_velocity=0.0;
+ step=114;
+ }
/////////////////////////////////////////////////////////////////////////
#endif
mesure_state(); /*位置測定*/
@@ -212,12 +267,14 @@
#endif
//Swing
swingFollowing();
- /*if(square){
+#ifdef MESURE
+ if(down){
IndicatorAuto=0;
fclose(fp_r);
}
fprintf(fp_r, "time:%1.3f, %f[rad/s], pwm:%f\r\n",time,swingRadVelocity,cont);
- time+=0.01;*/
+ time+=0.01;
+#endif
}
/***update state***/
#ifdef IM920
