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:
- DeguNaoto
- Date:
- 2015-10-17
- Revision:
- 102:e483c7453f52
- Parent:
- 101:b67d33e56b66
File content as of revision 102:e483c7453f52:
/**
* This program is written in main micro computer "mbed" for 2015 NHK Robot Contest (Bteam).
*/
//速度コントローラと向きコントローラはそのまま
//大和田にPIC1のシリンダの信号をタイマーで書かせる
//回転機構wait PI/4 追加で待つ
//射出をスキップする機能を追加
//スタートゾーンにマシンを戻した後に暴走しないようにする
//手動時の操作性の改善
//一度手動モードになったらスタートゾーンに戻るまで再び自動モードにならないようにする
//flagfで前進・後退切り替え
//大和田射角
//午後に一定回転制御の実験
//モード切替時に時間を待つ
/***コース選択***/
#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"
Serial pc(USBTX, USBRX);
#ifdef MESURE
LocalFileSystem local("local");
#endif
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;
#ifdef MESURE
FILE *fp_r = fopen("/local/velocity.dat", "w");
double time=0.0;
#endif
wait(0.3);
sendData(7,255);
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)) {
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>3000.0)&&(CStep==1)) {
if(!skip) sendData(1,1);
CStep=2;
}
if((x>5890.0)&&(CStep==2)) {
if(!skip) sendData(1,3);
CStep=3;
}
if((x>7650.0)&&(CStep==3)) {
if(!skip) sendData(1,2);
CStep=4;
}
if((x<6730.0)&&(CStep==4)) {
if(!skip) sendData(1,5);
CStep=5;
}
if((x<6170.0)&&(CStep==5)) {
if(!skip) sendData(1,4);
CStep=6;
}
if((x<4000.0)&&(CStep==6)){
sendData(7,255);
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)&&(CStep==7)){
sendData(1,5);
CStep=8;
}
if((x>5820.0)&&(CStep==8)){
sendData(1,4);
CStep=9;
}
if((x<4000.0)&&(CStep==9)){
sendData(7,255);
CStep=114;
}
/////////////////////////////////////////////////////////////////////////
/*****************************Opponents Mode****************************/
if(dspeed>0.0){
dspeed-=speed/50.0;
targ_velocity=dspeed;
}
if((step==15)&&((5700.0>x)&&(x>60.0))) {
targ_sita=0.025;
step=16;
}
// if((step==16)&&(x>3200.0)){
// if((step==16)&&(x>2850.0)){
if((step==16)&&(x>1520.0)){
targ_velocity=0.0;
// dspeed=speed;
step=17;
}
// if((step==17)&&((velocity<5000.0)&&(velocity>-5000.0))){
if((step==17)&&((velocity<1000.0)&&(velocity>-1000.0))){
flagf=0;
spcount=0.0;
targ_sita=0.035;
step=18;
dspeed=0.0;
}
if((step==18)&&(x<-100.0)){
targ_velocity=0.0;
targ_sita=PI/4.0;
step=114;
}
if((x>1570.0)&&(CStep==15)){
sendData(1,5);
CStep=16;
}
if((x<1300.0)&&(CStep==16)){
sendData(7,255);
CStep=114;
}
/*if((x>3000.0)&&(CStep==16)){
sendData(1,4);
CStep=17;
}
if((x<1300.0)&&(CStep==17)){
sendData(7,255);
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.015;
step=1;
}
if((step==1)&&(x>8600.0)) {
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.02;
}
if((step==3)&&(x<2050.0)) {
targ_sita=-PI/4;
step=4;
sendData(7,255);
}
if((step==4)&&(x<0.0)) {
targ_velocity=0.0;
step=114;
}
if((x>3100.0)&&(CStep==1)) {
if(!skip) sendData(1,1);
CStep=2;
}
if((x>5940.0)&&(CStep==2)) {
if(!skip) sendData(1,2);
CStep=3;
}
if((x>7550.0)&&(CStep==3)) {
if(!skip) sendData(1,3);
CStep=6;
}
if((x<6700.0)&&(CStep==4)) {
if(!skip) sendData(1,4);
CStep=5;
}
if((x<6160.0)&&(CStep==5)) {
if(!skip) sendData(1,5);
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;
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;
sendData(7,255);
}
if((step==9)&&(x<-10.0)) {
targ_velocity=0.0;
step=114;
}
if((x>4620.0)&&(CStep==6)){
sendData(1,4);
CStep=7;
}
if((x>5720.0)&&(CStep==7)){
sendData(1,5);
CStep=114;
}
/////////////////////////////////////////////////////////////////////////
/*****************************Opponents Mode****************************/
if((step==15)&&((5700.0>x)&&(x>70.0))) {
targ_sita=0.0;
step=16;
}
if((step==16)&&(x>2800.0)){
targ_velocity=0.0;
step=17;
}
if((step==17)&&((velocity<500.0)&&(velocity>-500.0))){
flagf=0;
spcount=0.0;
targ_sita=0.0;
step=18;
}
if((step==18)&&(x<-100.0)){
targ_velocity=0.0;
targ_sita=-PI/4.0;
step=114;
}
if((x>950.0)&&(CStep==15)){
sendData(1,4);
CStep=16;
}
if((x>2350.0)&&(CStep==16)){
sendData(1,5);
CStep=114;
}
/////////////////////////////////////////////////////////////////////////
#endif
mesure_state(); /*位置測定*/
move_following(); /*移動制御*/
}
else if(!autoflag) {
#ifdef IM920
manualMoveIM920(); /*analogStick*/
manualIM920(); /*IM920 button*/
#else
manualMovePS3(); /*analogStick*/
manualPS3(); /*PS3 button*/
#endif
//Swing
swingFollowing();
flaga=0;
#ifdef MESURE
if(down){
IndicatorAuto=0;
fclose(fp_r);
}
fprintf(fp_r, "time:%1.3f, %f[rad/s], pwm:%f\r\n",time,swingRadVelocity,cont);
time+=0.01;
#endif
}
/***update state***/
#ifdef IM920
readIM920();
#endif
mesureSwing();
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());
}
}
