CatPot 2015-2016 / Mbed 2 deprecated CatPot_2v10_T_Main

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

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/setup_command_active/setup.cpp@21:378470320524, 2016-03-16 (annotated)

Committer:
lilac0112_1
Date:
Wed Mar 16 11:27:58 2016 +0000
Revision:
21:378470320524
Parent:
19:967207de919d
Child:
24:34ef6379b0df
PING IR COMPASS >> INFO

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 9:c966191926c5 94 }
lilac0112_1 9:c966191926c5 95 else{
lilac0112_1 9:c966191926c5 96 kicker=1;
lilac0112_1 9:c966191926c5 97 wait(0.25);
lilac0112_1 9:c966191926c5 98 kicker=0;
lilac0112_1 9:c966191926c5 99 }
lilac0112_1 21:378470320524 100 }
lilac0112_1 9:c966191926c5 101 void SetPidAndMotor(void){
lilac0112_1 9:c966191926c5 102 if(sys.KickOffFlag==1){
lilac0112_1 9:c966191926c5 103 //compass
lilac0112_1 10:6df631c39f9b 104 hmc_reset=HMC_RUN;
lilac0112_1 9:c966191926c5 105 for(int i=0; i<5; i++){
lilac0112_1 9:c966191926c5 106 ReadCmps();
lilac0112_1 9:c966191926c5 107 cmps_set.CmpsInitialValue = cmps_set.cmps;
lilac0112_1 9:c966191926c5 108 wait_ms(10);
lilac0112_1 9:c966191926c5 109 }
lilac0112_1 19:967207de919d 110 cmps_set.CmpsDiff = REFERENCE - cmps_set.cmps;
lilac0112_1 10:6df631c39f9b 111 if(sys.TurnStartFlag==1){
lilac0112_1 18:3a42a931c95a 112 //正．．．右回転
lilac0112_1 18:3a42a931c95a 113 //負．．．左回転
lilac0112_1 10:6df631c39f9b 114 //cmps_set.FrontDeg=-180;
lilac0112_1 18:3a42a931c95a 115 //cmps_set.FrontDeg=179;
lilac0112_1 18:3a42a931c95a 116 //cmps_set.FrontDeg=180;
lilac0112_1 19:967207de919d 117 cmps_set.FrontDeg=-180;
lilac0112_1 10:6df631c39f9b 118 }
lilac0112_1 10:6df631c39f9b 119 else{
lilac0112_1 10:6df631c39f9b 120 cmps_set.FrontDeg=0;
lilac0112_1 10:6df631c39f9b 121 }
lilac0112_1 10:6df631c39f9b 122 Motor_ticker.attach(&ValidTx_motor, 0.020);
lilac0112_1 19:967207de919d 123 //pidupdate.attach(&ValidPidUpdate, PID_CYCLE);
lilac0112_1 9:c966191926c5 124 }
lilac0112_1 9:c966191926c5 125 else{
lilac0112_1 19:967207de919d 126 //pidupdate.detach();
lilac0112_1 9:c966191926c5 127 Motor_ticker.detach();
lilac0112_1 9:c966191926c5 128 ////Hmc_ticker.detach();
lilac0112_1 9:c966191926c5 129 motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
lilac0112_1 9:c966191926c5 130 move(0,0,0);
lilac0112_1 9:c966191926c5 131 cmps_set.OutputPID=0;
lilac0112_1 9:c966191926c5 132 cmps_set.InputPID=REFERENCE;
lilac0112_1 21:378470320524 133 HmcReset();
lilac0112_1 9:c966191926c5 134 }
lilac0112_1 9:c966191926c5 135 }
lilac0112_1 19:967207de919d 136 void SetInfo(void){
lilac0112_1 19:967207de919d 137 if(sys.KickOffFlag==1){
lilac0112_1 19:967207de919d 138 Info_ticker.attach(&ValidInfo, 0.050);
lilac0112_1 19:967207de919d 139 }
lilac0112_1 19:967207de919d 140 else{
lilac0112_1 19:967207de919d 141 Info_ticker.detach();
lilac0112_1 19:967207de919d 142 }
lilac0112_1 19:967207de919d 143 }
lilac0112_1 9:c966191926c5 144 void SetLine(void){
lilac0112_1 9:c966191926c5 145 if(sys.KickOffFlag==1){
lilac0112_1 9:c966191926c5 146 //Line_ticker.attach(&ReadLine, 0.005);
lilac0112_1 16:4fadb7a87497 147 /*Line[A_SPOT].rise(&LineCall_A);
lilac0112_1 9:c966191926c5 148 Line[B_SPOT].rise(&LineCall_B);
lilac0112_1 16:4fadb7a87497 149 Line[C_SPOT].rise(&LineCall_C);*/
lilac0112_1 16:4fadb7a87497 150
lilac0112_1 16:4fadb7a87497 151 LineHolding[A_SPOT].rise(&LineRanking_A);
lilac0112_1 16:4fadb7a87497 152 LineHolding[B_SPOT].rise(&LineRanking_B);
lilac0112_1 16:4fadb7a87497 153 LineHolding[C_SPOT].rise(&LineRanking_C);
lilac0112_1 9:c966191926c5 154 }
lilac0112_1 9:c966191926c5 155 else{
lilac0112_1 9:c966191926c5 156 ////Line_ticker.detach();
lilac0112_1 9:c966191926c5 157 }
lilac0112_1 10:6df631c39f9b 158 }