strategy2.cpp

00001 #include "mbed.h"
00002 #include "extern.h"
00003 
00004 //Debug
00005 void modeDebug0(void){
00006     return;
00007 }
00008 void modeDebug1(void){//ChaseOnly NonLine
00009 int vx,vy,vs;
00010     if(data.IrFlag==1){
00011         ReadIr();
00012         data.IrFlag=0;
00013     }
00014     if(data.PidFlag==1){
00015         PidUpdate();
00016         data.PidFlag=0;
00017     }
00018     
00019     vx = ir_move_val[data.irNotice][data.irPosition][IR_X];
00020     vy = ir_move_val[data.irNotice][data.irPosition][IR_Y];
00021     vs = data.OutputPID;
00022     if(data.irPosition<8){
00023         vx *= data.l_pow;
00024         vy *= data.l_pow;
00025     }
00026     else{
00027         vx *= data.s_pow;
00028         vy *= data.s_pow;
00029     }
00030     
00031     
00032     move(vx, vy, vs);
00033     
00034     if(data.MotorFlag==1){
00035         tx_motor();
00036         data.MotorFlag=0;
00037     }
00038     return;
00039 }
00040 void modeDebug2(void){//LineRestoringForce
00041     int vx,vy,vs;
00042     uint8_t LineStop[2];
00043     int LineForce[2];
00044     if(data.IrFlag==1){
00045         ReadIr();
00046         data.IrFlag=0;
00047     }
00048     if(data.PidFlag==1){
00049         PidUpdate();
00050         data.PidFlag=0;
00051     }
00052     vx = ir_move_val[data.irNotice][data.irPosition][IR_X];
00053     vy = ir_move_val[data.irNotice][data.irPosition][IR_Y];
00054     vs = data.OutputPID;
00055     if(data.irPosition<8){
00056         vx *= data.l_pow;
00057         vy *= data.l_pow;
00058     }
00059     else{
00060         vx *= data.s_pow;
00061         vy *= data.s_pow;
00062     }
00063     
00064     
00065     LineStop[X_AXIS] = (!((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1))))*(!((vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1))));
00066     LineStop[Y_AXIS] = (!((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1))))*(!((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1))));
00067     
00068     LineForce[X_AXIS] = LineForce[Y_AXIS] = 0;
00069     if(((vx>0)&&((data.lnFlag[A_SPOT]==1)&&(1)))){
00070         LineForce[X_AXIS] = -LINE_RF;
00071     }
00072     if(((vx<0)&&((data.lnFlag[B_SPOT]==1)&&(1)))){
00073         LineForce[X_AXIS] = LINE_RF;
00074     }
00075     if(((vy>0)&&((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1)))){
00076         LineForce[Y_AXIS] = -LINE_RF;
00077     }
00078     if(((vy<0)&&((data.lnFlag[C_SPOT]==1)&&(1)))){
00079         LineForce[Y_AXIS] = LINE_RF;
00080     }
00081     
00082     move(
00083         vx*LineStop[X_AXIS] + LineForce[X_AXIS],
00084         vy*LineStop[Y_AXIS] + LineForce[Y_AXIS],
00085         vs
00086     );
00087     if(data.MotorFlag==1){
00088         tx_motor();
00089         data.MotorFlag=0;
00090     }
00091     return;
00092 }
00093 void modeDebug3(void){//movesum
00094     int vx,vy,vs;
00095     uint8_t LineStop[2];
00096     static uint8_t moveLnFlag[4]={1, 1, 1, 1};
00097     static int moveLnlog[4];
00098     if(data.IrFlag==1){
00099         ReadIr();
00100         data.IrFlag=0;
00101     }
00102     if(data.PidFlag==1){
00103         PidUpdate();
00104         data.PidFlag=0;
00105     }
00106     vx = ir_move_val[data.irNotice][data.irPosition][IR_X];
00107     vy = ir_move_val[data.irNotice][data.irPosition][IR_Y];
00108     vs = data.OutputPID;
00109     if(data.irPosition<8){
00110         vx *= data.l_pow;
00111         vy *= data.l_pow;
00112     }
00113     else{
00114         vx *= data.s_pow;
00115         vy *= data.s_pow;
00116     }
00117     
00118     data.motorlog[X_AXIS] += vx;
00119     data.motorlog[Y_AXIS] += vy;
00120     
00121     if((data.lnFlag[A_SPOT]==1)&&(1)){
00122         moveLnFlag[A_SPOT]=0;
00123         moveLnlog[A_SPOT] = data.motorlog[X_AXIS];
00124     }
00125     if((data.lnFlag[B_SPOT]==1)&&(1)){
00126         moveLnFlag[B_SPOT]=0;
00127         moveLnlog[B_SPOT] = data.motorlog[X_AXIS];
00128     }
00129     if((data.lnFlag[C_SPOT]==1)&&(1)){
00130         moveLnFlag[C_SPOT]=0;
00131         moveLnlog[C_SPOT] = data.motorlog[Y_AXIS];
00132     }
00133     if((data.lnFlag[A_SPOT]==1)&&(data.lnFlag[B_SPOT]==1)){
00134         moveLnFlag[AB_SPOT]=0;
00135         moveLnlog[AB_SPOT] = data.motorlog[Y_AXIS];
00136     }
00137     
00138     if(((data.motorlog[X_AXIS]-moveLnlog[A_SPOT])<(-RELEASE_VAL))&&(moveLnFlag[A_SPOT]==0)) moveLnFlag[A_SPOT]=1;
00139     if(((data.motorlog[X_AXIS]-moveLnlog[B_SPOT])>(RELEASE_VAL))&&(moveLnFlag[B_SPOT]==0)) moveLnFlag[B_SPOT]=1;
00140     if(((data.motorlog[Y_AXIS]-moveLnlog[C_SPOT])>(RELEASE_VAL))&&(moveLnFlag[C_SPOT]==0)) moveLnFlag[C_SPOT]=1;
00141     if(((data.motorlog[Y_AXIS]-moveLnlog[AB_SPOT])<(-RELEASE_VAL))&&(moveLnFlag[AB_SPOT]==0)) moveLnFlag[AB_SPOT]=1;
00142     
00143     LineStop[X_AXIS] = (!((vx>0)&&(moveLnFlag[A_SPOT]==0)))*(!((vx<0)&&(moveLnFlag[B_SPOT]==0)));
00144     LineStop[Y_AXIS] = (!((vy>0)&&(moveLnFlag[C_SPOT]==0)))*(!((vy<0)&&(moveLnFlag[AB_SPOT]==0)));
00145     
00146     move(
00147         vx*LineStop[X_AXIS],
00148         vy*LineStop[Y_AXIS],
00149         vs
00150     );
00151     if(data.MotorFlag==1){
00152         tx_motor();
00153         data.MotorFlag=0;
00154     }
00155     return;
00156 }
00157 void modeDebug4(void){//solenoid
00158     if(data.KickFlag==1){
00159         DriveSolenoid();
00160     }
00161     return;
00162 }
00163 void modeDebug5(void){//cmpsTest
00164     if(data.IrFlag==1){
00165         ReadIr();
00166         data.IrFlag=0;
00167     }
00168     if(data.PidFlag==1){
00169         PidUpdate();
00170         data.PidFlag=0;
00171     }
00172     move(0,0,10);
00173     LED[0]=1;
00174     LED[3]=1;
00175     if(data.MotorFlag==1){
00176         tx_motor();
00177         data.MotorFlag=0;
00178     }
00179     
00180     return;
00181 }