Takeshi Nishimura
ランサー
Dependencies: SB1602E SDFileSystem mbed
Fork of
Seeed_SDCard_Shield
by 
Shields
main.cpp
- Committer:
- MCR_Xavier
- Date:
- 2018-04-29
- Revision:
- 12:2cb9082e2d40
- Parent:
- 11:5ec2507020da
File content as of revision 12:2cb9082e2d40:
//Lancer2016
//ヘッダー設定
#include "mbed.h"
#include "SDFileSystem.h"
#include "SB1602E.h"
#include "stdlib.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 LA_DA LancFree_Flg=1 //槍無効
#define LA_EN LancFree_Flg=0 //槍有効
#define DSs 8 //LineセンサA/Dシフト数
#define ASs 6 //トレース用LineセンサA/Dシフト数
#define ThS 80 //Line白黒判別閾値
#define ANGLE 0 //角度制御モード
#define TRACE 1 //トレースモード
#define WTRACE 2 //拡張トレースモード
#define NUM_DATA 10 //ソート用データ数
#define WWIDCLR W_WidsR=W_WidsL=0
SB1602E lcd(PB_9,PB_8); //LCD_I2C設定(SDA, SCL)
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 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; //センサー用タイマー
//プロトタイプ宣言
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 ErrChk(void); //エラーチェック
void LanceAng(int Ang); //槍角度制御
void Line_Dec(void); //白線判定
void enCalc(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); //角度制御モード
void Localization(void); // 位置推定
void led_color(char R , char G , char B ); //LED色制御
void ModeSelect(); //モード選択
//グローバル変数の宣言
int EnCount=0;
long EnCountI=0; //エンコーダカウント
int EnCount_1R=0; //コース１周分のエンコーダカウント毎週リセットされる
unsigned char Rev=0; //エンコーダ回転方向判定
unsigned char SD_Flag=0; //SDカードの状態判定
unsigned char Line_Flg; //ライン情報フラグ
unsigned char Shortcut_Flg=0; //ショートカットフラグ
unsigned char Turbo_Flg=0; //コーナー出口のターボフラグ
unsigned char R_Go = 15;
unsigned char TestDrive_Flg=0; //駆動輪フリー
unsigned char LancFree_Flg=0; //槍フリー
int timer1=0,timer2=0,Tmpcnt=0,Log_timer=0; //タイマー
int Stime=0;
char StartFlg=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,StrPot_R[10]; //ポテンショ
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デューティ
int ErrSt=0;
int Speed=0,Angle=0,STAngle=0;
int iAng=0;
long iV=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;
char ModeSelect_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;
long 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;
unsigned char R_No=1;
int ParamS=1;
int SPCom=4500,SPYobi=4500,SPCor=3200,loopJump=1000000,linewide=100;
int TripTime =60000; //走行時間
int AccelStopCnt = 1000; //駆動停止カウント
int MotorStopCnt = 2000; //モータ停止カウント
char SteerAngle;
char SteerMode=0;
char DefFlg=0;
int FrontPot[NUM_DATA + 1],RearPot[NUM_DATA + 1];
char Log_table[10][10];
int Log_W_Wids[300][2];
char Log_W_WidsLR[300];
char Log_Count_R[300];
char Log_tmp[100][6];
char LLog_tmp[300];
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[]={
0.99,0.98,0.97,0.96,0.95,0.95,0.94,0.93,0.92,0.92,0.91,0.91,0.90,
0.90,0.89,0.89,0.88,0.88,0.88,0.88,0.87,0.87,0.87,0.87,0.87,0.87,
0.87,0.87,0.88,0.88,0.88,0.89,0.89,0.90,0.90,0.91,0.92,0.93,0.94,
0.95,0.96,0.97,0.99,1.01,1.02,1.05,1.07,1.09,1.12,1.15};
float Motor_TableB[]={
1.01,1.02,1.03,1.04,1.06,1.07,1.08,1.10,1.11,1.13,1.14,1.16,1.18,
1.19,1.21,1.23,1.25,1.27,1.29,1.31,1.34,1.36,1.39,1.41,1.44,1.47,
1.50,1.53,1.56,1.59,1.63,1.67,1.70,1.75,1.79,1.83,1.88,1.93,1.98,
2.04,2.10,2.16,2.23,2.30,2.38,2.46,2.55,2.64,2.74,2.85};
float Motor_TableC[]={
0.99,0.98,0.97,0.96,0.95,0.94,0.93,0.92,0.91,0.90,0.88,0.87,0.86,
0.85,0.84,0.83,0.82,0.81,0.79,0.78,0.77,0.76,0.74,0.73,0.72,0.70,
0.69,0.67,0.66,0.64,0.63,0.61,0.59,0.58,0.56,0.54,0.52,0.50,0.48,
0.45,0.43,0.41,0.38,0.35,0.32,0.29,0.26,0.23,0.19,0.15};
float Motor_TableD[]={
1.01,1.02,1.03,1.04,1.05,1.06,1.07,1.08,1.09,1.10,1.12,1.13,1.14,
1.15,1.16,1.17,1.18,1.19,1.21,1.22,1.23,1.24,1.26,1.27,1.28,1.30,
1.31,1.33,1.34,1.36,1.37,1.39,1.41,1.42,1.44,1.46,1.48,1.50,1.52,
1.55,1.57,1.59,1.62,1.65,1.68,1.71,1.74,1.77,1.81,1.85};
////////////////////////////////////////////////
//-------------ベースプログラム-----------------//
////////////////////////////////////////////////
//----------モード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 >= TripTime && StartFlg && Count_R == 12) ErrSt = AccelStopCnt;
if(Pot_F >= 47 || Pot_F <= (-47)) ErrSt++;
if(DSensData[3] <= 250 && DSensData[4] <= 250 && StartFlg && SteerMode != ANGLE ) ErrSt = ErrSt + 5;
else if(ErrSt < 1000 && Pot_F == 0 ) ErrSt=0;
if(ErrSt >= MotorStopCnt ) ErrSt = MotorStopCnt;
}
//-----------エンコーダカウント------------------
void s0_trigger_rise() { // A相立上り変化割込み時の処理 (Low -> High)
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_1R = EnCount_1R + EnCount;
EnCount = 0;
}
//----------フルカラーLED制御--------------------
void led_color(char R , char G , char B )
{
if(LancFree_Flg)R=G=B=1;
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;
//Hpsort(FrontPot, NUM_DATA);
//Hpsort(RearPot, NUM_DATA);
//Get_Pot();
}
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;
}
//FrontPot[Stime] = Steer_Ptm.read_u16()>>6;
//RearPot[Stime] = Lance_Ptm.read_u16()>>6;
Stime++;
}
//----------ポテンショ値の取得---------------
void Get_Pot(){
Pot_F2 = (DFP-(Steer_Ptm.read_u16()>>6));
//Pot_F2 = DFP - FrontPot[4];
Pot_F = int(Pot_F2/3);
//Pot_R = RearPot[4];
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 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_Count_R[WidsNo] = Count_R;
Log_W_Wids[WidsNo][0] = W_WidsR;
//Log_W_Wids[WidsNo][1] = EnCountI;
LLog_tmp[WidsNo] = EnCount;
Log_W_Wids[WidsNo][1] = EnCount_1R;
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_Count_R[WidsNo] = Count_R;
Log_W_Wids[WidsNo][0] = W_WidsL;
//Log_W_Wids[WidsNo][1] = EnCountI;
LLog_tmp[WidsNo] = EnCount;
Log_W_Wids[WidsNo][1] = EnCount_1R;
WidsNo++;
}
}
//----------モータ総合管理--------------------
void MotorCtrl(){
if(ErrSt >= MotorStopCnt){ //異常判定
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{
if(FL_PWM >= 1000) FL_PWM = 1000;
if(FR_PWM >= 1000) FR_PWM = 1000;
if(RR_PWM >= 1000) RR_PWM = 1000;
if(RL_PWM >= 1000) RL_PWM = 1000;
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){
long dV;
int PT,SpeedBuf;
if(SpeedBuf != V) iV = 0;
SpeedBuf = V;
dV=EnCount*32;
if(ErrSt >= AccelStopCnt ){
iV = 0;
V = 0;}
dV = (V - dV)*8;
if(dV == 0) iV = 0;
iV = iV + (dV/1000);
dV = dV + iV;
if(dV <= 0){
dV=dV*(-1);
FL_Rev=FR_Rev=RR_Rev=RL_Rev=BW;
}
else FL_Rev=FR_Rev=RR_Rev=RL_Rev=FW;
if(dV>=1000) dV=1000;
if(Pot_F < 0){
PT = Pot_F * (-1);
//PT = int (PT*2);
if(PT >= 50 ) PT=50;
//if(!DefFlg) PT = 0;
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*2);
if(PT >= 50 ) PT=50;
//if(!DefFlg) PT = 0;
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 * 1;
iAng = iAng + (dAng/50);
dAng = dAng + iAng;
if(dAng >= 60) dAng = 60;
if(LancFree_Flg && dAng >= 3)dAng = 3;
if(Ang >= 400) dAng=60;
LA_PWM = dAng;
}
//------ステアリングコントロール-----------
void SteerCtrl(char STMode){
if(STMode == ANGLE) AngleCtrl(STAngle);
else LineTrace();
}
//-----------角度コントロール-------------
void AngleCtrl(int STAngle){
int Angle_V=0;
Angle_P=((STAngle*3) - Pot_F2)*(200);
Angle_I = Angle_I + (Angle_P / (70));
if(Angle_I >= 10000) Angle_I = 10000;
if(Angle_I <= (-10000)) Angle_I = (-10000);
Angle_D = Angle_B - Angle_P;
Angle_B = Angle_P;
Angle_V = Angle_P + (Angle_I ) - (Angle_D *0);
Angle_V = Angle_V / 100;
/*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 ) - (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;
if(SteerMode == WTRACE) TraceI=((DSensData[4]+DSensData[5]+DSensData[6]+DSensData[7])-(DSensData[3]+DSensData[2]+DSensData[1]+DSensData[0]));
else TraceI=((DSensData[4]+DSensData[5])-(DSensData[3]+DSensData[2]));
/* 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 * (10);
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 Localization(void){
if(Count_R == 0 && W_WidsR >= linewide){ //右1
WWIDCLR;
EnCount_1R =0;
Count_R++;}
else if(!Count_R && EnCount_1R >= 10000) Count_R++;
if(Count_R == 1 && W_WidsR >= linewide && EnCount_1R >= 54000){ //右2
WWIDCLR;
EnCount_1R = 57000;
Count_R++;}
else if(Count_R == 1 && EnCount_1R >= 60000) Count_R++;
if(Count_R == 2 && EnCount_1R >= 80000 && Shortcut_Flg ){ //予備減速
Count_R++;
EnCount_1R = 130000;
}
else if(Count_R == 2 && EnCount_1R >= 130000)Count_R++; //予備減速
if(Count_R == 3 && EnCount_1R >= 150000 && ( W_WidsR >= linewide || Shortcut_Flg )){ //コーナ1IN
WWIDCLR;
EnCount_1R = 153000;
Count_R++;}
else if(Count_R == 3 && EnCount_1R >= 156000) Count_R++;
if(Count_R == 4 && EnCount_1R >= 205000 && W_WidsR >= linewide && !Shortcut_Flg){ //コーナ1OUT
WWIDCLR;
EnCount_1R = 208000;
Count_R++;}
else if(Count_R == 4 && EnCount_1R >= 221000 && !Shortcut_Flg ) Count_R++;
else if(Count_R == 4 && EnCount_1R >= 170000 && (DSensData[3] >= 400 || DSensData[4] >= 400 || DSensData[5] >= 400 || DSensData[6] >= 100 || DSensData[7] >= 100 || DSensData[0] >= 100 || DSensData[1] >= 100 || DSensData[2] >= 400) && Shortcut_Flg ){
SteerMode = WTRACE;
WWIDCLR;
EnCount_1R = 239000;
Count_R++;}
else if(Count_R == 4 && EnCount_1R >= 200000 && Shortcut_Flg ) {
SteerMode = TRACE;
WWIDCLR;
ErrSt = 1000;
EnCount_1R = 239000;
Count_R++;}
if(Count_R == 5 && EnCount_1R >= 239000 && ( W_WidsR >= linewide || Shortcut_Flg)){ //右3
WWIDCLR;
EnCount_1R = 242000;
Count_R++;}
else if(Count_R == 5 && EnCount_1R >= 245000) Count_R++;
if(Count_R == 6 && W_WidsL >= linewide && EnCount_1R >= 290000){ //左1
WWIDCLR;
EnCount_1R = 299000;
Count_R++;}
else if(Count_R == 6 && EnCount_1R >= 302000) Count_R++;
if(Count_R == 7 && W_WidsR >= linewide && EnCount_1R >= 330000){ //右3
WWIDCLR;
EnCount_1R = 342000;
Count_R++;}
else if(Count_R == 7 && EnCount_1R >= 350000) Count_R++;
if(Count_R == 8 && W_WidsL >= linewide && EnCount_1R >= 380000){ //左2
WWIDCLR;
EnCount_1R = 385000;
Count_R++;}
else if(Count_R == 8 && EnCount_1R >= 388000) Count_R++;
if(Count_R == 9 && EnCount_1R >= 400000)Count_R++; //予備減速
if(Count_R == 10 && W_WidsR >= linewide && EnCount_1R >= 422000){ //コーナ1IN
WWIDCLR;
EnCount_1R = 425000;
Count_R++;}
else if(Count_R == 10 && EnCount_1R >= 428000) Count_R++;
if(Count_R == 11 && W_WidsR >= linewide && EnCount_1R >= 470000 && R_No != R_Go){ //コーナ1OUT
WWIDCLR;
EnCount_1R = 480000;
Count_R++;}
else if(Count_R == 11 && EnCount_1R >= 483000 && R_No != R_Go) Count_R++;
if(Count_R >= 12 && W_WidsR >= linewide && EnCount_1R >= 500000){ //右1
Count_C++;
WWIDCLR;
EnCount_1R =0;
Count_R = 1;
R_No ++;}
else if(Count_R >= 12 && EnCount_1R >= 550000){
Count_C++;
EnCount_1R = 3000;
Count_R = 1;
R_No ++;}
}
//----------タイマー割り込み---------------
void flip(){
timer1++;
timer2++;
//Log_timer++;
ErrChk();
Get_Pot();
DSens();
SteerCtrl(SteerMode);
if(!TestDrive_Flg)SpeedCtrl(Speed);
MotorCtrl();
Line_Dec();
LanceAng(Angle);
//if(ModeSW()==0 && !SW_W) SD_Mount();
//if(ModeSW()==1 && !SW_W) SD_Unmount();
enCalc();
//if(Log_timer >= 9 && writeflg && Tmpcnt <= 5999) TmpLog();
//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;
}*/
}
//----------Tmpログモード--------------------
void TmpLog(){
LLog_tmp[Tmpcnt] = EnCount;
/*Log_tmp[0][Tmpcnt] = Count_R;
Log_tmp[1][Tmpcnt] = Count_R;
Log_tmp[2][Tmpcnt] = W_WidsR;
Log_tmp[3][Tmpcnt] = W_WidsL;
Log_tmp[4][Tmpcnt] = DSensData[3];
Log_tmp[5][Tmpcnt] = DSensData[4];
Log_tmp[6][Tmpcnt] = Pot_R>>2;
Log_tmp[7][Tmpcnt] = 0;
Log_tmp[8][Tmpcnt] = 0;
Log_tmp[9][Tmpcnt] = 0;*/
Tmpcnt++;
Log_timer = 0;
}
//----------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%-10d\r\n",EnCountI,timer1,Count_C);
fprintf(fp, "Encount PWM1 PWM2 PWM3 PWM4 F_Pot R_Pot \r\n");
for(i=0;i<=Tmpcnt;i++){
//fprintf(fp, "%-10d%-4d%-2d\r\n",W_WidsR[i][0],Log_W_Wids[i][1],Log_W_WidsLR[i]);
//fprintf(fp, "%-10d%-4d%-4d%-4d%-4d%-4d%-4d%-4d%-4d%-4d\r\n",Log_tmp[0][i],Log_tmp[1][i],Log_tmp[2][i],Log_tmp[3][i],Log_tmp[4][i],Log_tmp[5][i],Log_tmp[6][i],Log_tmp[7][i],Log_tmp[8][i],Log_tmp[9][i]);
//fprintf(fp, "%-10d%-6d%-6d%-6d%-6d%-6d%-6d\r\n",LLog_tmp[i],Log_tmp[0][i],Log_tmp[1][i],Log_tmp[2][i],Log_tmp[3][i],Log_tmp[4][i],Log_tmp[5][i]);
//fprintf(fp, "%-10d\r\n",LLog_tmp[i]);
}
fprintf(fp, "Wids EnCount_1R WidsLR \r\n");
for(i=0;i<=WidsNo;i++){
fprintf(fp, "%-20d%-20d%-4d%-4d%-4d\r\n",Log_W_Wids[i][0],Log_W_Wids[i][1],Log_W_WidsLR[i],Log_Count_R[i],LLog_tmp[i]);
}
}
}
//---------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); //汎用タイマー割り込み
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(); //マイコン初期設定
wait(1);
ST_EN;
SteerMode = TRACE;
lcd.contrast( 0x2E ); //LCDコントラスト設定
wait(0.5);
if(ModeSelect_Flg == 0) ModeSelect();
wait(1);
while(SW_R){
if(!SW_W && !SW_B && !DebugFlg) Debug();
lcd.clear();
lcd.printf(0,"%dms",TripTime);
lcd.printf(1,"%dmm/s",SPCom);
//if(!SW_W) SD_Mount();
wait(0.1);
}
lcd.clear();
lcd.printf(0,"Ready!");
lcd.printf(1,"%x",SD_Flag);
wait(1);
lcd.clear();
lcd.printf(0,"Go!");
OTH_EN;
W_WidsL = W_WidsR = 0;
//Speed = SPCom;
Angle = 90;
Speed = SPCom;
EnCount_1R = -40000;
timer1 =0;
StartFlg = 1;
//writeflg = 1;
while(1){
if(!SW_W && !SW_B && !DebugFlg && !EnCount) Debug();
/*if(timer2 >= 100){
timer2 = 0;
lcd.clear();
lcd.printf(0,"%d",timer1);
lcd.printf(1,"%d",ErrSt);
}*/
Localization();
//if(Log_timer >= 10 && writeflg && Tmpcnt <= 1000) TmpLog();
if(Count_R == 0 ){
Speed = SPCom;
led_color(WHI);
Angle = 90;}
if(Count_R == 1 ){
Speed = SPCom;
led_color(RED);
if(EnCount_1R >= 30000) Angle = 150;
else if(EnCount_1R >= 13000) Angle = 180;//11000
}
if(Count_R == 2 ){
Speed = SPCom;
led_color(GRE);
if(EnCount_1R >= 75000) Angle = 180; //75000
else Angle = 150;
}
if(Count_R == 3 ){
Speed = SPYobi;
Angle = 90;
led_color(BLU);}
if(Count_R == 4 ){
led_color(YEL);
if(EnCount_1R >= 200000 && Turbo_Flg) Speed = SPCom;
else Speed = SPCor;
Angle = 30;
STAngle = -27;
//if(Shortcut_Flg)SteerMode = ANGLE;
}
if(Count_R == 5 ){
Speed = SPCom;
Angle = 125;
led_color(RED);}
if(Count_R == 6 ){
if(Shortcut_Flg)SteerMode = WTRACE;
Speed = SPCom;
led_color(GRE);
if(EnCount_1R >= 270000){
LA_EN;
Angle = 52;}
else if(EnCount_1R >= 240000) LA_DA;//Angle = 130;
}
if(Count_R == 7 ){
if(Shortcut_Flg)SteerMode = TRACE;
Speed = SPCom;
led_color(PUR);
if(EnCount_1R >= 320000){
LA_EN;
Angle = 135;}
else LA_DA;//Angle = 52;
}
if(Count_R == 8 ){
Speed = SPCom;
led_color(RED);
if(EnCount_1R >= 365000){
LA_EN;
Angle = 49;}
else LA_DA;//Angle = 132;
}
if(Count_R == 9 ){
LA_EN;
Speed = SPCom;
Angle = 49;
led_color(GRE);}
if(Count_R == 10){
Speed = SPYobi;
led_color(WHI);}
if(Count_R == 11){
led_color(BLU);
Angle = 90;
STAngle = -6;
if(R_No == R_Go){
if(EnCount_1R <= 470000){
Angle = 400;
SteerMode = ANGLE;}
else {
SteerMode = TRACE;
ErrSt = AccelStopCnt;
if(EnCount==0)Angle = 90;
}
}
if(EnCount_1R >= 470000 && Turbo_Flg) Speed = SPCom;
else Speed = SPCor;
}
if(Count_R == 12){
SteerMode = TRACE;
Speed = SPCom;
led_color(YEL);}
}
}
//----------モード選択--------------------
void ModeSelect(void){
int DebugModeSW;
writeflg = 0;
ModeSelect_Flg=1;
lcd.clear();
lcd.printf(0,"Select");
lcd.printf(1,"TripTime");
wait(1);
lcd.clear();
while( SW_R ){
DebugModeSW = ModeSW();
switch(DebugModeSW){
case 0:
lcd.clear();
lcd.printf(0,"Turbo_ON");
Turbo_Flg = 1;
wait(0.1);
break;
case 1:
lcd.clear();
lcd.printf(0,"Round30");
TripTime =30000;
R_Go = 7;
wait(0.1);
break;
case 2:
lcd.clear();
lcd.printf(0,"Round60");
TripTime =60000;
R_Go = 15;
wait(0.1);
break;
case 3:
lcd.clear();
lcd.printf(0,"Round10");
TripTime =10000;
R_Go = 2;
wait(0.1);
break;
case 4:
lcd.clear();
lcd.printf(0,"Round20");
TripTime =20000;
wait(0.1);
break;
case 5:
lcd.clear();
lcd.printf(0,"Shortcut60");
TripTime =60000;
Shortcut_Flg = 1;
wait(0.1);
break;
default:
lcd.clear();
lcd.printf(0,"Select");
lcd.printf(1,"1-5");
wait(0.1);
break;
}
}
wait(1);
lcd.clear();
while( SW_R){
DebugModeSW = ModeSW();
switch(DebugModeSW){
case 1:
lcd.clear();
lcd.printf(0,"STSpeed");
lcd.printf(1,"5m/s");
SPCom=5000;
wait(0.1);
break;
case 4:
lcd.clear();
lcd.printf(0,"STSpeed");
lcd.printf(1,"4.5m/s");
wait(1);
SPCom=4500;
SPCor=2800;
break;
case 3:
lcd.clear();
lcd.printf(0,"STSpeed");
lcd.printf(1,"6m/s");
SPCom=6000;
wait(0.1);
break;
case 2:
lcd.clear();
lcd.printf(0,"STSpeed");
lcd.printf(1,"5.2m/s");
SPCom=5200;
wait(0.1);
break;
case 5:
lcd.clear();
lcd.printf(0,"AllSpeed");
lcd.printf(1,"3m/s");
wait(1);
SPCom=3000;
SPYobi=3000;
SPCor=3000;
wait(0.1);
break;
case 6:
lcd.clear();
lcd.printf(0,"FreeDrive");
TestDrive_Flg=1;
wait(0.1);
break;
default:
lcd.clear();
lcd.printf(0,"Select");
lcd.printf(1,"1-6");
wait(0.1);
break;
}
}
}
//----------デバッグモード--------------------
void Debug(void){
int DebugModeSW;
int PotTest1,PotTest2;
writeflg = 0;
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,"Speed7");
wait(0.1);
break;
case 10:
lcd.clear();
lcd.printf(0,"Speed2");
wait(0.1);
break;
case 11:
lcd.clear();
lcd.printf(0,"Speed0");
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();
writeflg = 1;
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(1);
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(0,"%3d,%3d",DSensData[3],DSensData[4]);
lcd.printf(1,"%3d,%3d",DSensData[2],DSensData[5]);
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:
PotTest1 = (DFP-(Steer_Ptm.read_u16()>>6))/3;
PotTest2 = ((Lance_Ptm.read_u16()>>6)-DRP)/3;
lcd.clear();
lcd.printf(0,"%3d,%3d",Pot_F,PotTest1);
lcd.printf(1,"%3d,%3d",Pot_R,PotTest2);
/*lcd.printf(0,"%3d,%3d",RearPot[1],RearPot[4]);
lcd.printf(1,"%3d,%3d",RearPot[5],RearPot[9]);*/
wait(0.1);
break;
case 8:
SPCom=5000;
lcd.clear();
lcd.printf(0,"Speed5");
lcd.printf(1,"%d",SPCom);
wait(0.1);
//SPYobi=3500;
//SPCor=3000;
//ParamS=2000;
break;
case 9:
SPCom=7000;
lcd.clear();
lcd.printf(0,"Speed7");
lcd.printf(1,"%d",SPCom);
wait(0.1);
//SPYobi=3000;
//SPCor=3000;
break;
case 10:
SPCom=2000;
SPYobi=2000;
SPCor=2000;
lcd.clear();
lcd.printf(0,"Speed2");
lcd.printf(1,"%d",SPCom);
wait(0.1);
break;
case 11:
SPCom=0;
SPYobi=0;
SPCor=0;
lcd.clear();
lcd.printf(0,"Speed0");
lcd.printf(1,"%d",SPCom);
wait(0.1);
break;
case 12:
lcd.clear();
lcd.printf(0,"R_No");
lcd.printf(1,"%d",R_No);
wait(0.1);
linewide=100;
break;
case 13:
lcd.clear();
lcd.printf(0,"ErrSt");
lcd.printf(1,"%d",ErrSt);
wait(0.1);
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;
}