CatPot 2015-2016 / Mbed 2 deprecated CatPot_2v10_T_Main

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

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/setup_command_active/setup.cpp@27:825c6835e3db, 2016-03-21 (annotated)

Committer:
lilac0112_1
Date:
Mon Mar 21 17:05:08 2016 +0000
Revision:
27:825c6835e3db
Parent:
24:34ef6379b0df
Child:
30:5998ba42237e
somehow

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