CatPot 2015-2016
Main Program
Dependencies: mbed AQM1602 HMC6352 PID
Diff: main_processing/strategy/strategy.cpp
- Revision:
- 28:8ac6c3c1e643
- Parent:
- 27:769cb5a7ea37
- Child:
- 29:b413b0bb07a1
--- a/main_processing/strategy/strategy.cpp Sun Jan 31 15:46:19 2016 +0000
+++ b/main_processing/strategy/strategy.cpp Mon Feb 01 02:05:44 2016 +0000
@@ -3,13 +3,90 @@
//Atk
void modeAttack0(void){
+ double ir_x, ir_y;
int vx,vy,vs;
+ uint8_t LineDir[4];
uint8_t LineStop[2];
- static uint8_t moveLnFlag[4]={1, 1, 1, 1};
- static int moveLnlog[4];
- //int LineForce[2];
- //int Return;
- //int line
+ uint8_t IrRange[4];
+ uint8_t LineBind[4];
+ if(data.IrFlag==1){
+ ReadIr();
+ data.IrFlag=0;
+ }
+ if(data.PidFlag==1){
+ Line_ticker.detach();
+ PidUpdate();
+ Line_ticker.attach(&ReadLine, 0.005);
+ data.PidFlag=0;
+ }
+ ir_x = ir_move_val[data.irNotice][data.irPosition][IR_X];
+ ir_y = ir_move_val[data.irNotice][data.irPosition][IR_Y];
+ if(data.irPosition<8){
+ ir_x *= data.l_pow;
+ ir_y *= data.l_pow;
+ }
+ else{
+ ir_x *= data.s_pow;
+ ir_y *= data.s_pow;
+ }
+
+ LineDir[A_SPOT] = (!((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==0))));
+ LineDir[B_SPOT] = (!((vx<0)&&((data.lnFlag[A_SPOT]==0)&&(data.lnFlag[B_SPOT]==1))));
+ LineDir[C_SPOT] = (!((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1))));
+ LineDir[AB_SPOT] = (!((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))));
+
+ LineStop[X_AXIS] = LineDir[A_SPOT]*LineDir[B_SPOT];
+ LineStop[Y_AXIS] = LineDir[C_SPOT]*LineDir[AB_SPOT];
+
+
+
+ IrRange[A_SPOT] = (data.irPosition==19)||(data.irPosition==8)||(data.irPosition==9);
+ IrRange[B_SPOT] = (data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12);
+ IrRange[C_SPOT] = (data.irPosition==13)||(data.irPosition==14)||(data.irPosition==15);
+ IrRange[AB_SPOT] = (data.irPosition==16)||(data.irPosition==17)||(data.irPosition==18);
+
+ LineBind[A_SPOT] = ((IrRange[A_SPOT]==1)&&((LineDir[A_SPOT]==0)||(LineBind[A_SPOT]==1)));
+ LineBind[B_SPOT] = ((IrRange[B_SPOT]==1)&&((LineDir[B_SPOT]==0)||(LineBind[B_SPOT]==1)));
+ LineBind[C_SPOT] = ((IrRange[C_SPOT]==1)&&((LineDir[C_SPOT]==0)||(LineBind[C_SPOT]==1)));
+ LineBind[AB_SPOT] = ((IrRange[AB_SPOT]==1)&&((LineDir[AB_SPOT]==0)||(LineBind[AB_SPOT]==1)));
+
+ vx = ir_x*(!(LineBind[A_SPOT]==1))*(!(LineBind[B_SPOT]==1))*(!(LineBind[C_SPOT]==1))*(!(LineBind[AB_SPOT]==1));
+ vy = ir_y*(!(LineBind[A_SPOT]==1))*(!(LineBind[B_SPOT]==1))*(!(LineBind[C_SPOT]==1))*(!(LineBind[AB_SPOT]==1));;
+ vs = data.OutputPID;
+
+ move(
+ vx*LineStop[X_AXIS] + (LINE_RF)*(vx!=0)*(LineDir[C_SPOT]==0) + (-LINE_RF)*(vx!=0)*(LineDir[AB_SPOT]==0),
+ vy*LineStop[Y_AXIS] + (-LINE_RF)*(vy!=0)*(LineDir[A_SPOT]==0) + (LINE_RF)*(vy!=0)*(LineDir[B_SPOT]==0),
+ vs
+ );
+
+ if(data.MotorFlag==1){
+ tx_motor();
+ data.MotorFlag=0;
+ }
+ return;
+}
+void modeAttack1(void){
+ return;
+}
+void modeAttack2(void){
+ return;
+}
+void modeAttack3(void){
+ return;
+}
+void modeAttack4(void){
+ return;
+}
+void modeAttack5(void){
+ return;
+}
+//Debug
+void modeDebug0(void){
+ return;
+}
+void modeDebug1(void){//ChaseOnly NonLine
+int vx,vy,vs;
if(data.IrFlag==1){
ReadIr();
data.IrFlag=0;
@@ -18,16 +95,105 @@
PidUpdate();
data.PidFlag=0;
}
- //move(30,30,data.OutputPID);
- //move(30,0,data.OutputPID);
- /*move(
- ir_move_val[data.irNotice][data.irPosition][IR_X],
- ir_move_val[data.irNotice][data.irPosition][IR_Y],
- data.OutputPID
- );*/
+
+ vx = ir_move_val[data.irNotice][data.irPosition][IR_X];
+ vy = ir_move_val[data.irNotice][data.irPosition][IR_Y];
+ vs = data.OutputPID;
+ if(data.irPosition<8){
+ vx *= data.l_pow;
+ vy *= data.l_pow;
+ }
+ else{
+ vx *= data.s_pow;
+ vy *= data.s_pow;
+ }
+
+
+ move(vx, vy, vs);
+
+ if(data.MotorFlag==1){
+ tx_motor();
+ data.MotorFlag=0;
+ }
+ return;
+}
+void modeDebug2(void){//LineRestoringForce
+ int vx,vy,vs;
+ uint8_t LineStop[2];
+ int LineForce[2];
+ if(data.IrFlag==1){
+ ReadIr();
+ data.IrFlag=0;
+ }
+ if(data.PidFlag==1){
+ PidUpdate();
+ data.PidFlag=0;
+ }
vx = ir_move_val[data.irNotice][data.irPosition][IR_X];
vy = ir_move_val[data.irNotice][data.irPosition][IR_Y];
vs = data.OutputPID;
+ if(data.irPosition<8){
+ vx *= data.l_pow;
+ vy *= data.l_pow;
+ }
+ else{
+ vx *= data.s_pow;
+ vy *= data.s_pow;
+ }
+
+
+ LineStop[X_AXIS] = (!((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1))))*(!((vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1))));
+ LineStop[Y_AXIS] = (!((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))))*(!((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1))));
+
+ LineForce[X_AXIS] = LineForce[Y_AXIS] = 0;
+ if(((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1)))){
+ LineForce[X_AXIS] = -LINE_RF;
+ }
+ if(((vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1)))){
+ LineForce[X_AXIS] = LINE_RF;
+ }
+ if(((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1)))){
+ LineForce[Y_AXIS] = -LINE_RF;
+ }
+ if(((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1)))){
+ LineForce[Y_AXIS] = LINE_RF;
+ }
+
+ move(
+ vx*LineStop[X_AXIS] + LineForce[X_AXIS],
+ vy*LineStop[Y_AXIS] + LineForce[Y_AXIS],
+ vs
+ );
+ if(data.MotorFlag==1){
+ tx_motor();
+ data.MotorFlag=0;
+ }
+ return;
+}
+void modeDebug3(void){//movesum
+ int vx,vy,vs;
+ uint8_t LineStop[2];
+ static uint8_t moveLnFlag[4]={1, 1, 1, 1};
+ static int moveLnlog[4];
+ if(data.IrFlag==1){
+ ReadIr();
+ data.IrFlag=0;
+ }
+ if(data.PidFlag==1){
+ PidUpdate();
+ data.PidFlag=0;
+ }
+ vx = ir_move_val[data.irNotice][data.irPosition][IR_X];
+ vy = ir_move_val[data.irNotice][data.irPosition][IR_Y];
+ vs = data.OutputPID;
+ if(data.irPosition<8){
+ vx *= data.l_pow;
+ vy *= data.l_pow;
+ }
+ else{
+ vx *= data.s_pow;
+ vy *= data.s_pow;
+ }
data.motorlog[X_AXIS] += vx;
data.motorlog[Y_AXIS] += vy;
@@ -54,9 +220,6 @@
if(((data.motorlog[Y_AXIS]-moveLnlog[C_SPOT])>(RELEASE_VAL))&&(moveLnFlag[C_SPOT]==0)) moveLnFlag[C_SPOT]=1;
if(((data.motorlog[Y_AXIS]-moveLnlog[AB_SPOT])<(-RELEASE_VAL))&&(moveLnFlag[AB_SPOT]==0)) moveLnFlag[AB_SPOT]=1;
- //LineStop[X_AXIS] = (!((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1))))*(!((vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1))));
- //LineStop[Y_AXIS] = (!((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))))*(!((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1))));
-
LineStop[X_AXIS] = (!((vx>0)&&(moveLnFlag[A_SPOT]==0)))*(!((vx<0)&&(moveLnFlag[B_SPOT]==0)));
LineStop[Y_AXIS] = (!((vy>0)&&(moveLnFlag[C_SPOT]==0)))*(!((vy<0)&&(moveLnFlag[AB_SPOT]==0)));
@@ -65,72 +228,32 @@
vy*LineStop[Y_AXIS],
vs
);
- /*
- LineForce[X_AXIS] = LineForce[Y_AXIS] = 0;
- if(((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1)))){
- LineForce[X_AXIS] = -LINE_RF;
- }
- if(((vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1)))){
- LineForce[X_AXIS] = LINE_RF;
- }
- if(((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1)))){
- LineForce[Y_AXIS] = -LINE_RF;
- }
- if(((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1)))){
- LineForce[Y_AXIS] = LINE_RF;
- }
-
- move(
- vx*LineStop[X_AXIS] + LineForce[X_AXIS],
- vy*LineStop[Y_AXIS] + LineForce[Y_AXIS],
- vs
- );*/
-
- /*
- move(
- 0,//vx*LineStop[X_AXIS] + LineForce[X_AXIS],
- 0,//vy*LineStop[Y_AXIS] + LineForce[Y_AXIS],
- vs
- );
- */
if(data.MotorFlag==1){
tx_motor();
data.MotorFlag=0;
}
- //move(0,0,data.OutputPID);
- return;
-}
-void modeAttack1(void){
return;
}
-void modeAttack2(void){
- return;
-}
-void modeAttack3(void){
- return;
-}
-void modeAttack4(void){
- return;
-}
-void modeAttack5(void){
+void modeDebug4(void){//solenoid
+ if(data.KickFlag==1){
+ DriveSolenoid();
+ }
return;
}
-//Debug
-void modeDebug0(void){
- return;
-}
-void modeDebug1(void){
- return;
-}
-void modeDebug2(void){
+void modeDebug5(void){//cmpsTest
+ if(data.IrFlag==1){
+ ReadIr();
+ data.IrFlag=0;
+ }
+ if(data.PidFlag==1){
+ PidUpdate();
+ data.PidFlag=0;
+ }
+ move(0,0,data.OutputPID);
+ if(data.MotorFlag==1){
+ tx_motor();
+ data.MotorFlag=0;
+ }
+
return;
}
-void modeDebug3(void){
- return;
-}
-void modeDebug4(void){
- return;
-}
-void modeDebug5(void){
- return;
-}