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-07
- Revision:
- 22:4127fcf8c7d7
- Parent:
- 20:39a51962ed38
File content as of revision 22:4127fcf8c7d7:
/**
* This program is written in main micro computer "mbed" for 2015 NHK Robot Contest (Bteam).
*/
/***コース選択***/
#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
short mesureflag=0;
LocalFileSystem local("local");
#endif
int main() {
Com.attach(&Call,RATE);
#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);
initializeSwing();
while(1) {
if(autoflag){
#ifdef IM920
autoIM920(); /*IM920 button*/
#else
autoPS3(); /*PS3 button*/
#endif
#ifdef BLUE
//Blue
/********************************Nomal Mode*********************************/
if((step==0)&&((8650.0>x)&&(x>800.0))) {
// targ_sita=0.0;
targ_sita=-0.05;
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<2000.0)) {
if((step==3)&&(x<1900.0)) {
targ_sita=PI/4;
step=4;
}
if((step==4)&&(x<100.0)) {
targ_velocity=0.0;
step=114;
}
/***Cylinder***/
if((x>3130.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.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>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.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;
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>1400.0+deff)&&(CStep==15)){
sendData(1,4);
CStep=16;
}
if((x>2700.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(down) fclose(fp_r);
if(b==9) fclose(fp_r);
#endif
// mesure_state(); /*位置測定*/
// move_following(); /*移動制御*/
}
else if(!autoflag) {
flaga=0;
mesureSwing();
// swingFollowing();
#ifdef IM920
manualMoveIM920(); /*analogStick*/
manualIM920(); /*IM920 button*/
#else
manualMovePS3(); /*analogStick*/
manualPS3(); /*PS3 button*/
#endif
pc.printf("cont:%f\r\n",cont);
}
/***update state***/
#ifdef IM920
readIM920();
#endif
// pc.printf("x:%f ,y:%f ,sita:%f\r\n",x,y,sita);
// wait(RATE);
// pc.printf("%f\r\n",(float)((((2.0 * PI) / swingRadVelocity) / 4.0)));
}
}