ランサー
Dependencies: SB1602E SDFileSystem mbed
Fork of Seeed_SDCard_Shield by
main.cpp
- Committer:
- MCR_Xavier
- Date:
- 2017-04-15
- Revision:
- 9:8ab0d3e46c3c
- Parent:
- 8:352404951de8
- Child:
- 10:adefba925268
File content as of revision 9:8ab0d3e46c3c:
//Lancer2016 //ヘッダー設定 #include "mbed.h" #include "SDFileSystem.h" #include "SB1602E.h" //シンボル定義 #define CLR 0,0,0 //無 RGBパターン #define RED 1,0,0 //赤 #define GRE 0,1,0 //緑 #define YEL 1,1,0 //黄 #define BLU 0,0,1 //青 #define PUR 1,0,1 //紫 #define CYA 0,1,1 //水 #define WHI 1,1,1 //白 #define DFP 508 //フロントポテンショメータ機械原点 #define DRP 234 //リヤポテンショメータ機械原点 #define FW 1 //駆動輪前進 #define BW 0 //駆動輪後進 #define ST_EN ST_CTR = 0 #define ST_DA ST_CTR = 1 #define OTH_EN OTH_CTR = 0 #define OTH_DA OTH_CTR = 1 #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) #define TRACE 0 //トレースモード #define ANGLE 1 //角度制御モード SB1602E lcd(PB_9,PB_8); //LCD_I2C設定(SDA, SCL) //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 DigitalIn RS2(PA_15); //ロータリースイッチbit2 DigitalIn RS3(PA_12); //ロータリースイッチbit3 DigitalIn RS4(PA_9); //ロータリースイッチbit4 AnalogIn AMS(PC_5); //アナログモードスイッチ AnalogIn Line0(PC_0); //アナログラインセンサ AnalogIn Line1(PC_1); //アナログラインセンサ AnalogIn Line2(PC_2); //アナログラインセンサ AnalogIn Line3(PC_3); //アナログラインセンサ AnalogIn Line4(PA_0); //アナログラインセンサ AnalogIn Line5(PA_1); //アナログラインセンサ AnalogIn Line6(PA_2); //アナログラインセンサ AnalogIn Line7(PA_3); //アナログラインセンサ AnalogIn Steer_Ptm(PA_4); //ステアリングポテンショ AnalogIn Lance_Ptm(PC_4); //槍ポテンショ DigitalIn SW_W(PB_5); //スイッチ白 DigitalIn SW_R(PC_6); //スイッチ赤 DigitalIn SW_B(PC_12); //スイッチ黒 InterruptIn s0signal(D6); //ロータリエンコーダA相 InterruptIn s1signal(D7); //ロータリエンコーダB相 //モータ制御 PwmOut PWM_CH1(PC_7); //3/2槍 PwmOut PWM_CH2(PA_11); //1/4前左 PwmOut PWM_CH3(PB_2); //2/4前右 PwmOut PWM_CH4(PB_7); //4/2後右 PwmOut PWM_CH5(PA_10); //1/3後左 PwmOut PWM_CH6(PB_4); //3/1操舵 DigitalOut Rev_CH1(PB_12); //操舵モータDIR DigitalOut Rev_CH2(PB_13); //前左モータDIR DigitalOut Rev_CH3(PC_8); //前右モータDIR DigitalOut Rev_CH4(PC_9); //後右モータDIR DigitalOut Rev_CH5(PB_14); //後左モータDIR DigitalOut Rev_CH6(PB_15); //槍モータDIR DigitalOut SensLED(PC_11); //センサ用LED DigitalOut ST_CTR(PB_0); //STEER_EnableCtrl DigitalOut OTH_CTR(PB_1); //OTHER_EnableCtrl //デバッグ DigitalOut LED_R(PA_13); //LED_R DigitalOut LED_G(PA_14); //LED_G DigitalOut LED_B(PB_3); //LED_B //割り込み定義 Ticker flipper; //汎用タイマー Ticker sensget; //センサー用タイマー Ticker SDLogflip; //エンコーダ割り込み //Ticker encLerp; //エンコーダ割り込み //プロトタイプ宣言 void init(void); void MotorCtrl(void); //モータ総合管理 void enc(); //エンコーダ処理 int ModeSW(); //モードスイッチ int Get_SW(); //スイッチ情報取得 void Get_Pot(); //ポテンショメータA/D取得 void SpeedCtrl(int V); //速度制御 int PotTest(); //ポテンショテスト float PwmTest(); //PWMテスト void SD_Mount(); //SDカードをマウントする void SD_Unmount(); //SDカードをアンマウントする void DSens(); //センサ変調処理 void LineTrace(); //ライントレース void led_color(char R , char G , char B ); //LED色制御 void ErrChk(void); //エラーチェック void LanceAng(int Ang); //槍角度制御 void Line_Dec(void); //白線判定 void enCalc(void); //エンコーダ計算とか void CornerSeq(void); //コーナーモード void L_LineSeq(void); void R_LineSeq(void); void HolSeq1(void); void HolSeq2(void); void Debug(void); //デバッグモード void SD_Log(void); //SDログ void SD_WriteSq(void); //SD書き込み void TmpLog(void); //内部メモリへのログ void SteerCtrl(char STMode); //ステアリングコントロール void AngleCtrl(int STAngle); //角度制御モード //グローバル変数の宣言 int EnCount=0,EnCountBuf=0,EnCountI=0; //エンコーダカウント int EnCount_C=0,EnCount_R=0,EnCount_L=0,EnCount_H=0,EnCount_F=0; int PPms=0,RPM_Test=0,Rev_Flag=0; int ZCount=0; //エンコーダ停止判定カウンタ int EncFlag=0; unsigned char Rev=0; //エンコーダ回転方向検知 int encA=0,encB=0; //エンコーダ相状態確認 unsigned char SD_Flag=0;//SDカードの状態判定 unsigned char Line_Flg; //ライン情報フラグ 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];//ラインセンサ値 int Pot_F=0,Pot_R=0,Pot_F2=0; //ポテンショ char LA_Rev,FL_Rev,FR_Rev,RR_Rev,RL_Rev,ST_Rev; //モータ回転方向 int LA_PWM=0,FL_PWM=0,FR_PWM=0,RR_PWM=0,RL_PWM=0,ST_PWM=0; //モータPWMデューティ char ErrSt=0; int Speed=0,Angle=0; int iV=0,iAng=0; int TraceI,Trace_P=0,Trace_I=0,Trace_D=0,Trace_B=0; int Angle_P=0,Angle_I=0,Angle_D=0,Angle_B=0; int RSW_Value; char Turn_Flg=0; unsigned int R_LineCnt=0,L_LineCnt=0,R_LineCntF=0,L_LineCntF=0; char R_LineFlg=0,L_LineFlg=0; char DebugFlg=0; int W_WidsR=0,W_WidsL=0,W_WidsRCnt=0,W_WidsLCnt=0,dAng; unsigned char Count_C=0,Count_R=0,Count_L=0,Count_H=0; int ParamS=1; int SPCom=3200,SPYobi=4000,SPCor=3200,loopJump=1000000,linewide=100; char SteerAngle; char SteerMode=0; 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, 22,23,24,25,27,28,30,31,33,34,36,38,39,41,43,45,47,49,51,53,56, 58,60,63,65,68,70,73,76,79,82,85,88,91,94,98,101,104,108,112,115, 119,123}; 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, 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, 9,10,10,10,10,11,11,11,12,12,12,13,13,13,14,14,14,15,15,15,16,16, 16,17,17,18,18,18,19,19,19,20,20}; float Motor_TableA[]={ 1,0.99,0.98,0.98,0.97,0.96,0.95,0.95,0.94,0.93,0.93,0.92,0.92,0.91, 0.91,0.90,0.90,0.90,0.89,0.89,0.89,0.88,0.88,0.88,0.88,0.87,0.87, 0.87,0.87,0.87,0.87,0.87,0.87,0.87,0.87,0.88,0.88,0.88,0.89,0.89, 0.89,0.90,0.90,0.91,0.92,0.92,0.93,0.94,0.95,0.96,0.97}; float Motor_TableB[]={ 1,1.01,1.02,1.03,1.04,1.05,1.06,1.07,1.08,1.09,1.10,1.11,1.13,1.14, 1.15,1.17,1.18,1.20,1.21,1.23,1.24,1.26,1.28,1.29,1.31,1.33,1.35, 1.37,1.39,1.42,1.44,1.46,1.49,1.51,1.54,1.56,1.59,1.62,1.65,1.68, 1.72,1.75,1.79,1.83,1.86,1.91,1.95,1.99,2.04,2.09,2.14}; float Motor_TableC[]={ 1,0.99,0.98,0.97,0.97,0.96,0.95,0.94,0.93,0.92,0.91,0.90,0.90,0.89, 0.88,0.87,0.86,0.85,0.84,0.83,0.82,0.81,0.80,0.79,0.78,0.77,0.76, 0.75,0.74,0.73,0.72,0.70,0.69,0.68,0.67,0.66,0.64,0.63,0.62,0.60, 0.59,0.57,0.56,0.54,0.53,0.51,0.49,0.47,0.45,0.43,0.41}; float Motor_TableD[]={ 1,1.01,1.02,1.03,1.03,1.04,1.05,1.06,1.07,1.08,1.09,1.10,1.10,1.11, 1.12,1.13,1.14,1.15,1.16,1.17,1.18,1.19,1.20,1.21,1.22,1.23,1.24, 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; if(RS1)RSW += 1; if(RS2)RSW += 2; if(RS3)RSW += 4; if(RS4)RSW += 8; RSW_Value=int(RSW); return RSW;} //----------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; if(Pot_F >= 45 || Pot_F <= (-45)) ErrSt++; 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++; } //----------ポテンショ値の取得--------------- void Get_Pot(){ Pot_F2 = (DFP-(Steer_Ptm.read_u16()>>6)); Pot_F = Pot_F2/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; Count_C++; led_color(YEL); Speed = 1000; Angle = 90; while(EnCount_C <= 1000 || (W_WidsR <= 100 && W_WidsL <= 100) ){wait(0.1);} } //----------左ラインモード-------------------- void L_LineSeq(void){ EnCount_L = 0; Count_L++; led_color(BLU); Speed = 2000; Angle = 122; while(EnCount_L <= 1000){ if(W_WidsR >= 300){ CornerSeq(); Count_L--; } } } //----------右ラインモード-------------------- void R_LineSeq(void){ EnCount_R = 0; Count_R++; led_color(RED); Speed = 2000; Angle = 213; while(EnCount_R <= 1000 ){ if(W_WidsL >= 300){ CornerSeq(); Count_R--; break; } } } //----------水平1-------------------- void HolSeq1(void){ //float HitCnt=0; EnCount_H = 0; Count_R++; Count_H++; //led_color(RED); //Speed = 2000; //while(EnCount_H <= 33137 ){ Angle = 270; while(EnCount_H <= (30000-(ParamS*2))){wait(0.001);} //led_color(CYA); Angle = 188; /*while(EnCount_H <= 35000 ){ HitCnt = int(225+((EnCount_H - 33137)*0.0183)); Angle= HitCnt; }*/ } //----------水平2-------------------- void HolSeq2(void){ //float HitCnt=0; EnCount_H = 0; Count_R++; Count_H++; //led_color(RED); //Speed = 2000; //while(EnCount_H <= 33137 ){ Angle = 230; while(EnCount_H <= (27000-(ParamS*2))){wait(0.001);} //led_color(CYA); Angle = 170; /*while(EnCount_H <= 35000 ){ 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++;} else { R_LineCnt = 0; R_LineCntF++;} if(TFSensData[6] && TFSensData[7] ) { L_LineCntF = 0; L_LineCnt++;} else { L_LineCnt = 0; L_LineCntF++;} if(R_LineCnt >= 2 && !R_LineFlg ){ R_LineFlg = 1; W_WidsRCnt = EnCountI; } 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; W_WidsLCnt = EnCountI; } 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 MotorCtrl(){ if(ErrSt >= 100){ //異常判定 PWM_CH1.pulsewidth_us(0); //操舵PWM (0~100) PWM_CH2.pulsewidth_us(0); //前左PWM (0~1000) PWM_CH3.pulsewidth_us(0); //前右PWM (0~1000) PWM_CH4.pulsewidth_us(0); //後右PWM (0~1000) PWM_CH5.pulsewidth_us(0); //後左PWM (0~1000) PWM_CH6.pulsewidth_us(0); //槍舵PWM (0~100) OTH_DA; } else{ PWM_CH1.pulsewidth_us(ST_PWM); //槍PWM (0~100) PWM_CH2.pulsewidth_us(FL_PWM); //前左PWM (0~1000) PWM_CH3.pulsewidth_us(FR_PWM); //前右PWM (0~1000) 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) Rev_CH1 = ST_Rev; //槍回転方向 (H:CW) Rev_CH2 = !FL_Rev; //前左回転方向(L:FW) Rev_CH3 = FR_Rev; //前右回転方向(H:FW) Rev_CH4 = RR_Rev; //後右回転方向(H:FW) Rev_CH5 = !RL_Rev; //後左回転方向(L:FW) Rev_CH6 = LA_Rev; //操舵回転方向(H:CW) } } //----------速度制御-------------------- void SpeedCtrl(int V){ float dV; char PT; dV=EnCount*32; dV = (V - dV); if(dV <= 0){ iV = 0; dV=dV*(-1); FL_Rev=FR_Rev=RR_Rev=RL_Rev=BW; } else FL_Rev=FR_Rev=RR_Rev=RL_Rev=FW; iV = iV + (dV/10); dV = dV + iV; if(dV>=1000) dV=1000; if(Pot_F < 0){ PT = Pot_F * (-1); PT = int (PT*2); if(PT >= 50 ) PT=50; FL_PWM = int( dV * Motor_TableA[PT]); FR_PWM = int( dV * Motor_TableB[PT]); RL_PWM = int( dV * Motor_TableC[PT]); RR_PWM = int( dV * Motor_TableD[PT]); } else{ PT = Pot_F; PT = int (PT*1.5); if(PT >= 50 ) PT=50; FR_PWM = int( dV * Motor_TableA[PT]); FL_PWM = int( dV * Motor_TableB[PT]); RR_PWM = int( dV * Motor_TableC[PT]); RL_PWM = int( dV * Motor_TableD[PT]); } } //----------槍角度制御-------------------- void LanceAng(int Ang){ //int dAng; dAng = Pot_R - (DRP + (Ang * 3)); if(dAng <= 0){ //CW iAng = 0; LA_Rev = 1; dAng = dAng * (-1); } else{ //CCW LA_Rev = 0; } dAng = dAng * 2; iAng = iAng + (dAng/100); //dAng = dAng + iAng if(dAng >= 60) dAng = 60; LA_PWM = dAng; } //------ステアリングコントロール----------- void SteerCtrl(char STMode){ if(STMode == ANGLE) AngleCtrl(-35); else LineTrace(); } //-----------角度コントロール------------- void AngleCtrl(int STAngle){ int Angle_V=0; Angle_P=((STAngle*3) - Pot_F2); Angle_D = Angle_B - Angle_P; Angle_B = Angle_P; Angle_P = Angle_P*(100+(10*RSW_Value)); Angle_I = Angle_I + (Angle_P / (10*RSW_Value)); if(Angle_I >= 10000) Angle_I = 10000; if(Angle_I <= (-10000)) Angle_I = (-10000); Angle_V = Angle_P + (Angle_I*0 ) - (Angle_D * 0); Angle_V = Angle_V / 100; if(Angle_V <= -1){ Angle_V = Angle_V*(-1); ST_Rev=0; if(!Turn_Flg && Angle_V >= 10) Angle_I = 0; Turn_Flg=1; } else { ST_Rev=1; if(Turn_Flg && Angle_V >= 10) Angle_I = 0; Turn_Flg=0; } if(Angle_V >= 100) Angle_V=100; ST_PWM=Angle_V; } //----------ライントレース--------------- void LineTrace(){ int Trace_V; TraceI=(DSensData[4]-DSensData[3]); /* if(TraceI <= -1){ TraceI = TraceI*(-1); ST_Rev=1; if(!Turn_Flg) Trace_I = 0; Turn_Flg=1; } else { ST_Rev=0; if(Turn_Flg) Trace_I = 0; Turn_Flg=0; }*/ Trace_P = TraceI * (12); Trace_I = Trace_I + (Trace_P / (400)); if(Trace_I >= 10000) Trace_I = 10000; if(Trace_I <= (-10000)) Trace_I = (-10000); Trace_D = Trace_B - TraceI; Trace_B = TraceI; Trace_V = Trace_P + Trace_I - (Trace_D * 50); Trace_V = Trace_V / 100; if(Trace_V <= -1){ Trace_V = Trace_V*(-1); ST_Rev=1; if(!Turn_Flg && Trace_V >= 10) Trace_I = 0; Turn_Flg=1; } else { ST_Rev=0; if(Turn_Flg && Trace_V >= 10) Trace_I = 0; Turn_Flg=0; } if(Trace_V >= 100) Trace_V=100; ST_PWM=Trace_V; } //----------タイマー割り込み--------------- void flip(){ timer1++; timer2++; //10msで1cnt timer3++; Log_timer++; //ErrChk(); Get_Pot(); DSens(); //LineTrace(); SteerCtrl(SteerMode); SpeedCtrl(Speed); 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; //OTH_EN; Angle = Pot_F + 90; } else if(ModeSW()==0) { ST_DA; OTH_DA; }*/ 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); } //----------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 lcd.clear(); lcd.printf(0,"Success"); } else SD_Flag |= 0x10; //fail } 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){ //I/O設定 RS1.mode(PullDown); //ロータリスイッチbit1 RS2.mode(PullDown); //ロータリスイッチbit2 RS3.mode(PullDown); //ロータリスイッチbit3 RS4.mode(PullDown); //ロータリスイッチbit4 SW_W.mode(PullUp); //スイッチ白 SW_R.mode(PullUp); //スイッチ赤 SW_B.mode(PullUp); //スイッチ黒 s0signal.rise(&s0_trigger_rise); //エンコーダA相立ち上がり s0signal.fall(&s0_trigger_fall); //エンコーダA相立ち下がり s1signal.rise(&s1_trigger_rise); //エンコーダB相立ち上がり s1signal.fall(&s1_trigger_fall); //エンコーダB相立ち下がり //割り込み処理開始 flipper.attach_us(&flip,1000); //汎用タイマー割り込み SDLogflip.attach_us(&SDLog,10000); //ログ取り割り込み sensget.attach_us(&sensGet,100); //センサー用タイマー割り込み //PWM周期設定 PWM_CH1.period(0.0001); PWM_CH2.period(0.001); PWM_CH3.period(0.001); PWM_CH4.period(0.001); PWM_CH5.period(0.001); PWM_CH6.period(0.0001); ST_DA; OTH_DA; } //----------------メイン--------------------- int main() { init(); //マイコン初期設定 while(1){ wait(1); ST_EN; lcd.contrast( 0x2E ); //LCDコントラスト設定 wait(0.5); while(SW_R){ if(!SW_W && !SW_B && !DebugFlg) Debug(); lcd.clear(); lcd.printf(0,"Wait..."); lcd.printf(1,"%x",SD_Flag); //if(!SW_W) SD_Mount(); wait(0.5); } lcd.clear(); lcd.printf(0,"Ready!"); lcd.printf(1,"%x",SD_Flag); writeflg = 1; wait(1); lcd.clear(); lcd.printf(0,"Go!"); OTH_EN; W_WidsL = W_WidsR = 0; Speed = SPCom; Angle = 90; SteerMode = TRACE; while(1){ /*lcd.clear(); lcd.printf(0,"%d",dAng ); lcd.printf(1,"%d,%d",Count_L,Count_R ); wait(0.1);*/ if(Count_C >= 6 ) { if(!SW_W && !SW_B && !DebugFlg) Debug(); led_color(PUR); //OTH_DA; Speed = 0; lcd.clear(); lcd.printf(0,"%d",EnCountI ); lcd.printf(1,"%d,%d",Count_L,Count_R ); wait(0.1); } EnCount_F = 0; while(W_WidsR <= linewide && EnCountI <= loopJump){ //右1 if(!SW_W && !SW_B && !DebugFlg) Debug(); led_color(RED); wait(0.01); } EnCount_F = 0; W_WidsL = W_WidsR = 0; while(EnCount_F <= 1000){wait(0.01);} HolSeq1(); while(W_WidsR <= linewide&& EnCountI <= loopJump){ //右2 led_color(GRE); wait(0.01); } EnCount_F = 0; W_WidsL = W_WidsR = 0; while(EnCount_F <= 25000){wait(0.01);} HolSeq2(); EnCount_F = 0; W_WidsL = W_WidsR = 0; while(EnCount_F <= 1000){ //予備減速 Speed = SPYobi; wait(0.01);} Speed = SPCor; Angle = 90; while((W_WidsR <= linewide|| W_WidsL <= linewide )&& EnCountI <= loopJump){ //コーナーIN led_color(BLU); wait(0.01); } EnCount_F = 0; W_WidsL = W_WidsR = 0; while(EnCount_F <= 1000){wait(0.01);} while((W_WidsR <= linewide || W_WidsL <= linewide)&& EnCountI <= loopJump){ //コーナーOUT led_color(YEL); wait(0.01); } Speed = SPCom; Count_C++; EnCount_F = 0; W_WidsL = W_WidsR = 0; led_color(CLR); wait(0.01); while(W_WidsR <= linewide && EnCountI <= loopJump){ // 右3 led_color(RED); wait(0.01); } EnCount_F = 0; W_WidsL = W_WidsR = 0; Angle = 130; while(EnCount_F <= (20000-ParamS) && EnCountI <= loopJump){wait(0.01);} Angle = 90; while(W_WidsL <= linewide && EnCountI <= loopJump){ //左1 led_color(GRE); wait(0.01); } EnCount_F = 0; W_WidsL = W_WidsR = 0; Angle = 50; while(EnCount_F <= (19000-ParamS)){wait(0.01);} Angle = 90; while(W_WidsR <= linewide && EnCountI <= loopJump){ //右4 led_color(BLU); wait(0.01); } EnCount_F = 0; W_WidsL = W_WidsR = 0; Angle = 133; while(EnCount_F <= (20000-ParamS)&& EnCountI <= loopJump){wait(0.01);} Angle = 90; while(W_WidsL <= linewide && EnCountI <= loopJump){ //左2 led_color(RED); wait(0.01); } EnCount_F = 0; W_WidsL = W_WidsR = 0; Angle = 52; while(EnCount_F <= (5000-ParamS)&& EnCountI <= loopJump){wait(0.01);} Angle = 90; EnCount_F = 0; W_WidsL = W_WidsR = 0; while(EnCount_F <= 1000&& EnCountI <= loopJump){ //予備減速 Speed = SPYobi; wait(0.01);} Speed = SPCor; while((W_WidsR <= linewide || W_WidsL <= linewide)&& EnCountI <= loopJump){ //コーナーIN2 led_color(GRE); wait(0.01); } EnCount_F = 0; W_WidsL = W_WidsR = 0; while(EnCount_F <= 1000){wait(0.01);} while((W_WidsR <= linewide || W_WidsL <= linewide)&& EnCountI <= loopJump){ //コーナーOUT2 led_color(PUR); wait(0.01); } Speed = SPCom; Count_C++; EnCount_F = 0; W_WidsL = W_WidsR = 0; led_color(CLR); EnCountI =0; wait(0.01); /* if(W_WidsR >= 100 && W_WidsL >= 100 && Pot_F >= (-10)){ CornerSeq(); W_WidsL = W_WidsR = 0; } if(W_WidsR >= 300 && Pot_F >= (-10) && !Count_R ){ HolSeq1(); W_WidsL = W_WidsR = 0; } if(W_WidsR >= 300 && Pot_F >= (-10) && Count_R == 1 ){ HolSeq2(); W_WidsL = W_WidsR = 0; } if(W_WidsR >= 300 && Pot_F >= (-10) && Count_R >= 2 ){ R_LineSeq(); //if(Count_R >= 4) Count_R = 0; W_WidsL = W_WidsR = 0; } if(W_WidsL >= 300 && Pot_F >= (-10)){ L_LineSeq(); W_WidsL = W_WidsR = 0; }*/ } } } //----------デバッグモード-------------------- 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_F2); 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,"Speed7"); wait(0.1); SPCom=7000; //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; }