CatPot 2015-2016 / Mbed 2 deprecated CatPot_2v10_T_Main

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

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/setup_command_active/setup.cpp@9:c966191926c5, 2016-03-08 (annotated)

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