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:
- 23:26f9483439fe
- Parent:
- 21:bdf8ac5c200c
- Child:
- 24:6d2573d6f2b6
--- a/main_ps3.cpp Fri Nov 06 11:35:39 2015 +0000
+++ b/main_ps3.cpp Sat Nov 07 01:03:46 2015 +0000
@@ -20,9 +20,9 @@
short testflag=0;
FILE *fp_r = fopen("/local/velocity.dat", "w");
#endif
-#include "mbed.h"
+/*#include "mbed.h"
LocalFileSystem local("local");
-FILE *fp_r = fopen("/local/velocity.dat", "w");
+FILE *fp_r = fopen("/local/velocity.dat", "w");*/
#include "machine_ps3.h"
@@ -52,17 +52,14 @@
while(1) {
if(autoflag){
autoIM920(); /*IM920 button*/
-#ifdef BLUE
- Indicator2=1;
- //Blue
/********************************Nomal Mode*********************************/
- if((step==0)&&((9000.0>x)&&(x>800.0))) {
- targ_sita=-0.05;
+ if((step==0)&&((8600.0>x)&&(x>800.0))) {
+ targ_sita=-0.02;
// targ_sita=0.0;
step=1;
}
// if((step==1)&&(x>8600.0+deff)) {
- if((step==1)&&(x>9000.0)) {
+ if((step==1)&&(x>8600.0)) {
targ_velocity=0.0;
direction_controller.setBias(0.0);
direction_controller.reset();
@@ -89,14 +86,7 @@
/***Cylinder***/
if((x>3130.0+deff)&&(CStep==1)) {
// if((x>2030.0+deff)&&(CStep==1)) {
-// if(!skip){ sendData(1,1);
- if(!skip){
- sendData(1,5);
- fprintf(fp_r, "x:%f\r\n",x);
-#ifdef MESURE
- testflag=1;
-#endif
- }
+ if(!skip) sendData(1,1);
CStep=2;
}
if((x>5900.0+deff)&&(CStep==2)) {
@@ -119,223 +109,6 @@
sendData(7,0);
CStep=114;
}
- /////////////////////////////////////////////////////////////////////////
- /******************************Middle Mode******************************/
- if((step==5)&&((5700.0>x)&&(x>800.0))) {
-// targ_sita=0.025;
- targ_sita=0.0;
- step=6;
- }
- if((step==6)&&(x>7000.0)){
- targ_velocity=0.0;
- direction_controller.reset();
- direction_controller.setBias(0.0);
- step=7;
- }
- if((step==7)&&((velocity<500.0)&&(velocity>-500.0))){
- flagf=0;
- spcount=0.0;
- targ_sita=0.0;
- 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.0;
- 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.0;
- step=18;
- direction_controller.reset();
- direction_controller.setBias(0.0);
- }
- if((step==18)&&(x<-100.0)){
- targ_velocity=0.0;
- targ_sita=PI/4.0;
- step=114;
- }
-
- if((x>1050.0+deff)&&(CStep==15)){
- sendData(1,5);
- CStep=16;
- }
- if((x>2505.0+deff)&&(CStep==16)){
- sendData(1,4);
- CStep=17;
- }
- if((x<2400.0+deff)&&(CStep==17)){
- sendData(7,0);
- CStep=114;
- }
-#else
- //Red
- /********************************Nomal Mode*********************************/
- if((step==0)&&((8650.0>x)&&(x>800.0))) {
- targ_sita=0.0;
- step=1;
- }
- if((step==1)&&(x>8600.0+deff)) {
- targ_velocity=0.0;
- direction_controller.setBias(0.0);
- direction_controller.reset();
- velocity_controller.setBias(0.0);
- velocity_controller.reset();
- step=2;
- }
- if((step==2)&&((velocity<500.0)&&(velocity>-500.0))){
- step=3;
- spcount=0.0;
- flagf=0;
- targ_sita=0.0;
- }
- if((step==3)&&(x<1850.0)) {
- targ_sita=-PI/4;
- step=4;
- }
- if((step==4)&&(x<100.0)) {
- targ_velocity=0.0;
- step=114;
- }
- /***Cylinder***/
- 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;
- direction_controller.reset();
- direction_controller.setBias(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;
- direction_controller.reset();
- direction_controller.setBias(0.0);
- }
- 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
-#ifdef MESURE
- if(b==9) fclose(fp_r);
-#endif
- if(b==9) fclose(fp_r);
}
else if(!autoflag) {
flaga=0;