CatPot 2015-2016 / Mbed 2 deprecated CatPot_2v10_T_Main

ジャパンオープン用のメインプログラム

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/setup_command_active/setup.cpp@34:1c86c1299ea4, 2016-03-24 (annotated)

Committer:
lilac0112_1
Date:
Thu Mar 24 15:33:54 2016 +0000
Revision:
34:1c86c1299ea4
Parent:
32:367b16d69a32 
OK?It can be shipped; red......libero; green...atkOnly

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lilac0112_1 0:ea35c18c85fc 1 #include "mbed.h"
lilac0112_1 0:ea35c18c85fc 2 #include "extern.h"
lilac0112_1 0:ea35c18c85fc 3
lilac0112_1 0:ea35c18c85fc 4
lilac0112_1 0:ea35c18c85fc 5 void SetUp(void){//電源を入れた時の処理
lilac0112_1 0:ea35c18c85fc 6 //int i=1;
lilac0112_1 0:ea35c18c85fc 7 //solenoid
lilac0112_1 0:ea35c18c85fc 8 kicker=0;
lilac0112_1 0:ea35c18c85fc 9 //sw&lcd
lilac0112_1 0:ea35c18c85fc 10 Sw[0].mode(PullUp);
lilac0112_1 0:ea35c18c85fc 11 Sw[1].mode(PullUp);
lilac0112_1 0:ea35c18c85fc 12 Sw[2].mode(PullUp);
lilac0112_1 0:ea35c18c85fc 13 Sw[3].mode(PullUp);
lilac0112_1 0:ea35c18c85fc 14 Sw_ticker.attach(Sw_sample, 0.050);
lilac0112_1 0:ea35c18c85fc 15
lilac0112_1 0:ea35c18c85fc 16 //ball
lilac0112_1 0:ea35c18c85fc 17 BallChecker.mode(PullUp);
lilac0112_1 0:ea35c18c85fc 18
lilac0112_1 5:5ff3a7d5d8c2 19 //system,flag
lilac0112_1 5:5ff3a7d5d8c2 20 sys.strategy=0;
lilac0112_1 5:5ff3a7d5d8c2 21 sys.jump_flag=JUMP_TAG_MAX;//0<=x<=JUMP_TAG_NUM
lilac0112_1 5:5ff3a7d5d8c2 22 sys.stopflag=0;
lilac0112_1 5:5ff3a7d5d8c2 23 sys.KickOffFlag=0;
lilac0112_1 10:6df631c39f9b 24 sys.TurnStartFlag=0;
lilac0112_1 5:5ff3a7d5d8c2 25 sys.DribbleFlag=0;
lilac0112_1 5:5ff3a7d5d8c2 26 sys.MotorFlag=0;
lilac0112_1 19:967207de919d 27 sys.InfoFlag=0;
lilac0112_1 21:378470320524 28 //sys.PidFlag=0;
lilac0112_1 5:5ff3a7d5d8c2 29 //motor
lilac0112_1 7:efe90188a49d 30 sys.pow_num = 1;
lilac0112_1 7:efe90188a49d 31 sys.s_pow = ir_pow_val[sys.pow_num][POW_SHORT];
lilac0112_1 7:efe90188a49d 32 sys.m_pow = ir_pow_val[sys.pow_num][POW_MIDDLE];
lilac0112_1 7:efe90188a49d 33 sys.l_pow = ir_pow_val[sys.pow_num][POW_LONG];
lilac0112_1 7:efe90188a49d 34 /*sys.s_pow = 30;
lilac0112_1 6:f9167470d819 35 sys.m_pow = 30;
lilac0112_1 7:efe90188a49d 36 sys.l_pow = 30;*/
lilac0112_1 21:378470320524 37 sys.dri_pow = 100;
lilac0112_1 2:635947de1583 38 sys.ir_pow_table = 0;
lilac0112_1 2:635947de1583 39
lilac0112_1 30:5998ba42237e 40 ///////important
lilac0112_1 30:5998ba42237e 41 ///Blindシリーズ．1を代入であらゆるセンサーなどの値を反映させないようにする．
lilac0112_1 30:5998ba42237e 42 sys.IrBlind=0;
lilac0112_1 30:5998ba42237e 43 sys.LineBlind=0;
lilac0112_1 30:5998ba42237e 44 sys.PingBlind=0;
lilac0112_1 30:5998ba42237e 45
lilac0112_1 30:5998ba42237e 46 sys.HomeBlind=1;
lilac0112_1 30:5998ba42237e 47 sys.DriBlind=1;
lilac0112_1 34:1c86c1299ea4 48 sys.DriMotorBlind=0;
lilac0112_1 32:367b16d69a32 49 sys.TurnAtkBlind=0;
lilac0112_1 32:367b16d69a32 50 sys.TurnDriBlind=0;
lilac0112_1 32:367b16d69a32 51 sys.TurnHoldBlind=0;
lilac0112_1 32:367b16d69a32 52 sys.KickBlind=0;
lilac0112_1 30:5998ba42237e 53 //defence
lilac0112_1 30:5998ba42237e 54 sys.DefenceFlag=0;
lilac0112_1 30:5998ba42237e 55 ////////important
lilac0112_1 30:5998ba42237e 56
lilac0112_1 2:635947de1583 57 //ir
lilac0112_1 2:635947de1583 58 data.irNotice=IR_NONE;
lilac0112_1 0:ea35c18c85fc 59
lilac0112_1 0:ea35c18c85fc 60 //spi
lilac0112_1 0:ea35c18c85fc 61 spi.format(16, 3);
lilac0112_1 0:ea35c18c85fc 62 spi.frequency(1000000);
lilac0112_1 0:ea35c18c85fc 63 spi_ss[0]=spi_ss[1]=spi_ss[2]=spi_ss[3]=1;
lilac0112_1 0:ea35c18c85fc 64
lilac0112_1 0:ea35c18c85fc 65 //motor
lilac0112_1 0:ea35c18c85fc 66 motor.baud(115200); //ボーレート設定
lilac0112_1 0:ea35c18c85fc 67 motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
lilac0112_1 0:ea35c18c85fc 68 //compass
lilac0112_1 10:6df631c39f9b 69 hmc_reset=HMC_RUN;
lilac0112_1 32:367b16d69a32 70 /*for(int i=0; i<5; i++){
lilac0112_1 0:ea35c18c85fc 71 ReadCmps();
lilac0112_1 0:ea35c18c85fc 72 cmps_set.CmpsInitialValue = cmps_set.cmps;
lilac0112_1 0:ea35c18c85fc 73 wait_ms(10);
lilac0112_1 0:ea35c18c85fc 74 }
lilac0112_1 32:367b16d69a32 75 cmps_set.CmpsDiff = REFERENCE - cmps_set.CmpsInitialValue;*/
lilac0112_1 32:367b16d69a32 76 for(int i=0; i<5; i++){
lilac0112_1 32:367b16d69a32 77 ReadCmps();
lilac0112_1 32:367b16d69a32 78 cmps_set.CmpsInitialValue = cmps_set.cmps;
lilac0112_1 32:367b16d69a32 79 cmps_set.CmpsInitialValue0 = cmps_set.cmps;
lilac0112_1 32:367b16d69a32 80 wait_ms(10);
lilac0112_1 32:367b16d69a32 81 }
lilac0112_1 32:367b16d69a32 82 cmps_set.CmpsDiff = REFERENCE - cmps_set.cmps;
lilac0112_1 32:367b16d69a32 83
lilac0112_1 0:ea35c18c85fc 84 cmps_set.FrontDeg=0;
lilac0112_1 19:967207de919d 85 cmps_set.AtkDeg=0;
lilac0112_1 0:ea35c18c85fc 86
lilac0112_1 0:ea35c18c85fc 87 pid.setInputLimits(MINIMUM,MAXIMUM); //pid sed def
lilac0112_1 0:ea35c18c85fc 88 pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT); //pid sed def
lilac0112_1 0:ea35c18c85fc 89 pid.setBias(PID_BIAS); //pid sed def
lilac0112_1 0:ea35c18c85fc 90 pid.setMode(AUTO_MODE); //pid sed def
lilac0112_1 0:ea35c18c85fc 91 pid.setSetPoint(REFERENCE); //pid sed def
lilac0112_1 0:ea35c18c85fc 92 //pidupdate.attach(&PidUpdate, PID_CYCLE);
lilac0112_1 0:ea35c18c85fc 93 }
lilac0112_1 0:ea35c18c85fc 94 void SetUp2(void){//スタートした時の処理
lilac0112_1 0:ea35c18c85fc 95 __enable_irq(); // 許可
lilac0112_1 21:378470320524 96
lilac0112_1 9:c966191926c5 97 sys.KickOffFlag=1;
lilac0112_1 21:378470320524 98 SetKick();
lilac0112_1 10:6df631c39f9b 99 SetPidAndMotor();
lilac0112_1 19:967207de919d 100 SetInfo();
lilac0112_1 10:6df631c39f9b 101 SetLine();
lilac0112_1 0:ea35c18c85fc 102 }
lilac0112_1 0:ea35c18c85fc 103 void StopProcess(void){//コマンドに戻るときの処理
lilac0112_1 10:6df631c39f9b 104
lilac0112_1 10:6df631c39f9b 105 sys.KickOffFlag=0;
lilac0112_1 21:378470320524 106 SetKick();
lilac0112_1 10:6df631c39f9b 107 SetPidAndMotor();
lilac0112_1 19:967207de919d 108 SetInfo();
lilac0112_1 10:6df631c39f9b 109 SetLine();
lilac0112_1 0:ea35c18c85fc 110
lilac0112_1 0:ea35c18c85fc 111 __disable_irq(); // 禁止
lilac0112_1 0:ea35c18c85fc 112 __enable_irq(); // 許可
lilac0112_1 0:ea35c18c85fc 113 Sw_ticker.attach(Sw_sample, 0.050);
lilac0112_1 0:ea35c18c85fc 114 }
lilac0112_1 21:378470320524 115
lilac0112_1 9:c966191926c5 116 void SetKick(void){
lilac0112_1 9:c966191926c5 117 if(sys.KickOffFlag==1){
lilac0112_1 9:c966191926c5 118 kicker=0;
lilac0112_1 24:34ef6379b0df 119 ValidSolenoid();
lilac0112_1 32:367b16d69a32 120 BallChecker.fall(&DriveSolenoidJudge);
lilac0112_1 9:c966191926c5 121 }
lilac0112_1 9:c966191926c5 122 else{
lilac0112_1 9:c966191926c5 123 kicker=1;
lilac0112_1 9:c966191926c5 124 wait(0.25);
lilac0112_1 9:c966191926c5 125 kicker=0;
lilac0112_1 9:c966191926c5 126 }
lilac0112_1 21:378470320524 127 }
lilac0112_1 9:c966191926c5 128 void SetPidAndMotor(void){
lilac0112_1 9:c966191926c5 129 if(sys.KickOffFlag==1){
lilac0112_1 9:c966191926c5 130 //compass
lilac0112_1 24:34ef6379b0df 131 ValidTurn();
lilac0112_1 10:6df631c39f9b 132 hmc_reset=HMC_RUN;
lilac0112_1 10:6df631c39f9b 133 if(sys.TurnStartFlag==1){
lilac0112_1 18:3a42a931c95a 134 //正．．．右回転
lilac0112_1 18:3a42a931c95a 135 //負．．．左回転
lilac0112_1 10:6df631c39f9b 136 //cmps_set.FrontDeg=-180;
lilac0112_1 18:3a42a931c95a 137 //cmps_set.FrontDeg=179;
lilac0112_1 18:3a42a931c95a 138 //cmps_set.FrontDeg=180;
lilac0112_1 32:367b16d69a32 139 //cmps_set.CmpsDiff = REFERENCE - cmps_set.CmpsInitialValue;
lilac0112_1 32:367b16d69a32 140
lilac0112_1 32:367b16d69a32 141 /*for(int i=0; i<5; i++){
lilac0112_1 32:367b16d69a32 142 ReadCmps();
lilac0112_1 32:367b16d69a32 143 cmps_set.CmpsInitialValue2 = cmps_set.cmps;
lilac0112_1 32:367b16d69a32 144 wait_ms(10);
lilac0112_1 32:367b16d69a32 145 }*/
lilac0112_1 32:367b16d69a32 146 //cmps_set.CmpsDiff = REFERENCE - cmps_set.CmpsInitialValue2;
lilac0112_1 32:367b16d69a32 147
lilac0112_1 32:367b16d69a32 148 cmps_set.FrontDeg=-180;//-(cmps_set.CmpsInitialValue2-cmps_set.CmpsInitialValue0);
lilac0112_1 10:6df631c39f9b 149 }
lilac0112_1 10:6df631c39f9b 150 else{
lilac0112_1 32:367b16d69a32 151 for(int i=0; i<5; i++){
lilac0112_1 32:367b16d69a32 152 ReadCmps();
lilac0112_1 32:367b16d69a32 153 cmps_set.CmpsInitialValue = cmps_set.cmps;
lilac0112_1 32:367b16d69a32 154 wait_ms(10);
lilac0112_1 32:367b16d69a32 155 }
lilac0112_1 32:367b16d69a32 156 cmps_set.CmpsDiff = REFERENCE - cmps_set.cmps;
lilac0112_1 32:367b16d69a32 157
lilac0112_1 32:367b16d69a32 158
lilac0112_1 10:6df631c39f9b 159 cmps_set.FrontDeg=0;
lilac0112_1 10:6df631c39f9b 160 }
lilac0112_1 10:6df631c39f9b 161 Motor_ticker.attach(&ValidTx_motor, 0.020);
lilac0112_1 19:967207de919d 162 //pidupdate.attach(&ValidPidUpdate, PID_CYCLE);
lilac0112_1 9:c966191926c5 163 }
lilac0112_1 9:c966191926c5 164 else{
lilac0112_1 19:967207de919d 165 //pidupdate.detach();
lilac0112_1 9:c966191926c5 166 Motor_ticker.detach();
lilac0112_1 9:c966191926c5 167 ////Hmc_ticker.detach();
lilac0112_1 9:c966191926c5 168 motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
lilac0112_1 9:c966191926c5 169 move(0,0,0);
lilac0112_1 9:c966191926c5 170 cmps_set.OutputPID=0;
lilac0112_1 9:c966191926c5 171 cmps_set.InputPID=REFERENCE;
lilac0112_1 21:378470320524 172 HmcReset();
lilac0112_1 9:c966191926c5 173 }
lilac0112_1 9:c966191926c5 174 }
lilac0112_1 19:967207de919d 175 void SetInfo(void){
lilac0112_1 19:967207de919d 176 if(sys.KickOffFlag==1){
lilac0112_1 19:967207de919d 177 Info_ticker.attach(&ValidInfo, 0.050);
lilac0112_1 19:967207de919d 178 }
lilac0112_1 19:967207de919d 179 else{
lilac0112_1 19:967207de919d 180 Info_ticker.detach();
lilac0112_1 19:967207de919d 181 }
lilac0112_1 19:967207de919d 182 }
lilac0112_1 9:c966191926c5 183 void SetLine(void){
lilac0112_1 9:c966191926c5 184 if(sys.KickOffFlag==1){
lilac0112_1 9:c966191926c5 185 //Line_ticker.attach(&ReadLine, 0.005);
lilac0112_1 16:4fadb7a87497 186 /*Line[A_SPOT].rise(&LineCall_A);
lilac0112_1 9:c966191926c5 187 Line[B_SPOT].rise(&LineCall_B);
lilac0112_1 16:4fadb7a87497 188 Line[C_SPOT].rise(&LineCall_C);*/
lilac0112_1 30:5998ba42237e 189 /*
lilac0112_1 16:4fadb7a87497 190 LineHolding[A_SPOT].rise(&LineRanking_A);
lilac0112_1 16:4fadb7a87497 191 LineHolding[B_SPOT].rise(&LineRanking_B);
lilac0112_1 16:4fadb7a87497 192 LineHolding[C_SPOT].rise(&LineRanking_C);
lilac0112_1 30:5998ba42237e 193 */
lilac0112_1 27:825c6835e3db 194 /*
lilac0112_1 27:825c6835e3db 195 Line[A_SPOT].rise(&LineRawCall_A);
lilac0112_1 27:825c6835e3db 196 Line[B_SPOT].rise(&LineRawCall_B);
lilac0112_1 27:825c6835e3db 197 Line[C_SPOT].rise(&LineRawCall_C);
lilac0112_1 27:825c6835e3db 198 */
lilac0112_1 27:825c6835e3db 199
lilac0112_1 27:825c6835e3db 200 Line[A_SPOT].rise(&LineRawRanking_A);
lilac0112_1 27:825c6835e3db 201 Line[B_SPOT].rise(&LineRawRanking_B);
lilac0112_1 27:825c6835e3db 202 Line[C_SPOT].rise(&LineRawRanking_C);
lilac0112_1 9:c966191926c5 203 }
lilac0112_1 9:c966191926c5 204 else{
lilac0112_1 9:c966191926c5 205 ////Line_ticker.detach();
lilac0112_1 9:c966191926c5 206 }
lilac0112_1 10:6df631c39f9b 207 }