Takeshi Nishimura
ランサー
Dependencies: SB1602E SDFileSystem mbed
Fork of
Seeed_SDCard_Shield
by 
Shields
Diff: main.cpp
- Revision:
- 7:c99fe0eb544a
- Parent:
- 6:32698402be64
- Child:
- 8:352404951de8
diff -r 32698402be64 -r c99fe0eb544a main.cpp
--- 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;
+}