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
main_ps3.cpp
- Committer:
- unicore32
- Date:
- 2015-10-10
- Revision:
- 78:abb760e0a935
- Parent:
- 68:2b2b88ecdcce
- Parent:
- 77:7dd154b5ace3
- Child:
- 79:7f86e18f40ef
File content as of revision 78:abb760e0a935:
/**
* This program is written in main micro computer "mbed" for 2015 NHK Robot Contest (Bteam).
*/
//速度コントローラと向きコントローラはそのまま
//大和田にPIC1のシリンダの信号をタイマーで書かせる
//回転機構wait PI/4 追加で待つ
//射出をスキップする機能を追加
//スタートゾーンにマシンを戻した後に暴走しないようにする
//手動時の操作性の改善
//一度手動モードになったらスタートゾーンに戻るまで再び自動モードにならないようにする
/***コース選択***/
//#define BLUE
#define RED
/***コントローラ選択***/
//#define IM920
#define PS3
#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"
Serial pc(USBTX, USBRX);
//LocalFileSystem local("local");
Timeout MStop;
void Restart(){
if(step==10) step=11;
}
int main() {
#ifdef IM920
initializeIM920();
#else
initializeSBDBT();
#endif
initializeMotors();
initializeControllers();
initializeSwing();
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;
// FILE *fp_r = fopen("/local/velocity.dat", "w");
wait(0.3);
sendData(7,255);
// double time=0.0;
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>900.0))) targ_sita=0.03,step=1; /*nomal*/
if((step==1)&&(x>8650.0)) targ_velocity=-speed,step=2,flagf=0;
if((step==2)&&(x<1700.0)) {
targ_sita=PI/4;
step=3;
sendData(7,255);
}
if((step==3)&&(x<-100.0)) {
targ_velocity=0.0;
step=114;-
autoflag = 0;
}
/***Cylinder***/
if((x>2900.0)&&(CStep==1)) { /*nomal*/
if(!skip) sendData(1,1);
CStep=2;
}
if((x>5850.0)&&(CStep==2)) {
if(!skip) sendData(1,2);
CStep=3;
}
if((x>7600.0)&&(CStep==3)) {
if(!skip) sendData(1,3);
CStep=4;
}
if((x<6600.0)&&(CStep==4)) {
if(!skip) sendData(1,5);
CStep=5;
}
if((x<6100.0)&&(CStep==5)) {
if(!skip) sendData(1,4);
CStep=6;
}
/////////////////////////////////////////////////////////////////////////
/******************************Middle Mode******************************/
if((step==4)&&((5700.0>x)&&(x>600.0))) {/*middle*/
targ_sita=0.03;
step=5;
}
if((step==5)&&(x>5000.0)) {
targ_velocity=-speed;
step=6;
sendData(1,5);
flagf=0;
}
if((step==6)&&(x<1600.0)) {
targ_sita=PI/4;
step=7;
sendData(7,255);
}
if((step==7)&&(x<-100.0)) {
targ_velocity=0.0;
step=114;
autoflag = 0;
}
/////////////////////////////////////////////////////////////////////////
/*****************************Opponents Mode****************************/
if((step==8)&&(x>1500.0)) {
targ_sita=-PI/2.0;
step=9;
}
if((step==9)&&(y<-1000.0)) {
targ_velocity = 0.0;
targ_sita = 4.0 * ( PI / 9.0 );
step=10;
}
if((step==10)&&(abs(sita-targ_sita)<0.05)) {
sendData(1,4);
MStop.attach(Restart,0.5);
}
if(step==11){
targ_sita = 0.0;
step = 12;
}
if((step==12)&&(abs(sita-targ_sita)<0.05)){
targ_velocity = -speed;
flagf = 0;
step = 13;
}
if((step==13)&&(y>0.0)){
targ_sita = 0.0;
step = 14;
}
if((step==14)&&(x<-100.0)){
targ_velocity = 0.0;
targ_sita = PI/4.0;
step = 114;
}
/////////////////////////////////////////////////////////////////////////
#else
//Red
/********************************Nomal Mode*********************************/
if((step==0)&&((8650.0>x)&&(x>900.0))) targ_sita=-0.03,step=1; /*nomal*/
if((step==1)&&(x>8650.0)) targ_velocity=-speed,step=2,flagf=0;
if((step==2)&&(x<1700.0)) {
targ_sita=-PI/4;
step=3;
sendData(7,31);
}
if((step==3)&&(x<-100.0)) {
targ_velocity=0.0;
step=8;
autoflag=0;
}
/***Cylinder***/
if((x>2900.0)&&(CStep==1)) { /*nomal*/
if(!skip) sendData(1,1);
CStep=2;
}
if((x>5850.0)&&(CStep==2)) {
if(!skip) sendData(1,3);
CStep=3;
}
if((x>7600.0)&&(CStep==3)) {
if(!skip) sendData(1,2);
CStep=6;
}
if((x<6600.0)&&(CStep==4)) {
if(!skip) sendData(1,4);
CStep=5;
}
if((x<6100.0)&&(CStep==5)) {
if(!skip) sendData(1,5);
CStep=6;
}
/////////////////////////////////////////////////////////////////////////
/******************************Middle Mode******************************/
if((step==4)&&((5700.0>x)&&(x>600.0))) { /*middle*/
targ_sita=-0.03;
step=5;
medge=1;
}
if((step==5)&&(x>5000.0)) {
targ_velocity=-speed;
step=6;
sendData(1,4);
flagf=0;
}
if((step==6)&&(x<2000.0)) {
targ_sita=-PI/4;
step=7;
sendData(7,255);
}
if((step==7)&&(x<-100.0)) {
targ_velocity=0.0;
step=8;
autoflag=0;
}
/////////////////////////////////////////////////////////////////////////
#endif
mesure_state(); /*位置測定*/
move_following(); /*移動制御*/
}
else if(!autoflag) {
resetState();
#ifdef IM920
manualMoveIM920(); /*analogStick*/
manualIM920(); /*IM920 button*/
#else
manualMovePS3(); /*analogStick*/
manualPS3(); /*PS3 button*/
#endif
//Swing
swingFollowing();
/*if(square){
IndicatorAuto=0;
fclose(fp_r);
}
fprintf(fp_r, "time:%1.3f, %f[rad/s], pwm:%f\r\n",time,swingRadVelocity,cont);
time+=0.01;*/
}
/***update state***/
#ifdef IM920
readIM920();
#endif
mesureSwing();
wait(RATE);
// pc.printf("sita:%f, x:%f, y:%f ,x1:%f, x2:%f ,velocity:%f\r\n",sita,x,y,x1,x2,velocity);
// pc.printf("A2 = %d, X = %d, Y = %d, B = %d, dead = %d\r\n", a2, X, Y, b, deadflag);
// pc.printf("%f %f\r\n",cont,swingRadVelocity);
}
}
