ジャパンオープン用のメインプログラム
Dependencies: mbed AQM1602 HMC6352 PID
command_functions.cpp
00001 #include "mbed.h" 00002 #include "extern.h" 00003 00004 uint8_t ZeroFunction(uint8_t x){ 00005 char buf[LCD_COLUMN_NUM]; 00006 if(x==1){ 00007 sprintf(buf, "%2d%.14s", sys.strategy, act[sys.strategy].LcdStr); 00008 Lcd.locate(0, 1);Lcd.print(buf);wait_ms(50);return 0; 00009 } 00010 else if(x==2){ 00011 sprintf(buf, "%2d:S%2d M%2d L%2d", 00012 sys.pow_num, 00013 ir_pow_val[sys.pow_num][POW_SHORT], 00014 ir_pow_val[sys.pow_num][POW_MIDDLE], 00015 ir_pow_val[sys.pow_num][POW_LONG] 00016 ); 00017 Lcd.locate(0, 1);Lcd.print(buf); 00018 return 0; 00019 } 00020 else{//x==3 00021 sprintf(buf, "SeeYouAgain!"); 00022 Lcd.locate(0, 1);Lcd.print(buf);wait(0.5); 00023 NVIC_SystemReset(); 00024 return 1; 00025 } 00026 } 00027 uint8_t Start(uint8_t x){ 00028 char buf[LCD_COLUMN_NUM]; 00029 uint8_t temp; 00030 temp = sys.strategy; 00031 sprintf(buf, "CatPot"); 00032 Lcd.locate(0, 1);Lcd.print(buf); 00033 sys.strategy=0; 00034 sys.TurnStartFlag=0; 00035 Active(); 00036 sys.strategy=temp; 00037 //reset 00038 NVIC_SystemReset(); 00039 return 1; 00040 } 00041 uint8_t TurnAndStart(uint8_t x){ 00042 char buf[LCD_COLUMN_NUM]; 00043 uint8_t temp; 00044 temp = sys.strategy; 00045 sprintf(buf, "CatPot"); 00046 Lcd.locate(0, 1);Lcd.print(buf); 00047 sys.strategy=0; 00048 sys.TurnStartFlag=1; 00049 Active(); 00050 sys.strategy=temp; 00051 //reset 00052 NVIC_SystemReset(); 00053 return 1; 00054 } 00055 uint8_t GetIr(uint8_t x){ 00056 char buf[LCD_COLUMN_NUM]; 00057 ReadIr(); 00058 00059 //if(x==1) sprintf(buf, "SHRT:%2d, LNG:%2d", data.irSpot[0], data.irSpot[1]); 00060 //if(x==2) sprintf(buf, "NOTE:%2d", data.irNotice); 00061 //if(x==3) sprintf(buf, "POSITION:%2d", data.irPosition); 00062 if(x==1) sprintf(buf, "POSI:%2d NOTE:%1d", data.irPosition, data.irNotice); 00063 if(x==2) sprintf(buf, "PH_L:%1d PH_S:%1d", data.irValPhase[IR_LONG], data.irValPhase[IR_SHORT]); 00064 if(x==3) sprintf(buf, "DIF:%1d KEY:%2d", data.irDif[IR_LONG], data.irKey); 00065 Lcd.locate(0, 1);Lcd.print(buf);wait_ms(50);return 0; 00066 } 00067 uint8_t GetSns0(uint8_t x){ 00068 char buf[LCD_COLUMN_NUM]; 00069 uint8_t raw[3]; 00070 uint8_t held[3]; 00071 if(x==1){ 00072 LineKeeper=LINE_FIX;//line 00073 00074 raw[2] = GetBit(LineRaw, 3); 00075 raw[1] = GetBit(LineRaw, 2); 00076 raw[0] = GetBit(LineRaw, 1); 00077 00078 held[2] = GetBit(LineHold, 3); 00079 held[1] = GetBit(LineHold, 2); 00080 held[0] = GetBit(LineHold, 1); 00081 //BusOut LineKeeper(lineInA, lineInB, lineInC); 00082 //BusIn LineRaw(lineA2, lineB2, lineC2); 00083 //BusIn LineHold(lineA1, lineB1, lineC1); 00084 sprintf(buf, "A:%1d%1d B:%1d%1d C:%1d%1d ", raw[0],held[0],raw[1],held[1],raw[2],held[2]); 00085 //sprintf(buf, "A:%1d B:%1d C:%1d ", Line[0].read(), Line[1].read(), Line[2].read()); 00086 Lcd.locate(0, 1);Lcd.print(buf);wait_ms(50);return 0; 00087 } 00088 if(x==2){ 00089 ReadPing2(); 00090 ReadPing(); 00091 sprintf(buf, "L%3dR%3dF%3dB%3d", data.ping[L_PING], data.ping[R_PING], data.ping[F_PING], data.ping[B_PING]); 00092 Lcd.locate(0, 1);Lcd.print(buf);wait_ms(50);return 0; 00093 } 00094 if(x==3){ 00095 //ReadIr(); 00096 //sprintf(buf, "IR_KEY:%2d", data.irKey); 00097 //sprintf(buf, "BALL:%1d", BallChecker.read()); 00098 sprintf(buf, "BALLA%6d", BallCheckerA.read_u16()); 00099 Lcd.locate(0, 1);Lcd.print(buf);wait_ms(50);return 0; 00100 } 00101 return 0; 00102 } 00103 uint8_t RwPid(uint8_t x){ 00104 char buf[LCD_COLUMN_NUM]; 00105 if(x==1){ 00106 PidUpdate(); 00107 sprintf(buf, "In:%03.0f Out:%+02d", cmps_set.InputPID, cmps_set.OutputPID); 00108 Lcd.locate(0, 1);Lcd.print(buf);wait_ms(50);return 0; 00109 } 00110 if(x==2){ 00111 sys.jump_flag=START; 00112 sprintf(buf, "FaceToFront!"); 00113 Lcd.locate(0, 1);Lcd.print(buf); 00114 hmc_reset = HMC_RST; 00115 wait_ms(100); 00116 hmc_reset = HMC_RUN; 00117 for(int i=0; i<5; i++){ 00118 ReadCmps(); 00119 cmps_set.CmpsInitialValue = cmps_set.cmps; 00120 wait_ms(100); 00121 } 00122 cmps_set.CmpsDiff = REFERENCE - cmps_set.cmps; 00123 cmps_set.FrontDeg=0; 00124 return 1; 00125 } 00126 return 1; 00127 } 00128 uint8_t CalibrationEnterOrExit(uint8_t x){ 00129 static uint8_t state=0; 00130 char buf[LCD_COLUMN_NUM]; 00131 00132 if(x==1){ 00133 if(state==0){ 00134 sprintf(buf, "Exit>>Enter"); 00135 } 00136 if(state==1){ 00137 sprintf(buf, "Enter>>Exit"); 00138 } 00139 Lcd.locate(0, 1);Lcd.print(buf); 00140 return 0; 00141 } 00142 if(x==2){ 00143 if(state==0){ 00144 hmc.setCalibrationMode(HMC6352_ENTER_CALIB); 00145 state=1; 00146 return 1; 00147 } 00148 if(state==1){ 00149 hmc.setCalibrationMode(HMC6352_EXIT_CALIB); 00150 state=0; 00151 return 1; 00152 } 00153 } 00154 return 1; 00155 } 00156 uint8_t CalibrationTurn(uint8_t x){ 00157 char buf[LCD_COLUMN_NUM]; 00158 sprintf(buf, "CalibrationTurn"); 00159 Lcd.locate(0, 1);Lcd.print(buf); 00160 hmc.setCalibrationMode(HMC6352_ENTER_CALIB);wait_ms(20); 00161 move(0,0,5);tx_motor(); 00162 wait(5); 00163 move(0,0,0);tx_motor(); 00164 hmc.setCalibrationMode(HMC6352_EXIT_CALIB);wait_ms(20); 00165 sys.jump_flag=GET_PID_VALUE; 00166 return 1; 00167 } 00168 uint8_t SetPowerUp(uint8_t x){ 00169 char buf[LCD_COLUMN_NUM]; 00170 uint8_t i=(sys.pow_num+(-1+POW_COMBI_NUM))%POW_COMBI_NUM; 00171 if(x==1){ 00172 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]); 00173 Lcd.locate(0, 1);Lcd.print(buf); 00174 return 0; 00175 } 00176 else{//x==2 00177 sys.pow_num = i; 00178 sys.s_pow = ir_pow_val[sys.pow_num][POW_SHORT]; 00179 sys.m_pow = ir_pow_val[sys.pow_num][POW_MIDDLE]; 00180 sys.l_pow = ir_pow_val[sys.pow_num][POW_LONG]; 00181 return 1; 00182 } 00183 } 00184 uint8_t SetPowerDown(uint8_t x){ 00185 char buf[LCD_COLUMN_NUM]; 00186 uint8_t i=(sys.pow_num+1)%POW_COMBI_NUM; 00187 if(x==1){ 00188 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]); 00189 Lcd.locate(0, 1);Lcd.print(buf); 00190 return 0; 00191 } 00192 else{//x==2 00193 sys.pow_num = i; 00194 sys.s_pow = ir_pow_val[sys.pow_num][POW_SHORT]; 00195 sys.m_pow = ir_pow_val[sys.pow_num][POW_MIDDLE]; 00196 sys.l_pow = ir_pow_val[sys.pow_num][POW_LONG]; 00197 return 1; 00198 } 00199 } 00200 uint8_t SoftReset(uint8_t x){ 00201 char buf[LCD_COLUMN_NUM]; 00202 sprintf(buf, "SeeYouAgain!"); 00203 Lcd.locate(0, 1);Lcd.print(buf);wait(0.5); 00204 NVIC_SystemReset(); 00205 return 1; 00206 } 00207 uint8_t SetStrategyUp(uint8_t x){ 00208 char buf[LCD_COLUMN_NUM]; 00209 uint8_t i=(sys.strategy+(-1+STRATEGY_NUM))%STRATEGY_NUM; 00210 if(x==1){ 00211 sprintf(buf, "%2d:%.12s", i, act[i].LcdStr); 00212 Lcd.locate(0, 1);Lcd.print(buf); 00213 return 0; 00214 } 00215 else{//x==2 00216 sys.strategy = i; 00217 return 1; 00218 } 00219 } 00220 uint8_t SetStrategyDown(uint8_t x){ 00221 char buf[LCD_COLUMN_NUM]; 00222 uint8_t i=(sys.strategy+1)%STRATEGY_NUM; 00223 if(x==1){ 00224 sprintf(buf, "%2d:%.12s", i, act[i].LcdStr); 00225 Lcd.locate(0, 1);Lcd.print(buf); 00226 return 0; 00227 } 00228 else{//x==2 00229 sys.strategy = i; 00230 return 1; 00231 } 00232 } 00233 uint8_t Start2(uint8_t x){ 00234 char buf[LCD_COLUMN_NUM]; 00235 sprintf(buf, "CatPotForDebug"); 00236 Lcd.locate(0, 1);Lcd.print(buf); 00237 sys.TurnStartFlag=0; 00238 Active(); 00239 return 1; 00240 } 00241 uint8_t DriveKicker(uint8_t x){ 00242 char buf[LCD_COLUMN_NUM]; 00243 sprintf(buf, "KickForDebug"); 00244 Lcd.locate(0, 1);Lcd.print(buf); 00245 wait(2); 00246 kicker = 1; 00247 wait(.5); 00248 kicker = 0; 00249 wait(.5); 00250 return 1; 00251 } 00252 uint8_t DriveDribblerAndKicker(uint8_t x){ 00253 char buf[LCD_COLUMN_NUM]; 00254 sprintf(buf, "DribbleForDebug"); 00255 Lcd.locate(0, 1);Lcd.print(buf); 00256 sys.DribbleFlag=1;wait_ms(20);move(0,0,0);tx_motor(); 00257 wait(5); 00258 kicker = 1; 00259 wait(.5); 00260 kicker = 0; 00261 wait(2); 00262 sys.DribbleFlag=0;wait_ms(20);move(0,0,0);tx_motor(); 00263 return 1; 00264 }
Generated on Wed Jul 13 2022 02:59:03 by 1.7.2