CatPot 2015-2016 / Mbed 2 deprecated CatPot_2v10_T_Main

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

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/setup_command_active/setup.cpp@30:5998ba42237e, 2016-03-23 (annotated)

Committer:
lilac0112_1
Date:
Wed Mar 23 11:25:22 2016 +0000
Revision:
30:5998ba42237e
Parent:
27:825c6835e3db
Child:
32:367b16d69a32
03/23 ping slow OK

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 30:5998ba42237e 48 //defence
lilac0112_1 30:5998ba42237e 49 sys.DefenceFlag=0;
lilac0112_1 30:5998ba42237e 50 ////////important
lilac0112_1 30:5998ba42237e 51
lilac0112_1 2:635947de1583 52 //ir
lilac0112_1 2:635947de1583 53 data.irNotice=IR_NONE;
lilac0112_1 0:ea35c18c85fc 54
lilac0112_1 0:ea35c18c85fc 55 //spi
lilac0112_1 0:ea35c18c85fc 56 spi.format(16, 3);
lilac0112_1 0:ea35c18c85fc 57 spi.frequency(1000000);
lilac0112_1 0:ea35c18c85fc 58 spi_ss[0]=spi_ss[1]=spi_ss[2]=spi_ss[3]=1;
lilac0112_1 0:ea35c18c85fc 59
lilac0112_1 0:ea35c18c85fc 60 //motor
lilac0112_1 0:ea35c18c85fc 61 motor.baud(115200); //ボーレート設定
lilac0112_1 0:ea35c18c85fc 62 motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
lilac0112_1 0:ea35c18c85fc 63 //compass
lilac0112_1 10:6df631c39f9b 64 hmc_reset=HMC_RUN;
lilac0112_1 0:ea35c18c85fc 65 for(int i=0; i<5; i++){
lilac0112_1 0:ea35c18c85fc 66 ReadCmps();
lilac0112_1 0:ea35c18c85fc 67 cmps_set.CmpsInitialValue = cmps_set.cmps;
lilac0112_1 0:ea35c18c85fc 68 wait_ms(10);
lilac0112_1 0:ea35c18c85fc 69 }
lilac0112_1 19:967207de919d 70 cmps_set.CmpsDiff = REFERENCE - cmps_set.CmpsInitialValue;
lilac0112_1 0:ea35c18c85fc 71 cmps_set.FrontDeg=0;
lilac0112_1 19:967207de919d 72 cmps_set.AtkDeg=0;
lilac0112_1 0:ea35c18c85fc 73
lilac0112_1 0:ea35c18c85fc 74 pid.setInputLimits(MINIMUM,MAXIMUM); //pid sed def
lilac0112_1 0:ea35c18c85fc 75 pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT); //pid sed def
lilac0112_1 0:ea35c18c85fc 76 pid.setBias(PID_BIAS); //pid sed def
lilac0112_1 0:ea35c18c85fc 77 pid.setMode(AUTO_MODE); //pid sed def
lilac0112_1 0:ea35c18c85fc 78 pid.setSetPoint(REFERENCE); //pid sed def
lilac0112_1 0:ea35c18c85fc 79 //pidupdate.attach(&PidUpdate, PID_CYCLE);
lilac0112_1 0:ea35c18c85fc 80 }
lilac0112_1 0:ea35c18c85fc 81 void SetUp2(void){//スタートした時の処理
lilac0112_1 0:ea35c18c85fc 82 __enable_irq(); // 許可
lilac0112_1 21:378470320524 83
lilac0112_1 9:c966191926c5 84 sys.KickOffFlag=1;
lilac0112_1 21:378470320524 85 SetKick();
lilac0112_1 10:6df631c39f9b 86 SetPidAndMotor();
lilac0112_1 19:967207de919d 87 SetInfo();
lilac0112_1 10:6df631c39f9b 88 SetLine();
lilac0112_1 0:ea35c18c85fc 89 }
lilac0112_1 0:ea35c18c85fc 90 void StopProcess(void){//コマンドに戻るときの処理
lilac0112_1 10:6df631c39f9b 91
lilac0112_1 10:6df631c39f9b 92 sys.KickOffFlag=0;
lilac0112_1 21:378470320524 93 SetKick();
lilac0112_1 10:6df631c39f9b 94 SetPidAndMotor();
lilac0112_1 19:967207de919d 95 SetInfo();
lilac0112_1 10:6df631c39f9b 96 SetLine();
lilac0112_1 0:ea35c18c85fc 97
lilac0112_1 0:ea35c18c85fc 98 __disable_irq(); // 禁止
lilac0112_1 0:ea35c18c85fc 99 __enable_irq(); // 許可
lilac0112_1 0:ea35c18c85fc 100 Sw_ticker.attach(Sw_sample, 0.050);
lilac0112_1 0:ea35c18c85fc 101 }
lilac0112_1 21:378470320524 102
lilac0112_1 9:c966191926c5 103 void SetKick(void){
lilac0112_1 9:c966191926c5 104 if(sys.KickOffFlag==1){
lilac0112_1 9:c966191926c5 105 kicker=0;
lilac0112_1 24:34ef6379b0df 106 ValidSolenoid();
lilac0112_1 9:c966191926c5 107 }
lilac0112_1 9:c966191926c5 108 else{
lilac0112_1 9:c966191926c5 109 kicker=1;
lilac0112_1 9:c966191926c5 110 wait(0.25);
lilac0112_1 9:c966191926c5 111 kicker=0;
lilac0112_1 9:c966191926c5 112 }
lilac0112_1 21:378470320524 113 }
lilac0112_1 9:c966191926c5 114 void SetPidAndMotor(void){
lilac0112_1 9:c966191926c5 115 if(sys.KickOffFlag==1){
lilac0112_1 9:c966191926c5 116 //compass
lilac0112_1 24:34ef6379b0df 117 ValidTurn();
lilac0112_1 10:6df631c39f9b 118 hmc_reset=HMC_RUN;
lilac0112_1 9:c966191926c5 119 for(int i=0; i<5; i++){
lilac0112_1 9:c966191926c5 120 ReadCmps();
lilac0112_1 9:c966191926c5 121 cmps_set.CmpsInitialValue = cmps_set.cmps;
lilac0112_1 9:c966191926c5 122 wait_ms(10);
lilac0112_1 9:c966191926c5 123 }
lilac0112_1 19:967207de919d 124 cmps_set.CmpsDiff = REFERENCE - cmps_set.cmps;
lilac0112_1 10:6df631c39f9b 125 if(sys.TurnStartFlag==1){
lilac0112_1 18:3a42a931c95a 126 //正．．．右回転
lilac0112_1 18:3a42a931c95a 127 //負．．．左回転
lilac0112_1 10:6df631c39f9b 128 //cmps_set.FrontDeg=-180;
lilac0112_1 18:3a42a931c95a 129 //cmps_set.FrontDeg=179;
lilac0112_1 18:3a42a931c95a 130 //cmps_set.FrontDeg=180;
lilac0112_1 19:967207de919d 131 cmps_set.FrontDeg=-180;
lilac0112_1 10:6df631c39f9b 132 }
lilac0112_1 10:6df631c39f9b 133 else{
lilac0112_1 10:6df631c39f9b 134 cmps_set.FrontDeg=0;
lilac0112_1 10:6df631c39f9b 135 }
lilac0112_1 10:6df631c39f9b 136 Motor_ticker.attach(&ValidTx_motor, 0.020);
lilac0112_1 19:967207de919d 137 //pidupdate.attach(&ValidPidUpdate, PID_CYCLE);
lilac0112_1 9:c966191926c5 138 }
lilac0112_1 9:c966191926c5 139 else{
lilac0112_1 19:967207de919d 140 //pidupdate.detach();
lilac0112_1 9:c966191926c5 141 Motor_ticker.detach();
lilac0112_1 9:c966191926c5 142 ////Hmc_ticker.detach();
lilac0112_1 9:c966191926c5 143 motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
lilac0112_1 9:c966191926c5 144 move(0,0,0);
lilac0112_1 9:c966191926c5 145 cmps_set.OutputPID=0;
lilac0112_1 9:c966191926c5 146 cmps_set.InputPID=REFERENCE;
lilac0112_1 21:378470320524 147 HmcReset();
lilac0112_1 9:c966191926c5 148 }
lilac0112_1 9:c966191926c5 149 }
lilac0112_1 19:967207de919d 150 void SetInfo(void){
lilac0112_1 19:967207de919d 151 if(sys.KickOffFlag==1){
lilac0112_1 19:967207de919d 152 Info_ticker.attach(&ValidInfo, 0.050);
lilac0112_1 19:967207de919d 153 }
lilac0112_1 19:967207de919d 154 else{
lilac0112_1 19:967207de919d 155 Info_ticker.detach();
lilac0112_1 19:967207de919d 156 }
lilac0112_1 19:967207de919d 157 }
lilac0112_1 9:c966191926c5 158 void SetLine(void){
lilac0112_1 9:c966191926c5 159 if(sys.KickOffFlag==1){
lilac0112_1 9:c966191926c5 160 //Line_ticker.attach(&ReadLine, 0.005);
lilac0112_1 16:4fadb7a87497 161 /*Line[A_SPOT].rise(&LineCall_A);
lilac0112_1 9:c966191926c5 162 Line[B_SPOT].rise(&LineCall_B);
lilac0112_1 16:4fadb7a87497 163 Line[C_SPOT].rise(&LineCall_C);*/
lilac0112_1 30:5998ba42237e 164 /*
lilac0112_1 16:4fadb7a87497 165 LineHolding[A_SPOT].rise(&LineRanking_A);
lilac0112_1 16:4fadb7a87497 166 LineHolding[B_SPOT].rise(&LineRanking_B);
lilac0112_1 16:4fadb7a87497 167 LineHolding[C_SPOT].rise(&LineRanking_C);
lilac0112_1 30:5998ba42237e 168 */
lilac0112_1 27:825c6835e3db 169 /*
lilac0112_1 27:825c6835e3db 170 Line[A_SPOT].rise(&LineRawCall_A);
lilac0112_1 27:825c6835e3db 171 Line[B_SPOT].rise(&LineRawCall_B);
lilac0112_1 27:825c6835e3db 172 Line[C_SPOT].rise(&LineRawCall_C);
lilac0112_1 27:825c6835e3db 173 */
lilac0112_1 27:825c6835e3db 174
lilac0112_1 27:825c6835e3db 175 Line[A_SPOT].rise(&LineRawRanking_A);
lilac0112_1 27:825c6835e3db 176 Line[B_SPOT].rise(&LineRawRanking_B);
lilac0112_1 27:825c6835e3db 177 Line[C_SPOT].rise(&LineRawRanking_C);
lilac0112_1 9:c966191926c5 178 }
lilac0112_1 9:c966191926c5 179 else{
lilac0112_1 9:c966191926c5 180 ////Line_ticker.detach();
lilac0112_1 9:c966191926c5 181 }
lilac0112_1 10:6df631c39f9b 182 }