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:
- 38:f4e9893641ac
- Parent:
- 37:75fcd28f48c7
- Child:
- 39:5ba406327aa3
--- a/main_ps3.cpp Sun Nov 15 11:45:29 2015 +0000
+++ b/main_ps3.cpp Mon Nov 16 09:37:31 2015 +0000
@@ -3,8 +3,8 @@
*/
/***コース選択***/
-#define BLUE
-//#define RED
+//#define BLUE
+#define RED
/***マシン状態計測***/
//#define MESURE
@@ -15,7 +15,7 @@
#include "machine_ps3.h"
-Serial pc(USBTX, USBRX);
+//Serial pc(USBTX, USBRX);
//LocalFileSystem local("local");
@@ -191,8 +191,9 @@
#else
/********************************Own & Middle Mode*********************************/
- if((step==0)&&((10000.0>x)&&(x>800.0))) {
- targ_sita=0.03;
+// if((step==0)&&((10000.0>x)&&(x>1000.0))) {
+ if((step==0)&&((10000.0>x)&&(x>850.0))) {
+ targ_sita=0.02;
// targ_sita=0.0;
step=1;
}
@@ -208,9 +209,9 @@
spcount=0.0;
flagf=0;
// targ_sita=0.0;
- targ_sita=0.03;
+ targ_sita=0.025;
}
- if((step==3)&&(x<1700.0)) {
+ if((step==3)&&(x<2000.0)) {
targ_sita=-PI/4;
step=4;
}
@@ -220,15 +221,15 @@
}
//Cylinder
- if((x>3100.0)&&(CStep==0)) {
+ if((x>3250.0)&&(CStep==0)) {
if(!skip) sendData(1,1);
CStep=1;
}
- if((x>6050.0)&&(CStep==1)) {
+ if((x>6100.0)&&(CStep==1)) {
if(!skip) sendData(1,3);
CStep=2;
}
- if((x>7650.0)&&(CStep==2)) {
+ if((x>7750.0)&&(CStep==2)) {
if(!skip) sendData(1,2);
CStep=3;
}
@@ -236,26 +237,42 @@
if(!skip) sendData(1,8);
CStep=4;
}
- if((x<7200.0)&&(CStep==4)) {
- if(!skip) sendData(1,6); //over
+ if((x>8500.0)&&(CStep==4)) CStep=5;
+ /////////////////////////////////////////
+ /*if((x<7200.0)&&(CStep==4)) {
+ if(!skip) sendData(1,4); //over
CStep=5;
}
if((x<6950.0)&&(CStep==5)) {
- if(!skip) sendData(1,4); //front
+ if(!skip) sendData(1,6); //fornt
CStep=6;
}
if((x<6700.0)&&(CStep==6)) {
if(!skip) sendData(1,5); //middle
CStep=7;
+ }*/
+ /////////////////////////////////////////
+ if((x<7970.0)&&(CStep==5)) {
+ if(!skip) sendData(1,4); //front
+ CStep=6;
}
- if((x<3000.0)&&(CStep==7)){
+ if((x<6830.0)&&(CStep==6)) {
+ if(!skip) sendData(1,5); //middle
+ CStep=7;
+ }
+ if((x<5580.0)&&(CStep==7)) {
+ if(!skip) sendData(1,6); //over
+ CStep=8;
+ }
+ /////////////////////////////////////////
+ if((x<3000.0)&&(CStep==8)){
sendData(7,0);
CStep=114;
}
/********************************Own & Opponent Mode*********************************/
- if((step==10)&&((9800.0>x)&&(x>700.0))) {
- targ_sita=-0.03;
+ if((step==10)&&((10000.0>x)&&(x>630.0))) {
+ targ_sita=0.025;
// targ_sita=0.0;
step=11;
}
@@ -271,48 +288,50 @@
flagf=0;
// flagf=3;
// targ_sita=0.0;
- targ_sita=-0.03;
+ targ_sita=0.02;
}
- if((step==13)&&(x<1400.0)) {
- targ_sita=PI/4;
+ if((step==13)&&(x<1450.0)) {
+ targ_sita=-PI/4;
step=14;
}
- if((step==14)&&(x<600.0)) {
+ if((step==14)&&(x<700.0)) {
dpcount=speed;
step=114;
}
//Cylinderd
- if((x>3200.0)&&(CStep==10)) {
+ if((x>3100.0)&&(CStep==10)) {
if(!skip) sendData(1,1);
CStep=11;
}
- if((x>6050.0)&&(CStep==11)) {
+ if((x>6000.0)&&(CStep==11)) {
if(!skip) sendData(1,3);
CStep=12;
}
- if((x>7700.0)&&(CStep==12)) {
+ if((x>7650.0)&&(CStep==12)) {
if(!skip) sendData(1,2);
CStep=13;
}
- if((x>9500.0)&&(CStep==13)) {
- if(!skip) sendData(1,8);
+ if((x>9800.0)&&(CStep==13)) {
+ sendData(1,8);
CStep=14;
}
- if((x<9400.0)&&(CStep==14)) {
- if(!skip) sendData(1,6); //over
- CStep=15;
- }
- if((x<6600.0)&&(CStep==15)) {
- if(!skip) sendData(1,5); //middle
+ if((x>9900.0)&&(CStep==14)) CStep=15;
+ if((x<9750.0)&&(CStep==15)) { //7.8
+ if(!skip) sendData(1,6);
CStep=16;
}
- if((x<4820.0)&&(CStep==16)) {
-// if((x<4850.0)&&(CStep==16)) {
- if(!skip) sendData(1,4); //front
+ if((x<7900.0)&&(CStep==16)) { //7.8
+// if((x<7950.0)&&(CStep==16)) { //7.8
+ if(!skip) sendData(1,5);
CStep=17;
}
- if((x<1000.0)&&(CStep==17)){
+ if((x<5670.0)&&(CStep==17)) { //7.8
+// if((x<5750.0)&&(CStep==17)) { //7.8
+ if(!skip) sendData(1,4);
+ CStep=18;
+ }
+ if((x<1000.0)&&(CStep==18)){
sendData(7,0);
CStep=114;
}
@@ -393,19 +412,23 @@
}
/********************************Swing Mode(front)*********************************/
- if((mstep==20)&&((10000.0>x)&&(x>1500.0))) {
+ if((mstep==20)&&((10000.0>x)&&(x>1550.0))) {
dpcount=speed;
mstep=21;
}
if((mstep==21)&&((velocity<5.0)&&(velocity>-5.0))){
- targ_sita=PI/4.0+PI/16.0;
- swingmoved=3;
- mstep=114;
+ targ_sita=PI/4.0-PI/8.0;
+ swingmoved=2;
+ mstep=253;
+ }
+ if((mstep==252)&&(((targ_sita-sita)<0.01)&&((targ_sita-sita)>-0.01))){
+ flagf=2;
}
if(mstep==22){
mstep=23;
- targ_sita=3.0*PI/4.0;
+ targ_sita=3.0*PI/4.0-PI/8.0;
sendData(7,0);
+ flagf=0;
}
if((mstep==23)&&(((targ_sita-sita)<0.05)&&((targ_sita-sita)>-0.05))){
flagf=1;
@@ -413,19 +436,124 @@
mstep=24;
}
if((mstep==24)&&(x<1300.0)){
- targ_sita=PI;
+ targ_sita=5*PI/6.0;
mstep=25;
}
if((mstep==25)&&(x<-10.0)){
targ_velocity=0.0;
+ targ_sita=3.0*PI/4.0;
mstep=114;
}
#else
+ /********************************Swing Mode(middle)*********************************/
+ if((mstep==0)&&((10000.0>x)&&(x>1550.0))) {
+ dpcount=speed;
+ mstep=1;
+ }
+ if((mstep==1)&&((velocity<5.0)&&(velocity>-5.0))){
+ targ_sita=-PI/4.0;
+ swingmoved=2;
+ mstep=251;
+ }
+ if((mstep==251)&&(((targ_sita-sita)<0.01)&&((targ_sita-sita)>-0.01))){
+ flagf=2;
+ }
+ if(mstep==2){
+ mstep=3;
+ targ_sita=-PI/18.0;
+ sendData(7,0);
+ flagf=0;
+ }
+ if((mstep==3)&&(((targ_sita-sita)<0.05)&&((targ_sita-sita)>-0.05))){
+ flagf=1;
+ targ_velocity=speed;
+ mstep=4;
+ }
+ if((mstep==4)&&(x<1600.0)){
+ targ_sita=-5.0*PI/6.0;
+ mstep=5;
+ }
+ if((mstep==5)&&(x<-10.0)){
+ targ_velocity=0.0;
+ targ_sita=-3.0*PI/4.0;
+ mstep=114;
+ }
+
+
+ /********************************Swing Mode(over)*********************************/
+// if((mstep==10)&&((10000.0>x)&&(x>1500.0))) {
+ if((mstep==10)&&((10000.0>x)&&(x>1550.0))) {
+ dpcount=speed;
+ mstep=11;
+ }
+ if((mstep==11)&&((velocity<5.0)&&(velocity>-5.0))){
+ targ_sita=-PI/3.0;
+ targ_sita=-PI/4.0+PI/8.0;
+ swingmoved=2;
+ mstep=252;
+ }
+ if((mstep==252)&&(((targ_sita-sita)<0.01)&&((targ_sita-sita)>-0.01))){
+ flagf=2;
+ }
+ if(mstep==12){
+ mstep=13;
+ targ_sita=-3.0*PI/4.0+PI/8.0;
+ sendData(7,0);
+ flagf=0;
+ }
+ if((mstep==13)&&(((targ_sita-sita)<0.05)&&((targ_sita-sita)>-0.05))){
+ flagf=1;
+ targ_velocity=speed;
+ mstep=14;
+ }
+ if((mstep==14)&&(x<1600.0)){
+ targ_sita=-5.0*PI/6.0;
+ mstep=15;
+ }
+ if((mstep==15)&&(x<-10.0)){
+ targ_velocity=0.0;
+ targ_sita=-3.0*PI/4.0;
+ mstep=114;
+ }
+
+ /********************************Swing Mode(front)*********************************/
+ if((mstep==20)&&((10000.0>x)&&(x>1550.0))) {
+ dpcount=speed;
+ mstep=21;
+ }
+ if((mstep==21)&&((velocity<5.0)&&(velocity>-5.0))){
+ targ_sita=0.0;
+ swingmoved=2;
+ mstep=253;
+ }
+ if((mstep==252)&&(((targ_sita-sita)<0.01)&&((targ_sita-sita)>-0.01))){
+ flagf=2;
+ }
+ if(mstep==22){
+ mstep=23;
+ targ_sita=PI/6.0;
+ sendData(7,0);
+ flagf=0;
+ }
+ if((mstep==23)&&(((targ_sita-sita)<0.05)&&((targ_sita-sita)>-0.05))){
+ flagf=1;
+ targ_velocity=speed;
+ mstep=24;
+ }
+ if((mstep==24)&&(x<1600.0)){
+ targ_sita=-5.0*PI/6.0;
+ mstep=25;
+ }
+ if((mstep==25)&&(x<-10.0)){
+ targ_velocity=0.0;
+ targ_sita=-3.0*PI/4.0;
+ mstep=114;
+ }
#endif
}
// pc.printf("b:%d\r\n",swingRadVelocity);
// pc.printf("Swing:%f\r\n",SwingSens.getPulses());
- pc.printf("x:%f ,y:%f ,sita:%f ,r:%f\r\n",x,y,sita,Pulses_move_r);
+// pc.printf("x:%f ,y:%f ,sita:%f ,r:%f\r\n",x,y,sita,Pulses_move_r);
}
}
\ No newline at end of file