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.
main_ps3.cpp
- Committer:
- DeguNaoto
- Date:
- 2015-11-13
- Revision:
- 34:aa2a5c888a27
- Parent:
- 33:a4323c20494b
- Child:
- 35:7b6786193aa2
File content as of revision 34:aa2a5c888a27:
/**
* This program is written in main micro computer "mbed" for 2015 NHK Robot Contest (Bteam).
*/
/***コース選択***/
#define BLUE
//#define RED
/***マシン状態計測***/
//#define MESURE
#if defined(BLUE) && defined(RED)
#error Caution, You should define either BLUE or RED
#endif
#include "machine_ps3.h"
Serial pc(USBTX, USBRX);
//LocalFileSystem local("local");
int main() {
// FILE *fp_r = fopen("/local/velocity.dat", "w");
Com.attach(&Call,RATE);
initializeMotors();
initializeControllers();
initializeRS485();
initializeMbedSerial();
initializeSwing();
#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;
wait(0.3);
sendData(7,0);
while(1) {
// if(modeflag){
if(autoflag){
autoIM920(); /*IM920 button*/
#ifdef BLUE
/********************************Own & Middle Mode*********************************/
if((step==0)&&((10000.0>x)&&(x>1000.0))) {
targ_sita=-0.03;
// targ_sita=0.0;
step=1;
}
if((step==1)&&(x>9000.0)) {
targ_velocity=0.0;
velocity_controller.setBias(0.0);
velocity_controller.reset();
// dpcount=speed;
step=2;
}
if((step==2)&&((velocity<500.0)&&(velocity>-500.0))){
step=3;
spcount=0.0;
flagf=0;
// targ_sita=0.0;
targ_sita=-0.03;
}
if((step==3)&&(x<1700.0)) {
targ_sita=PI/4;
step=4;
}
if((step==4)&&(x<800.0)) {
dpcount=speed;
step=114;
}
//Cylinder
if((x>3200.0)&&(CStep==0)) {
if(!skip) sendData(1,1);
CStep=1;
}
if((x>6150.0)&&(CStep==1)) {
if(!skip) sendData(1,2);
CStep=2;
}
if((x>7750.0)&&(CStep==2)) {
if(!skip) sendData(1,3);
CStep=3;
}
if((x<7050.0)&&(CStep==3)) {
if(!skip) sendData(1,4); //over
CStep=4;
}
if((x<6950.0)&&(CStep==4)) {
if(!skip) sendData(1,5); //middle
CStep=5;
}
if((x<6950.0)&&(CStep==5)) {
if(!skip) sendData(1,6); //front
CStep=6;
}
if((x<3000.0)&&(CStep==6)){
sendData(7,0);
CStep=114;
}
/********************************Own & Opponent Mode*********************************/
if((step==10)&&((10000.0>x)&&(x>700.0))) {
targ_sita=-0.03;
// targ_sita=0.0;
step=11;
}
if((step==11)&&(x>10000.0)) {
targ_velocity=0.0;
velocity_controller.setBias(0.0);
velocity_controller.reset();
step=12;
}
if((step==12)&&((velocity<500.0)&&(velocity>-500.0))){
step=13;
spcount=0.0;
flagf=0;
// targ_sita=0.0;
targ_sita=-0.03;
}
if((step==13)&&(x<1400.0)) {
targ_sita=PI/4;
step=14;
}
if((step==14)&&(x<600.0)) {
dpcount=speed;
step=114;
}
//Cylinderd
if((x>9500.0)&&(CStep==10)) CStep=11;
if((x<9400.0)&&(CStep==11)) {
if(!skip) sendData(1,4);
CStep=12;
}
if((x<6900.0)&&(CStep==12)) {
if(!skip) sendData(1,5);
CStep=13;
}
if((x<5350.0)&&(CStep==13)) {
if(!skip) sendData(1,6);
CStep=14;
}
if((x<1000.0)&&(CStep==14)){
sendData(7,0);
CStep=114;
}
#else
/********************************Own & Middle Mode*********************************/
if((step==0)&&((10000.0>x)&&(x>1000.0))) {
targ_sita=0.03;
// targ_sita=0.0;
step=1;
}
if((step==1)&&(x>9000.0)) {
targ_velocity=0.0;
velocity_controller.setBias(0.0);
velocity_controller.reset();
// dpcount=speed;
step=2;
}
if((step==2)&&((velocity<500.0)&&(velocity>-500.0))){
step=3;
spcount=0.0;
flagf=0;
// targ_sita=0.0;
targ_sita=-0.03;
}
if((step==3)&&(x<1700.0)) {
targ_sita=-PI/4;
step=4;
}
if((step==4)&&(x<800.0)) {
dpcount=speed;
step=114;
}
//Cylinder
if((x>3250.0)&&(CStep==0)) {
if(!skip) sendData(1,1);
CStep=1;
}
if((x>6200.0)&&(CStep==1)) {
if(!skip) sendData(1,3);
CStep=2;
}
if((x>7800.0)&&(CStep==2)) {
if(!skip) sendData(1,2);
CStep=3;
}
if((x<7000.0)&&(CStep==3)) {
if(!skip) sendData(1,6); //over
CStep=4;
}
if((x<6920.0)&&(CStep==4)) {
if(!skip) sendData(1,5); //middle
CStep=5;
}
if((x<6900.0)&&(CStep==5)) {
if(!skip) sendData(1,4); //front
CStep=6;
}
if((x<3000.0)&&(CStep==6)){
sendData(7,0);
CStep=114;
}
#endif
}
// else if(!modeflag) {
else if(!autoflag){
manualIM920(); /*IM920 button*/
#ifdef BLUE
/********************************Swing Mode*********************************/
if((mstep==0)&&((10000.0>x)&&(x>1700.0))) {
dpcount=speed;
mstep=1;
}
if((mstep==1)&&((velocity<5.0)&&(velocity>-5.0))){
targ_sita=PI/4.0;
mstep=114;
}
/********************************Opponent Struct Mode*********************************/
if((mstep==10)&&((10000.0>x)&&(x>1150.0))) {
targ_sita=-0.03;
// targ_sita=0.0;
mstep=11;
}
/*if((mstep==11)&&(x>8000.0)) {
targ_velocity=0.0;
mstep=13;
}*/
if((mstep==11)&&(x>8000.0)) {
targ_sita=-PI/4.0;
mstep=12;
}
if((mstep==12)&&(x>8700.0)) {
targ_velocity=0.0;
mstep=13;
}
if((mstep==13)&&((velocity<5.0)&&(velocity>-5.0))){
targ_sita=0.0;
mstep=14;
}
/*if((mstep==14)&&((sita<0.01)&&(sita>-0.01))){
sendData(1,6);
mstep=114;
}*/
/*if((mCStep==0)&&(x>7100.0)){
sendData(1,4);
mCStep=114;
}*/
#else
#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);
}
}