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

Dependencies:   mbed AQM1602 HMC6352 PID

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers command_functions.cpp Source File

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 }