CatPot 2015-2016 / Mbed 2 deprecated CatPot_2v10_T_Main

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

Dependencies:   mbed AQM1602 HMC6352 PID

main_processing/setup_command_active/command_functions.cpp@38:67bc78f3c0ab, 2016-03-27 (annotated)

Committer:
lilac0112_1
Date:
Sun Mar 27 13:04:39 2016 +0000
Revision:
38:67bc78f3c0ab
Parent:
36:b1ebcc2c1f70 
JapanSoccerOpen2016 CatPot Program(main)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lilac0112_1 3:c49ea7ad8e91 1 #include "mbed.h"
lilac0112_1 3:c49ea7ad8e91 2 #include "extern.h"
lilac0112_1 3:c49ea7ad8e91 3
lilac0112_1 4:52da8da146d4 4 uint8_t ZeroFunction(uint8_t x){
lilac0112_1 7:efe90188a49d 5 char buf[LCD_COLUMN_NUM];
lilac0112_1 4:52da8da146d4 6 if(x==1){
lilac0112_1 14:b510adcb6065 7 sprintf(buf, "%2d%.14s", sys.strategy, act[sys.strategy].LcdStr);
lilac0112_1 4:52da8da146d4 8 Lcd.locate(0, 1);Lcd.print(buf);wait_ms(50);return 0;
lilac0112_1 4:52da8da146d4 9 }
lilac0112_1 10:6df631c39f9b 10 else if(x==2){
lilac0112_1 10:6df631c39f9b 11 sprintf(buf, "%2d:S%2d M%2d L%2d",
lilac0112_1 10:6df631c39f9b 12 sys.pow_num,
lilac0112_1 10:6df631c39f9b 13 ir_pow_val[sys.pow_num][POW_SHORT],
lilac0112_1 10:6df631c39f9b 14 ir_pow_val[sys.pow_num][POW_MIDDLE],
lilac0112_1 10:6df631c39f9b 15 ir_pow_val[sys.pow_num][POW_LONG]
lilac0112_1 10:6df631c39f9b 16 );
lilac0112_1 10:6df631c39f9b 17 Lcd.locate(0, 1);Lcd.print(buf);
lilac0112_1 10:6df631c39f9b 18 return 0;
lilac0112_1 10:6df631c39f9b 19 }
lilac0112_1 10:6df631c39f9b 20 else{//x==3
lilac0112_1 4:52da8da146d4 21 sprintf(buf, "SeeYouAgain!");
lilac0112_1 4:52da8da146d4 22 Lcd.locate(0, 1);Lcd.print(buf);wait(0.5);
lilac0112_1 4:52da8da146d4 23 NVIC_SystemReset();
lilac0112_1 4:52da8da146d4 24 return 1;
lilac0112_1 4:52da8da146d4 25 }
lilac0112_1 4:52da8da146d4 26 }
lilac0112_1 4:52da8da146d4 27 uint8_t Start(uint8_t x){
lilac0112_1 7:efe90188a49d 28 char buf[LCD_COLUMN_NUM];
lilac0112_1 8:25c8c75d182a 29 uint8_t temp;
lilac0112_1 8:25c8c75d182a 30 temp = sys.strategy;
lilac0112_1 4:52da8da146d4 31 sprintf(buf, "CatPot");
lilac0112_1 4:52da8da146d4 32 Lcd.locate(0, 1);Lcd.print(buf);
lilac0112_1 4:52da8da146d4 33 sys.strategy=0;
lilac0112_1 10:6df631c39f9b 34 sys.TurnStartFlag=0;
lilac0112_1 10:6df631c39f9b 35 Active();
lilac0112_1 10:6df631c39f9b 36 sys.strategy=temp;
lilac0112_1 32:367b16d69a32 37 //reset
lilac0112_1 32:367b16d69a32 38 NVIC_SystemReset();
lilac0112_1 10:6df631c39f9b 39 return 1;
lilac0112_1 10:6df631c39f9b 40 }
lilac0112_1 10:6df631c39f9b 41 uint8_t TurnAndStart(uint8_t x){
lilac0112_1 10:6df631c39f9b 42 char buf[LCD_COLUMN_NUM];
lilac0112_1 10:6df631c39f9b 43 uint8_t temp;
lilac0112_1 10:6df631c39f9b 44 temp = sys.strategy;
lilac0112_1 10:6df631c39f9b 45 sprintf(buf, "CatPot");
lilac0112_1 10:6df631c39f9b 46 Lcd.locate(0, 1);Lcd.print(buf);
lilac0112_1 10:6df631c39f9b 47 sys.strategy=0;
lilac0112_1 10:6df631c39f9b 48 sys.TurnStartFlag=1;
lilac0112_1 5:5ff3a7d5d8c2 49 Active();
lilac0112_1 8:25c8c75d182a 50 sys.strategy=temp;
lilac0112_1 32:367b16d69a32 51 //reset
lilac0112_1 32:367b16d69a32 52 NVIC_SystemReset();
lilac0112_1 4:52da8da146d4 53 return 1;
lilac0112_1 4:52da8da146d4 54 }
lilac0112_1 4:52da8da146d4 55 uint8_t GetIr(uint8_t x){
lilac0112_1 7:efe90188a49d 56 char buf[LCD_COLUMN_NUM];
lilac0112_1 4:52da8da146d4 57 ReadIr();
lilac0112_1 4:52da8da146d4 58
lilac0112_1 4:52da8da146d4 59 //if(x==1) sprintf(buf, "SHRT:%2d, LNG:%2d", data.irSpot[0], data.irSpot[1]);
lilac0112_1 4:52da8da146d4 60 //if(x==2) sprintf(buf, "NOTE:%2d", data.irNotice);
lilac0112_1 4:52da8da146d4 61 //if(x==3) sprintf(buf, "POSITION:%2d", data.irPosition);
lilac0112_1 4:52da8da146d4 62 if(x==1) sprintf(buf, "POSI:%2d NOTE:%1d", data.irPosition, data.irNotice);
lilac0112_1 4:52da8da146d4 63 if(x==2) sprintf(buf, "PH_L:%1d PH_S:%1d", data.irValPhase[IR_LONG], data.irValPhase[IR_SHORT]);
lilac0112_1 4:52da8da146d4 64 if(x==3) sprintf(buf, "DIF:%1d KEY:%2d", data.irDif[IR_LONG], data.irKey);
lilac0112_1 4:52da8da146d4 65 Lcd.locate(0, 1);Lcd.print(buf);wait_ms(50);return 0;
lilac0112_1 4:52da8da146d4 66 }
lilac0112_1 10:6df631c39f9b 67 uint8_t GetSns0(uint8_t x){
lilac0112_1 7:efe90188a49d 68 char buf[LCD_COLUMN_NUM];
lilac0112_1 4:52da8da146d4 69 uint8_t raw[3];
lilac0112_1 4:52da8da146d4 70 uint8_t held[3];
lilac0112_1 4:52da8da146d4 71 if(x==1){
lilac0112_1 11:3efae754e6ef 72 LineKeeper=LINE_FIX;//line
lilac0112_1 10:6df631c39f9b 73
lilac0112_1 4:52da8da146d4 74 raw[2] = GetBit(LineRaw, 3);
lilac0112_1 4:52da8da146d4 75 raw[1] = GetBit(LineRaw, 2);
lilac0112_1 4:52da8da146d4 76 raw[0] = GetBit(LineRaw, 1);
lilac0112_1 4:52da8da146d4 77
lilac0112_1 4:52da8da146d4 78 held[2] = GetBit(LineHold, 3);
lilac0112_1 4:52da8da146d4 79 held[1] = GetBit(LineHold, 2);
lilac0112_1 4:52da8da146d4 80 held[0] = GetBit(LineHold, 1);
lilac0112_1 4:52da8da146d4 81 //BusOut LineKeeper(lineInA, lineInB, lineInC);
lilac0112_1 4:52da8da146d4 82 //BusIn LineRaw(lineA2, lineB2, lineC2);
lilac0112_1 4:52da8da146d4 83 //BusIn LineHold(lineA1, lineB1, lineC1);
lilac0112_1 4:52da8da146d4 84 sprintf(buf, "A:%1d%1d B:%1d%1d C:%1d%1d ", raw[0],held[0],raw[1],held[1],raw[2],held[2]);
lilac0112_1 4:52da8da146d4 85 //sprintf(buf, "A:%1d B:%1d C:%1d ", Line[0].read(), Line[1].read(), Line[2].read());
lilac0112_1 4:52da8da146d4 86 Lcd.locate(0, 1);Lcd.print(buf);wait_ms(50);return 0;
lilac0112_1 4:52da8da146d4 87 }
lilac0112_1 4:52da8da146d4 88 if(x==2){
lilac0112_1 10:6df631c39f9b 89 ReadPing2();
lilac0112_1 4:52da8da146d4 90 ReadPing();
lilac0112_1 10:6df631c39f9b 91 sprintf(buf, "L%3dR%3dF%3dB%3d", data.ping[L_PING], data.ping[R_PING], data.ping[F_PING], data.ping[B_PING]);
lilac0112_1 4:52da8da146d4 92 Lcd.locate(0, 1);Lcd.print(buf);wait_ms(50);return 0;
lilac0112_1 4:52da8da146d4 93 }
lilac0112_1 4:52da8da146d4 94 if(x==3){
lilac0112_1 4:52da8da146d4 95 //ReadIr();
lilac0112_1 4:52da8da146d4 96 //sprintf(buf, "IR_KEY:%2d", data.irKey);
lilac0112_1 4:52da8da146d4 97 //sprintf(buf, "BALL:%1d", BallChecker.read());
lilac0112_1 4:52da8da146d4 98 sprintf(buf, "BALLA%6d", BallCheckerA.read_u16());
lilac0112_1 4:52da8da146d4 99 Lcd.locate(0, 1);Lcd.print(buf);wait_ms(50);return 0;
lilac0112_1 4:52da8da146d4 100 }
lilac0112_1 4:52da8da146d4 101 return 0;
lilac0112_1 4:52da8da146d4 102 }
lilac0112_1 10:6df631c39f9b 103 uint8_t RwPid(uint8_t x){
lilac0112_1 10:6df631c39f9b 104 char buf[LCD_COLUMN_NUM];
lilac0112_1 10:6df631c39f9b 105 if(x==1){
lilac0112_1 10:6df631c39f9b 106 PidUpdate();
lilac0112_1 10:6df631c39f9b 107 sprintf(buf, "In:%03.0f Out:%+02d", cmps_set.InputPID, cmps_set.OutputPID);
lilac0112_1 10:6df631c39f9b 108 Lcd.locate(0, 1);Lcd.print(buf);wait_ms(50);return 0;
lilac0112_1 10:6df631c39f9b 109 }
lilac0112_1 10:6df631c39f9b 110 if(x==2){
lilac0112_1 10:6df631c39f9b 111 sys.jump_flag=START;
lilac0112_1 10:6df631c39f9b 112 sprintf(buf, "FaceToFront!");
lilac0112_1 10:6df631c39f9b 113 Lcd.locate(0, 1);Lcd.print(buf);
lilac0112_1 10:6df631c39f9b 114 hmc_reset = HMC_RST;
lilac0112_1 28:f246a5fbf6a5 115 wait_ms(100);
lilac0112_1 10:6df631c39f9b 116 hmc_reset = HMC_RUN;
lilac0112_1 10:6df631c39f9b 117 for(int i=0; i<5; i++){
lilac0112_1 10:6df631c39f9b 118 ReadCmps();
lilac0112_1 10:6df631c39f9b 119 cmps_set.CmpsInitialValue = cmps_set.cmps;
lilac0112_1 10:6df631c39f9b 120 wait_ms(100);
lilac0112_1 10:6df631c39f9b 121 }
lilac0112_1 10:6df631c39f9b 122 cmps_set.CmpsDiff = REFERENCE - cmps_set.cmps;
lilac0112_1 10:6df631c39f9b 123 cmps_set.FrontDeg=0;
lilac0112_1 10:6df631c39f9b 124 return 1;
lilac0112_1 10:6df631c39f9b 125 }
lilac0112_1 10:6df631c39f9b 126 return 1;
lilac0112_1 10:6df631c39f9b 127 }
lilac0112_1 10:6df631c39f9b 128 uint8_t CalibrationEnterOrExit(uint8_t x){
lilac0112_1 10:6df631c39f9b 129 static uint8_t state=0;
lilac0112_1 10:6df631c39f9b 130 char buf[LCD_COLUMN_NUM];
lilac0112_1 10:6df631c39f9b 131
lilac0112_1 10:6df631c39f9b 132 if(x==1){
lilac0112_1 10:6df631c39f9b 133 if(state==0){
lilac0112_1 10:6df631c39f9b 134 sprintf(buf, "Exit>>Enter");
lilac0112_1 10:6df631c39f9b 135 }
lilac0112_1 10:6df631c39f9b 136 if(state==1){
lilac0112_1 10:6df631c39f9b 137 sprintf(buf, "Enter>>Exit");
lilac0112_1 10:6df631c39f9b 138 }
lilac0112_1 10:6df631c39f9b 139 Lcd.locate(0, 1);Lcd.print(buf);
lilac0112_1 10:6df631c39f9b 140 return 0;
lilac0112_1 10:6df631c39f9b 141 }
lilac0112_1 10:6df631c39f9b 142 if(x==2){
lilac0112_1 10:6df631c39f9b 143 if(state==0){
lilac0112_1 10:6df631c39f9b 144 hmc.setCalibrationMode(HMC6352_ENTER_CALIB);
lilac0112_1 10:6df631c39f9b 145 state=1;
lilac0112_1 10:6df631c39f9b 146 return 1;
lilac0112_1 10:6df631c39f9b 147 }
lilac0112_1 10:6df631c39f9b 148 if(state==1){
lilac0112_1 10:6df631c39f9b 149 hmc.setCalibrationMode(HMC6352_EXIT_CALIB);
lilac0112_1 10:6df631c39f9b 150 state=0;
lilac0112_1 10:6df631c39f9b 151 return 1;
lilac0112_1 10:6df631c39f9b 152 }
lilac0112_1 10:6df631c39f9b 153 }
lilac0112_1 10:6df631c39f9b 154 return 1;
lilac0112_1 10:6df631c39f9b 155 }
lilac0112_1 10:6df631c39f9b 156 uint8_t CalibrationTurn(uint8_t x){
lilac0112_1 10:6df631c39f9b 157 char buf[LCD_COLUMN_NUM];
lilac0112_1 10:6df631c39f9b 158 sprintf(buf, "CalibrationTurn");
lilac0112_1 10:6df631c39f9b 159 Lcd.locate(0, 1);Lcd.print(buf);
lilac0112_1 10:6df631c39f9b 160 hmc.setCalibrationMode(HMC6352_ENTER_CALIB);wait_ms(20);
lilac0112_1 10:6df631c39f9b 161 move(0,0,5);tx_motor();
lilac0112_1 10:6df631c39f9b 162 wait(5);
lilac0112_1 10:6df631c39f9b 163 move(0,0,0);tx_motor();
lilac0112_1 10:6df631c39f9b 164 hmc.setCalibrationMode(HMC6352_EXIT_CALIB);wait_ms(20);
lilac0112_1 10:6df631c39f9b 165 sys.jump_flag=GET_PID_VALUE;
lilac0112_1 10:6df631c39f9b 166 return 1;
lilac0112_1 10:6df631c39f9b 167 }
lilac0112_1 7:efe90188a49d 168 uint8_t SetPowerUp(uint8_t x){
lilac0112_1 7:efe90188a49d 169 char buf[LCD_COLUMN_NUM];
lilac0112_1 7:efe90188a49d 170 uint8_t i=(sys.pow_num+(-1+POW_COMBI_NUM))%POW_COMBI_NUM;
lilac0112_1 7:efe90188a49d 171 if(x==1){
lilac0112_1 7:efe90188a49d 172 sprintf(buf, "%2d:S%2d M%2d L%2d", i, ir_pow_val[i][POW_SHORT], ir_pow_val[i][POW_MIDDLE], ir_pow_val[i][POW_LONG]);
lilac0112_1 7:efe90188a49d 173 Lcd.locate(0, 1);Lcd.print(buf);
lilac0112_1 7:efe90188a49d 174 return 0;
lilac0112_1 7:efe90188a49d 175 }
lilac0112_1 7:efe90188a49d 176 else{//x==2
lilac0112_1 7:efe90188a49d 177 sys.pow_num = i;
lilac0112_1 7:efe90188a49d 178 sys.s_pow = ir_pow_val[sys.pow_num][POW_SHORT];
lilac0112_1 7:efe90188a49d 179 sys.m_pow = ir_pow_val[sys.pow_num][POW_MIDDLE];
lilac0112_1 7:efe90188a49d 180 sys.l_pow = ir_pow_val[sys.pow_num][POW_LONG];
lilac0112_1 7:efe90188a49d 181 return 1;
lilac0112_1 7:efe90188a49d 182 }
lilac0112_1 7:efe90188a49d 183 }
lilac0112_1 7:efe90188a49d 184 uint8_t SetPowerDown(uint8_t x){
lilac0112_1 7:efe90188a49d 185 char buf[LCD_COLUMN_NUM];
lilac0112_1 7:efe90188a49d 186 uint8_t i=(sys.pow_num+1)%POW_COMBI_NUM;
lilac0112_1 7:efe90188a49d 187 if(x==1){
lilac0112_1 7:efe90188a49d 188 sprintf(buf, "%2d:S%2d M%2d L%2d", i, ir_pow_val[i][POW_SHORT], ir_pow_val[i][POW_MIDDLE], ir_pow_val[i][POW_LONG]);
lilac0112_1 7:efe90188a49d 189 Lcd.locate(0, 1);Lcd.print(buf);
lilac0112_1 7:efe90188a49d 190 return 0;
lilac0112_1 7:efe90188a49d 191 }
lilac0112_1 7:efe90188a49d 192 else{//x==2
lilac0112_1 7:efe90188a49d 193 sys.pow_num = i;
lilac0112_1 7:efe90188a49d 194 sys.s_pow = ir_pow_val[sys.pow_num][POW_SHORT];
lilac0112_1 7:efe90188a49d 195 sys.m_pow = ir_pow_val[sys.pow_num][POW_MIDDLE];
lilac0112_1 7:efe90188a49d 196 sys.l_pow = ir_pow_val[sys.pow_num][POW_LONG];
lilac0112_1 7:efe90188a49d 197 return 1;
lilac0112_1 7:efe90188a49d 198 }
lilac0112_1 7:efe90188a49d 199 }
lilac0112_1 4:52da8da146d4 200 uint8_t SoftReset(uint8_t x){
lilac0112_1 7:efe90188a49d 201 char buf[LCD_COLUMN_NUM];
lilac0112_1 4:52da8da146d4 202 sprintf(buf, "SeeYouAgain!");
lilac0112_1 4:52da8da146d4 203 Lcd.locate(0, 1);Lcd.print(buf);wait(0.5);
lilac0112_1 4:52da8da146d4 204 NVIC_SystemReset();
lilac0112_1 4:52da8da146d4 205 return 1;
lilac0112_1 4:52da8da146d4 206 }
lilac0112_1 8:25c8c75d182a 207 uint8_t SetStrategyUp(uint8_t x){
lilac0112_1 8:25c8c75d182a 208 char buf[LCD_COLUMN_NUM];
lilac0112_1 8:25c8c75d182a 209 uint8_t i=(sys.strategy+(-1+STRATEGY_NUM))%STRATEGY_NUM;
lilac0112_1 8:25c8c75d182a 210 if(x==1){
lilac0112_1 8:25c8c75d182a 211 sprintf(buf, "%2d:%.12s", i, act[i].LcdStr);
lilac0112_1 8:25c8c75d182a 212 Lcd.locate(0, 1);Lcd.print(buf);
lilac0112_1 8:25c8c75d182a 213 return 0;
lilac0112_1 8:25c8c75d182a 214 }
lilac0112_1 8:25c8c75d182a 215 else{//x==2
lilac0112_1 8:25c8c75d182a 216 sys.strategy = i;
lilac0112_1 8:25c8c75d182a 217 return 1;
lilac0112_1 8:25c8c75d182a 218 }
lilac0112_1 8:25c8c75d182a 219 }
lilac0112_1 8:25c8c75d182a 220 uint8_t SetStrategyDown(uint8_t x){
lilac0112_1 8:25c8c75d182a 221 char buf[LCD_COLUMN_NUM];
lilac0112_1 8:25c8c75d182a 222 uint8_t i=(sys.strategy+1)%STRATEGY_NUM;
lilac0112_1 8:25c8c75d182a 223 if(x==1){
lilac0112_1 8:25c8c75d182a 224 sprintf(buf, "%2d:%.12s", i, act[i].LcdStr);
lilac0112_1 8:25c8c75d182a 225 Lcd.locate(0, 1);Lcd.print(buf);
lilac0112_1 8:25c8c75d182a 226 return 0;
lilac0112_1 8:25c8c75d182a 227 }
lilac0112_1 8:25c8c75d182a 228 else{//x==2
lilac0112_1 8:25c8c75d182a 229 sys.strategy = i;
lilac0112_1 8:25c8c75d182a 230 return 1;
lilac0112_1 8:25c8c75d182a 231 }
lilac0112_1 8:25c8c75d182a 232 }
lilac0112_1 4:52da8da146d4 233 uint8_t Start2(uint8_t x){
lilac0112_1 7:efe90188a49d 234 char buf[LCD_COLUMN_NUM];
lilac0112_1 4:52da8da146d4 235 sprintf(buf, "CatPotForDebug");
lilac0112_1 4:52da8da146d4 236 Lcd.locate(0, 1);Lcd.print(buf);
lilac0112_1 36:b1ebcc2c1f70 237 sys.TurnStartFlag=0;
lilac0112_1 5:5ff3a7d5d8c2 238 Active();
lilac0112_1 4:52da8da146d4 239 return 1;
lilac0112_1 9:c966191926c5 240 }
lilac0112_1 9:c966191926c5 241 uint8_t DriveKicker(uint8_t x){
lilac0112_1 9:c966191926c5 242 char buf[LCD_COLUMN_NUM];
lilac0112_1 10:6df631c39f9b 243 sprintf(buf, "KickForDebug");
lilac0112_1 9:c966191926c5 244 Lcd.locate(0, 1);Lcd.print(buf);
lilac0112_1 9:c966191926c5 245 wait(2);
lilac0112_1 9:c966191926c5 246 kicker = 1;
lilac0112_1 9:c966191926c5 247 wait(.5);
lilac0112_1 9:c966191926c5 248 kicker = 0;
lilac0112_1 9:c966191926c5 249 wait(.5);
lilac0112_1 9:c966191926c5 250 return 1;
lilac0112_1 10:6df631c39f9b 251 }
lilac0112_1 10:6df631c39f9b 252 uint8_t DriveDribblerAndKicker(uint8_t x){
lilac0112_1 10:6df631c39f9b 253 char buf[LCD_COLUMN_NUM];
lilac0112_1 10:6df631c39f9b 254 sprintf(buf, "DribbleForDebug");
lilac0112_1 10:6df631c39f9b 255 Lcd.locate(0, 1);Lcd.print(buf);
lilac0112_1 10:6df631c39f9b 256 sys.DribbleFlag=1;wait_ms(20);move(0,0,0);tx_motor();
lilac0112_1 10:6df631c39f9b 257 wait(5);
lilac0112_1 10:6df631c39f9b 258 kicker = 1;
lilac0112_1 10:6df631c39f9b 259 wait(.5);
lilac0112_1 10:6df631c39f9b 260 kicker = 0;
lilac0112_1 10:6df631c39f9b 261 wait(2);
lilac0112_1 10:6df631c39f9b 262 sys.DribbleFlag=0;wait_ms(20);move(0,0,0);tx_motor();
lilac0112_1 10:6df631c39f9b 263 return 1;
lilac0112_1 3:c49ea7ad8e91 264 }