Revision 32:367b16d69a32, committed 2016-03-24

Comitter:

lilac0112_1

Date:

Thu Mar 24 04:54:39 2016 +0000

Parent:

31:745a775cfc20

Child:

33:aa115c30892e

Commit message:

turn start are renewed

Changed in this revision

--- a/main_processing/setup_command_active/command.cpp	Wed Mar 23 13:01:43 2016 +0000
+++ b/main_processing/setup_command_active/command.cpp	Thu Mar 24 04:54:39 2016 +0000
@@ -72,6 +72,13 @@
         }
         sys.jump_flag=JUMP_TAG_MAX;
     }
+    
+    ReadCmps();
+    //cmps_set.CmpsInitialValue = cmps_set.cmps;
+    wait_ms(10);
+    cmps_set.CmpsDiff = REFERENCE - cmps_set.cmps;
+    cmps_set.FrontDeg=0;
+    
     LineKeeper=LINE_FREE;//line
     return 0;
 }

--- a/main_processing/setup_command_active/command_functions.cpp	Wed Mar 23 13:01:43 2016 +0000
+++ b/main_processing/setup_command_active/command_functions.cpp	Thu Mar 24 04:54:39 2016 +0000
@@ -34,6 +34,8 @@
     sys.TurnStartFlag=0;
     Active();
     sys.strategy=temp;
