CatPot 2015-2016 / Mbed 2 deprecated CatPot_2v10_T_Main

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

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/setup_command_active/setup.cpp@16:4fadb7a87497, 2016-03-14 (annotated)

Committer:
lilac0112_1
Date:
Mon Mar 14 07:51:37 2016 +0000
Revision:
16:4fadb7a87497
Parent:
10:6df631c39f9b
Child:
18:3a42a931c95a
line line line

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.KickFlag=0;
lilac0112_1 5:5ff3a7d5d8c2 27 sys.MotorFlag=0;
lilac0112_1 5:5ff3a7d5d8c2 28 sys.IrFlag=0;
lilac0112_1 5:5ff3a7d5d8c2 29 sys.UswFlag=0;
lilac0112_1 10:6df631c39f9b 30 sys.UswFlag2=0;
lilac0112_1 5:5ff3a7d5d8c2 31 sys.PidFlag=0;
lilac0112_1 5:5ff3a7d5d8c2 32 //motor
lilac0112_1 7:efe90188a49d 33 sys.pow_num = 1;
lilac0112_1 7:efe90188a49d 34 sys.s_pow = ir_pow_val[sys.pow_num][POW_SHORT];
lilac0112_1 7:efe90188a49d 35 sys.m_pow = ir_pow_val[sys.pow_num][POW_MIDDLE];
lilac0112_1 7:efe90188a49d 36 sys.l_pow = ir_pow_val[sys.pow_num][POW_LONG];
lilac0112_1 7:efe90188a49d 37 /*sys.s_pow = 30;
lilac0112_1 6:f9167470d819 38 sys.m_pow = 30;
lilac0112_1 7:efe90188a49d 39 sys.l_pow = 30;*/
lilac0112_1 10:6df631c39f9b 40 sys.dri_pow = 50;
lilac0112_1 2:635947de1583 41 sys.ir_pow_table = 0;
lilac0112_1 2:635947de1583 42
lilac0112_1 2:635947de1583 43 //ir
lilac0112_1 2:635947de1583 44 data.irNotice=IR_NONE;
lilac0112_1 0:ea35c18c85fc 45
lilac0112_1 0:ea35c18c85fc 46 //spi
lilac0112_1 0:ea35c18c85fc 47 spi.format(16, 3);
lilac0112_1 0:ea35c18c85fc 48 spi.frequency(1000000);
lilac0112_1 0:ea35c18c85fc 49 spi_ss[0]=spi_ss[1]=spi_ss[2]=spi_ss[3]=1;
lilac0112_1 0:ea35c18c85fc 50
lilac0112_1 0:ea35c18c85fc 51 //motor
lilac0112_1 0:ea35c18c85fc 52 motor.baud(115200); //ボーレート設定
lilac0112_1 0:ea35c18c85fc 53 motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
lilac0112_1 0:ea35c18c85fc 54 //compass
lilac0112_1 10:6df631c39f9b 55 hmc_reset=HMC_RUN;
lilac0112_1 0:ea35c18c85fc 56 for(int i=0; i<5; i++){
lilac0112_1 0:ea35c18c85fc 57 ReadCmps();
lilac0112_1 0:ea35c18c85fc 58 cmps_set.CmpsInitialValue = cmps_set.cmps;
lilac0112_1 0:ea35c18c85fc 59 wait_ms(10);
lilac0112_1 0:ea35c18c85fc 60 }
lilac0112_1 0:ea35c18c85fc 61 cmps_set.CmpsDiff = REFERENCE - cmps_set.cmps;
lilac0112_1 0:ea35c18c85fc 62 cmps_set.FrontDeg=0;
lilac0112_1 0:ea35c18c85fc 63
lilac0112_1 0:ea35c18c85fc 64 pid.setInputLimits(MINIMUM,MAXIMUM); //pid sed def
lilac0112_1 0:ea35c18c85fc 65 pid.setOutputLimits(-OUT_LIMIT, OUT_LIMIT); //pid sed def
lilac0112_1 0:ea35c18c85fc 66 pid.setBias(PID_BIAS); //pid sed def
lilac0112_1 0:ea35c18c85fc 67 pid.setMode(AUTO_MODE); //pid sed def
lilac0112_1 0:ea35c18c85fc 68 pid.setSetPoint(REFERENCE); //pid sed def
lilac0112_1 0:ea35c18c85fc 69 //pidupdate.attach(&PidUpdate, PID_CYCLE);
lilac0112_1 0:ea35c18c85fc 70 }
lilac0112_1 0:ea35c18c85fc 71 void SetUp2(void){//スタートした時の処理
lilac0112_1 0:ea35c18c85fc 72
lilac0112_1 0:ea35c18c85fc 73 __enable_irq(); // 許可
lilac0112_1 9:c966191926c5 74 sys.KickOffFlag=1;
lilac0112_1 0:ea35c18c85fc 75
lilac0112_1 10:6df631c39f9b 76 SetKick();
lilac0112_1 10:6df631c39f9b 77 SetPidAndMotor();
lilac0112_1 10:6df631c39f9b 78 SetIr();
lilac0112_1 10:6df631c39f9b 79 SetPing();
lilac0112_1 10:6df631c39f9b 80 SetLine();
lilac0112_1 10:6df631c39f9b 81 /*
lilac0112_1 9:c966191926c5 82 //solenoid
lilac0112_1 0:ea35c18c85fc 83 sys.KickFlag=0;
lilac0112_1 0:ea35c18c85fc 84 kicker=0;
lilac0112_1 0:ea35c18c85fc 85 //compass
lilac0112_1 10:6df631c39f9b 86 hmc_reset=HMC_RUN;
lilac0112_1 0:ea35c18c85fc 87 for(int i=0; i<5; i++){
lilac0112_1 0:ea35c18c85fc 88 ReadCmps();
lilac0112_1 0:ea35c18c85fc 89 cmps_set.CmpsInitialValue = cmps_set.cmps;
lilac0112_1 0:ea35c18c85fc 90 wait_ms(10);
lilac0112_1 0:ea35c18c85fc 91 }
lilac0112_1 0:ea35c18c85fc 92 cmps_set.CmpsDiff = REFERENCE - cmps_set.cmps;
lilac0112_1 0:ea35c18c85fc 93 cmps_set.FrontDeg=0;
lilac0112_1 0:ea35c18c85fc 94
lilac0112_1 0:ea35c18c85fc 95 //Line_ticker.attach(&ReadLine, 0.005);
lilac0112_1 0:ea35c18c85fc 96 Line[A_SPOT].rise(&LineCall_A);
lilac0112_1 0:ea35c18c85fc 97 Line[B_SPOT].rise(&LineCall_B);
lilac0112_1 0:ea35c18c85fc 98 Line[C_SPOT].rise(&LineCall_C);
lilac0112_1 0:ea35c18c85fc 99 Solenoid_ticker.attach(&AvailableSolenoid, 6.0);
lilac0112_1 0:ea35c18c85fc 100 Ir_ticker.attach(&ValidIr, 0.050);
lilac0112_1 0:ea35c18c85fc 101 Ping_ticker.attach(&ValidPing, 0.050);
lilac0112_1 0:ea35c18c85fc 102 //Hmc_ticker.attach(&HmcReset, 1.0);
lilac0112_1 0:ea35c18c85fc 103 Motor_ticker.attach(&ValidTx_motor, 0.020);
lilac0112_1 0:ea35c18c85fc 104 pidupdate.attach(&ValidPidUpdate, PID_CYCLE);
lilac0112_1 10:6df631c39f9b 105 */
lilac0112_1 0:ea35c18c85fc 106 }
lilac0112_1 0:ea35c18c85fc 107 void StopProcess(void){//コマンドに戻るときの処理
lilac0112_1 10:6df631c39f9b 108
lilac0112_1 10:6df631c39f9b 109 sys.KickOffFlag=0;
lilac0112_1 10:6df631c39f9b 110
lilac0112_1 10:6df631c39f9b 111 SetKick();
lilac0112_1 10:6df631c39f9b 112 SetPidAndMotor();
lilac0112_1 10:6df631c39f9b 113 SetIr();
lilac0112_1 10:6df631c39f9b 114 SetPing();
lilac0112_1 10:6df631c39f9b 115 SetLine();
lilac0112_1 10:6df631c39f9b 116 /*
lilac0112_1 0:ea35c18c85fc 117 pidupdate.detach();
lilac0112_1 0:ea35c18c85fc 118 Motor_ticker.detach();
lilac0112_1 0:ea35c18c85fc 119 ////Line_ticker.detach();
lilac0112_1 0:ea35c18c85fc 120 Ir_ticker.detach();
lilac0112_1 0:ea35c18c85fc 121 Ping_ticker.detach();
lilac0112_1 0:ea35c18c85fc 122 ////Hmc_ticker.detach();
lilac0112_1 0:ea35c18c85fc 123 motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
lilac0112_1 0:ea35c18c85fc 124 move(0,0,0);
lilac0112_1 0:ea35c18c85fc 125 cmps_set.OutputPID=0;
lilac0112_1 0:ea35c18c85fc 126 cmps_set.InputPID=REFERENCE;
lilac0112_1 0:ea35c18c85fc 127 //solenoid
lilac0112_1 9:c966191926c5 128 Solenoid_ticker.detach();
lilac0112_1 0:ea35c18c85fc 129 kicker=1;
lilac0112_1 0:ea35c18c85fc 130 wait(0.25);
lilac0112_1 0:ea35c18c85fc 131 kicker=0;
lilac0112_1 10:6df631c39f9b 132 */
lilac0112_1 0:ea35c18c85fc 133
lilac0112_1 0:ea35c18c85fc 134 __disable_irq(); // 禁止
lilac0112_1 0:ea35c18c85fc 135 __enable_irq(); // 許可
lilac0112_1 0:ea35c18c85fc 136 Sw_ticker.attach(Sw_sample, 0.050);
lilac0112_1 0:ea35c18c85fc 137
lilac0112_1 0:ea35c18c85fc 138 }
lilac0112_1 9:c966191926c5 139 void SetKick(void){
lilac0112_1 9:c966191926c5 140 if(sys.KickOffFlag==1){
lilac0112_1 9:c966191926c5 141 //solenoid
lilac0112_1 9:c966191926c5 142 sys.KickFlag=0;
lilac0112_1 9:c966191926c5 143 kicker=0;
lilac0112_1 9:c966191926c5 144 Solenoid_ticker.attach(&AvailableSolenoid, 6.0);
lilac0112_1 9:c966191926c5 145 }
lilac0112_1 9:c966191926c5 146 else{
lilac0112_1 9:c966191926c5 147 //solenoid
lilac0112_1 9:c966191926c5 148 Solenoid_ticker.detach();
lilac0112_1 9:c966191926c5 149 kicker=1;
lilac0112_1 9:c966191926c5 150 wait(0.25);
lilac0112_1 9:c966191926c5 151 kicker=0;
lilac0112_1 9:c966191926c5 152 }
lilac0112_1 9:c966191926c5 153 }
lilac0112_1 9:c966191926c5 154 void SetPidAndMotor(void){
lilac0112_1 9:c966191926c5 155 if(sys.KickOffFlag==1){
lilac0112_1 9:c966191926c5 156 //compass
lilac0112_1 10:6df631c39f9b 157 hmc_reset=HMC_RUN;
lilac0112_1 9:c966191926c5 158 for(int i=0; i<5; i++){
lilac0112_1 9:c966191926c5 159 ReadCmps();
lilac0112_1 9:c966191926c5 160 cmps_set.CmpsInitialValue = cmps_set.cmps;
lilac0112_1 9:c966191926c5 161 wait_ms(10);
lilac0112_1 9:c966191926c5 162 }
lilac0112_1 10:6df631c39f9b 163 if(sys.TurnStartFlag==1){
lilac0112_1 10:6df631c39f9b 164 //cmps_set.FrontDeg=-180;
lilac0112_1 10:6df631c39f9b 165 cmps_set.FrontDeg=179;
lilac0112_1 10:6df631c39f9b 166 }
lilac0112_1 10:6df631c39f9b 167 else{
lilac0112_1 10:6df631c39f9b 168 cmps_set.FrontDeg=0;
lilac0112_1 10:6df631c39f9b 169 }
lilac0112_1 10:6df631c39f9b 170 FrontHere();
lilac0112_1 10:6df631c39f9b 171 Motor_ticker.attach(&ValidTx_motor, 0.020);
lilac0112_1 9:c966191926c5 172 pidupdate.attach(&ValidPidUpdate, PID_CYCLE);
lilac0112_1 9:c966191926c5 173 }
lilac0112_1 9:c966191926c5 174 else{
lilac0112_1 9:c966191926c5 175 pidupdate.detach();
lilac0112_1 9:c966191926c5 176 Motor_ticker.detach();
lilac0112_1 9:c966191926c5 177 ////Hmc_ticker.detach();
lilac0112_1 9:c966191926c5 178 motor.printf("1F0002F0003F0004F000\r\n"); //モータ停止
lilac0112_1 9:c966191926c5 179 move(0,0,0);
lilac0112_1 9:c966191926c5 180 cmps_set.OutputPID=0;
lilac0112_1 9:c966191926c5 181 cmps_set.InputPID=REFERENCE;
lilac0112_1 9:c966191926c5 182 }
lilac0112_1 9:c966191926c5 183 }
lilac0112_1 9:c966191926c5 184 void SetIr(void){
lilac0112_1 9:c966191926c5 185 if(sys.KickOffFlag==1){
lilac0112_1 9:c966191926c5 186 Ir_ticker.attach(&ValidIr, 0.050);
lilac0112_1 9:c966191926c5 187 }
lilac0112_1 9:c966191926c5 188 else{
lilac0112_1 9:c966191926c5 189 Ir_ticker.detach();
lilac0112_1 9:c966191926c5 190 }
lilac0112_1 9:c966191926c5 191 }
lilac0112_1 9:c966191926c5 192 void SetPing(void){
lilac0112_1 9:c966191926c5 193 if(sys.KickOffFlag==1){
lilac0112_1 9:c966191926c5 194 Ping_ticker.attach(&ValidPing, 0.050);
lilac0112_1 10:6df631c39f9b 195 Ping_ticker2.attach(&ValidPing2, 0.050);
lilac0112_1 9:c966191926c5 196 }
lilac0112_1 9:c966191926c5 197 else{
lilac0112_1 9:c966191926c5 198 Ping_ticker.detach();
lilac0112_1 10:6df631c39f9b 199 Ping_ticker2.detach();
lilac0112_1 9:c966191926c5 200 }
lilac0112_1 9:c966191926c5 201 }
lilac0112_1 9:c966191926c5 202 void SetLine(void){
lilac0112_1 9:c966191926c5 203 if(sys.KickOffFlag==1){
lilac0112_1 9:c966191926c5 204 //Line_ticker.attach(&ReadLine, 0.005);
lilac0112_1 16:4fadb7a87497 205 /*Line[A_SPOT].rise(&LineCall_A);
lilac0112_1 9:c966191926c5 206 Line[B_SPOT].rise(&LineCall_B);
lilac0112_1 16:4fadb7a87497 207 Line[C_SPOT].rise(&LineCall_C);*/
lilac0112_1 16:4fadb7a87497 208
lilac0112_1 16:4fadb7a87497 209 LineHolding[A_SPOT].rise(&LineRanking_A);
lilac0112_1 16:4fadb7a87497 210 LineHolding[B_SPOT].rise(&LineRanking_B);
lilac0112_1 16:4fadb7a87497 211 LineHolding[C_SPOT].rise(&LineRanking_C);
lilac0112_1 9:c966191926c5 212
lilac0112_1 9:c966191926c5 213 //LineHolding[A_SPOT].rise(&LineCall_ABC);
lilac0112_1 9:c966191926c5 214 //LineHolding[B_SPOT].rise(&LineCall_ABC);
lilac0112_1 9:c966191926c5 215 //LineHolding[C_SPOT].rise(&LineCall_ABC);
lilac0112_1 9:c966191926c5 216 }
lilac0112_1 9:c966191926c5 217 else{
lilac0112_1 9:c966191926c5 218 ////Line_ticker.detach();
lilac0112_1 9:c966191926c5 219 }
lilac0112_1 10:6df631c39f9b 220 }