Takeshi Nishimura
ランサー
Dependencies: SB1602E SDFileSystem mbed
Fork of
Seeed_SDCard_Shield
by 
Shields
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 //Lancer2016 00002 00003 //ヘッダー設定 00004 #include "mbed.h" 00005 #include "SDFileSystem.h" 00006 #include "SB1602E.h" 00007 #include "stdlib.h" 00008 00009 //シンボル定義 00010 #define CLR 0,0,0 //無 RGBパターン 00011 #define RED 1,0,0 //赤 00012 #define GRE 0,1,0 //緑 00013 #define YEL 1,1,0 //黄 00014 #define BLU 0,0,1 //青 00015 #define PUR 1,0,1 //紫 00016 #define CYA 0,1,1 //水 00017 #define WHI 1,1,1 //白 00018 #define DFP 508 //フロントポテンショメータ機械原点 00019 #define DRP 234 //リヤポテンショメータ機械原点 00020 #define FW 1 //駆動輪前進 00021 #define BW 0 //駆動輪後進 00022 #define ST_EN ST_CTR = 0 //ステアリングモータ有効 00023 #define ST_DA ST_CTR = 1 //ステアリングモータ無効 00024 #define OTH_EN OTH_CTR = 0 //その他モータ有効 00025 #define OTH_DA OTH_CTR = 1 //その他モータ無効 00026 #define LA_DA LancFree_Flg=1 //槍無効 00027 #define LA_EN LancFree_Flg=0 //槍有効 00028 #define DSs 8 //LineセンサA/Dシフト数 00029 #define ASs 6 //トレース用LineセンサA/Dシフト数 00030 #define ThS 80 //Line白黒判別閾値 00031 #define ANGLE 0 //角度制御モード 00032 #define TRACE 1 //トレースモード 00033 #define WTRACE 2 //拡張トレースモード 00034 #define NUM_DATA 10 //ソート用データ数 00035 #define WWIDCLR W_WidsR=W_WidsL=0 00036 00037 SB1602E lcd(PB_9,PB_8); //LCD_I2C設定(SDA, SCL) 00038 SDFileSystem sd(PA_7,PA_6,PA_5,PB_6,"sd"); // MOSI, MISO, SCK, CS 00039 FILE *fp; 00040 00041 //センサ入力 00042 DigitalIn RS1(PC_10); //ロータリースイッチbit1 00043 DigitalIn RS2(PA_15); //ロータリースイッチbit2 00044 DigitalIn RS3(PA_12); //ロータリースイッチbit3 00045 DigitalIn RS4(PA_9); //ロータリースイッチbit4 00046 AnalogIn Line0(PC_0); //アナログラインセンサ 00047 AnalogIn Line1(PC_1); //アナログラインセンサ 00048 AnalogIn Line2(PC_2); //アナログラインセンサ 00049 AnalogIn Line3(PC_3); //アナログラインセンサ 00050 AnalogIn Line4(PA_0); //アナログラインセンサ 00051 AnalogIn Line5(PA_1); //アナログラインセンサ 00052 AnalogIn Line6(PA_2); //アナログラインセンサ 00053 AnalogIn Line7(PA_3); //アナログラインセンサ 00054 AnalogIn Steer_Ptm(PA_4); //ステアリングポテンショ 00055 AnalogIn Lance_Ptm(PC_4); //槍ポテンショ 00056 DigitalIn SW_W(PB_5); //スイッチ白 00057 DigitalIn SW_R(PC_6); //スイッチ赤 00058 DigitalIn SW_B(PC_12); //スイッチ黒 00059 InterruptIn s0signal(D6); //ロータリエンコーダA相 00060 InterruptIn s1signal(D7); //ロータリエンコーダB相 00061 00062 //モータ制御 00063 PwmOut PWM_CH1(PC_7); //3/2槍 00064 PwmOut PWM_CH2(PA_11); //1/4前左 00065 PwmOut PWM_CH3(PB_2); //2/4前右 00066 PwmOut PWM_CH4(PB_7); //4/2後右 00067 PwmOut PWM_CH5(PA_10); //1/3後左 00068 PwmOut PWM_CH6(PB_4); //3/1操舵 00069 DigitalOut Rev_CH1(PB_12); //操舵モータDIR 00070 DigitalOut Rev_CH2(PB_13); //前左モータDIR 00071 DigitalOut Rev_CH3(PC_8); //前右モータDIR 00072 DigitalOut Rev_CH4(PC_9); //後右モータDIR 00073 DigitalOut Rev_CH5(PB_14); //後左モータDIR 00074 DigitalOut Rev_CH6(PB_15); //槍モータDIR 00075 DigitalOut SensLED(PC_11); //センサ用LED 00076 DigitalOut ST_CTR(PB_0); //STEER_EnableCtrl 00077 DigitalOut OTH_CTR(PB_1); //OTHER_EnableCtrl 00078 00079 //デバッグ 00080 DigitalOut LED_R(PA_13); //LED_R 00081 DigitalOut LED_G(PA_14); //LED_G 00082 DigitalOut LED_B(PB_3); //LED_B 00083 00084 //割り込み定義 00085 Ticker flipper; //汎用タイマー 00086 Ticker sensget; //センサー用タイマー 00087 00088 //プロトタイプ宣言 00089 void init(void); //マイコン初期設定 00090 void MotorCtrl(void); //モータ管理 00091 void enc(); //エンコーダ処理 00092 int ModeSW(); //モードスイッチ値取得 00093 int Get_SW(); //スイッチ値取得 00094 void Get_Pot(); //ポテンショメータA/D取得 00095 void SpeedCtrl(int V); //速度制御 00096 int PotTest(); //ポテンショテスト 00097 float PwmTest(); //PWMテスト 00098 void SD_Mount(); //SDカードをマウントする 00099 void SD_Unmount(); //SDカードをアンマウントする 00100 void DSens(); //センサ変調処理 00101 void LineTrace(); //ライントレース 00102 void ErrChk(void); //エラーチェック 00103 void LanceAng(int Ang); //槍角度制御 00104 void Line_Dec(void); //白線判定 00105 void enCalc(void); //エンコーダ計算とか 00106 void Debug(void); //デバッグモード 00107 void SD_Log(void); //SDログ 00108 void SD_WriteSq(void); //SD書き込み 00109 void TmpLog(void); //内部メモリへのログ 00110 void SteerCtrl(char STMode); //ステアリングコントロール 00111 void AngleCtrl(int STAngle); //角度制御モード 00112 void Localization(void); // 位置推定 00113 void led_color(char R , char G , char B ); //LED色制御 00114 void ModeSelect(); //モード選択 00115 00116 00117 //グローバル変数の宣言 00118 int EnCount=0; 00119 long EnCountI=0; //エンコーダカウント 00120 int EnCount_1R=0; //コース１周分のエンコーダカウント毎週リセットされる 00121 unsigned char Rev=0; //エンコーダ回転方向判定 00122 unsigned char SD_Flag=0; //SDカードの状態判定 00123 unsigned char Line_Flg; //ライン情報フラグ 00124 unsigned char Shortcut_Flg=0; //ショートカットフラグ 00125 unsigned char Turbo_Flg=0; //コーナー出口のターボフラグ 00126 unsigned char R_Go = 15; 00127 unsigned char TestDrive_Flg=0; //駆動輪フリー 00128 unsigned char LancFree_Flg=0; //槍フリー 00129 int timer1=0,timer2=0,Tmpcnt=0,Log_timer=0; //タイマー 00130 int Stime=0; 00131 char StartFlg=0; 00132 unsigned long long cntT=0; 00133 int ONSensData[8],OFFSensData[8],DSensData[8],TFSensData[8];//ラインセンサ値 00134 int Pot_F=0,Pot_R=0,Pot_F2=0,StrPot_R[10]; //ポテンショ 00135 char LA_Rev,FL_Rev,FR_Rev,RR_Rev,RL_Rev,ST_Rev; //モータ回転方向 00136 int LA_PWM=0,FL_PWM=0,FR_PWM=0,RR_PWM=0,RL_PWM=0,ST_PWM=0; //モータPWMデューティ 00137 int ErrSt=0; 00138 int Speed=0,Angle=0,STAngle=0; 00139 int iAng=0; 00140 long iV=0; 00141 int TraceI,Trace_P=0,Trace_I=0,Trace_D=0,Trace_B=0; 00142 int Angle_P=0,Angle_I=0,Angle_D=0,Angle_B=0; 00143 int RSW_Value; 00144 char Turn_Flg=0; 00145 char ModeSelect_Flg=0; 00146 unsigned int R_LineCnt=0,L_LineCnt=0,R_LineCntF=0,L_LineCntF=0; 00147 char R_LineFlg=0,L_LineFlg=0; 00148 char DebugFlg=0; 00149 long W_WidsR=0,W_WidsL=0,W_WidsRCnt=0,W_WidsLCnt=0,dAng; 00150 unsigned char Count_C=0,Count_R=0,Count_L=0,Count_H=0; 00151 unsigned char R_No=1; 00152 int ParamS=1; 00153 int SPCom=4500,SPYobi=4500,SPCor=3200,loopJump=1000000,linewide=100; 00154 int TripTime =60000; //走行時間 00155 int AccelStopCnt = 1000; //駆動停止カウント 00156 int MotorStopCnt = 2000; //モータ停止カウント 00157 char SteerAngle; 00158 char SteerMode=0; 00159 char DefFlg=0; 00160 int FrontPot[NUM_DATA + 1],RearPot[NUM_DATA + 1]; 00161 00162 char Log_table[10][10]; 00163 int Log_W_Wids[300][2]; 00164 char Log_W_WidsLR[300]; 00165 char Log_Count_R[300]; 00166 char Log_tmp[100][6]; 00167 char LLog_tmp[300]; 00168 int Sensecnt=0,writeflg=0; 00169 00170 unsigned char WidsNo=0; 00171 char Logcnt=0; 00172 char Trace_table[]={ 00173 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,1,1,1,2,2,2,2,3,3, 00174 3,4,4,4,5,5,6,6,7,7,8,8,9,10,10,11,12,13,13,14,15,16,17,18,19,20, 00175 22,23,24,25,27,28,30,31,33,34,36,38,39,41,43,45,47,49,51,53,56, 00176 58,60,63,65,68,70,73,76,79,82,85,88,91,94,98,101,104,108,112,115, 00177 119,123}; 00178 char Trace_table2[]={0,0,1,1,1,1,1,1,1,1,1,1,1,1,2,2,2,2,2,2,2,2, 00179 3,3,3,3,3,3,4,4,4,4,4,4,5,5,5,5,5,6,6,6,6,7,7,7,7,8,8,8,8,9,9,9, 00180 9,10,10,10,10,11,11,11,12,12,12,13,13,13,14,14,14,15,15,15,16,16, 00181 16,17,17,18,18,18,19,19,19,20,20}; 00182 00183 float Motor_TableA[]={ 00184 0.99,0.98,0.97,0.96,0.95,0.95,0.94,0.93,0.92,0.92,0.91,0.91,0.90, 00185 0.90,0.89,0.89,0.88,0.88,0.88,0.88,0.87,0.87,0.87,0.87,0.87,0.87, 00186 0.87,0.87,0.88,0.88,0.88,0.89,0.89,0.90,0.90,0.91,0.92,0.93,0.94, 00187 0.95,0.96,0.97,0.99,1.01,1.02,1.05,1.07,1.09,1.12,1.15}; 00188 00189 float Motor_TableB[]={ 00190 1.01,1.02,1.03,1.04,1.06,1.07,1.08,1.10,1.11,1.13,1.14,1.16,1.18, 00191 1.19,1.21,1.23,1.25,1.27,1.29,1.31,1.34,1.36,1.39,1.41,1.44,1.47, 00192 1.50,1.53,1.56,1.59,1.63,1.67,1.70,1.75,1.79,1.83,1.88,1.93,1.98, 00193 2.04,2.10,2.16,2.23,2.30,2.38,2.46,2.55,2.64,2.74,2.85}; 00194 00195 float Motor_TableC[]={ 00196 0.99,0.98,0.97,0.96,0.95,0.94,0.93,0.92,0.91,0.90,0.88,0.87,0.86, 00197 0.85,0.84,0.83,0.82,0.81,0.79,0.78,0.77,0.76,0.74,0.73,0.72,0.70, 00198 0.69,0.67,0.66,0.64,0.63,0.61,0.59,0.58,0.56,0.54,0.52,0.50,0.48, 00199 0.45,0.43,0.41,0.38,0.35,0.32,0.29,0.26,0.23,0.19,0.15}; 00200 00201 float Motor_TableD[]={ 00202 1.01,1.02,1.03,1.04,1.05,1.06,1.07,1.08,1.09,1.10,1.12,1.13,1.14, 00203 1.15,1.16,1.17,1.18,1.19,1.21,1.22,1.23,1.24,1.26,1.27,1.28,1.30, 00204 1.31,1.33,1.34,1.36,1.37,1.39,1.41,1.42,1.44,1.46,1.48,1.50,1.52, 00205 1.55,1.57,1.59,1.62,1.65,1.68,1.71,1.74,1.77,1.81,1.85}; 00206 00207 //////////////////////////////////////////////// 00208 //-------------ベースプログラム-----------------// 00209 //////////////////////////////////////////////// 00210 //----------モードSW-------------------- 00211 int ModeSW(){ 00212 char RSW=0; 00213 if(RS1)RSW += 1; 00214 if(RS2)RSW += 2; 00215 if(RS3)RSW += 4; 00216 if(RS4)RSW += 8; 00217 RSW_Value=int(RSW); 00218 return RSW;} 00219 //----------SW情報取得-------------------- 00220 int Get_SW(){ 00221 char SW_Val=0; 00222 if(SW_W)SW_Val |= 0x01; 00223 if(SW_R)SW_Val |= 0x02; 00224 if(SW_B)SW_Val |= 0x04; 00225 return SW_Val;} 00226 //----------エラーチェック-------------------- 00227 void ErrChk(void){ 00228 if(timer1 >= TripTime && StartFlg && Count_R == 12) ErrSt = AccelStopCnt; 00229 if(Pot_F >= 47 || Pot_F <= (-47)) ErrSt++; 00230 if(DSensData[3] <= 250 && DSensData[4] <= 250 && StartFlg && SteerMode != ANGLE ) ErrSt = ErrSt + 5; 00231 else if(ErrSt < 1000 && Pot_F == 0 ) ErrSt=0; 00232 if(ErrSt >= MotorStopCnt ) ErrSt = MotorStopCnt; 00233 } 00234 //-----------エンコーダカウント------------------ 00235 void s0_trigger_rise() { // A相立上り変化割込み時の処理 (Low -> High) 00236 Rev |= 0x01; 00237 if(Rev&0x10) EnCount--; 00238 else EnCount++; 00239 } 00240 void s0_trigger_fall() { // A相立下り変化割込み時の処理 (High -> Low) 00241 Rev &= ~0x01; 00242 if(Rev&0x10) EnCount++; 00243 else EnCount--; 00244 } 00245 void s1_trigger_rise() { // B相立上り変化割込み時の処理 (Low -> High) 00246 Rev |= 0x10; 00247 if(Rev&0x01) EnCount++; 00248 else EnCount--; 00249 } 00250 void s1_trigger_fall() { // B相立下り変化割込み時の処理 (High -> Low) 00251 Rev &= ~0x10; 00252 if(Rev&0x01) EnCount--; 00253 else EnCount++; 00254 } 00255 //----------エンコーダ計算-------------------- 00256 void enCalc(void){ 00257 EnCountI = EnCountI + EnCount; 00258 EnCount_1R = EnCount_1R + EnCount; 00259 EnCount = 0; 00260 } 00261 //----------フルカラーLED制御-------------------- 00262 void led_color(char R , char G , char B ) 00263 { 00264 if(LancFree_Flg)R=G=B=1; 00265 if ( R ) LED_R = 1; 00266 else LED_R = 0; 00267 if ( G ) LED_G = 1; 00268 else LED_G = 0; 00269 if ( B ) LED_B = 1; 00270 else LED_B = 0; 00271 } 00272 //----------センサー値の取得--------------- 00273 void sensGet(){ 00274 if(Stime >= 10) { 00275 Stime=0; 00276 //Hpsort(FrontPot, NUM_DATA); 00277 //Hpsort(RearPot, NUM_DATA); 00278 //Get_Pot(); 00279 } 00280 if(Stime == 0 ) SensLED=0; 00281 if(Stime == 5 ) SensLED=1; 00282 if(Stime == 4 ) { 00283 OFFSensData[0] = Line0.read_u16()>>DSs; 00284 OFFSensData[1] = Line1.read_u16()>>DSs; 00285 OFFSensData[2] = Line2.read_u16()>>ASs; 00286 OFFSensData[3] = Line3.read_u16()>>ASs; 00287 OFFSensData[4] = Line4.read_u16()>>ASs; 00288 OFFSensData[5] = Line5.read_u16()>>ASs; 00289 OFFSensData[6] = Line6.read_u16()>>DSs; 00290 OFFSensData[7] = Line7.read_u16()>>DSs; 00291 } 00292 if(Stime == 9 ) { 00293 ONSensData[0] = Line0.read_u16()>>DSs; 00294 ONSensData[1] = Line1.read_u16()>>DSs; 00295 ONSensData[2] = Line2.read_u16()>>ASs; 00296 ONSensData[3] = Line3.read_u16()>>ASs; 00297 ONSensData[4] = Line4.read_u16()>>ASs; 00298 ONSensData[5] = Line5.read_u16()>>ASs; 00299 ONSensData[6] = Line6.read_u16()>>DSs; 00300 ONSensData[7] = Line7.read_u16()>>DSs; 00301 } 00302 //FrontPot[Stime] = Steer_Ptm.read_u16()>>6; 00303 //RearPot[Stime] = Lance_Ptm.read_u16()>>6; 00304 Stime++; 00305 } 00306 //----------ポテンショ値の取得--------------- 00307 void Get_Pot(){ 00308 00309 Pot_F2 = (DFP-(Steer_Ptm.read_u16()>>6)); 00310 //Pot_F2 = DFP - FrontPot[4]; 00311 Pot_F = int(Pot_F2/3); 00312 //Pot_R = RearPot[4]; 00313 Pot_R = Lance_Ptm.read_u16()>>6; 00314 } 00315 //----------センサー値差分の計算--------------- 00316 void DSens(){ 00317 00318 int sensorno; 00319 for(sensorno=0;sensorno<=7;sensorno++){ 00320 DSensData[sensorno] = OFFSensData[sensorno] - ONSensData[sensorno]; 00321 if(DSensData[sensorno] >= ThS ) TFSensData[sensorno] = 1; 00322 else TFSensData[sensorno] = 0; 00323 } 00324 } 00325 00326 //////////////////////////////////////////////// 00327 //-------------便利機能プログラム---------------// 00328 //////////////////////////////////////////////// 00329 //----------白線判別-------------------- 00330 void Line_Dec(void){ 00331 char LR_Flg=0; 00332 if(TFSensData[0] && TFSensData[1] ) { 00333 R_LineCntF = 0; 00334 R_LineCnt++;} 00335 else { 00336 R_LineCnt = 0; 00337 R_LineCntF++;} 00338 if(TFSensData[6] && TFSensData[7] ) { 00339 L_LineCntF = 0; 00340 L_LineCnt++;} 00341 else { 00342 L_LineCnt = 0; 00343 L_LineCntF++;} 00344 00345 if(R_LineCnt >= 2 && !R_LineFlg ){ 00346 R_LineFlg = 1; 00347 W_WidsRCnt = EnCountI; 00348 } 00349 else if(!R_LineCnt && R_LineFlg && R_LineCntF >= 4 ){ 00350 R_LineFlg = 0; 00351 LR_Flg = 1; 00352 W_WidsR = EnCountI - W_WidsRCnt; 00353 Log_W_WidsLR[WidsNo] = LR_Flg; 00354 Log_Count_R[WidsNo] = Count_R; 00355 Log_W_Wids[WidsNo][0] = W_WidsR; 00356 //Log_W_Wids[WidsNo][1] = EnCountI; 00357 LLog_tmp[WidsNo] = EnCount; 00358 Log_W_Wids[WidsNo][1] = EnCount_1R; 00359 WidsNo++; 00360 } 00361 if(L_LineCnt >= 2 && !L_LineFlg ){ 00362 L_LineFlg = 1; 00363 W_WidsLCnt = EnCountI; 00364 } 00365 else if(!L_LineCnt && L_LineFlg && L_LineCntF >= 4 ){ 00366 L_LineFlg = 0; 00367 LR_Flg = 0; 00368 W_WidsL = EnCountI - W_WidsLCnt; 00369 Log_W_WidsLR[WidsNo] = LR_Flg; 00370 Log_Count_R[WidsNo] = Count_R; 00371 Log_W_Wids[WidsNo][0] = W_WidsL; 00372 //Log_W_Wids[WidsNo][1] = EnCountI; 00373 LLog_tmp[WidsNo] = EnCount; 00374 Log_W_Wids[WidsNo][1] = EnCount_1R; 00375 WidsNo++; 00376 } 00377 } 00378 00379 //----------モータ総合管理-------------------- 00380 void MotorCtrl(){ 00381 if(ErrSt >= MotorStopCnt){ //異常判定 00382 PWM_CH1.pulsewidth_us(0); //操舵PWM (0~100) 00383 PWM_CH2.pulsewidth_us(0); //前左PWM (0~1000) 00384 PWM_CH3.pulsewidth_us(0); //前右PWM (0~1000) 00385 PWM_CH4.pulsewidth_us(0); //後右PWM (0~1000) 00386 PWM_CH5.pulsewidth_us(0); //後左PWM (0~1000) 00387 PWM_CH6.pulsewidth_us(0); //槍舵PWM (0~100) 00388 OTH_DA; 00389 } 00390 else{ 00391 if(FL_PWM >= 1000) FL_PWM = 1000; 00392 if(FR_PWM >= 1000) FR_PWM = 1000; 00393 if(RR_PWM >= 1000) RR_PWM = 1000; 00394 if(RL_PWM >= 1000) RL_PWM = 1000; 00395 PWM_CH1.pulsewidth_us(ST_PWM); //槍PWM (0~100) 00396 PWM_CH2.pulsewidth_us(FL_PWM); //前左PWM (0~1000) 00397 PWM_CH3.pulsewidth_us(FR_PWM); //前右PWM (0~1000) 00398 PWM_CH4.pulsewidth_us(RR_PWM); //後右PWM (0~1000) 00399 PWM_CH5.pulsewidth_us(RL_PWM); //後左PWM (0~1000) 00400 PWM_CH6.pulsewidth_us(LA_PWM); //操舵PWM (0~100) 00401 Rev_CH1 = ST_Rev; //槍回転方向 （H：CW） 00402 Rev_CH2 = !FL_Rev; //前左回転方向（L：FW） 00403 Rev_CH3 = FR_Rev; //前右回転方向（H：FW） 00404 Rev_CH4 = RR_Rev; //後右回転方向（H：FW） 00405 Rev_CH5 = !RL_Rev; //後左回転方向（L：FW） 00406 Rev_CH6 = LA_Rev; //操舵回転方向（H：CW） 00407 } 00408 } 00409 //----------速度制御-------------------- 00410 void SpeedCtrl(int V){ 00411 long dV; 00412 int PT,SpeedBuf; 00413 00414 if(SpeedBuf != V) iV = 0; 00415 SpeedBuf = V; 00416 dV=EnCount*32; 00417 if(ErrSt >= AccelStopCnt ){ 00418 iV = 0; 00419 V = 0;} 00420 dV = (V - dV)*8; 00421 if(dV == 0) iV = 0; 00422 iV = iV + (dV/1000); 00423 dV = dV + iV; 00424 if(dV <= 0){ 00425 dV=dV*(-1); 00426 FL_Rev=FR_Rev=RR_Rev=RL_Rev=BW; 00427 } 00428 else FL_Rev=FR_Rev=RR_Rev=RL_Rev=FW; 00429 if(dV>=1000) dV=1000; 00430 if(Pot_F < 0){ 00431 PT = Pot_F * (-1); 00432 //PT = int (PT*2); 00433 if(PT >= 50 ) PT=50; 00434 //if(!DefFlg) PT = 0; 00435 FL_PWM = int( dV * Motor_TableA[PT]); 00436 FR_PWM = int( dV * Motor_TableB[PT]); 00437 RL_PWM = int( dV * Motor_TableC[PT]); 00438 RR_PWM = int( dV * Motor_TableD[PT]); 00439 } 00440 else{ 00441 PT = Pot_F; 00442 //PT = int (PT*2); 00443 if(PT >= 50 ) PT=50; 00444 //if(!DefFlg) PT = 0; 00445 FR_PWM = int( dV * Motor_TableA[PT]); 00446 FL_PWM = int( dV * Motor_TableB[PT]); 00447 RR_PWM = int( dV * Motor_TableC[PT]); 00448 RL_PWM = int( dV * Motor_TableD[PT]); 00449 } 00450 00451 } 00452 //----------槍角度制御-------------------- 00453 void LanceAng(int Ang){ 00454 //int dAng; 00455 00456 dAng = Pot_R - (DRP + (Ang * 3)); 00457 if(dAng <= 0){ //CW 00458 iAng = 0; 00459 LA_Rev = 1; 00460 dAng = dAng * (-1); 00461 } 00462 else{ //CCW 00463 LA_Rev = 0; 00464 } 00465 dAng = dAng * 1; 00466 iAng = iAng + (dAng/50); 00467 dAng = dAng + iAng; 00468 if(dAng >= 60) dAng = 60; 00469 if(LancFree_Flg && dAng >= 3)dAng = 3; 00470 00471 if(Ang >= 400) dAng=60; 00472 LA_PWM = dAng; 00473 } 00474 //------ステアリングコントロール----------- 00475 void SteerCtrl(char STMode){ 00476 if(STMode == ANGLE) AngleCtrl(STAngle); 00477 else LineTrace(); 00478 } 00479 00480 //-----------角度コントロール------------- 00481 void AngleCtrl(int STAngle){ 00482 int Angle_V=0; 00483 00484 Angle_P=((STAngle*3) - Pot_F2)*(200); 00485 Angle_I = Angle_I + (Angle_P / (70)); 00486 if(Angle_I >= 10000) Angle_I = 10000; 00487 if(Angle_I <= (-10000)) Angle_I = (-10000); 00488 Angle_D = Angle_B - Angle_P; 00489 Angle_B = Angle_P; 00490 Angle_V = Angle_P + (Angle_I ) - (Angle_D *0); 00491 Angle_V = Angle_V / 100; 00492 00493 /*Angle_P=((STAngle*3) - Pot_F2); 00494 Angle_D = Angle_B - Angle_P; 00495 Angle_B = Angle_P; 00496 //Angle_P = Angle_P*(100+(10*RSW_Value)); 00497 Angle_I = Angle_I + (Angle_P / (10*RSW_Value)); 00498 if(Angle_I >= 10000) Angle_I = 10000; 00499 if(Angle_I <= (-10000)) Angle_I = (-10000); 00500 Angle_V = Angle_P + (Angle_I ) - (Angle_D * 0); 00501 Angle_V = Angle_V / 100;*/ 00502 00503 if(Angle_V <= -1){ 00504 Angle_V = Angle_V*(-1); 00505 ST_Rev=0; 00506 if(!Turn_Flg && Angle_V >= 10) Angle_I = 0; 00507 Turn_Flg=1; 00508 } 00509 else { 00510 ST_Rev=1; 00511 if(Turn_Flg && Angle_V >= 10) Angle_I = 0; 00512 Turn_Flg=0; 00513 } 00514 00515 if(Angle_V >= 100) Angle_V=100; 00516 ST_PWM=Angle_V; 00517 } 00518 //----------ライントレース--------------- 00519 void LineTrace(){ 00520 int Trace_V; 00521 if(SteerMode == WTRACE) TraceI=((DSensData[4]+DSensData[5]+DSensData[6]+DSensData[7])-(DSensData[3]+DSensData[2]+DSensData[1]+DSensData[0])); 00522 else TraceI=((DSensData[4]+DSensData[5])-(DSensData[3]+DSensData[2])); 00523 /* if(TraceI <= -1){ 00524 TraceI = TraceI*(-1); 00525 ST_Rev=1; 00526 if(!Turn_Flg) Trace_I = 0; 00527 Turn_Flg=1; 00528 } 00529 else { 00530 ST_Rev=0; 00531 if(Turn_Flg) Trace_I = 0; 00532 Turn_Flg=0; 00533 }*/ 00534 Trace_P = TraceI * (10); 00535 Trace_I = Trace_I + (Trace_P / (400)); 00536 if(Trace_I >= 10000) Trace_I = 10000; 00537 if(Trace_I <= (-10000)) Trace_I = (-10000); 00538 Trace_D = Trace_B - TraceI; 00539 Trace_B = TraceI; 00540 Trace_V = Trace_P + Trace_I - (Trace_D * 50); 00541 Trace_V = Trace_V / 100; 00542 00543 if(Trace_V <= -1){ 00544 Trace_V = Trace_V*(-1); 00545 ST_Rev=1; 00546 if(!Turn_Flg && Trace_V >= 10) Trace_I = 0; 00547 Turn_Flg=1; 00548 } 00549 else { 00550 ST_Rev=0; 00551 if(Turn_Flg && Trace_V >= 10) Trace_I = 0; 00552 Turn_Flg=0; 00553 } 00554 00555 if(Trace_V >= 100) Trace_V=100; 00556 ST_PWM=Trace_V; 00557 } 00558 //----------コース上自己位置推定--------------- 00559 void Localization(void){ 00560 00561 if(Count_R == 0 && W_WidsR >= linewide){ //右1 00562 WWIDCLR; 00563 EnCount_1R =0; 00564 Count_R++;} 00565 else if(!Count_R && EnCount_1R >= 10000) Count_R++; 00566 00567 if(Count_R == 1 && W_WidsR >= linewide && EnCount_1R >= 54000){ //右2 00568 WWIDCLR; 00569 EnCount_1R = 57000; 00570 Count_R++;} 00571 else if(Count_R == 1 && EnCount_1R >= 60000) Count_R++; 00572 00573 if(Count_R == 2 && EnCount_1R >= 80000 && Shortcut_Flg ){ //予備減速 00574 Count_R++; 00575 EnCount_1R = 130000; 00576 } 00577 else if(Count_R == 2 && EnCount_1R >= 130000)Count_R++; //予備減速 00578 00579 if(Count_R == 3 && EnCount_1R >= 150000 && ( W_WidsR >= linewide || Shortcut_Flg )){ //コーナ1IN 00580 WWIDCLR; 00581 EnCount_1R = 153000; 00582 Count_R++;} 00583 else if(Count_R == 3 && EnCount_1R >= 156000) Count_R++; 00584 00585 if(Count_R == 4 && EnCount_1R >= 205000 && W_WidsR >= linewide && !Shortcut_Flg){ //コーナ1OUT 00586 WWIDCLR; 00587 EnCount_1R = 208000; 00588 Count_R++;} 00589 else if(Count_R == 4 && EnCount_1R >= 221000 && !Shortcut_Flg ) Count_R++; 00590 else if(Count_R == 4 && EnCount_1R >= 170000 && (DSensData[3] >= 400 || DSensData[4] >= 400 || DSensData[5] >= 400 || DSensData[6] >= 100 || DSensData[7] >= 100 || DSensData[0] >= 100 || DSensData[1] >= 100 || DSensData[2] >= 400) && Shortcut_Flg ){ 00591 SteerMode = WTRACE; 00592 WWIDCLR; 00593 EnCount_1R = 239000; 00594 Count_R++;} 00595 else if(Count_R == 4 && EnCount_1R >= 200000 && Shortcut_Flg ) { 00596 SteerMode = TRACE; 00597 WWIDCLR; 00598 ErrSt = 1000; 00599 EnCount_1R = 239000; 00600 Count_R++;} 00601 00602 00603 if(Count_R == 5 && EnCount_1R >= 239000 && ( W_WidsR >= linewide || Shortcut_Flg)){ //右3 00604 WWIDCLR; 00605 EnCount_1R = 242000; 00606 Count_R++;} 00607 else if(Count_R == 5 && EnCount_1R >= 245000) Count_R++; 00608 00609 if(Count_R == 6 && W_WidsL >= linewide && EnCount_1R >= 290000){ //左1 00610 WWIDCLR; 00611 EnCount_1R = 299000; 00612 Count_R++;} 00613 else if(Count_R == 6 && EnCount_1R >= 302000) Count_R++; 00614 00615 if(Count_R == 7 && W_WidsR >= linewide && EnCount_1R >= 330000){ //右3 00616 WWIDCLR; 00617 EnCount_1R = 342000; 00618 Count_R++;} 00619 else if(Count_R == 7 && EnCount_1R >= 350000) Count_R++; 00620 00621 if(Count_R == 8 && W_WidsL >= linewide && EnCount_1R >= 380000){ //左2 00622 WWIDCLR; 00623 EnCount_1R = 385000; 00624 Count_R++;} 00625 else if(Count_R == 8 && EnCount_1R >= 388000) Count_R++; 00626 00627 if(Count_R == 9 && EnCount_1R >= 400000)Count_R++; //予備減速 00628 00629 if(Count_R == 10 && W_WidsR >= linewide && EnCount_1R >= 422000){ //コーナ1IN 00630 WWIDCLR; 00631 EnCount_1R = 425000; 00632 Count_R++;} 00633 else if(Count_R == 10 && EnCount_1R >= 428000) Count_R++; 00634 00635 if(Count_R == 11 && W_WidsR >= linewide && EnCount_1R >= 470000 && R_No != R_Go){ //コーナ1OUT 00636 WWIDCLR; 00637 EnCount_1R = 480000; 00638 Count_R++;} 00639 else if(Count_R == 11 && EnCount_1R >= 483000 && R_No != R_Go) Count_R++; 00640 00641 if(Count_R >= 12 && W_WidsR >= linewide && EnCount_1R >= 500000){ //右1 00642 Count_C++; 00643 WWIDCLR; 00644 EnCount_1R =0; 00645 Count_R = 1; 00646 R_No ++;} 00647 else if(Count_R >= 12 && EnCount_1R >= 550000){ 00648 Count_C++; 00649 EnCount_1R = 3000; 00650 Count_R = 1; 00651 R_No ++;} 00652 00653 } 00654 //----------タイマー割り込み--------------- 00655 void flip(){ 00656 timer1++; 00657 timer2++; 00658 //Log_timer++; 00659 ErrChk(); 00660 Get_Pot(); 00661 DSens(); 00662 SteerCtrl(SteerMode); 00663 if(!TestDrive_Flg)SpeedCtrl(Speed); 00664 MotorCtrl(); 00665 Line_Dec(); 00666 LanceAng(Angle); 00667 //if(ModeSW()==0 && !SW_W) SD_Mount(); 00668 //if(ModeSW()==1 && !SW_W) SD_Unmount(); 00669 enCalc(); 00670 //if(Log_timer >= 9 && writeflg && Tmpcnt <= 5999) TmpLog(); 00671 //if(timer1 >= 1000 && !SW_W && !SW_B && !DebugFlg) Debug(); 00672 //ST_EN; 00673 /*if(ModeSW()==2){ 00674 ST_EN; 00675 //OTH_EN; 00676 Angle = Pot_F + 90; 00677 } 00678 else if(ModeSW()==0) { 00679 ST_DA; 00680 OTH_DA; 00681 }*/ 00682 00683 } 00684 //----------Tmpログモード-------------------- 00685 void TmpLog(){ 00686 00687 LLog_tmp[Tmpcnt] = EnCount; 00688 /*Log_tmp[0][Tmpcnt] = Count_R; 00689 Log_tmp[1][Tmpcnt] = Count_R; 00690 Log_tmp[2][Tmpcnt] = W_WidsR; 00691 Log_tmp[3][Tmpcnt] = W_WidsL; 00692 Log_tmp[4][Tmpcnt] = DSensData[3]; 00693 Log_tmp[5][Tmpcnt] = DSensData[4]; 00694 Log_tmp[6][Tmpcnt] = Pot_R>>2; 00695 Log_tmp[7][Tmpcnt] = 0; 00696 Log_tmp[8][Tmpcnt] = 0; 00697 Log_tmp[9][Tmpcnt] = 0;*/ 00698 Tmpcnt++; 00699 Log_timer = 0; 00700 } 00701 //----------SDログモード-------------------- 00702 void SDLog(){ 00703 //char i=0; 00704 if( SD_Flag & 0x02){ 00705 //fprintf(fp, "%-10d%-10d\r\n",EnCount,EnCountI); 00706 //fprintf(fp, "%-4d%-4d%-4d%-4d\r\n",Log_table[0][0],Log_table[0][1],Log_table[0][6],Log_table[0][7]); 00707 //fprintf(fp, "%-4d%-4d%-4d%-4d%-4d%-4d%-4d%-4d\r\n",ONSensData[0],OFFSensData[0],ONSensData[1],OFFSensData[1],ONSensData[6],OFFSensData[6],ONSensData[7],OFFSensData[7]); 00708 /*for(i=0;i<=10;i++){ 00709 fprintf(fp, "%-4d%-4d%-4d%-4d\r\n",Log_table[i][0],Log_table[i][1],Log_table[i][6],Log_table[i][7]); 00710 }*/ 00711 } 00712 else Log_timer=0; 00713 00714 } 00715 //----------SD書き込みシーケンス-------------------- 00716 void SD_WriteSq(){ 00717 int i=0; 00718 if( SD_Flag & 0x02){ 00719 //fprintf(fp, "\r\nLineWidsL\r\n"); 00720 fprintf(fp, "EncountI time \r\n"); 00721 fprintf(fp, "%-10d%-10d%-10d\r\n",EnCountI,timer1,Count_C); 00722 fprintf(fp, "Encount PWM1 PWM2 PWM3 PWM4 F_Pot R_Pot \r\n"); 00723 for(i=0;i<=Tmpcnt;i++){ 00724 //fprintf(fp, "%-10d%-4d%-2d\r\n",W_WidsR[i][0],Log_W_Wids[i][1],Log_W_WidsLR[i]); 00725 //fprintf(fp, "%-10d%-4d%-4d%-4d%-4d%-4d%-4d%-4d%-4d%-4d\r\n",Log_tmp[0][i],Log_tmp[1][i],Log_tmp[2][i],Log_tmp[3][i],Log_tmp[4][i],Log_tmp[5][i],Log_tmp[6][i],Log_tmp[7][i],Log_tmp[8][i],Log_tmp[9][i]); 00726 //fprintf(fp, "%-10d%-6d%-6d%-6d%-6d%-6d%-6d\r\n",LLog_tmp[i],Log_tmp[0][i],Log_tmp[1][i],Log_tmp[2][i],Log_tmp[3][i],Log_tmp[4][i],Log_tmp[5][i]); 00727 //fprintf(fp, "%-10d\r\n",LLog_tmp[i]); 00728 } 00729 fprintf(fp, "Wids EnCount_1R WidsLR \r\n"); 00730 for(i=0;i<=WidsNo;i++){ 00731 fprintf(fp, "%-20d%-20d%-4d%-4d%-4d\r\n",Log_W_Wids[i][0],Log_W_Wids[i][1],Log_W_WidsLR[i],Log_Count_R[i],LLog_tmp[i]); 00732 } 00733 } 00734 00735 } 00736 //---------SDカードをマウント------------- 00737 void SD_Mount(){ 00738 if(SD_Flag == 0x00){ 00739 led_color(RED); 00740 lcd.clear(); 00741 lcd.printf(0,"Mounting"); 00742 fp = fopen("/sd/Log.txt", "w"); 00743 led_color(BLU); 00744 SD_Flag |= 0x01; 00745 if (fp != NULL){ 00746 SD_Flag |= 0x02; //success 00747 lcd.clear(); 00748 lcd.printf(0,"Success"); 00749 } 00750 else SD_Flag |= 0x10; //fail 00751 } 00752 led_color(GRE); 00753 } 00754 00755 //---------SDカードをアンマウント------------- 00756 void SD_Unmount(){ 00757 if(SD_Flag & 0x01){ 00758 led_color(RED); 00759 lcd.clear(); 00760 lcd.printf(0,"UnMnt..."); 00761 SD_WriteSq(); 00762 SD_Flag = 0; 00763 fclose(fp); 00764 led_color(WHI); 00765 lcd.clear(); 00766 lcd.printf(0,"Success"); 00767 wait(1); 00768 }} 00769 //----------マイコン初期設定--------------- 00770 void init(void){ 00771 //I/O設定 00772 RS1.mode(PullDown); //ロータリスイッチbit1 00773 RS2.mode(PullDown); //ロータリスイッチbit2 00774 RS3.mode(PullDown); //ロータリスイッチbit3 00775 RS4.mode(PullDown); //ロータリスイッチbit4 00776 SW_W.mode(PullUp); //スイッチ白 00777 SW_R.mode(PullUp); //スイッチ赤 00778 SW_B.mode(PullUp); //スイッチ黒 00779 s0signal.rise(&s0_trigger_rise); //エンコーダA相立ち上がり 00780 s0signal.fall(&s0_trigger_fall); //エンコーダA相立ち下がり 00781 s1signal.rise(&s1_trigger_rise); //エンコーダB相立ち上がり 00782 s1signal.fall(&s1_trigger_fall); //エンコーダB相立ち下がり 00783 //割り込み処理開始 00784 flipper.attach_us(&flip,1000); //汎用タイマー割り込み 00785 sensget.attach_us(&sensGet,100); //センサー用タイマー割り込み 00786 //PWM周期設定 00787 PWM_CH1.period(0.0001); 00788 PWM_CH2.period(0.001); 00789 PWM_CH3.period(0.001); 00790 PWM_CH4.period(0.001); 00791 PWM_CH5.period(0.001); 00792 PWM_CH6.period(0.0001); 00793 ST_DA; 00794 OTH_DA; 00795 } 00796 //----------------メイン--------------------- 00797 int main(){ 00798 init(); //マイコン初期設定 00799 wait(1); 00800 ST_EN; 00801 SteerMode = TRACE; 00802 lcd.contrast( 0x2E ); //LCDコントラスト設定 00803 wait(0.5); 00804 if(ModeSelect_Flg == 0) ModeSelect(); 00805 wait(1); 00806 while(SW_R){ 00807 if(!SW_W && !SW_B && !DebugFlg) Debug(); 00808 lcd.clear(); 00809 lcd.printf(0,"%dms",TripTime); 00810 lcd.printf(1,"%dmm/s",SPCom); 00811 //if(!SW_W) SD_Mount(); 00812 wait(0.1); 00813 } 00814 lcd.clear(); 00815 lcd.printf(0,"Ready!"); 00816 lcd.printf(1,"%x",SD_Flag); 00817 wait(1); 00818 lcd.clear(); 00819 lcd.printf(0,"Go!"); 00820 OTH_EN; 00821 W_WidsL = W_WidsR = 0; 00822 //Speed = SPCom; 00823 Angle = 90; 00824 Speed = SPCom; 00825 EnCount_1R = -40000; 00826 timer1 =0; 00827 StartFlg = 1; 00828 //writeflg = 1; 00829 while(1){ 00830 00831 if(!SW_W && !SW_B && !DebugFlg && !EnCount) Debug(); 00832 /*if(timer2 >= 100){ 00833 timer2 = 0; 00834 lcd.clear(); 00835 lcd.printf(0,"%d",timer1); 00836 lcd.printf(1,"%d",ErrSt); 00837 }*/ 00838 Localization(); 00839 //if(Log_timer >= 10 && writeflg && Tmpcnt <= 1000) TmpLog(); 00840 00841 if(Count_R == 0 ){ 00842 Speed = SPCom; 00843 led_color(WHI); 00844 Angle = 90;} 00845 if(Count_R == 1 ){ 00846 Speed = SPCom; 00847 led_color(RED); 00848 if(EnCount_1R >= 30000) Angle = 150; 00849 else if(EnCount_1R >= 13000) Angle = 180;//11000 00850 } 00851 if(Count_R == 2 ){ 00852 Speed = SPCom; 00853 led_color(GRE); 00854 if(EnCount_1R >= 75000) Angle = 180; //75000 00855 else Angle = 150; 00856 } 00857 if(Count_R == 3 ){ 00858 Speed = SPYobi; 00859 Angle = 90; 00860 led_color(BLU);} 00861 if(Count_R == 4 ){ 00862 led_color(YEL); 00863 if(EnCount_1R >= 200000 && Turbo_Flg) Speed = SPCom; 00864 else Speed = SPCor; 00865 Angle = 30; 00866 STAngle = -27; 00867 //if(Shortcut_Flg)SteerMode = ANGLE; 00868 } 00869 if(Count_R == 5 ){ 00870 Speed = SPCom; 00871 Angle = 125; 00872 led_color(RED);} 00873 if(Count_R == 6 ){ 00874 if(Shortcut_Flg)SteerMode = WTRACE; 00875 Speed = SPCom; 00876 led_color(GRE); 00877 if(EnCount_1R >= 270000){ 00878 LA_EN; 00879 Angle = 52;} 00880 else if(EnCount_1R >= 240000) LA_DA;//Angle = 130; 00881 } 00882 if(Count_R == 7 ){ 00883 if(Shortcut_Flg)SteerMode = TRACE; 00884 Speed = SPCom; 00885 led_color(PUR); 00886 if(EnCount_1R >= 320000){ 00887 LA_EN; 00888 Angle = 135;} 00889 else LA_DA;//Angle = 52; 00890 } 00891 if(Count_R == 8 ){ 00892 Speed = SPCom; 00893 led_color(RED); 00894 if(EnCount_1R >= 365000){ 00895 LA_EN; 00896 Angle = 49;} 00897 else LA_DA;//Angle = 132; 00898 } 00899 if(Count_R == 9 ){ 00900 LA_EN; 00901 Speed = SPCom; 00902 Angle = 49; 00903 led_color(GRE);} 00904 if(Count_R == 10){ 00905 Speed = SPYobi; 00906 led_color(WHI);} 00907 if(Count_R == 11){ 00908 led_color(BLU); 00909 Angle = 90; 00910 STAngle = -6; 00911 if(R_No == R_Go){ 00912 if(EnCount_1R <= 470000){ 00913 Angle = 400; 00914 SteerMode = ANGLE;} 00915 else { 00916 SteerMode = TRACE; 00917 ErrSt = AccelStopCnt; 00918 if(EnCount==0)Angle = 90; 00919 } 00920 } 00921 if(EnCount_1R >= 470000 && Turbo_Flg) Speed = SPCom; 00922 else Speed = SPCor; 00923 } 00924 if(Count_R == 12){ 00925 SteerMode = TRACE; 00926 Speed = SPCom; 00927 led_color(YEL);} 00928 00929 } 00930 } 00931 00932 //----------モード選択-------------------- 00933 void ModeSelect(void){ 00934 int DebugModeSW; 00935 writeflg = 0; 00936 ModeSelect_Flg=1; 00937 lcd.clear(); 00938 lcd.printf(0,"Select"); 00939 lcd.printf(1,"TripTime"); 00940 wait(1); 00941 lcd.clear(); 00942 while( SW_R ){ 00943 DebugModeSW = ModeSW(); 00944 switch(DebugModeSW){ 00945 case 0: 00946 lcd.clear(); 00947 lcd.printf(0,"Turbo_ON"); 00948 Turbo_Flg = 1; 00949 wait(0.1); 00950 break; 00951 case 1: 00952 lcd.clear(); 00953 lcd.printf(0,"Round30"); 00954 TripTime =30000; 00955 R_Go = 7; 00956 wait(0.1); 00957 break; 00958 case 2: 00959 lcd.clear(); 00960 lcd.printf(0,"Round60"); 00961 TripTime =60000; 00962 R_Go = 15; 00963 wait(0.1); 00964 break; 00965 case 3: 00966 lcd.clear(); 00967 lcd.printf(0,"Round10"); 00968 TripTime =10000; 00969 R_Go = 2; 00970 wait(0.1); 00971 break; 00972 case 4: 00973 lcd.clear(); 00974 lcd.printf(0,"Round20"); 00975 TripTime =20000; 00976 wait(0.1); 00977 break; 00978 case 5: 00979 lcd.clear(); 00980 lcd.printf(0,"Shortcut60"); 00981 TripTime =60000; 00982 Shortcut_Flg = 1; 00983 wait(0.1); 00984 break; 00985 00986 default: 00987 lcd.clear(); 00988 lcd.printf(0,"Select"); 00989 lcd.printf(1,"1-5"); 00990 wait(0.1); 00991 break; 00992 } 00993 } 00994 wait(1); 00995 lcd.clear(); 00996 while( SW_R){ 00997 DebugModeSW = ModeSW(); 00998 switch(DebugModeSW){ 00999 case 1: 01000 lcd.clear(); 01001 lcd.printf(0,"STSpeed"); 01002 lcd.printf(1,"5m/s"); 01003 SPCom=5000; 01004 wait(0.1); 01005 break; 01006 case 4: 01007 lcd.clear(); 01008 lcd.printf(0,"STSpeed"); 01009 lcd.printf(1,"4.5m/s"); 01010 wait(1); 01011 SPCom=4500; 01012 SPCor=2800; 01013 break; 01014 case 3: 01015 lcd.clear(); 01016 lcd.printf(0,"STSpeed"); 01017 lcd.printf(1,"6m/s"); 01018 SPCom=6000; 01019 wait(0.1); 01020 break; 01021 case 2: 01022 lcd.clear(); 01023 lcd.printf(0,"STSpeed"); 01024 lcd.printf(1,"5.2m/s"); 01025 SPCom=5200; 01026 wait(0.1); 01027 break; 01028 case 5: 01029 lcd.clear(); 01030 lcd.printf(0,"AllSpeed"); 01031 lcd.printf(1,"3m/s"); 01032 wait(1); 01033 SPCom=3000; 01034 SPYobi=3000; 01035 SPCor=3000; 01036 wait(0.1); 01037 break; 01038 01039 case 6: 01040 lcd.clear(); 01041 lcd.printf(0,"FreeDrive"); 01042 TestDrive_Flg=1; 01043 wait(0.1); 01044 break; 01045 01046 default: 01047 lcd.clear(); 01048 lcd.printf(0,"Select"); 01049 lcd.printf(1,"1-6"); 01050 wait(0.1); 01051 break; 01052 } 01053 } 01054 } 01055 01056 //----------デバッグモード-------------------- 01057 void Debug(void){ 01058 int DebugModeSW; 01059 int PotTest1,PotTest2; 01060 writeflg = 0; 01061 DebugFlg=1; 01062 lcd.clear(); 01063 lcd.printf(0,"DBGMode"); 01064 wait(2); 01065 while( SW_R ){ 01066 DebugModeSW = ModeSW(); 01067 switch(DebugModeSW){ 01068 case 1: 01069 lcd.clear(); 01070 lcd.printf(0,"SensT/F"); 01071 wait(0.1); 01072 break; 01073 case 2: 01074 lcd.clear(); 01075 lcd.printf(0,"SensA/D"); 01076 wait(0.1); 01077 break; 01078 case 3: 01079 lcd.clear(); 01080 lcd.printf(0,"Trip"); 01081 wait(0.1); 01082 break; 01083 case 4: 01084 lcd.clear(); 01085 lcd.printf(0,"LinWidLR"); 01086 wait(0.1); 01087 break; 01088 case 5: 01089 lcd.clear(); 01090 lcd.printf(0,"LineCnr"); 01091 wait(0.1); 01092 break; 01093 case 6: 01094 lcd.clear(); 01095 lcd.printf(0,"LineA/D"); 01096 wait(0.1); 01097 break; 01098 case 7: 01099 lcd.clear(); 01100 lcd.printf(0,"PotA/D"); 01101 wait(0.1); 01102 break; 01103 case 8: 01104 lcd.clear(); 01105 lcd.printf(0,"Speed5"); 01106 wait(0.1); 01107 break; 01108 case 9: 01109 lcd.clear(); 01110 lcd.printf(0,"Speed7"); 01111 wait(0.1); 01112 break; 01113 case 10: 01114 lcd.clear(); 01115 lcd.printf(0,"Speed2"); 01116 wait(0.1); 01117 break; 01118 case 11: 01119 lcd.clear(); 01120 lcd.printf(0,"Speed0"); 01121 wait(0.1); 01122 break; 01123 case 12: 01124 lcd.clear(); 01125 lcd.printf(0,"linewide"); 01126 lcd.printf(1,"100"); 01127 wait(0.1); 01128 break; 01129 case 13: 01130 lcd.clear(); 01131 lcd.printf(0,"SD_Mount?"); 01132 lcd.printf(1,"%x",SD_Flag); 01133 wait(0.1); 01134 if(!SW_W) SD_Mount(); 01135 writeflg = 1; 01136 break; 01137 case 14: 01138 lcd.clear(); 01139 lcd.printf(0,"SD_UnMount?"); 01140 wait(0.1); 01141 if(!SW_W) SD_Unmount(); 01142 break; 01143 case 15: 01144 lcd.clear(); 01145 lcd.printf(0,"DSPSens"); 01146 wait(0.1); 01147 break; 01148 default: 01149 break; 01150 } 01151 } 01152 wait(1); 01153 while( SW_R){ 01154 DebugModeSW = ModeSW(); 01155 switch(DebugModeSW){ 01156 case 1: 01157 lcd.clear(); 01158 lcd.printf(0,"0167RL"); 01159 lcd.printf(1,"%1d%1d%1d%1d%1d%1d",TFSensData[0],TFSensData[1],TFSensData[6],TFSensData[7],R_LineFlg ,L_LineFlg); 01160 wait(0.1); 01161 break; 01162 case 2: 01163 lcd.clear(); 01164 //lcd.printf(0,"SensA/D"); 01165 lcd.printf(0,"%3d,%3d",DSensData[3],DSensData[4]); 01166 lcd.printf(1,"%3d,%3d",DSensData[2],DSensData[5]); 01167 wait(0.1); 01168 break; 01169 case 3: 01170 lcd.clear(); 01171 lcd.printf(0,"%d",EnCount ); 01172 lcd.printf(1,"%d",EnCountI); 01173 wait(0.1); 01174 break; 01175 case 4: 01176 lcd.clear(); 01177 lcd.printf(0,"%d",W_WidsL ); 01178 lcd.printf(1,"%d",W_WidsR ); 01179 wait(0.1); 01180 break; 01181 case 5: 01182 lcd.clear(); 01183 lcd.printf(0,"%d",R_LineCnt ); 01184 lcd.printf(1,"%d",L_LineCnt ); 01185 wait(0.1); 01186 break; 01187 case 6: 01188 lcd.clear(); 01189 lcd.printf(0,"%3d,%3d",DSensData[0],DSensData[1]); 01190 lcd.printf(1,"%3d,%3d",DSensData[6],DSensData[7]); 01191 wait(0.5); 01192 break; 01193 case 7: 01194 PotTest1 = (DFP-(Steer_Ptm.read_u16()>>6))/3; 01195 PotTest2 = ((Lance_Ptm.read_u16()>>6)-DRP)/3; 01196 lcd.clear(); 01197 lcd.printf(0,"%3d,%3d",Pot_F,PotTest1); 01198 lcd.printf(1,"%3d,%3d",Pot_R,PotTest2); 01199 /*lcd.printf(0,"%3d,%3d",RearPot[1],RearPot[4]); 01200 lcd.printf(1,"%3d,%3d",RearPot[5],RearPot[9]);*/ 01201 wait(0.1); 01202 break; 01203 case 8: 01204 SPCom=5000; 01205 lcd.clear(); 01206 lcd.printf(0,"Speed5"); 01207 lcd.printf(1,"%d",SPCom); 01208 wait(0.1); 01209 //SPYobi=3500; 01210 //SPCor=3000; 01211 //ParamS=2000; 01212 break; 01213 case 9: 01214 SPCom=7000; 01215 lcd.clear(); 01216 lcd.printf(0,"Speed7"); 01217 lcd.printf(1,"%d",SPCom); 01218 wait(0.1); 01219 //SPYobi=3000; 01220 //SPCor=3000; 01221 break; 01222 case 10: 01223 SPCom=2000; 01224 SPYobi=2000; 01225 SPCor=2000; 01226 lcd.clear(); 01227 lcd.printf(0,"Speed2"); 01228 lcd.printf(1,"%d",SPCom); 01229 wait(0.1); 01230 break; 01231 case 11: 01232 SPCom=0; 01233 SPYobi=0; 01234 SPCor=0; 01235 lcd.clear(); 01236 lcd.printf(0,"Speed0"); 01237 lcd.printf(1,"%d",SPCom); 01238 wait(0.1); 01239 break; 01240 case 12: 01241 lcd.clear(); 01242 lcd.printf(0,"R_No"); 01243 lcd.printf(1,"%d",R_No); 01244 wait(0.1); 01245 linewide=100; 01246 break; 01247 case 13: 01248 lcd.clear(); 01249 lcd.printf(0,"ErrSt"); 01250 lcd.printf(1,"%d",ErrSt); 01251 wait(0.1); 01252 break; 01253 case 15: 01254 lcd.clear(); 01255 lcd.printf(0,"%d",DSensData[0]); 01256 wait(0.1); 01257 break; 01258 default: 01259 break; 01260 } 01261 } 01262 01263 01264 while( SW_W ){ 01265 lcd.clear(); 01266 lcd.printf(0,"Exit?"); 01267 wait(0.1); 01268 } 01269 lcd.clear(); 01270 lcd.printf(0,"Bye-Bye"); 01271 wait(2); 01272 lcd.clear(); 01273 DebugFlg=0; 01274 }
Generated on Sat Jul 16 2022 19:14:57 by
1.7.2