+    //reset
+    NVIC_SystemReset();
     return 1;
 }
 uint8_t TurnAndStart(uint8_t x){
@@ -46,6 +48,8 @@
     sys.TurnStartFlag=1;
     Active();
     sys.strategy=temp;
+    //reset
+    NVIC_SystemReset();
     return 1;
 }
 uint8_t GetIr(uint8_t x){

--- a/main_processing/setup_command_active/setup.cpp	Wed Mar 23 13:01:43 2016 +0000
+++ b/main_processing/setup_command_active/setup.cpp	Thu Mar 24 04:54:39 2016 +0000
@@ -45,6 +45,10 @@
     
     sys.HomeBlind=1;
     sys.DriBlind=1;
+    sys.TurnAtkBlind=0;
+    sys.TurnDriBlind=0;
+    sys.TurnHoldBlind=0;
+    sys.KickBlind=0;
     //defence
     sys.DefenceFlag=0;
     ////////important
@@ -62,12 +66,20 @@
     motor.printf("1F0002F0003F0004F000\r\n");       //モータ停止
     //compass
     hmc_reset=HMC_RUN;
-    for(int i=0; i<5; i++){
+    /*for(int i=0; i<5; i++){
         ReadCmps();
         cmps_set.CmpsInitialValue = cmps_set.cmps;
         wait_ms(10);
     }
-    cmps_set.CmpsDiff = REFERENCE - cmps_set.CmpsInitialValue;
+    cmps_set.CmpsDiff = REFERENCE - cmps_set.CmpsInitialValue;*/
+    for(int i=0; i<5; i++){
+        ReadCmps();
+        cmps_set.CmpsInitialValue = cmps_set.cmps;
+        cmps_set.CmpsInitialValue0 = cmps_set.cmps;
+        wait_ms(10);
+    }
+    cmps_set.CmpsDiff = REFERENCE - cmps_set.cmps;
+    
     cmps_set.FrontDeg=0;
     cmps_set.AtkDeg=0;
     
@@ -104,6 +116,7 @@
     if(sys.KickOffFlag==1){
         kicker=0;
         ValidSolenoid();
+        BallChecker.fall(&DriveSolenoidJudge);
     }
     else{
         kicker=1;
@@ -116,21 +129,32 @@
         //compass
         ValidTurn();
         hmc_reset=HMC_RUN;
-        for(int i=0; i<5; i++){
-            ReadCmps();
-            cmps_set.CmpsInitialValue = cmps_set.cmps;
-            wait_ms(10);
-        }
-        cmps_set.CmpsDiff = REFERENCE - cmps_set.cmps;
         if(sys.TurnStartFlag==1){
             //正．．．右回転
             //負．．．左回転
             //cmps_set.FrontDeg=-180;
             //cmps_set.FrontDeg=179;
             //cmps_set.FrontDeg=180;
-            cmps_set.FrontDeg=-180;
+            //cmps_set.CmpsDiff = REFERENCE - cmps_set.CmpsInitialValue;
+            
+            /*for(int i=0; i<5; i++){
+                ReadCmps();
+                cmps_set.CmpsInitialValue2 = cmps_set.cmps;
+                wait_ms(10);
+            }*/
+            //cmps_set.CmpsDiff = REFERENCE - cmps_set.CmpsInitialValue2;
+            
+            cmps_set.FrontDeg=-180;//-(cmps_set.CmpsInitialValue2-cmps_set.CmpsInitialValue0);
         }
         else{
+            for(int i=0; i<5; i++){
+                ReadCmps();
+                cmps_set.CmpsInitialValue = cmps_set.cmps;
+                wait_ms(10);
+            }
+            cmps_set.CmpsDiff = REFERENCE - cmps_set.cmps;
+            
+            
             cmps_set.FrontDeg=0;
         }
         Motor_ticker.attach(&ValidTx_motor, 0.020);

--- a/main_processing/strategy/old_strategy.cpp	Wed Mar 23 13:01:43 2016 +0000
+++ b/main_processing/strategy/old_strategy.cpp	Thu Mar 24 04:54:39 2016 +0000
@@ -2,17 +2,28 @@
 #include "extern.h"
 
 //Atk
-void modeAttack0(void){
+void modeAttack0(void){//red(atk)
     ////初期値を決める等
     if(sys.KickOffFlag==1){
         
         ///Blindシリーズ．1を代入であらゆるセンサーなどの値を反映させないようにする．
+        /*sys.IrBlind=0;
+        sys.LineBlind=0;
+        sys.PingBlind=0;
+        
+        sys.HomeBlind=1;
+        sys.DriBlind=1;*/
+        
         sys.IrBlind=0;
         sys.LineBlind=0;
         sys.PingBlind=0;
         
         sys.HomeBlind=1;
-        sys.DriBlind=1;
+        sys.DriBlind=0;
+        sys.TurnAtkBlind=1;
+        sys.TurnDriBlind=1;
+        sys.TurnHoldBlind=1;
+        sys.KickBlind=0;
         //defence
         sys.DefenceFlag=0;
         
@@ -22,21 +33,36 @@
     }
     modeAttack4();
 }
-void modeAttack1(void){
+void modeAttack1(void){//green(libero)
     ////初期値を決める等
     if(sys.KickOffFlag==1){
         
         ///Blindシリーズ．1を代入であらゆるセンサーなどの値を反映させないようにする．
+        
         sys.IrBlind=0;
         sys.LineBlind=0;
         sys.PingBlind=0;
         
         sys.HomeBlind=0;
         sys.DriBlind=1;
+        sys.TurnAtkBlind=1;
+        sys.TurnDriBlind=1;
+        sys.TurnHoldBlind=1;
+        sys.KickBlind=0;
+        /*
+        sys.IrBlind=0;
+        sys.LineBlind=0;
+        sys.PingBlind=0;
+        
+        sys.HomeBlind=0;
+        sys.DriBlind=1;
+        */
         //defence
         sys.DefenceFlag=0;
         
         
+        
+        
         //初期値設定の終了
         //sys.KickOffFlag=0;
     }

--- a/main_processing/strategy/strategy.cpp	Wed Mar 23 13:01:43 2016 +0000
+++ b/main_processing/strategy/strategy.cpp	Thu Mar 24 04:54:39 2016 +0000
@@ -244,6 +244,25 @@
                 }
             }
         }
+        if((sys.HomeBlind==0)){
+            if(((data.irNotice==IR_CLOSE)||(data.irNotice==IR_CLOSER))&&(data.ping[B_PING]<60)&&
+                (
+                    (data.irPosition==11)||
+                    (data.irPosition==(ir_posi_s[(11-8+24+1)%12]))||
+                    (data.irPosition==(ir_posi_s[(11-8+24-1)%12]))||
+                    (data.irPosition==(ir_posi_s[(11-8+24+2)%12]))||
+                    (data.irPosition==(ir_posi_s[(11-8+24-2)%12]))||
+                    (data.irPosition==(ir_posi_s[(11-8+24+3)%12]))||
+                    (data.irPosition==(ir_posi_s[(11-8+24-3)%12]))
+                )
+            ){
+                sys.ir_pow_table=2;//直進
+                ir_x_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_X_DIR];
+                ir_y_dir = ir_move_val[2][data.irNotice][data.irPosition][IR_Y_DIR];
+                ir_x_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_X_TURN];
+                ir_y_turn = ir_move_val[2][data.irNotice][data.irPosition][IR_Y_TURN];
+            }
+        }
     }
     
     //Irの検出値によって出力を調整
