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-10-28
- Revision:
- 0:b613dc16f27d
- Child:
- 1:3ac2087996f3
File content as of revision 0:b613dc16f27d:
/**
* This program is written in main micro computer "mbed" for 2015 NHK Robot Contest (Bteam).
*/
//回転機構wait PI/4 追加で待つ
//射出をスキップする機能を追加
//スタートゾーンにマシンを戻した後に暴走しないようにする
//手動時の操作性の改善
//一度手動モードになったらスタートゾーンに戻るまで再び自動モードにならないようにする
//flagfで前進・後退切り替え
//大和田射角
//午後に一定回転制御の実験
//モード切替時に時間を待つ
//赤チーム側を青チーム側のプログラムに合わせて実装
//横幅調整済み
//相手妨害モードいらない new!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
//赤チームモード、相手チーム側の射出位置調整
/***コース選択***/
//#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
LocalFileSystem local("local");
#endif
short cal=0;
Ticker Com;
void Call(){
mesure_state();
if(autoflag) cal=1;
}
int main() {
Com.attach(&Call,0.01);
#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);
while(1) {
if(autoflag){
#ifdef IM920
autoIM920(); /*IM920 button*/
#else
autoPS3(); /*PS3 button*/
#endif
#ifdef BLUE
//Blue
if(spcount<speed){
spcount+=speed/100.0;
targ_velocity=spcount;
}
/********************************Nomal Mode*********************************/
if((step==0)&&((8650.0>x)&&(x>800.0))) {
targ_sita=0.025;
step=1;
}
if((step==1)&&(x>8600.0+deff)) {
targ_velocity=0.0;
step=2;
}
if((step==2)&&((velocity<500.0)&&(velocity>-500.0))){
step=3;
spcount=0.0;
flagf=0;
targ_sita=0.035;
}
if((step==3)&&(x<2000.0)) {
targ_sita=PI/4;
step=4;
}
if((step==4)&&(x<0.0)) {
targ_velocity=0.0;
step=114;
}
/***Cylinder***/
if((x>3030.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.025;
step=6;
}
if((step==6)&&(x>7000.0)){
targ_velocity=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<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.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;
}
if((step==18)&&(x<-100.0)){
targ_velocity=0.0;
targ_sita=PI/4.0;
step=114;
}
// if((x>1200.0+deff)&&(CStep==15)){
// if((x>1000.0+deff)&&(CStep==15)){
if((x>1050.0+deff)&&(CStep==15)){
sendData(1,5);
CStep=16;
}
if((x>2505.0+deff)&&(CStep==16)){
// if((x>2455.0+deff)&&(CStep==16)){
// if((x>2630.0)&&(CStep==16)){
sendData(1,4);
CStep=17;
}
if((x<2400.0+deff)&&(CStep==17)){
sendData(7,0);
CStep=114;
}
#else
//Red
if(spcount<speed){
spcount+=speed/100.0;
targ_velocity=spcount;
}
/********************************Nomal Mode*********************************/
if((step==0)&&((8650.0>x)&&(x>800.0))) {
targ_sita=-0.025;
step=1;
}
if((step==1)&&(x>8600.0+deff)) {
targ_velocity=0.0;
step=2;
}
if((step==2)&&((velocity<500.0)&&(velocity>-500.0))){
step=3;
spcount=0.0;
flagf=0;
targ_sita=-0.035;
}
if((step==3)&&(x<2000.0)) {
targ_sita=-PI/4;
step=4;
}
if((step==4)&&(x<0.0)) {
targ_velocity=0.0;
step=114;
}
/***Cylinder***/
// if((x>3030.0+deff)&&(CStep==1)) {
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;
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;
}
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
// mesure_state(); /*位置測定*/
// move_following(); /*移動制御*/
if(cal){
cal=0;
move_following();
}
}
else if(!autoflag) {
flaga=0;
#ifdef IM920
manualMoveIM920(); /*analogStick*/
manualIM920(); /*IM920 button*/
#else
manualMovePS3(); /*analogStick*/
manualPS3(); /*PS3 button*/
#endif
#ifdef MESURE
if(down){
IndicatorAuto=0;
fclose(fp_r);
}
time+=0.01;
#endif
}
/***update state***/
#ifdef IM920
readIM920();
#endif
// mesureSwing();
pc.printf("x:%f,y:%f\r\n",x,y);
// wait(RATE);
// pc.printf("%f\r\n",(float)((((2.0 * PI) / swingRadVelocity) / 4.0)));
// 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 %f\r\n",cont,swingRadVelocity, (double)SwingSens.getPulses());
}
}