Takeshi Nishimura
ランサー
Dependencies: SB1602E SDFileSystem mbed
Fork of
Seeed_SDCard_Shield
by 
Shields
main.cpp
- Committer:
- MCR_Xavier
- Date:
- 2017-04-02
- Revision:
- 7:c99fe0eb544a
- Parent:
- 6:32698402be64
- Child:
- 8:352404951de8
File content as of revision 7:c99fe0eb544a:
//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）
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 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); //内部メモリへのログ
//グローバル変数の宣言
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; //ポテンショ
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 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=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,
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++;
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;
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;
}
}
}
//----------水平１--------------------
void HolSeq1(void){
//float HitCnt=0;
EnCount_H = 0;
Count_R++;
Count_H++;
//led_color(RED);
//Speed = 2000;
//while(EnCount_H <= 33137 ){
Angle = 250;
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 = 210;
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);
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;
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 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 * 7;
Trace_I = Trace_I + (Trace_P / (400));
if(Trace_I >= 100000) Trace_I = 100000;
Trace_D = Trace_B - TraceI;
Trace_B = TraceI;
Trace_V = Trace_P + Trace_I - (Trace_D * 30);
Trace_V = Trace_V / 100;
if(Trace_V >= 100) Trace_V=100;
ST_PWM=Trace_V;
}
//----------タイマー割り込み---------------
void flip(){
timer1++;
timer2++; //10msで1cnt
timer3++;
Log_timer++;
//ErrChk();
DSens();
LineTrace();
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;
while(1){
/*lcd.clear();
lcd.printf(0,"%d",dAng );
lcd.printf(1,"%d,%d",Count_L,Count_R );
wait(0.1);*/
if(Count_C >= 4 ) {
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 <= 14000){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 <= (19000-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_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;
}