@@ -287,21 +306,23 @@
     //ホールド判定
     JudgeBallHolding();
     //ドリブラー駆動
-    if((sys.DriBlind==0)&&(data.lnRawOrderLog1[0]!=LINE_EMPTY)&&(data.lnRawOrder[0]==LINE_EMPTY)){
-        if(
-            ((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT))||
-            ((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT))
-        ){
-            cmps_set.GoalDeg=0;
-        }
-        else if((data.lnRawOrderLog1[0]==A_SPOT)&&(1)){
-            cmps_set.GoalDeg=-30;
-        }
-        else if((data.lnRawOrderLog1[0]==B_SPOT)&&(1)){
-            cmps_set.GoalDeg=30;
-        }
-        else{
-            cmps_set.GoalDeg=0;
+    if((sys.TurnDriBlind==0)&&(data.lnRawOrderLog1[0]!=LINE_EMPTY)&&(data.lnRawOrder[0]==LINE_EMPTY)){
+        if(sys.TurnHoldBlind==0){
+            if(
+                ((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]==B_SPOT))||
+                ((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]==A_SPOT))
+            ){
+                cmps_set.GoalDeg=0;
+            }
+            else if((data.lnRawOrderLog1[0]==A_SPOT)&&(1)){
+                cmps_set.GoalDeg=-30;
+            }
+            else if((data.lnRawOrderLog1[0]==B_SPOT)&&(1)){
+                cmps_set.GoalDeg=30;
+            }
+            else{
+                cmps_set.GoalDeg=0;
+            }
         }
     }
     if((sys.DriBlind==0)&&(data.irNotice==IR_CLOSER)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){
@@ -343,8 +364,8 @@
         (data.ping[R_PING]<30)&&
         (data.ping[L_PING]<30)
     ){
-        DriveTurn();
-        DriveSolenoid();
+        //DriveTurn();
+        //DriveSolenoid();
     }
     
     //Irの細かい補正値の処理．基本的にLineの補正値を演算する処理が始まる前にIrのモーター出力値補正をすべて終わらせる．
@@ -378,7 +399,13 @@
             ir_y = -1;
         }
         else{
-            ir_y = 0;
+            if((data.ping[B_PING]>40)&&(data.ping[R_PING]>55)&&(data.ping[L_PING]>55)){
+                ir_y = -1;
+                ir_pow_y = 15;
+            }
+            else{
+                ir_y = 0;
+            }
             //sys.HomeStayFlag[Y_PING]=1;
         }
     }
