ランサー
Dependencies: SB1602E SDFileSystem mbed
Fork of Seeed_SDCard_Shield by
Diff: main.cpp
- Revision:
- 7:c99fe0eb544a
- Parent:
- 6:32698402be64
- Child:
- 8:352404951de8
--- a/main.cpp Tue Mar 28 11:47:29 2017 +0000 +++ b/main.cpp Sun Apr 02 08:23:38 2017 +0000 @@ -22,11 +22,18 @@ #define ST_DA ST_CTR = 1 #define OTH_EN OTH_CTR = 0 #define OTH_DA OTH_CTR = 1 -#define Ss 6 -#define ThS 400 //Line白黒判別閾値 +#define DSs 8 //LineセンサA/Dシフト数 +#define ASs 6 //トレース用LineセンサA/Dシフト数 +#define ThS 80 //Line白黒判別閾値 +#define DSPA0 0.00748 //ローパスフィルタ定数(200HzHaming窓N5) +#define DSPA1 0.16347 //ローパスフィルタ定数(200HzHaming窓N5) +#define DSPA2 0.4 //ローパスフィルタ定数(200HzHaming窓N5) +#define DSPA3 0.16347 //ローパスフィルタ定数(200HzHaming窓N5) +#define DSPA4 0.00748 //ローパスフィルタ定数(200HzHaming窓N5) SB1602E lcd(PB_9,PB_8); //LCD_I2C設定(SDA, SCL) -SDFileSystem sd(D11, D12, D13, D10,"sd"); // MOSI, MISO, SCK, CS +//SDFileSystem sd(D11, D12, D13, D10,"sd"); // MOSI, MISO, SCK, CS +SDFileSystem sd(PA_7,PA_6,PA_5,PB_6,"sd"); // MOSI, MISO, SCK, CS FILE *fp; //センサ入力 DigitalIn RS1(PC_10); //ロータリースイッチbit1 @@ -70,9 +77,10 @@ DigitalOut LED_G(PA_14); //LED_G DigitalOut LED_B(PB_3); //LED_B //割り込み定義 -Ticker flipper; //汎用タイマー -Ticker sensget; //センサー用タイマー -Ticker encLerp; //エンコーダ割り込み +Ticker flipper; //汎用タイマー +Ticker sensget; //センサー用タイマー +Ticker SDLogflip; //エンコーダ割り込み +//Ticker encLerp; //エンコーダ割り込み //プロトタイプ宣言 void init(void); @@ -98,6 +106,9 @@ void HolSeq1(void); void HolSeq2(void); void Debug(void); //デバッグモード +void SD_Log(void); //SDログ +void SD_WriteSq(void); //SD書き込み +void TmpLog(void); //内部メモリへのログ //グローバル変数の宣言 int EnCount=0,EnCountBuf=0,EnCountI=0; //エンコーダカウント @@ -109,7 +120,7 @@ int encA=0,encB=0; //エンコーダ相状態確認 unsigned char SD_Flag=0;//SDカードの状態判定 unsigned char Line_Flg; //ライン情報フラグ -int timer1=0,timer2=0,timer3=0,timer4=0,cntM=0,Ext_timer=0; //タイマー +int timer1=0,timer2=0,timer3=0,Tmpcnt=0,Log_timer=0; //タイマー int Stime=0; unsigned long long cntT=0; int ONSensData[8],OFFSensData[8],DSensData[8],TFSensData[8];//ラインセンサ値 @@ -130,6 +141,14 @@ int ParamS=1; int SPCom=3000,SPYobi=3000,SPCor=3000,loopJump=1000000,linewide=100; +char Log_table[10][10]; +int Log_W_Wids[100][2]; +char Log_W_WidsLR[100]; +char Log_tmp[10][10000]; +int Sensecnt=0,writeflg=0; + +unsigned char WidsNo=0; +char Logcnt=0; char Trace_table[]={ 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, 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, @@ -162,7 +181,9 @@ 1.25,1.26,1.27,1.28,1.30,1.31,1.32,1.33,1.34,1.36,1.37,1.38,1.40, 1.41,1.43,1.44,1.46,1.47,1.49,1.51,1.53,1.55,1.57,1.59}; - +//////////////////////////////////////////////// +//-------------ベースプログラム-----------------// +//////////////////////////////////////////////// //----------モードSW-------------------- int ModeSW(){ char RSW=0; @@ -172,196 +193,13 @@ if(RS4)RSW += 8; RSW_Value=int(RSW); return RSW;} -//----------デバッグモード-------------------- -void Debug(void){ - int DebugModeSW; - DebugFlg=1; - lcd.clear(); - lcd.printf(0,"DBGMode"); - wait(2); - while( SW_R ){ - DebugModeSW = ModeSW(); - switch(DebugModeSW){ - case 1: - lcd.clear(); - lcd.printf(0,"SensT/F"); - wait(0.1); - break; - case 2: - lcd.clear(); - lcd.printf(0,"SensA/D"); - wait(0.1); - break; - case 3: - lcd.clear(); - lcd.printf(0,"Trip"); - wait(0.1); - break; - case 4: - lcd.clear(); - lcd.printf(0,"LinWidLR"); - wait(0.1); - break; - case 5: - lcd.clear(); - lcd.printf(0,"LineCnr"); - wait(0.1); - break; - case 6: - lcd.clear(); - lcd.printf(0,"LineA/D"); - wait(0.1); - break; - case 7: - lcd.clear(); - lcd.printf(0,"PotA/D"); - wait(0.1); - break; - case 8: - lcd.clear(); - lcd.printf(0,"Speed3"); - wait(0.1); - break; - case 9: - lcd.clear(); - lcd.printf(0,"Speed3-2"); - wait(0.1); - break; - case 10: - lcd.clear(); - lcd.printf(0,"LoopJump"); - wait(0.1); - break; - case 11: - lcd.clear(); - lcd.printf(0,"linewide"); - lcd.printf(1,"400"); - wait(0.1); - break; - case 12: - lcd.clear(); - lcd.printf(0,"linewide"); - lcd.printf(1,"100"); - wait(0.1); - break; - case 13: - lcd.clear(); - lcd.printf(0,"SD_Mount?"); - lcd.printf(1,"%x",SD_Flag); - wait(0.1); - if(!SW_W) SD_Mount(); - break; - case 14: - lcd.clear(); - lcd.printf(0,"SD_UnMount?"); - wait(0.1); - if(!SW_W) SD_Unmount(); - break; - default: - break; - } - } - wait(2); - while( SW_R){ - DebugModeSW = ModeSW(); - switch(DebugModeSW){ - case 1: - lcd.clear(); - lcd.printf(0,"0167RL"); - lcd.printf(1,"%1d%1d%1d%1d%1d%1d",TFSensData[0],TFSensData[1],TFSensData[6],TFSensData[7],R_LineFlg ,L_LineFlg); - wait(0.1); - break; - case 2: - lcd.clear(); - lcd.printf(0,"SensA/D"); - lcd.printf(1,"%3d,%3d",DSensData[3],DSensData[4]); - wait(0.1); - break; - case 3: - lcd.clear(); - lcd.printf(0,"%d",EnCount ); - lcd.printf(1,"%d",EnCountI); - wait(0.1); - break; - case 4: - lcd.clear(); - lcd.printf(0,"%d",W_WidsL ); - lcd.printf(1,"%d",W_WidsR ); - wait(0.1); - break; - case 5: - lcd.clear(); - lcd.printf(0,"%d",R_LineCnt ); - lcd.printf(1,"%d",L_LineCnt ); - wait(0.1); - break; - case 6: - lcd.clear(); - lcd.printf(0,"%3d,%3d",DSensData[0],DSensData[1]); - lcd.printf(1,"%3d,%3d",DSensData[6],DSensData[7]); - wait(0.5); - break; - case 7: - lcd.clear(); - lcd.printf(0,"%d",Pot_F); - lcd.printf(1,"%d",Pot_R); - wait(0.1); - break; - case 8: - lcd.clear(); - lcd.printf(0,"Speed5"); - wait(0.1); - SPCom=5000; - SPYobi=3500; - SPCor=3000; - ParamS=2000; - break; - case 9: - lcd.clear(); - lcd.printf(0,"Speed3-2"); - wait(0.1); - SPCom=3000; - SPYobi=3000; - SPCor=3000; - break; - case 10: - lcd.clear(); - lcd.printf(0,"LoopJump"); - wait(0.1); - loopJump = 1000000; - break; - case 11: - lcd.clear(); - lcd.printf(0,"linewide"); - lcd.printf(1,"400"); - wait(0.1); - linewide=400; - break; - case 12: - lcd.clear(); - lcd.printf(0,"linewide"); - lcd.printf(1,"100"); - wait(0.1); - linewide=100; - break; - - default: - break; - } - } - - - while( SW_W ){ - lcd.clear(); - lcd.printf(0,"Exit?"); - wait(0.1); - } - lcd.clear(); - lcd.printf(0,"Bye-Bye"); - wait(2); - lcd.clear(); - DebugFlg=0; -} +//----------SW情報取得-------------------- +int Get_SW(){ + char SW_Val=0; + if(SW_W)SW_Val |= 0x01; + if(SW_R)SW_Val |= 0x02; + if(SW_B)SW_Val |= 0x04; + return SW_Val;} //----------エラーチェック-------------------- void ErrChk(void){ if(timer1 >= 30000) ErrSt = 100; @@ -369,6 +207,91 @@ if(ErrSt < 100 && Pot_F == 0 ) ErrSt=0; if(ErrSt >= 120 ) ErrSt = 120; } + //-----------エンコーダカウント------------------ +void s0_trigger_rise() { // A相立上り変化割込み時の処理 (Low -> High) + EncFlag=1; //外部割込み停止検出 + Rev |= 0x01; + if(Rev&0x10) EnCount--; + else EnCount++; + } +void s0_trigger_fall() { // A相立下り変化割込み時の処理 (High -> Low) + Rev &= ~0x01; + if(Rev&0x10) EnCount++; + else EnCount--; + } +void s1_trigger_rise() { // B相立上り変化割込み時の処理 (Low -> High) + Rev |= 0x10; + if(Rev&0x01) EnCount++; + else EnCount--; + } +void s1_trigger_fall() { // B相立下り変化割込み時の処理 (High -> Low) + Rev &= ~0x10; + if(Rev&0x01) EnCount--; + else EnCount++; + } +//----------エンコーダ計算-------------------- +void enCalc(void){ + EnCountI = EnCountI + EnCount; + EnCount_C = EnCount_C + EnCount; + EnCount_L = EnCount_L + EnCount; + EnCount_R = EnCount_R + EnCount; + EnCount_H = EnCount_H + EnCount; + EnCount_F = EnCount_F + EnCount; + EnCount = 0; +} +//----------フルカラーLED制御-------------------- +void led_color(char R , char G , char B ) +{ + if ( R ) LED_R = 1; + else LED_R = 0; + if ( G ) LED_G = 1; + else LED_G = 0; + if ( B ) LED_B = 1; + else LED_B = 0; + } +//----------センサー値の取得--------------- +void sensGet(){ + if(Stime >= 10) Stime=0; + if(Stime == 0 ) SensLED=0; + if(Stime == 5 ) SensLED=1; + if(Stime == 4 ) { + OFFSensData[0] = Line0.read_u16()>>DSs; + OFFSensData[1] = Line1.read_u16()>>DSs; + OFFSensData[2] = Line2.read_u16()>>ASs; + OFFSensData[3] = Line3.read_u16()>>ASs; + OFFSensData[4] = Line4.read_u16()>>ASs; + OFFSensData[5] = Line5.read_u16()>>ASs; + OFFSensData[6] = Line6.read_u16()>>DSs; + OFFSensData[7] = Line7.read_u16()>>DSs; + } + if(Stime == 9 ) { + ONSensData[0] = Line0.read_u16()>>DSs; + ONSensData[1] = Line1.read_u16()>>DSs; + ONSensData[2] = Line2.read_u16()>>ASs; + ONSensData[3] = Line3.read_u16()>>ASs; + ONSensData[4] = Line4.read_u16()>>ASs; + ONSensData[5] = Line5.read_u16()>>ASs; + ONSensData[6] = Line6.read_u16()>>DSs; + ONSensData[7] = Line7.read_u16()>>DSs; + } + Stime++; + Pot_F = (DFP-(Steer_Ptm.read_u16()>>6))/3; + Pot_R = Lance_Ptm.read_u16()>>6; + } + //----------センサー値差分の計算--------------- +void DSens(){ + + int sensorno; + for(sensorno=0;sensorno<=7;sensorno++){ + DSensData[sensorno] = OFFSensData[sensorno] - ONSensData[sensorno]; + if(DSensData[sensorno] >= ThS ) TFSensData[sensorno] = 1; + else TFSensData[sensorno] = 0; + } + } + +//////////////////////////////////////////////// +//-------------行動計画------------------------// +//////////////////////////////////////////////// //----------コーナーモード-------------------- void CornerSeq(void){ EnCount_C = 0; @@ -441,8 +364,13 @@ HitCnt = int(225+((EnCount_H - 33137)*0.0183)); Angle= HitCnt; }*/ } + +//////////////////////////////////////////////// +//-------------便利機能プログラム---------------// +//////////////////////////////////////////////// //----------白線判別-------------------- void Line_Dec(void){ + char LR_Flg=0; if(TFSensData[0] && TFSensData[1] ) { R_LineCntF = 0; R_LineCnt++;} @@ -462,7 +390,12 @@ } else if(!R_LineCnt && R_LineFlg && R_LineCntF >= 4 ){ R_LineFlg = 0; + LR_Flg = 1; W_WidsR = EnCountI - W_WidsRCnt; + Log_W_WidsLR[WidsNo] = LR_Flg; + Log_W_Wids[WidsNo][0] = W_WidsR; + Log_W_Wids[WidsNo][1] = EnCountI; + WidsNo++; } if(L_LineCnt >= 2 && !L_LineFlg ){ L_LineFlg = 1; @@ -470,36 +403,15 @@ } else if(!L_LineCnt && L_LineFlg && L_LineCntF >= 4 ){ L_LineFlg = 0; + LR_Flg = 0; W_WidsL = EnCountI - W_WidsLCnt; + Log_W_WidsLR[WidsNo] = LR_Flg; + Log_W_Wids[WidsNo][0] = W_WidsL; + Log_W_Wids[WidsNo][1] = EnCountI; + WidsNo++; } } -//----------エンコーダ計算-------------------- -void enCalc(void){ - EnCountI = EnCountI + EnCount; - EnCount_C = EnCount_C + EnCount; - EnCount_L = EnCount_L + EnCount; - EnCount_R = EnCount_R + EnCount; - EnCount_H = EnCount_H + EnCount; - EnCount_F = EnCount_F + EnCount; - EnCount = 0; -} -//----------フルカラーLED制御-------------------- -void led_color(char R , char G , char B ) -{ - if ( R ) LED_R = 1; - else LED_R = 0; - if ( G ) LED_G = 1; - else LED_G = 0; - if ( B ) LED_B = 1; - else LED_B = 0; - } -//----------SW情報取得-------------------- -int Get_SW(){ - char SW_Val=0; - if(SW_W)SW_Val |= 0x01; - if(SW_R)SW_Val |= 0x02; - if(SW_B)SW_Val |= 0x04; - return SW_Val;} + //----------モータ総合管理-------------------- void MotorCtrl(){ if(ErrSt >= 100){ //異常判定 @@ -509,7 +421,6 @@ PWM_CH4.pulsewidth_us(0); //後右PWM (0~1000) PWM_CH5.pulsewidth_us(0); //後左PWM (0~1000) PWM_CH6.pulsewidth_us(0); //槍舵PWM (0~100) - //if (ErrSt >=120 ) ST_DA; OTH_DA; } else{ @@ -519,7 +430,6 @@ PWM_CH4.pulsewidth_us(RR_PWM); //後右PWM (0~1000) PWM_CH5.pulsewidth_us(RL_PWM); //後左PWM (0~1000) PWM_CH6.pulsewidth_us(LA_PWM); //操舵PWM (0~100) - //PWM_CH6.pulsewidth_us(LA_PWM); //操舵PWM (0~100) Rev_CH1 = ST_Rev; //槍回転方向 (H:CW) Rev_CH2 = !FL_Rev; //前左回転方向(L:FW) Rev_CH3 = FR_Rev; //前右回転方向(H:FW) @@ -540,20 +450,9 @@ FL_Rev=FR_Rev=RR_Rev=RL_Rev=BW; } else FL_Rev=FR_Rev=RR_Rev=RL_Rev=FW; - //dV=dV*10; - /*if(V >= 0){ - FL_Rev=FR_Rev=RR_Rev=RL_Rev=FW; - } - else{ - FL_Rev=FR_Rev=RR_Rev=RL_Rev=BW; - V=V*(-1); - }*/ iV = iV + (dV/10); dV = dV + iV; - //if(ST_EN && OTH_EN && iV >= 100) ErrSt++; - if(dV>=500) dV=500; - //FL_PWM=FR_PWM=RR_PWM=RL_PWM=dV; - //EnCount=0; + if(dV>=1000) dV=1000; if(Pot_F < 0){ PT = Pot_F * (-1); FL_PWM = int( dV * Motor_TableA[PT]); @@ -588,114 +487,10 @@ if(dAng >= 60) dAng = 60; LA_PWM = dAng; } -//-----------エンコーダ------------------ -void s0_trigger_rise() { // A相立上り変化割込み時の処理 (Low -> High) - EncFlag=1; //外部割込み停止検出 - Rev |= 0x01; - if(Rev&0x10) EnCount--; - else EnCount++; - } -void s0_trigger_fall() { // A相立下り変化割込み時の処理 (High -> Low) - Rev &= ~0x01; - if(Rev&0x10) EnCount++; - else EnCount--; - } -void s1_trigger_rise() { // B相立上り変化割込み時の処理 (Low -> High) - Rev |= 0x10; - if(Rev&0x01) EnCount++; - else EnCount--; - } -void s1_trigger_fall() { // B相立下り変化割込み時の処理 (High -> Low) - Rev &= ~0x10; - if(Rev&0x01) EnCount--; - else EnCount++; - } - //----------センサー値差分の計算--------------- -void DSens(){ - - int i; - for(i=0;i<=7;i++){ - DSensData[i]=OFFSensData[i]-ONSensData[i]; - if(DSensData[i] >= ThS ) TFSensData[i] = 1; - else TFSensData[i] = 0; - } - } -//----------センサー値の取得--------------- -void sensGet(){ - if(Stime >= 10) Stime=0; - if(Stime == 0 ) SensLED=0; - if(Stime == 5 ) SensLED=1; - if(Stime == 4 ) { - OFFSensData[0] = Line0.read_u16()>>Ss; - OFFSensData[1] = Line1.read_u16()>>Ss; - OFFSensData[2] = Line2.read_u16()>>Ss; - OFFSensData[3] = Line3.read_u16()>>Ss; - OFFSensData[4] = Line4.read_u16()>>Ss; - OFFSensData[5] = Line5.read_u16()>>Ss; - OFFSensData[6] = Line6.read_u16()>>Ss; - OFFSensData[7] = Line7.read_u16()>>Ss; - } - if(Stime == 9 ) { - ONSensData[0] = Line0.read_u16()>>Ss; - ONSensData[1] = Line1.read_u16()>>Ss; - ONSensData[2] = Line2.read_u16()>>Ss; - ONSensData[3] = Line3.read_u16()>>Ss; - ONSensData[4] = Line4.read_u16()>>Ss; - ONSensData[5] = Line5.read_u16()>>Ss; - ONSensData[6] = Line6.read_u16()>>Ss; - ONSensData[7] = Line7.read_u16()>>Ss; - } - Stime++; - Pot_F = (DFP-(Steer_Ptm.read_u16()>>6))/3; - Pot_R = Lance_Ptm.read_u16()>>6; - } -//----------エンコーダ値の補間--------------- -void encorderLerp(){ - if(EnCount==0){ - if(ZCount <= 20) EnCountI+=EnCountBuf; - else EnCountBuf=EnCount; - } - else{ - EnCountI+=EnCount; - if(ZCount <= 2) EnCountBuf=EnCount; - } - PPms+=EnCountBuf/10; - EnCount=0; - ZCount++; - if(EncFlag==1){ - ZCount=0; - EncFlag=0; - } - //ZCount++; - } + //----------ライントレース--------------- void LineTrace(){ int Trace_V; - /*int Trace1=0,Trace2=0; - char Trace_Mode,A; - //TraceI=(DSensData[5]-DSensData[2])+((DSensData[4]-DSensData[3])); - Trace1=(DSensData[5]-DSensData[2])/9; - Trace2=(DSensData[4]-DSensData[3])/7; - TraceT=Trace1; - A = RSW_Value*10; - if(Trace1 >= A || Trace1 <= (-A)){ //外センサトレース - Trace_Mode=1; - TraceI = Trace2;} - else{ //内センサトレース - Trace_Mode=0; - TraceI = Trace1;} - if(TraceI <= -1){ - TraceI = TraceI*(-1); - ST_Rev=1; - } - else ST_Rev=0; - //TraceI=TraceI/RSW_Value; - if(TraceI >= 100) TraceI=100; - if(Trace_Mode) ST_PWM=Trace_table[TraceI-A+10]; - else ST_PWM=Trace_table2[TraceI]; - //ST_PWM = TraceI;*/ - //TraceI=(DSensData[5]-DSensData[2])+((DSensData[4]-DSensData[3])); - //TraceI=(DSensData[5]-DSensData[2])/10; TraceI=(DSensData[4]-DSensData[3]); if(TraceI <= -1){ TraceI = TraceI*(-1); @@ -722,16 +517,21 @@ //----------タイマー割り込み--------------- void flip(){ timer1++; - timer2++; + timer2++; //10msで1cnt timer3++; + Log_timer++; //ErrChk(); DSens(); LineTrace(); SpeedCtrl(Speed); - enCalc(); MotorCtrl(); Line_Dec(); LanceAng(Angle); + if(ModeSW()==0 && !SW_W) SD_Mount(); + if(ModeSW()==1 && !SW_W) SD_Unmount(); + if(writeflg && Tmpcnt <= 9999) TmpLog(); + enCalc(); + //if(timer1 >= 1000 && !SW_W && !SW_B && !DebugFlg) Debug(); //ST_EN; /*if(ModeSW()==2){ ST_EN; @@ -742,36 +542,97 @@ ST_DA; OTH_DA; }*/ - if(timer3 >= 500){ + if(timer3 >= 9){ //lcd.clear(); //lcd.printf(0,"Mode:%d,%d",ModeSW(),Pot_R); //lcd.printf(0,"%d",timer1); //lcd.printf(1,"%d,%d",FL_PWM,EnCountI); + timer2++; timer3=0; } - if( SD_Flag & 0x02)fprintf(fp, "%-10d%-10d%-10d\r\n",EnCount,ZCount,EnCountBuf); + //if( SD_Flag & 0x02)fprintf(fp, "%-10d%-10d%-10d\r\n",EnCount,ZCount,EnCountBuf); + +} +//----------Tmpログモード-------------------- +void TmpLog(){ + + Log_tmp[0][Tmpcnt] = EnCount; + Log_tmp[1][Tmpcnt] = DSensData[0]>>1; + Log_tmp[2][Tmpcnt] = DSensData[1]>>1; + Log_tmp[3][Tmpcnt] = DSensData[6]>>1; + Log_tmp[4][Tmpcnt] = DSensData[7]>>1; + Log_tmp[5][Tmpcnt] = Pot_F; + Log_tmp[6][Tmpcnt] = Pot_R >> 2; + Log_tmp[7][Tmpcnt] = EnCount; + Log_tmp[8][Tmpcnt] = EnCount; + Log_tmp[9][Tmpcnt] = EnCount; + Tmpcnt++; +} +//----------SDログモード-------------------- +void SDLog(){ + //char i=0; + if( SD_Flag & 0x02){ + //fprintf(fp, "%-10d%-10d\r\n",EnCount,EnCountI); + //fprintf(fp, "%-4d%-4d%-4d%-4d\r\n",Log_table[0][0],Log_table[0][1],Log_table[0][6],Log_table[0][7]); + //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]); + /*for(i=0;i<=10;i++){ + fprintf(fp, "%-4d%-4d%-4d%-4d\r\n",Log_table[i][0],Log_table[i][1],Log_table[i][6],Log_table[i][7]); + }*/ + } + else Log_timer=0; + +} +//----------SD書き込みシーケンス-------------------- +void SD_WriteSq(){ + int i=0; + if( SD_Flag & 0x02){ + //fprintf(fp, "\r\nLineWidsL\r\n"); + fprintf(fp, "EncountI time \r\n"); + fprintf(fp, "%-10d%-10d\r\n",EnCountI,timer1); + fprintf(fp, "Encount Sens0 Sens1 Sens6 Sens7 Pot_F Pot_R \r\n"); + for(i=0;i<=9999;i++){ + //fprintf(fp, "%-10d%-4d%-2d\r\n",Log_W_Wids[i][0],Log_W_Wids[i][1],Log_W_WidsLR[i]); + fprintf(fp, "%-4d%-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]); + } + fprintf(fp, "EncountI Wids WidsLR \r\n"); + for(i=0;i<=99;i++){ + fprintf(fp, "%-10d%-10d%-4d\r\n",Log_W_Wids[i][0],Log_W_Wids[i][1],Log_W_WidsLR[i]); + } + } + } //---------SDカードをマウント------------- void SD_Mount(){ if(SD_Flag == 0x00){ led_color(RED); + lcd.clear(); + lcd.printf(0,"Mounting"); fp = fopen("/sd/Log.txt", "w"); led_color(BLU); SD_Flag |= 0x01; - if (fp != NULL)SD_Flag |= 0x02; //success + if (fp != NULL){ + SD_Flag |= 0x02; //success + lcd.clear(); + lcd.printf(0,"Success"); + } else SD_Flag |= 0x10; //fail } - Ext_timer=0; - timer1=0; - fprintf(fp, "AAAA\r\n"); led_color(GRE); } //---------SDカードをアンマウント------------- void SD_Unmount(){ if(SD_Flag & 0x01){ + led_color(RED); + lcd.clear(); + lcd.printf(0,"UnMnt..."); + SD_WriteSq(); SD_Flag = 0; fclose(fp); + led_color(WHI); + lcd.clear(); + lcd.printf(0,"Success"); + wait(1); }} //----------マイコン初期設定--------------- void init(void){ @@ -789,7 +650,7 @@ s1signal.fall(&s1_trigger_fall); //エンコーダB相立ち下がり //割り込み処理開始 flipper.attach_us(&flip,1000); //汎用タイマー割り込み -//encLerp.attach_us(&encorderLerp,1000); //エンコーダ割り込み +SDLogflip.attach_us(&SDLog,10000); //ログ取り割り込み sensget.attach_us(&sensGet,100); //センサー用タイマー割り込み //PWM周期設定 PWM_CH1.period(0.0001); @@ -808,10 +669,7 @@ wait(1); ST_EN; lcd.contrast( 0x2E ); //LCDコントラスト設定 - //wait(2); - //SD_Mount(); wait(0.5); - //SD_Unmount(); while(SW_R){ if(!SW_W && !SW_B && !DebugFlg) Debug(); lcd.clear(); @@ -823,6 +681,7 @@ lcd.clear(); lcd.printf(0,"Ready!"); lcd.printf(1,"%x",SD_Flag); + writeflg = 1; wait(1); lcd.clear(); lcd.printf(0,"Go!"); @@ -835,9 +694,11 @@ lcd.printf(0,"%d",dAng ); lcd.printf(1,"%d,%d",Count_L,Count_R ); wait(0.1);*/ - if(Count_C >= 40 ) { + if(Count_C >= 4 ) { + if(!SW_W && !SW_B && !DebugFlg) Debug(); led_color(PUR); - OTH_DA; + //OTH_DA; + Speed = 0; lcd.clear(); lcd.printf(0,"%d",EnCountI ); lcd.printf(1,"%d,%d",Count_L,Count_R ); @@ -845,6 +706,7 @@ } EnCount_F = 0; while(W_WidsR <= linewide && EnCountI <= loopJump){ //右1 + if(!SW_W && !SW_B && !DebugFlg) Debug(); led_color(RED); wait(0.01); } @@ -859,7 +721,7 @@ } EnCount_F = 0; W_WidsL = W_WidsR = 0; - while(EnCount_F <= 1000){wait(0.01);} + while(EnCount_F <= 14000){wait(0.01);} HolSeq2(); EnCount_F = 0; W_WidsL = W_WidsR = 0; @@ -924,7 +786,7 @@ EnCount_F = 0; W_WidsL = W_WidsR = 0; Angle = 52; - while(EnCount_F <= (20000-ParamS)&& EnCountI <= loopJump){wait(0.01);} + while(EnCount_F <= (19000-ParamS)&& EnCountI <= loopJump){wait(0.01);} Angle = 90; EnCount_F = 0; @@ -977,4 +839,203 @@ }*/ } } -} \ No newline at end of file +} +//----------デバッグモード-------------------- +void Debug(void){ + int DebugModeSW; + DebugFlg=1; + lcd.clear(); + lcd.printf(0,"DBGMode"); + wait(2); + while( SW_R ){ + DebugModeSW = ModeSW(); + switch(DebugModeSW){ + case 1: + lcd.clear(); + lcd.printf(0,"SensT/F"); + wait(0.1); + break; + case 2: + lcd.clear(); + lcd.printf(0,"SensA/D"); + wait(0.1); + break; + case 3: + lcd.clear(); + lcd.printf(0,"Trip"); + wait(0.1); + break; + case 4: + lcd.clear(); + lcd.printf(0,"LinWidLR"); + wait(0.1); + break; + case 5: + lcd.clear(); + lcd.printf(0,"LineCnr"); + wait(0.1); + break; + case 6: + lcd.clear(); + lcd.printf(0,"LineA/D"); + wait(0.1); + break; + case 7: + lcd.clear(); + lcd.printf(0,"PotA/D"); + wait(0.1); + break; + case 8: + lcd.clear(); + lcd.printf(0,"Speed5"); + wait(0.1); + break; + case 9: + lcd.clear(); + lcd.printf(0,"Speed6"); + wait(0.1); + break; + case 10: + lcd.clear(); + lcd.printf(0,"LoopJump"); + wait(0.1); + break; + case 11: + lcd.clear(); + lcd.printf(0,"linewide"); + lcd.printf(1,"400"); + wait(0.1); + break; + case 12: + lcd.clear(); + lcd.printf(0,"linewide"); + lcd.printf(1,"100"); + wait(0.1); + break; + case 13: + lcd.clear(); + lcd.printf(0,"SD_Mount?"); + lcd.printf(1,"%x",SD_Flag); + wait(0.1); + if(!SW_W) SD_Mount(); + break; + case 14: + lcd.clear(); + lcd.printf(0,"SD_UnMount?"); + wait(0.1); + if(!SW_W) SD_Unmount(); + break; + case 15: + lcd.clear(); + lcd.printf(0,"DSPSens"); + wait(0.1); + break; + default: + break; + } + } + wait(2); + while( SW_R){ + DebugModeSW = ModeSW(); + switch(DebugModeSW){ + case 1: + lcd.clear(); + lcd.printf(0,"0167RL"); + lcd.printf(1,"%1d%1d%1d%1d%1d%1d",TFSensData[0],TFSensData[1],TFSensData[6],TFSensData[7],R_LineFlg ,L_LineFlg); + wait(0.1); + break; + case 2: + lcd.clear(); + lcd.printf(0,"SensA/D"); + lcd.printf(1,"%3d,%3d",DSensData[3],DSensData[4]); + wait(0.1); + break; + case 3: + lcd.clear(); + lcd.printf(0,"%d",EnCount ); + lcd.printf(1,"%d",EnCountI); + wait(0.1); + break; + case 4: + lcd.clear(); + lcd.printf(0,"%d",W_WidsL ); + lcd.printf(1,"%d",W_WidsR ); + wait(0.1); + break; + case 5: + lcd.clear(); + lcd.printf(0,"%d",R_LineCnt ); + lcd.printf(1,"%d",L_LineCnt ); + wait(0.1); + break; + case 6: + lcd.clear(); + lcd.printf(0,"%3d,%3d",DSensData[0],DSensData[1]); + lcd.printf(1,"%3d,%3d",DSensData[6],DSensData[7]); + wait(0.5); + break; + case 7: + lcd.clear(); + lcd.printf(0,"%d",Pot_F); + lcd.printf(1,"%d",Pot_R); + wait(0.1); + break; + case 8: + lcd.clear(); + lcd.printf(0,"Speed5"); + wait(0.1); + SPCom=5000; + SPYobi=3500; + SPCor=3000; + ParamS=2000; + break; + case 9: + lcd.clear(); + lcd.printf(0,"Speed6"); + wait(0.1); + SPCom=6000; + SPYobi=3000; + SPCor=3000; + break; + case 10: + lcd.clear(); + lcd.printf(0,"LoopJump"); + wait(0.1); + loopJump = 1000000; + break; + case 11: + lcd.clear(); + lcd.printf(0,"linewide"); + lcd.printf(1,"400"); + wait(0.1); + linewide=400; + break; + case 12: + lcd.clear(); + lcd.printf(0,"linewide"); + lcd.printf(1,"100"); + wait(0.1); + linewide=100; + break; + case 15: + lcd.clear(); + lcd.printf(0,"%d",DSensData[0]); + wait(0.1); + break; + default: + break; + } + } + + + while( SW_W ){ + lcd.clear(); + lcd.printf(0,"Exit?"); + wait(0.1); + } + lcd.clear(); + lcd.printf(0,"Bye-Bye"); + wait(2); + lcd.clear(); + DebugFlg=0; +}