![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Main Program
Dependencies: mbed AQM1602 HMC6352 PID
Diff: main_processing/strategy/strategy2.cpp
- Revision:
- 42:02aaa806d929
- Child:
- 45:c23f25c00d0d
diff -r 3cdcdec9c885 -r 02aaa806d929 main_processing/strategy/strategy2.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main_processing/strategy/strategy2.cpp Thu Feb 04 20:45:41 2016 +0000 @@ -0,0 +1,179 @@ +#include "mbed.h" +#include "extern.h" + +//Debug +void modeDebug0(void){ + return; +} +void modeDebug1(void){//ChaseOnly NonLine +int vx,vy,vs; + 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; + } + + + 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; + + if((data.lnFlag[A_SPOT]==1)&&(1)){ + moveLnFlag[A_SPOT]=0; + moveLnlog[A_SPOT] = data.motorlog[X_AXIS]; + } + if((data.lnFlag[B_SPOT]==1)&&(1)){ + moveLnFlag[B_SPOT]=0; + moveLnlog[B_SPOT] = data.motorlog[X_AXIS]; + } + if((data.lnFlag[C_SPOT]==1)&&(1)){ + moveLnFlag[C_SPOT]=0; + moveLnlog[C_SPOT] = data.motorlog[Y_AXIS]; + } + if((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1)){ + moveLnFlag[AB_SPOT]=0; + moveLnlog[AB_SPOT] = data.motorlog[Y_AXIS]; + } + + if(((data.motorlog[X_AXIS]-moveLnlog[A_SPOT])<(-RELEASE_VAL))&&(moveLnFlag[A_SPOT]==0)) moveLnFlag[A_SPOT]=1; + if(((data.motorlog[X_AXIS]-moveLnlog[B_SPOT])>(RELEASE_VAL))&&(moveLnFlag[B_SPOT]==0)) moveLnFlag[B_SPOT]=1; + 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)&&(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))); + + move( + vx*LineStop[X_AXIS], + vy*LineStop[Y_AXIS], + vs + ); + if(data.MotorFlag==1){ + tx_motor(); + data.MotorFlag=0; + } + return; +} +void modeDebug4(void){//solenoid + if(data.KickFlag==1){ + DriveSolenoid(); + } + return; +} +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; +}