@@ -391,6 +418,7 @@
         if(ir_y>0) ir_y=0;
         if((data.ping[B_PING]>60)&&(1)){
             ir_y = -1;
+            ir_pow_y = 15;
         }
     }
     if(
@@ -434,11 +462,11 @@
     //LED = ((data.lnOrder[0]!=LINE_EMPTY)<<2) | ((data.lnOrder[1]!=LINE_EMPTY)<<1) | ((data.lnOrder[2]!=LINE_EMPTY)<<0);
     
     //LED = sys.BallHoldFlag*15;
-    //if(sys.BallHoldFlag==1) LED=15;
-    //else LED=10;
+    if(sys.BallHoldFlag==1) LED=15;
+    else LED=10;
     
-    if(data.lnRepeat>=1) LED=15;
-    else LED=10;
+    //if(data.lnRepeat>=1) LED=15;
+    //else LED=10;
     
     //LED = 0xFF*(data.ping[B_PING]<30);
     
@@ -449,8 +477,8 @@
     
     
     ////最終的なモーターの出力を演算
-    vx = (ir_pow_x*ir_x)*data.lnStop[X_LINE]*(data.lnStay[X_LINE])*LineSlowPower[X_LINE] + LineReturnPower[X_LINE];
-    vy = (ir_pow_y*ir_y)*data.lnStop[Y_LINE]*(data.lnStay[Y_LINE])*LineSlowPower[Y_LINE] + LineReturnPower[Y_LINE];
+    vx = (ir_pow_x*ir_x)*data.lnStop[X_LINE]*(data.lnStay[X_LINE])*LineSlowPower[X_LINE]*(sys.TurnStopFlag==0) + LineReturnPower[X_LINE];
+    vy = (ir_pow_y*ir_y)*data.lnStop[Y_LINE]*(data.lnStay[Y_LINE])*LineSlowPower[Y_LINE]*(sys.TurnStopFlag==0) + LineReturnPower[Y_LINE];
     vs = cmps_set.OutputPID;
     move(
         vx,

--- a/main_processing/strategy_parts/output.cpp	Wed Mar 23 13:01:43 2016 +0000
+++ b/main_processing/strategy_parts/output.cpp	Thu Mar 24 04:54:39 2016 +0000
@@ -5,7 +5,20 @@
 void PidUpdate(void)
 {
     cmps_set.cmps = hmc.sample()/10.0;
-    cmps_set.InputPID = fmod((cmps_set.cmps+(cmps_set.CmpsDiff-cmps_set.FrontDeg-cmps_set.AtkDeg-cmps_set.HoldDeg-cmps_set.GoalDeg)+7200.0), 360.0);//0<cmps_set.cmps<359.0
+    cmps_set.InputPID = 
+    fmod(
+        (
+            cmps_set.cmps+
+            (cmps_set.CmpsDiff
+                -cmps_set.FrontDeg
+                -cmps_set.AtkDeg*(sys.TurnAtkBlind==0)
+                -cmps_set.HoldDeg*(sys.TurnHoldBlind==0)
+                -cmps_set.GoalDeg*(sys.TurnDriBlind==0)
+            )+
+            7200.0
+        ),
+        360.0
+    );//0<cmps_set.cmps<359.0
     pid.setProcessValue(cmps_set.InputPID);
     cmps_set.OutputPID = -pid.compute();
     /*if((abs(cmps_set.OutputPID)<PID_OUT_MIN)&&(cmps_set.OutputPID!=0)){
@@ -22,28 +35,40 @@
     cmps_set.AtkDeg = 30;
 }
 void DriveTurn(void){
+    if(sys.TurnAtkBlind==1) return;
     if(sys.TurnStopFlag==1) return;
     
     if(
-        (data.ping[L_PING]<data.ping[R_PING])||
-        (data.lnRawOrderLog1[0]==B_SPOT)||
-        (data.lnRawOrder[0]==B_SPOT)
+        //(data.ping[L_PING]<data.ping[R_PING])||
+        ((data.lnRawOrderLog1[0]==B_SPOT)&&(data.lnRawOrderLog1[1]!=A_SPOT)&&(data.lnRawOrder[0]==LINE_EMPTY))
     ){
         cmps_set.AtkDeg = 150;
+        if((sys.TurnDriBlind==0)&&(cmps_set.GoalDeg>0)){
+            cmps_set.AtkDeg = -cmps_set.GoalDeg*2;
+        }
     }
     else if(
-        (data.ping[L_PING]>data.ping[R_PING])||
-        (data.lnRawOrderLog1[0]==A_SPOT)||
-        (data.lnRawOrder[0]==A_SPOT)
+        //(data.ping[L_PING]>data.ping[R_PING])||
+         ((data.lnRawOrderLog1[0]==A_SPOT)&&(data.lnRawOrderLog1[1]!=B_SPOT)&&(data.lnRawOrder[0]==LINE_EMPTY))
     ){
         cmps_set.AtkDeg = -150;
+        if((sys.TurnDriBlind==0)&&(cmps_set.GoalDeg<0)){
+            cmps_set.AtkDeg = cmps_set.GoalDeg*2;
+        }
     }
     else{
         cmps_set.AtkDeg = 0;
     }
+    if((sys.TurnDriBlind==0)&&(cmps_set.GoalDeg!=0)){
+        Turn_timeout.attach(&TurnSignal, .25);
+        Turn_stop.attach(&ValidTurn, .5);
+    }
+    else{
+        Turn_timeout.attach(&TurnSignal, .25);
+        Turn_stop.attach(&ValidTurn, .5);
+    }
     sys.TurnStopFlag=1;
-    Turn_timeout.attach(&TurnSignal, .5);
-    Turn_stop.attach(&ValidTurn, 2.0);
+    
 }
 void TurnSignal(void){
     cmps_set.AtkDeg = 0;
@@ -91,6 +116,7 @@
 }
 //solenoid
 void DriveSolenoid(void){
+    if(sys.KickBlind==1) return;
     if(sys.KickStopFlag==1) return;
     
     kicker=1;
@@ -98,6 +124,12 @@
     Solenoid_timeout.attach(&SolenoidSignal, .5);
     Kick_stop.attach(&ValidSolenoid, 2.0);
 }
+void DriveSolenoidJudge(void){
+    if((data.irNotice==IR_CLOSER)&&((data.irPosition==10)||(data.irPosition==11)||(data.irPosition==12))){
+        DriveTurn();
+        DriveSolenoid();
+    }
+}
 void SolenoidSignal(void){
     kicker=0;
 }

--- a/main_processing/strategy_parts/output.h	Wed Mar 23 13:01:43 2016 +0000
+++ b/main_processing/strategy_parts/output.h	Thu Mar 24 04:54:39 2016 +0000
@@ -17,6 +17,7 @@
 void move(int vx, int vy, int vs);
 //solenoid
 void DriveSolenoid(void);
+void DriveSolenoidJudge(void);
 void SolenoidSignal(void);
 void ValidSolenoid(void);
 //ball

--- a/setting/ActiveConfig.h	Wed Mar 23 13:01:43 2016 +0000
+++ b/setting/ActiveConfig.h	Thu Mar 24 04:54:39 2016 +0000
@@ -10,7 +10,7 @@
 
 const ActiveItem static act[STRATEGY_NUM]={
     {"NormalAtk_______", modeAttack0, MODE_ATTACK4},
-    {"defence_________", modeAttack1, MODE_ATTACK1},
+    {"Libero__________", modeAttack1, MODE_ATTACK1},
     {"Atk+Dribble_____", modeAttack2, MODE_ATTACK2},
     {"LineOnly________", modeAttack3, MODE_ATTACK3},
     {"modeAttack4_____", modeAttack4, MODE_ATTACK4},

--- a/setting/CommandConfig.h	Wed Mar 23 13:01:43 2016 +0000
+++ b/setting/CommandConfig.h	Thu Mar 24 04:54:39 2016 +0000
@@ -5,7 +5,6 @@
 #include "extern.h"
 
 const CommandItem static item[STATE_NUM_Y]={
-    {{"None__","Strgy_","Power_","SftRst"}, 4, ZeroFunction,        ZERO_FUNCTION},
     {{"Start_","Active","______","______"}, 2, Start,               START},
     {{"TrnSrt","Active","______","______"}, 2, TurnAndStart,        TURN_AND_START},
     {{"Ir____","IrSpot","IrNote","IrPstn"}, 4, GetIr,               GET_IR},
@@ -21,6 +20,7 @@
     {{"Kicker","DRIVE_","______","______"}, 2, DriveKicker,           DRIVE_KICKER},
     {{"Dribbl","DRIVE_","______","______"}, 2, DriveDribblerAndKicker,DRIVE_DRIBBLER_AND_KICKER},
     {{"Start2","Active","______","______"}, 2, Start2,              START2},
+    {{"None__","Strgy_","Power_","SftRst"}, 4, ZeroFunction,        ZERO_FUNCTION},
     
     //char LcdStr[STATE_NUM_X][BUFSIZE];
     //uint8_t str_num;

--- a/setting/def.h	Wed Mar 23 13:01:43 2016 +0000
+++ b/setting/def.h	Thu Mar 24 04:54:39 2016 +0000
@@ -2,8 +2,8 @@
 #define _DEF_H_
 
 //red or green
-//#define RED_CAT//jj
-#define GREEN_CAT//lily
+#define RED_CAT//jj
+//#define GREEN_CAT//lily
 
 //BT(BlueTooth)
 #define DATA_NUM 8+2//2byte→KEYCODE（拝啓）とCHECKCODE（敬具） 8byte→やりとりするデータ
@@ -218,7 +218,7 @@
 //データ
 typedef struct {
     //cmps&pid
-    double cmps, CmpsInitialValue, CmpsDiff;//0<x<360
+    double cmps, CmpsInitialValue, CmpsInitialValue2, CmpsInitialValue0, CmpsDiff;//0<x<360
     int16_t FrontDeg, AtkDeg, HoldDeg, GoalDeg;
     //正．．．右回転
     //負．．．左回転
@@ -298,7 +298,12 @@
     uint8_t PingBlind;
     
     uint8_t DriBlind;
+    uint8_t TurnAtkBlind;
+    uint8_t TurnDriBlind;
+    uint8_t TurnHoldBlind;
+    uint8_t KickBlind;
     uint8_t HomeBlind;
+    
     //motor 
     uint8_t pow_num;
     uint8_t s_pow;

--- a/setting/extern.h	Wed Mar 23 13:01:43 2016 +0000
+++ b/setting/extern.h	Thu Mar 24 04:54:39 2016 +0000
@@ -46,7 +46,7 @@
 extern InterruptIn Line[3];
 extern InterruptIn LineHolding[3];
 //ballcheck(bottom)
-extern DigitalIn BallChecker;
+extern InterruptIn BallChecker;
 extern AnalogIn BallCheckerA;
 //debug_switch(debug_board)
 extern DigitalIn Sw[4];

--- a/setting/main.h	Wed Mar 23 13:01:43 2016 +0000
+++ b/setting/main.h	Thu Mar 24 04:54:39 2016 +0000
@@ -15,7 +15,7 @@
 InterruptIn Line[3]={lineA2, lineB2, lineC2};
 InterruptIn LineHolding[3]={lineA1, lineB1, lineC1};
 //ballcheck(bottom)
-DigitalIn BallChecker(ballcheck);
+InterruptIn BallChecker(ballcheck);
 AnalogIn BallCheckerA(ballcheck);
 //debug_switch(debug_board)
 DigitalIn Sw[4] = {selectsw1, selectsw2, debugsw1, debugsw2};