Diff: main_processing/strategy/strategy2.cpp

Revision:

42:02aaa806d929

Child:

45:c23f25c00d0d

--- /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;
+}