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.
autoMode.h
- Committer:
- DeguNaoto
- Date:
- 2015-10-31
- Revision:
- 3:8d8c25c556ae
- Parent:
- 2:738b28f6a04b
- Child:
- 6:ca3a74a93ae2
File content as of revision 3:8d8c25c556ae:
#ifndef AUTOMODE_H
#define AUTOMODE_H
/***PID Controller***/
//PID velocity_controller(36.0,5274.0 ,0.0,RATE);
//PID direction_controller(36.0,3.0,0.0,RATE);
PID velocity_controller(9.0,5274.0,0.0,RATE);
PID direction_controller(36.0,3.0,0.0,RATE);
Timeout OpStart;
void OpponentsStart(){
spcount = 0.0;
step = 15;
CStep = 15;
flaga = 1;
flagf = 1;
}
#ifdef IM920
/***IM920 correspondence***/
void autoIM920() {
if(b==7){ /*mode change*/
if(edge7) {
Com.detach();
edge7=0;
autoflag=0;
Indicator4=0;
IndicatorAuto=1;
flaga=0;
Move_l(0.0);
Move_r(0.0);
}
}
else if((b==6)&&(!flaga)){ /*start*/
if(edge6){
edge6=0;
resetState();
flagf=1;
spcount=0.0;
direction_controller.setBias(0.0);
// targ_velocity=speed;
#ifdef BLUE
// sendData(5,69); //right
// sendData(5,70); //right
sendData(5,71); //right
wait(0.05);
sendData(4,69); //left
stateR = 69;
stateL = 69;
#else
sendData(5,69); //right
wait(0.05);
sendData(4,69); //left
stateR = 69;
stateL = 69;
#endif
wait(0.05);
step = 0;
CStep = 1;
flaga = 1;
}
}
else if((b==5)&&(!flaga)){ /*middle start*/
if(edge5){
edge5=0;
resetState();
flagf = 1;
spcount=0.0;
direction_controller.setBias(0.0);
// targ_velocity=speed;
#ifdef BLUE
sendData(5,60); //right
wait(0.05);
sendData(4,60); //left
stateR = 60;
stateL = 61;
#else
sendData(5,59); //right
wait(0.05);
sendData(4,59); //left
stateR = 59;
stateL = 59;
#endif
wait(0.05);
step = 5;
CStep = 7;
flaga = 1;
}
}
else if((b==8)&&(!flaga)){ /*opponents start*/
if(edge8){
edge8=0;
resetState();
direction_controller.setBias(0.0);
// targ_velocity = speed;
#ifdef BLUE
sendData(5,69); //right
wait(0.05);
sendData(4,69); //left
stateR = 71;
stateL = 71;
#else
/*sendData(5,66); //right
wait(0.05);
sendData(4,66); //left*/
/*sendData(4,71); //left
wait(0.05);
sendData(5,71); //right*/
sendData(4,73); //left
wait(0.05);
sendData(5,73); //right
stateR = 71;
stateL = 72;
#endif
wait(0.05);
step=114;
CStep=114;
OpStart.attach(&OpponentsStart,2.2);
// OpStart.attach(&OpponentsStart,2.0);
}
}
else if(b==1){ /*L down*/
if(edge1){
edge1=0;
if(stateL!=1) stateL--;
sendData(4,stateL);
}
}
else if(b==2){ /*L up*/
if(edge2){
edge2=0;
if(stateL!=MAX_VALUE) stateL++;
sendData(4,stateL);
}
}
else if(b==3){ /*R down*/
if(edge3){
edge3=0;
if(stateR!=1) stateR--;
sendData(5,stateR);
}
}
else if(b==4){ /*R up*/
if(edge4){
edge4=0;
if(stateR!=MAX_VALUE) stateR++;
sendData(5,stateR);
}
}
if(a2){
skip = 1;
}
else if(!a2){
skip = 0;
}
if(b!=1) edge1=1;
if(b!=2) edge2=1;
if(b!=3) edge3=1;
if(b!=4) edge4=1;
if(b!=5) edge5=1;
if(b!=6) edge6=1;
if(b!=7) edge7=1;
if(b!=8) edge8=1;
if(b!=9) edge9=1;
}
#else
void autoPS3(){
if(circle){ /*mode change*/
if(edge_circle){
edge_circle=0;
autoflag=0;
Indicator4=0;
IndicatorAuto=1;
flaga=0;
Move_l(0.0);
Move_r(0.0);
}
}
else if(triangle){ /*start*/
if(edge_triangle){
edge_triangle=0;
resetState();
flagf=1;
spcount=0.0;
// targ_velocity=speed;
#ifdef BLUE
// sendData(5,69); //right
sendData(5,71); //right
wait(0.05);
sendData(4,69); //left
stateR = 69;
stateL = 69;
#else
sendData(5,69); //right
wait(0.05);
sendData(4,69); //left
stateR = 69;
stateL = 69;
#endif
wait(0.05);
step = 0;
CStep = 1;
flaga = 1;
}
}
else if(square){ /*middle start*/
if(edge_square){
edge_square=0;
resetState();
flagf = 1;
spcount=0.0;
// targ_velocity=speed;
#ifdef BLUE
sendData(5,60); //right
wait(0.05);
sendData(4,60); //left
stateR = 60;
stateL = 61;
#else
sendData(5,59); //right
wait(0.05);
sendData(4,59); //left
stateR = 61;
stateL = 60;
#endif
wait(0.05);
step = 5;
CStep = 7;
flaga = 1;
}
}
else if(cross){ /*opponents start*/
if(edge_cross){
edge_cross=0;
resetState();
// targ_velocity = speed;
#ifdef BLUE
sendData(5,69); //right
wait(0.05);
sendData(4,69); //left
stateR = 71;
stateL = 71;
#else
/*sendData(5,66); //right
wait(0.05);
sendData(4,66); //left*/
/*sendData(4,71); //left
wait(0.05);
sendData(5,71); //right*/
sendData(4,73); //left
wait(0.05);
sendData(5,73); //right
stateR = 71;
stateL = 72;
#endif
wait(0.05);
step=114;
CStep=114;
OpStart.attach(&OpponentsStart,2.2);
// OpStart.attach(&OpponentsStart,2.0);
}
}
else if(l1){ /*L up*/
if(edge_l1){
edge_l1=0;
if(stateL!=92) stateL++;
sendData(4,stateL);
}
}
else if(l2){ /*L down*/
if(edge_l2){
edge_l2=0;
if(stateL!=1) stateL--;
sendData(4,stateL);
}
}
else if(r1){ /*R up*/
if(edge_r1){
edge_r1=0;
if(stateR!=92) stateR++;
sendData(5,stateR);
}
}
else if(r2){ /*R down*/
if(edge_r2){
edge_r2=0;
if(stateR!=1) stateR--;
sendData(5,stateR);
}
}
if(up){
skip = 1;
}
else if(!up){
skip = 0;
}
}
#endif
/***The function is PID controller initialize.***/
inline void initializeControllers()
{
velocity_controller.setInputLimits(-20000.0, 20000.0); //x1
direction_controller.setInputLimits(-PI, PI); //x2
//Pwm output from MOVE_OUTPUT_LIMIT_BOTTOM to MOVE_OUTPUT_LIMIT_TOP
velocity_controller.setOutputLimits(0.0, 1.0);
direction_controller.setOutputLimits(-1.0, 1.0);
//set bias. 初期値
velocity_controller.setBias(0.0);
direction_controller.setBias(0.0);
//set mode.
velocity_controller.setMode(AUTO_MODE);
direction_controller.setMode(AUTO_MODE);
}
/***The function is following move speed.***/
inline void velocity_following()
{
velocity_controller.setSetPoint((float)targ_velocity);
velocity_controller.setProcessValue((float)velocity);
x1 = (double)velocity_controller.compute();
}
inline void sita_following()
{
direction_controller.setSetPoint((float)targ_sita);
direction_controller.setProcessValue((float)sita);
x2 = (double)direction_controller.compute();
}
inline void move_following()
{
velocity_following();
sita_following();
if(flagf==0){
// Vr = ( 2.0*(-x1) + x2 ) / 3.0;
// Vl = ( 2.0*(-x1) - x2 ) / 3.0;
/*if(x2>0.0){
Vr = -x1;
Vl = -x1 - x2;
}
else{
Vr = -x1 + x2;
Vl = -x1;
}*/
if(x2>0.0){
Vr = -x1 + x2;
Vl = -x1;
}
else{
Vr = -x1;
Vl = -x1 - x2;
}
}
else if(flagf==1){
if(x2>0.0){
Vr = x1;
Vl = x1 - x2;
}
else{
Vr = x1 + x2;
Vl = x1;
}
}
Move_r( ( float ) Vr );
Move_l( ( float ) Vl );
}
#endif /*autoMode.h*/