Takeshi Nishimura / Mbed 2 deprecated Lancer2016

ランサー

Dependencies:   SB1602E SDFileSystem mbed

Fork of Seeed_SDCard_Shield by Shields

main.cpp@12:2cb9082e2d40, 2018-04-29 (annotated)

Committer:
MCR_Xavier
Date:
Sun Apr 29 10:40:19 2018 +0000
Revision:
12:2cb9082e2d40
Parent:
11:5ec2507020da 
Lancer2018????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MCR_Xavier 5:20bb1e34b354 1 //Lancer2016
screamer 3:5edc67dee8b7 2
MCR_Xavier 5:20bb1e34b354 3 //ヘッダー設定
screamer 0:525c842a3c89 4 #include "mbed.h"
screamer 0:525c842a3c89 5 #include "SDFileSystem.h"
MCR_Xavier 5:20bb1e34b354 6 #include "SB1602E.h"
MCR_Xavier 10:adefba925268 7 #include "stdlib.h"
screamer 0:525c842a3c89 8
MCR_Xavier 5:20bb1e34b354 9 //シンボル定義
MCR_Xavier 11:5ec2507020da 10 #define CLR 0,0,0 //無 RGBパターン
MCR_Xavier 11:5ec2507020da 11 #define RED 1,0,0 //赤
MCR_Xavier 11:5ec2507020da 12 #define GRE 0,1,0 //緑
MCR_Xavier 11:5ec2507020da 13 #define YEL 1,1,0 //黄
MCR_Xavier 11:5ec2507020da 14 #define BLU 0,0,1 //青
MCR_Xavier 11:5ec2507020da 15 #define PUR 1,0,1 //紫
MCR_Xavier 11:5ec2507020da 16 #define CYA 0,1,1 //水
MCR_Xavier 11:5ec2507020da 17 #define WHI 1,1,1 //白
MCR_Xavier 11:5ec2507020da 18 #define DFP 508 //フロントポテンショメータ機械原点
MCR_Xavier 11:5ec2507020da 19 #define DRP 234 //リヤポテンショメータ機械原点
MCR_Xavier 11:5ec2507020da 20 #define FW 1 //駆動輪前進
MCR_Xavier 11:5ec2507020da 21 #define BW 0 //駆動輪後進
MCR_Xavier 11:5ec2507020da 22 #define ST_EN ST_CTR = 0 //ステアリングモータ有効
MCR_Xavier 11:5ec2507020da 23 #define ST_DA ST_CTR = 1 //ステアリングモータ無効
MCR_Xavier 11:5ec2507020da 24 #define OTH_EN OTH_CTR = 0 //その他モータ有効
MCR_Xavier 11:5ec2507020da 25 #define OTH_DA OTH_CTR = 1 //その他モータ無効
MCR_Xavier 11:5ec2507020da 26 #define LA_DA LancFree_Flg=1 //槍無効
MCR_Xavier 11:5ec2507020da 27 #define LA_EN LancFree_Flg=0 //槍有効
MCR_Xavier 11:5ec2507020da 28 #define DSs 8 //LineセンサA/Dシフト数
MCR_Xavier 11:5ec2507020da 29 #define ASs 6 //トレース用LineセンサA/Dシフト数
MCR_Xavier 11:5ec2507020da 30 #define ThS 80 //Line白黒判別閾値
MCR_Xavier 11:5ec2507020da 31 #define ANGLE 0 //角度制御モード
MCR_Xavier 11:5ec2507020da 32 #define TRACE 1 //トレースモード
MCR_Xavier 11:5ec2507020da 33 #define WTRACE 2 //拡張トレースモード
MCR_Xavier 11:5ec2507020da 34 #define NUM_DATA 10 //ソート用データ数
MCR_Xavier 10:adefba925268 35 #define WWIDCLR W_WidsR=W_WidsL=0
MCR_Xavier 5:20bb1e34b354 36
MCR_Xavier 5:20bb1e34b354 37 SB1602E lcd(PB_9,PB_8); //LCD_I2C設定(SDA, SCL)
MCR_Xavier 7:c99fe0eb544a 38 SDFileSystem sd(PA_7,PA_6,PA_5,PB_6,"sd"); // MOSI, MISO, SCK, CS
screamer 0:525c842a3c89 39 FILE *fp;
MCR_Xavier 11:5ec2507020da 40
MCR_Xavier 5:20bb1e34b354 41 //センサ入力
MCR_Xavier 11:5ec2507020da 42 DigitalIn RS1(PC_10); //ロータリースイッチbit1
MCR_Xavier 11:5ec2507020da 43 DigitalIn RS2(PA_15); //ロータリースイッチbit2
MCR_Xavier 11:5ec2507020da 44 DigitalIn RS3(PA_12); //ロータリースイッチbit3
MCR_Xavier 5:20bb1e34b354 45 DigitalIn RS4(PA_9); //ロータリースイッチbit4
MCR_Xavier 5:20bb1e34b354 46 AnalogIn Line0(PC_0); //アナログラインセンサ
MCR_Xavier 5:20bb1e34b354 47 AnalogIn Line1(PC_1); //アナログラインセンサ
MCR_Xavier 5:20bb1e34b354 48 AnalogIn Line2(PC_2); //アナログラインセンサ
MCR_Xavier 5:20bb1e34b354 49 AnalogIn Line3(PC_3); //アナログラインセンサ
MCR_Xavier 5:20bb1e34b354 50 AnalogIn Line4(PA_0); //アナログラインセンサ
MCR_Xavier 5:20bb1e34b354 51 AnalogIn Line5(PA_1); //アナログラインセンサ
MCR_Xavier 5:20bb1e34b354 52 AnalogIn Line6(PA_2); //アナログラインセンサ
MCR_Xavier 5:20bb1e34b354 53 AnalogIn Line7(PA_3); //アナログラインセンサ
MCR_Xavier 5:20bb1e34b354 54 AnalogIn Steer_Ptm(PA_4); //ステアリングポテンショ
MCR_Xavier 5:20bb1e34b354 55 AnalogIn Lance_Ptm(PC_4); //槍ポテンショ
MCR_Xavier 5:20bb1e34b354 56 DigitalIn SW_W(PB_5); //スイッチ白
MCR_Xavier 5:20bb1e34b354 57 DigitalIn SW_R(PC_6); //スイッチ赤
MCR_Xavier 11:5ec2507020da 58 DigitalIn SW_B(PC_12); //スイッチ黒
MCR_Xavier 5:20bb1e34b354 59 InterruptIn s0signal(D6); //ロータリエンコーダA相
MCR_Xavier 5:20bb1e34b354 60 InterruptIn s1signal(D7); //ロータリエンコーダB相
MCR_Xavier 11:5ec2507020da 61
MCR_Xavier 5:20bb1e34b354 62 //モータ制御
MCR_Xavier 11:5ec2507020da 63 PwmOut PWM_CH1(PC_7); //3/2槍
MCR_Xavier 11:5ec2507020da 64 PwmOut PWM_CH2(PA_11); //1/4前左
MCR_Xavier 11:5ec2507020da 65 PwmOut PWM_CH3(PB_2); //2/4前右
MCR_Xavier 11:5ec2507020da 66 PwmOut PWM_CH4(PB_7); //4/2後右
MCR_Xavier 11:5ec2507020da 67 PwmOut PWM_CH5(PA_10); //1/3後左
MCR_Xavier 11:5ec2507020da 68 PwmOut PWM_CH6(PB_4); //3/1操舵
MCR_Xavier 11:5ec2507020da 69 DigitalOut Rev_CH1(PB_12); //操舵モータDIR
MCR_Xavier 11:5ec2507020da 70 DigitalOut Rev_CH2(PB_13); //前左モータDIR
MCR_Xavier 11:5ec2507020da 71 DigitalOut Rev_CH3(PC_8); //前右モータDIR
MCR_Xavier 11:5ec2507020da 72 DigitalOut Rev_CH4(PC_9); //後右モータDIR
MCR_Xavier 11:5ec2507020da 73 DigitalOut Rev_CH5(PB_14); //後左モータDIR
MCR_Xavier 11:5ec2507020da 74 DigitalOut Rev_CH6(PB_15); //槍モータDIR
MCR_Xavier 11:5ec2507020da 75 DigitalOut SensLED(PC_11); //センサ用LED
MCR_Xavier 11:5ec2507020da 76 DigitalOut ST_CTR(PB_0); //STEER_EnableCtrl
MCR_Xavier 11:5ec2507020da 77 DigitalOut OTH_CTR(PB_1); //OTHER_EnableCtrl
MCR_Xavier 11:5ec2507020da 78
MCR_Xavier 5:20bb1e34b354 79 //デバッグ
MCR_Xavier 11:5ec2507020da 80 DigitalOut LED_R(PA_13); //LED_R
MCR_Xavier 11:5ec2507020da 81 DigitalOut LED_G(PA_14); //LED_G
MCR_Xavier 11:5ec2507020da 82 DigitalOut LED_B(PB_3); //LED_B
MCR_Xavier 11:5ec2507020da 83
MCR_Xavier 5:20bb1e34b354 84 //割り込み定義
MCR_Xavier 11:5ec2507020da 85 Ticker flipper; //汎用タイマー
MCR_Xavier 11:5ec2507020da 86 Ticker sensget; //センサー用タイマー
MCR_Xavier 5:20bb1e34b354 87
MCR_Xavier 5:20bb1e34b354 88 //プロトタイプ宣言
MCR_Xavier 11:5ec2507020da 89 void init(void); //マイコン初期設定
MCR_Xavier 11:5ec2507020da 90 void MotorCtrl(void); //モータ管理
MCR_Xavier 11:5ec2507020da 91 void enc(); //エンコーダ処理
MCR_Xavier 11:5ec2507020da 92 int ModeSW(); //モードスイッチ値取得
MCR_Xavier 11:5ec2507020da 93 int Get_SW(); //スイッチ値取得
MCR_Xavier 11:5ec2507020da 94 void Get_Pot(); //ポテンショメータA/D取得
MCR_Xavier 11:5ec2507020da 95 void SpeedCtrl(int V); //速度制御
MCR_Xavier 11:5ec2507020da 96 int PotTest(); //ポテンショテスト
MCR_Xavier 11:5ec2507020da 97 float PwmTest(); //PWMテスト
MCR_Xavier 11:5ec2507020da 98 void SD_Mount(); //SDカードをマウントする
MCR_Xavier 11:5ec2507020da 99 void SD_Unmount(); //SDカードをアンマウントする
MCR_Xavier 11:5ec2507020da 100 void DSens(); //センサ変調処理
MCR_Xavier 11:5ec2507020da 101 void LineTrace(); //ライントレース
MCR_Xavier 11:5ec2507020da 102 void ErrChk(void); //エラーチェック
MCR_Xavier 11:5ec2507020da 103 void LanceAng(int Ang); //槍角度制御
MCR_Xavier 11:5ec2507020da 104 void Line_Dec(void); //白線判定
MCR_Xavier 11:5ec2507020da 105 void enCalc(void); //エンコーダ計算とか
MCR_Xavier 11:5ec2507020da 106 void Debug(void); //デバッグモード
MCR_Xavier 11:5ec2507020da 107 void SD_Log(void); //SDログ
MCR_Xavier 11:5ec2507020da 108 void SD_WriteSq(void); //SD書き込み
MCR_Xavier 11:5ec2507020da 109 void TmpLog(void); //内部メモリへのログ
MCR_Xavier 11:5ec2507020da 110 void SteerCtrl(char STMode); //ステアリングコントロール
MCR_Xavier 11:5ec2507020da 111 void AngleCtrl(int STAngle); //角度制御モード
MCR_Xavier 11:5ec2507020da 112 void Localization(void); // 位置推定
MCR_Xavier 11:5ec2507020da 113 void led_color(char R , char G , char B ); //LED色制御
MCR_Xavier 11:5ec2507020da 114 void ModeSelect(); //モード選択
MCR_Xavier 11:5ec2507020da 115
MCR_Xavier 5:20bb1e34b354 116
MCR_Xavier 5:20bb1e34b354 117 //グローバル変数の宣言
MCR_Xavier 11:5ec2507020da 118 int EnCount=0;
MCR_Xavier 11:5ec2507020da 119 long EnCountI=0; //エンコーダカウント
MCR_Xavier 11:5ec2507020da 120 int EnCount_1R=0; //コース１周分のエンコーダカウント毎週リセットされる
MCR_Xavier 11:5ec2507020da 121 unsigned char Rev=0; //エンコーダ回転方向判定
MCR_Xavier 11:5ec2507020da 122 unsigned char SD_Flag=0; //SDカードの状態判定
MCR_Xavier 11:5ec2507020da 123 unsigned char Line_Flg; //ライン情報フラグ
MCR_Xavier 11:5ec2507020da 124 unsigned char Shortcut_Flg=0; //ショートカットフラグ
MCR_Xavier 11:5ec2507020da 125 unsigned char Turbo_Flg=0; //コーナー出口のターボフラグ
MCR_Xavier 11:5ec2507020da 126 unsigned char R_Go = 15;
MCR_Xavier 11:5ec2507020da 127 unsigned char TestDrive_Flg=0; //駆動輪フリー
MCR_Xavier 11:5ec2507020da 128 unsigned char LancFree_Flg=0; //槍フリー
MCR_Xavier 11:5ec2507020da 129 int timer1=0,timer2=0,Tmpcnt=0,Log_timer=0; //タイマー
MCR_Xavier 5:20bb1e34b354 130 int Stime=0;
MCR_Xavier 11:5ec2507020da 131 char StartFlg=0;
MCR_Xavier 5:20bb1e34b354 132 unsigned long long cntT=0;
MCR_Xavier 5:20bb1e34b354 133 int ONSensData[8],OFFSensData[8],DSensData[8],TFSensData[8];//ラインセンサ値
MCR_Xavier 10:adefba925268 134 int Pot_F=0,Pot_R=0,Pot_F2=0,StrPot_R[10]; //ポテンショ
MCR_Xavier 5:20bb1e34b354 135 char LA_Rev,FL_Rev,FR_Rev,RR_Rev,RL_Rev,ST_Rev; //モータ回転方向
MCR_Xavier 5:20bb1e34b354 136 int LA_PWM=0,FL_PWM=0,FR_PWM=0,RR_PWM=0,RL_PWM=0,ST_PWM=0; //モータPWMデューティ
MCR_Xavier 11:5ec2507020da 137 int ErrSt=0;
MCR_Xavier 11:5ec2507020da 138 int Speed=0,Angle=0,STAngle=0;
MCR_Xavier 11:5ec2507020da 139 int iAng=0;
MCR_Xavier 11:5ec2507020da 140 long iV=0;
MCR_Xavier 5:20bb1e34b354 141 int TraceI,Trace_P=0,Trace_I=0,Trace_D=0,Trace_B=0;
MCR_Xavier 9:8ab0d3e46c3c 142 int Angle_P=0,Angle_I=0,Angle_D=0,Angle_B=0;
MCR_Xavier 5:20bb1e34b354 143 int RSW_Value;
MCR_Xavier 5:20bb1e34b354 144 char Turn_Flg=0;
MCR_Xavier 11:5ec2507020da 145 char ModeSelect_Flg=0;
MCR_Xavier 6:32698402be64 146 unsigned int R_LineCnt=0,L_LineCnt=0,R_LineCntF=0,L_LineCntF=0;
MCR_Xavier 5:20bb1e34b354 147 char R_LineFlg=0,L_LineFlg=0;
MCR_Xavier 5:20bb1e34b354 148 char DebugFlg=0;
MCR_Xavier 11:5ec2507020da 149 long W_WidsR=0,W_WidsL=0,W_WidsRCnt=0,W_WidsLCnt=0,dAng;
MCR_Xavier 6:32698402be64 150 unsigned char Count_C=0,Count_R=0,Count_L=0,Count_H=0;
MCR_Xavier 11:5ec2507020da 151 unsigned char R_No=1;
MCR_Xavier 6:32698402be64 152 int ParamS=1;
MCR_Xavier 11:5ec2507020da 153 int SPCom=4500,SPYobi=4500,SPCor=3200,loopJump=1000000,linewide=100;
MCR_Xavier 11:5ec2507020da 154 int TripTime =60000; //走行時間
MCR_Xavier 11:5ec2507020da 155 int AccelStopCnt = 1000; //駆動停止カウント
MCR_Xavier 11:5ec2507020da 156 int MotorStopCnt = 2000; //モータ停止カウント
MCR_Xavier 9:8ab0d3e46c3c 157 char SteerAngle;
MCR_Xavier 9:8ab0d3e46c3c 158 char SteerMode=0;
MCR_Xavier 11:5ec2507020da 159 char DefFlg=0;
MCR_Xavier 11:5ec2507020da 160 int FrontPot[NUM_DATA + 1],RearPot[NUM_DATA + 1];
MCR_Xavier 5:20bb1e34b354 161
MCR_Xavier 7:c99fe0eb544a 162 char Log_table[10][10];
MCR_Xavier 11:5ec2507020da 163 int Log_W_Wids[300][2];
MCR_Xavier 11:5ec2507020da 164 char Log_W_WidsLR[300];
MCR_Xavier 11:5ec2507020da 165 char Log_Count_R[300];
MCR_Xavier 11:5ec2507020da 166 char Log_tmp[100][6];
MCR_Xavier 12:2cb9082e2d40 167 char LLog_tmp[300];
MCR_Xavier 7:c99fe0eb544a 168 int Sensecnt=0,writeflg=0;
MCR_Xavier 7:c99fe0eb544a 169
MCR_Xavier 7:c99fe0eb544a 170 unsigned char WidsNo=0;
MCR_Xavier 7:c99fe0eb544a 171 char Logcnt=0;
MCR_Xavier 5:20bb1e34b354 172 char Trace_table[]={
MCR_Xavier 5:20bb1e34b354 173 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,
MCR_Xavier 5:20bb1e34b354 174 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,
MCR_Xavier 5:20bb1e34b354 175 22,23,24,25,27,28,30,31,33,34,36,38,39,41,43,45,47,49,51,53,56,
MCR_Xavier 5:20bb1e34b354 176 58,60,63,65,68,70,73,76,79,82,85,88,91,94,98,101,104,108,112,115,
MCR_Xavier 5:20bb1e34b354 177 119,123};
MCR_Xavier 5:20bb1e34b354 178 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,
MCR_Xavier 5:20bb1e34b354 179 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,
MCR_Xavier 5:20bb1e34b354 180 9,10,10,10,10,11,11,11,12,12,12,13,13,13,14,14,14,15,15,15,16,16,
MCR_Xavier 5:20bb1e34b354 181 16,17,17,18,18,18,19,19,19,20,20};
MCR_Xavier 5:20bb1e34b354 182
MCR_Xavier 5:20bb1e34b354 183 float Motor_TableA[]={
MCR_Xavier 11:5ec2507020da 184 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,
MCR_Xavier 11:5ec2507020da 185 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,
MCR_Xavier 11:5ec2507020da 186 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,
MCR_Xavier 11:5ec2507020da 187 0.95,0.96,0.97,0.99,1.01,1.02,1.05,1.07,1.09,1.12,1.15};
MCR_Xavier 11:5ec2507020da 188
MCR_Xavier 5:20bb1e34b354 189 float Motor_TableB[]={
MCR_Xavier 11:5ec2507020da 190 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,
MCR_Xavier 11:5ec2507020da 191 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,
MCR_Xavier 11:5ec2507020da 192 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,
MCR_Xavier 11:5ec2507020da 193 2.04,2.10,2.16,2.23,2.30,2.38,2.46,2.55,2.64,2.74,2.85};
MCR_Xavier 11:5ec2507020da 194
MCR_Xavier 5:20bb1e34b354 195 float Motor_TableC[]={
MCR_Xavier 11:5ec2507020da 196 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,
MCR_Xavier 11:5ec2507020da 197 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,
MCR_Xavier 11:5ec2507020da 198 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,
MCR_Xavier 11:5ec2507020da 199 0.45,0.43,0.41,0.38,0.35,0.32,0.29,0.26,0.23,0.19,0.15};
MCR_Xavier 11:5ec2507020da 200
MCR_Xavier 5:20bb1e34b354 201 float Motor_TableD[]={
MCR_Xavier 11:5ec2507020da 202 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,
MCR_Xavier 11:5ec2507020da 203 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,
MCR_Xavier 11:5ec2507020da 204 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,
MCR_Xavier 11:5ec2507020da 205 1.55,1.57,1.59,1.62,1.65,1.68,1.71,1.74,1.77,1.81,1.85};
MCR_Xavier 5:20bb1e34b354 206
MCR_Xavier 7:c99fe0eb544a 207 ////////////////////////////////////////////////
MCR_Xavier 7:c99fe0eb544a 208 //-------------ベースプログラム-----------------//
MCR_Xavier 7:c99fe0eb544a 209 ////////////////////////////////////////////////
MCR_Xavier 5:20bb1e34b354 210 //----------モードSW--------------------
MCR_Xavier 5:20bb1e34b354 211 int ModeSW(){
MCR_Xavier 5:20bb1e34b354 212 char RSW=0;
MCR_Xavier 5:20bb1e34b354 213 if(RS1)RSW += 1;
MCR_Xavier 5:20bb1e34b354 214 if(RS2)RSW += 2;
MCR_Xavier 5:20bb1e34b354 215 if(RS3)RSW += 4;
MCR_Xavier 5:20bb1e34b354 216 if(RS4)RSW += 8;
MCR_Xavier 5:20bb1e34b354 217 RSW_Value=int(RSW);
MCR_Xavier 5:20bb1e34b354 218 return RSW;}
MCR_Xavier 7:c99fe0eb544a 219 //----------SW情報取得--------------------
MCR_Xavier 7:c99fe0eb544a 220 int Get_SW(){
MCR_Xavier 7:c99fe0eb544a 221 char SW_Val=0;
MCR_Xavier 7:c99fe0eb544a 222 if(SW_W)SW_Val |= 0x01;
MCR_Xavier 7:c99fe0eb544a 223 if(SW_R)SW_Val |= 0x02;
MCR_Xavier 7:c99fe0eb544a 224 if(SW_B)SW_Val |= 0x04;
MCR_Xavier 7:c99fe0eb544a 225 return SW_Val;}
MCR_Xavier 5:20bb1e34b354 226 //----------エラーチェック--------------------
MCR_Xavier 5:20bb1e34b354 227 void ErrChk(void){
MCR_Xavier 11:5ec2507020da 228 if(timer1 >= TripTime && StartFlg && Count_R == 12) ErrSt = AccelStopCnt;
MCR_Xavier 11:5ec2507020da 229 if(Pot_F >= 47 || Pot_F <= (-47)) ErrSt++;
MCR_Xavier 11:5ec2507020da 230 if(DSensData[3] <= 250 && DSensData[4] <= 250 && StartFlg && SteerMode != ANGLE ) ErrSt = ErrSt + 5;
MCR_Xavier 11:5ec2507020da 231 else if(ErrSt < 1000 && Pot_F == 0 ) ErrSt=0;
MCR_Xavier 11:5ec2507020da 232 if(ErrSt >= MotorStopCnt ) ErrSt = MotorStopCnt;
MCR_Xavier 5:20bb1e34b354 233 }
MCR_Xavier 7:c99fe0eb544a 234 //-----------エンコーダカウント------------------
MCR_Xavier 7:c99fe0eb544a 235 void s0_trigger_rise() { // A相立上り変化割込み時の処理 (Low -> High)
MCR_Xavier 7:c99fe0eb544a 236 Rev |= 0x01;
MCR_Xavier 7:c99fe0eb544a 237 if(Rev&0x10) EnCount--;
MCR_Xavier 7:c99fe0eb544a 238 else EnCount++;
MCR_Xavier 7:c99fe0eb544a 239 }
MCR_Xavier 7:c99fe0eb544a 240 void s0_trigger_fall() { // A相立下り変化割込み時の処理 (High -> Low)
MCR_Xavier 7:c99fe0eb544a 241 Rev &= ~0x01;
MCR_Xavier 7:c99fe0eb544a 242 if(Rev&0x10) EnCount++;
MCR_Xavier 7:c99fe0eb544a 243 else EnCount--;
MCR_Xavier 7:c99fe0eb544a 244 }
MCR_Xavier 7:c99fe0eb544a 245 void s1_trigger_rise() { // B相立上り変化割込み時の処理 (Low -> High)
MCR_Xavier 7:c99fe0eb544a 246 Rev |= 0x10;
MCR_Xavier 7:c99fe0eb544a 247 if(Rev&0x01) EnCount++;
MCR_Xavier 7:c99fe0eb544a 248 else EnCount--;
MCR_Xavier 7:c99fe0eb544a 249 }
MCR_Xavier 7:c99fe0eb544a 250 void s1_trigger_fall() { // B相立下り変化割込み時の処理 (High -> Low)
MCR_Xavier 7:c99fe0eb544a 251 Rev &= ~0x10;
MCR_Xavier 7:c99fe0eb544a 252 if(Rev&0x01) EnCount--;
MCR_Xavier 7:c99fe0eb544a 253 else EnCount++;
MCR_Xavier 7:c99fe0eb544a 254 }
MCR_Xavier 7:c99fe0eb544a 255 //----------エンコーダ計算--------------------
MCR_Xavier 7:c99fe0eb544a 256 void enCalc(void){
MCR_Xavier 7:c99fe0eb544a 257 EnCountI = EnCountI + EnCount;
MCR_Xavier 10:adefba925268 258 EnCount_1R = EnCount_1R + EnCount;
MCR_Xavier 7:c99fe0eb544a 259 EnCount = 0;
MCR_Xavier 7:c99fe0eb544a 260 }
MCR_Xavier 7:c99fe0eb544a 261 //----------フルカラーLED制御--------------------
MCR_Xavier 7:c99fe0eb544a 262 void led_color(char R , char G , char B )
MCR_Xavier 7:c99fe0eb544a 263 {
MCR_Xavier 11:5ec2507020da 264 if(LancFree_Flg)R=G=B=1;
MCR_Xavier 7:c99fe0eb544a 265 if ( R ) LED_R = 1;
MCR_Xavier 7:c99fe0eb544a 266 else LED_R = 0;
MCR_Xavier 7:c99fe0eb544a 267 if ( G ) LED_G = 1;
MCR_Xavier 7:c99fe0eb544a 268 else LED_G = 0;
MCR_Xavier 7:c99fe0eb544a 269 if ( B ) LED_B = 1;
MCR_Xavier 7:c99fe0eb544a 270 else LED_B = 0;
MCR_Xavier 7:c99fe0eb544a 271 }
MCR_Xavier 7:c99fe0eb544a 272 //----------センサー値の取得---------------
MCR_Xavier 7:c99fe0eb544a 273 void sensGet(){
MCR_Xavier 10:adefba925268 274 if(Stime >= 10) {
MCR_Xavier 10:adefba925268 275 Stime=0;
MCR_Xavier 11:5ec2507020da 276 //Hpsort(FrontPot, NUM_DATA);
MCR_Xavier 11:5ec2507020da 277 //Hpsort(RearPot, NUM_DATA);
MCR_Xavier 11:5ec2507020da 278 //Get_Pot();
MCR_Xavier 10:adefba925268 279 }
MCR_Xavier 7:c99fe0eb544a 280 if(Stime == 0 ) SensLED=0;
MCR_Xavier 7:c99fe0eb544a 281 if(Stime == 5 ) SensLED=1;
MCR_Xavier 7:c99fe0eb544a 282 if(Stime == 4 ) {
MCR_Xavier 7:c99fe0eb544a 283 OFFSensData[0] = Line0.read_u16()>>DSs;
MCR_Xavier 7:c99fe0eb544a 284 OFFSensData[1] = Line1.read_u16()>>DSs;
MCR_Xavier 7:c99fe0eb544a 285 OFFSensData[2] = Line2.read_u16()>>ASs;
MCR_Xavier 7:c99fe0eb544a 286 OFFSensData[3] = Line3.read_u16()>>ASs;
MCR_Xavier 7:c99fe0eb544a 287 OFFSensData[4] = Line4.read_u16()>>ASs;
MCR_Xavier 7:c99fe0eb544a 288 OFFSensData[5] = Line5.read_u16()>>ASs;
MCR_Xavier 7:c99fe0eb544a 289 OFFSensData[6] = Line6.read_u16()>>DSs;
MCR_Xavier 7:c99fe0eb544a 290 OFFSensData[7] = Line7.read_u16()>>DSs;
MCR_Xavier 7:c99fe0eb544a 291 }
MCR_Xavier 7:c99fe0eb544a 292 if(Stime == 9 ) {
MCR_Xavier 7:c99fe0eb544a 293 ONSensData[0] = Line0.read_u16()>>DSs;
MCR_Xavier 7:c99fe0eb544a 294 ONSensData[1] = Line1.read_u16()>>DSs;
MCR_Xavier 7:c99fe0eb544a 295 ONSensData[2] = Line2.read_u16()>>ASs;
MCR_Xavier 7:c99fe0eb544a 296 ONSensData[3] = Line3.read_u16()>>ASs;
MCR_Xavier 7:c99fe0eb544a 297 ONSensData[4] = Line4.read_u16()>>ASs;
MCR_Xavier 7:c99fe0eb544a 298 ONSensData[5] = Line5.read_u16()>>ASs;
MCR_Xavier 7:c99fe0eb544a 299 ONSensData[6] = Line6.read_u16()>>DSs;
MCR_Xavier 7:c99fe0eb544a 300 ONSensData[7] = Line7.read_u16()>>DSs;
MCR_Xavier 7:c99fe0eb544a 301 }
MCR_Xavier 11:5ec2507020da 302 //FrontPot[Stime] = Steer_Ptm.read_u16()>>6;
MCR_Xavier 11:5ec2507020da 303 //RearPot[Stime] = Lance_Ptm.read_u16()>>6;
MCR_Xavier 7:c99fe0eb544a 304 Stime++;
MCR_Xavier 9:8ab0d3e46c3c 305 }
MCR_Xavier 9:8ab0d3e46c3c 306 //----------ポテンショ値の取得---------------
MCR_Xavier 9:8ab0d3e46c3c 307 void Get_Pot(){
MCR_Xavier 9:8ab0d3e46c3c 308
MCR_Xavier 9:8ab0d3e46c3c 309 Pot_F2 = (DFP-(Steer_Ptm.read_u16()>>6));
MCR_Xavier 11:5ec2507020da 310 //Pot_F2 = DFP - FrontPot[4];
MCR_Xavier 11:5ec2507020da 311 Pot_F = int(Pot_F2/3);
MCR_Xavier 11:5ec2507020da 312 //Pot_R = RearPot[4];
MCR_Xavier 11:5ec2507020da 313 Pot_R = Lance_Ptm.read_u16()>>6;
MCR_Xavier 9:8ab0d3e46c3c 314 }
MCR_Xavier 7:c99fe0eb544a 315 //----------センサー値差分の計算---------------
MCR_Xavier 7:c99fe0eb544a 316 void DSens(){
MCR_Xavier 7:c99fe0eb544a 317
MCR_Xavier 7:c99fe0eb544a 318 int sensorno;
MCR_Xavier 7:c99fe0eb544a 319 for(sensorno=0;sensorno<=7;sensorno++){
MCR_Xavier 7:c99fe0eb544a 320 DSensData[sensorno] = OFFSensData[sensorno] - ONSensData[sensorno];
MCR_Xavier 7:c99fe0eb544a 321 if(DSensData[sensorno] >= ThS ) TFSensData[sensorno] = 1;
MCR_Xavier 7:c99fe0eb544a 322 else TFSensData[sensorno] = 0;
MCR_Xavier 7:c99fe0eb544a 323 }
MCR_Xavier 7:c99fe0eb544a 324 }
MCR_Xavier 7:c99fe0eb544a 325
MCR_Xavier 7:c99fe0eb544a 326 ////////////////////////////////////////////////
MCR_Xavier 7:c99fe0eb544a 327 //-------------便利機能プログラム---------------//
MCR_Xavier 7:c99fe0eb544a 328 ////////////////////////////////////////////////
MCR_Xavier 5:20bb1e34b354 329 //----------白線判別--------------------
MCR_Xavier 5:20bb1e34b354 330 void Line_Dec(void){
MCR_Xavier 7:c99fe0eb544a 331 char LR_Flg=0;
MCR_Xavier 6:32698402be64 332 if(TFSensData[0] && TFSensData[1] ) {
MCR_Xavier 6:32698402be64 333 R_LineCntF = 0;
MCR_Xavier 6:32698402be64 334 R_LineCnt++;}
MCR_Xavier 6:32698402be64 335 else {
MCR_Xavier 6:32698402be64 336 R_LineCnt = 0;
MCR_Xavier 6:32698402be64 337 R_LineCntF++;}
MCR_Xavier 6:32698402be64 338 if(TFSensData[6] && TFSensData[7] ) {
MCR_Xavier 6:32698402be64 339 L_LineCntF = 0;
MCR_Xavier 6:32698402be64 340 L_LineCnt++;}
MCR_Xavier 6:32698402be64 341 else {
MCR_Xavier 6:32698402be64 342 L_LineCnt = 0;
MCR_Xavier 6:32698402be64 343 L_LineCntF++;}
MCR_Xavier 5:20bb1e34b354 344
MCR_Xavier 5:20bb1e34b354 345 if(R_LineCnt >= 2 && !R_LineFlg ){
MCR_Xavier 5:20bb1e34b354 346 R_LineFlg = 1;
MCR_Xavier 5:20bb1e34b354 347 W_WidsRCnt = EnCountI;
MCR_Xavier 5:20bb1e34b354 348 }
MCR_Xavier 6:32698402be64 349 else if(!R_LineCnt && R_LineFlg && R_LineCntF >= 4 ){
MCR_Xavier 5:20bb1e34b354 350 R_LineFlg = 0;
MCR_Xavier 7:c99fe0eb544a 351 LR_Flg = 1;
MCR_Xavier 6:32698402be64 352 W_WidsR = EnCountI - W_WidsRCnt;
MCR_Xavier 7:c99fe0eb544a 353 Log_W_WidsLR[WidsNo] = LR_Flg;
MCR_Xavier 11:5ec2507020da 354 Log_Count_R[WidsNo] = Count_R;
MCR_Xavier 7:c99fe0eb544a 355 Log_W_Wids[WidsNo][0] = W_WidsR;
MCR_Xavier 11:5ec2507020da 356 //Log_W_Wids[WidsNo][1] = EnCountI;
MCR_Xavier 12:2cb9082e2d40 357 LLog_tmp[WidsNo] = EnCount;
MCR_Xavier 11:5ec2507020da 358 Log_W_Wids[WidsNo][1] = EnCount_1R;
MCR_Xavier 7:c99fe0eb544a 359 WidsNo++;
MCR_Xavier 5:20bb1e34b354 360 }
MCR_Xavier 5:20bb1e34b354 361 if(L_LineCnt >= 2 && !L_LineFlg ){
MCR_Xavier 5:20bb1e34b354 362 L_LineFlg = 1;
MCR_Xavier 5:20bb1e34b354 363 W_WidsLCnt = EnCountI;
MCR_Xavier 5:20bb1e34b354 364 }
MCR_Xavier 6:32698402be64 365 else if(!L_LineCnt && L_LineFlg && L_LineCntF >= 4 ){
MCR_Xavier 5:20bb1e34b354 366 L_LineFlg = 0;
MCR_Xavier 7:c99fe0eb544a 367 LR_Flg = 0;
MCR_Xavier 5:20bb1e34b354 368 W_WidsL = EnCountI - W_WidsLCnt;
MCR_Xavier 7:c99fe0eb544a 369 Log_W_WidsLR[WidsNo] = LR_Flg;
MCR_Xavier 11:5ec2507020da 370 Log_Count_R[WidsNo] = Count_R;
MCR_Xavier 7:c99fe0eb544a 371 Log_W_Wids[WidsNo][0] = W_WidsL;
MCR_Xavier 11:5ec2507020da 372 //Log_W_Wids[WidsNo][1] = EnCountI;
MCR_Xavier 12:2cb9082e2d40 373 LLog_tmp[WidsNo] = EnCount;
MCR_Xavier 11:5ec2507020da 374 Log_W_Wids[WidsNo][1] = EnCount_1R;
MCR_Xavier 7:c99fe0eb544a 375 WidsNo++;
MCR_Xavier 5:20bb1e34b354 376 }
MCR_Xavier 5:20bb1e34b354 377 }
MCR_Xavier 7:c99fe0eb544a 378
MCR_Xavier 5:20bb1e34b354 379 //----------モータ総合管理--------------------
MCR_Xavier 5:20bb1e34b354 380 void MotorCtrl(){
MCR_Xavier 11:5ec2507020da 381 if(ErrSt >= MotorStopCnt){ //異常判定
MCR_Xavier 5:20bb1e34b354 382 PWM_CH1.pulsewidth_us(0); //操舵PWM　(0~100)
MCR_Xavier 5:20bb1e34b354 383 PWM_CH2.pulsewidth_us(0); //前左PWM　(0~1000)
MCR_Xavier 5:20bb1e34b354 384 PWM_CH3.pulsewidth_us(0); //前右PWM　(0~1000)
MCR_Xavier 5:20bb1e34b354 385 PWM_CH4.pulsewidth_us(0); //後右PWM　(0~1000)
MCR_Xavier 5:20bb1e34b354 386 PWM_CH5.pulsewidth_us(0); //後左PWM　(0~1000)
MCR_Xavier 5:20bb1e34b354 387 PWM_CH6.pulsewidth_us(0); //槍舵PWM　(0~100)
MCR_Xavier 5:20bb1e34b354 388 OTH_DA;
MCR_Xavier 5:20bb1e34b354 389 }
MCR_Xavier 5:20bb1e34b354 390 else{
MCR_Xavier 11:5ec2507020da 391 if(FL_PWM >= 1000) FL_PWM = 1000;
MCR_Xavier 11:5ec2507020da 392 if(FR_PWM >= 1000) FR_PWM = 1000;
MCR_Xavier 11:5ec2507020da 393 if(RR_PWM >= 1000) RR_PWM = 1000;
MCR_Xavier 11:5ec2507020da 394 if(RL_PWM >= 1000) RL_PWM = 1000;
MCR_Xavier 5:20bb1e34b354 395 PWM_CH1.pulsewidth_us(ST_PWM); //槍PWM　　(0~100)
MCR_Xavier 5:20bb1e34b354 396 PWM_CH2.pulsewidth_us(FL_PWM); //前左PWM　(0~1000)
MCR_Xavier 5:20bb1e34b354 397 PWM_CH3.pulsewidth_us(FR_PWM); //前右PWM　(0~1000)
MCR_Xavier 5:20bb1e34b354 398 PWM_CH4.pulsewidth_us(RR_PWM); //後右PWM　(0~1000)
MCR_Xavier 5:20bb1e34b354 399 PWM_CH5.pulsewidth_us(RL_PWM); //後左PWM　(0~1000)
MCR_Xavier 5:20bb1e34b354 400 PWM_CH6.pulsewidth_us(LA_PWM); //操舵PWM　(0~100)
MCR_Xavier 5:20bb1e34b354 401 Rev_CH1 = ST_Rev; //槍回転方向　（H：CW）
MCR_Xavier 5:20bb1e34b354 402 Rev_CH2 = !FL_Rev; //前左回転方向（L：FW）
MCR_Xavier 5:20bb1e34b354 403 Rev_CH3 = FR_Rev; //前右回転方向（H：FW）
MCR_Xavier 5:20bb1e34b354 404 Rev_CH4 = RR_Rev; //後右回転方向（H：FW）
MCR_Xavier 5:20bb1e34b354 405 Rev_CH5 = !RL_Rev; //後左回転方向（L：FW）
MCR_Xavier 5:20bb1e34b354 406 Rev_CH6 = LA_Rev; //操舵回転方向（H：CW）
MCR_Xavier 5:20bb1e34b354 407 }
MCR_Xavier 5:20bb1e34b354 408 }
MCR_Xavier 5:20bb1e34b354 409 //----------速度制御--------------------
MCR_Xavier 5:20bb1e34b354 410 void SpeedCtrl(int V){
MCR_Xavier 11:5ec2507020da 411 long dV;
MCR_Xavier 11:5ec2507020da 412 int PT,SpeedBuf;
MCR_Xavier 11:5ec2507020da 413
MCR_Xavier 11:5ec2507020da 414 if(SpeedBuf != V) iV = 0;
MCR_Xavier 11:5ec2507020da 415 SpeedBuf = V;
MCR_Xavier 5:20bb1e34b354 416 dV=EnCount*32;
MCR_Xavier 11:5ec2507020da 417 if(ErrSt >= AccelStopCnt ){
MCR_Xavier 11:5ec2507020da 418 iV = 0;
MCR_Xavier 11:5ec2507020da 419 V = 0;}
MCR_Xavier 11:5ec2507020da 420 dV = (V - dV)*8;
MCR_Xavier 11:5ec2507020da 421 if(dV == 0) iV = 0;
MCR_Xavier 11:5ec2507020da 422 iV = iV + (dV/1000);
MCR_Xavier 11:5ec2507020da 423 dV = dV + iV;
MCR_Xavier 5:20bb1e34b354 424 if(dV <= 0){
MCR_Xavier 5:20bb1e34b354 425 dV=dV*(-1);
MCR_Xavier 5:20bb1e34b354 426 FL_Rev=FR_Rev=RR_Rev=RL_Rev=BW;
MCR_Xavier 5:20bb1e34b354 427 }
MCR_Xavier 11:5ec2507020da 428 else FL_Rev=FR_Rev=RR_Rev=RL_Rev=FW;
MCR_Xavier 7:c99fe0eb544a 429 if(dV>=1000) dV=1000;
MCR_Xavier 5:20bb1e34b354 430 if(Pot_F < 0){
MCR_Xavier 5:20bb1e34b354 431 PT = Pot_F * (-1);
MCR_Xavier 11:5ec2507020da 432 //PT = int (PT*2);
MCR_Xavier 8:352404951de8 433 if(PT >= 50 ) PT=50;
MCR_Xavier 11:5ec2507020da 434 //if(!DefFlg) PT = 0;
MCR_Xavier 5:20bb1e34b354 435 FL_PWM = int( dV * Motor_TableA[PT]);
MCR_Xavier 5:20bb1e34b354 436 FR_PWM = int( dV * Motor_TableB[PT]);
MCR_Xavier 5:20bb1e34b354 437 RL_PWM = int( dV * Motor_TableC[PT]);
MCR_Xavier 5:20bb1e34b354 438 RR_PWM = int( dV * Motor_TableD[PT]);
MCR_Xavier 5:20bb1e34b354 439 }
MCR_Xavier 5:20bb1e34b354 440 else{
MCR_Xavier 5:20bb1e34b354 441 PT = Pot_F;
MCR_Xavier 11:5ec2507020da 442 //PT = int (PT*2);
MCR_Xavier 8:352404951de8 443 if(PT >= 50 ) PT=50;
MCR_Xavier 11:5ec2507020da 444 //if(!DefFlg) PT = 0;
MCR_Xavier 5:20bb1e34b354 445 FR_PWM = int( dV * Motor_TableA[PT]);
MCR_Xavier 5:20bb1e34b354 446 FL_PWM = int( dV * Motor_TableB[PT]);
MCR_Xavier 5:20bb1e34b354 447 RR_PWM = int( dV * Motor_TableC[PT]);
MCR_Xavier 5:20bb1e34b354 448 RL_PWM = int( dV * Motor_TableD[PT]);
MCR_Xavier 5:20bb1e34b354 449 }
screamer 0:525c842a3c89 450
MCR_Xavier 5:20bb1e34b354 451 }
MCR_Xavier 5:20bb1e34b354 452 //----------槍角度制御--------------------
MCR_Xavier 5:20bb1e34b354 453 void LanceAng(int Ang){
MCR_Xavier 6:32698402be64 454 //int dAng;
MCR_Xavier 11:5ec2507020da 455
MCR_Xavier 5:20bb1e34b354 456 dAng = Pot_R - (DRP + (Ang * 3));
MCR_Xavier 5:20bb1e34b354 457 if(dAng <= 0){ //CW
MCR_Xavier 5:20bb1e34b354 458 iAng = 0;
MCR_Xavier 5:20bb1e34b354 459 LA_Rev = 1;
MCR_Xavier 5:20bb1e34b354 460 dAng = dAng * (-1);
MCR_Xavier 5:20bb1e34b354 461 }
MCR_Xavier 5:20bb1e34b354 462 else{ //CCW
MCR_Xavier 5:20bb1e34b354 463 LA_Rev = 0;
MCR_Xavier 5:20bb1e34b354 464 }
MCR_Xavier 11:5ec2507020da 465 dAng = dAng * 1;
MCR_Xavier 11:5ec2507020da 466 iAng = iAng + (dAng/50);
MCR_Xavier 11:5ec2507020da 467 dAng = dAng + iAng;
MCR_Xavier 5:20bb1e34b354 468 if(dAng >= 60) dAng = 60;
MCR_Xavier 11:5ec2507020da 469 if(LancFree_Flg && dAng >= 3)dAng = 3;
MCR_Xavier 11:5ec2507020da 470
MCR_Xavier 11:5ec2507020da 471 if(Ang >= 400) dAng=60;
MCR_Xavier 5:20bb1e34b354 472 LA_PWM = dAng;
MCR_Xavier 5:20bb1e34b354 473 }
MCR_Xavier 9:8ab0d3e46c3c 474 //------ステアリングコントロール-----------
MCR_Xavier 9:8ab0d3e46c3c 475 void SteerCtrl(char STMode){
MCR_Xavier 11:5ec2507020da 476 if(STMode == ANGLE) AngleCtrl(STAngle);
MCR_Xavier 9:8ab0d3e46c3c 477 else LineTrace();
MCR_Xavier 9:8ab0d3e46c3c 478 }
MCR_Xavier 9:8ab0d3e46c3c 479
MCR_Xavier 9:8ab0d3e46c3c 480 //-----------角度コントロール-------------
MCR_Xavier 9:8ab0d3e46c3c 481 void AngleCtrl(int STAngle){
MCR_Xavier 9:8ab0d3e46c3c 482 int Angle_V=0;
MCR_Xavier 11:5ec2507020da 483
MCR_Xavier 11:5ec2507020da 484 Angle_P=((STAngle*3) - Pot_F2)*(200);
MCR_Xavier 11:5ec2507020da 485 Angle_I = Angle_I + (Angle_P / (70));
MCR_Xavier 11:5ec2507020da 486 if(Angle_I >= 10000) Angle_I = 10000;
MCR_Xavier 11:5ec2507020da 487 if(Angle_I <= (-10000)) Angle_I = (-10000);
MCR_Xavier 9:8ab0d3e46c3c 488 Angle_D = Angle_B - Angle_P;
MCR_Xavier 9:8ab0d3e46c3c 489 Angle_B = Angle_P;
MCR_Xavier 11:5ec2507020da 490 Angle_V = Angle_P + (Angle_I ) - (Angle_D *0);
MCR_Xavier 11:5ec2507020da 491 Angle_V = Angle_V / 100;
MCR_Xavier 11:5ec2507020da 492
MCR_Xavier 11:5ec2507020da 493 /*Angle_P=((STAngle*3) - Pot_F2);
MCR_Xavier 11:5ec2507020da 494 Angle_D = Angle_B - Angle_P;
MCR_Xavier 11:5ec2507020da 495 Angle_B = Angle_P;
MCR_Xavier 11:5ec2507020da 496 //Angle_P = Angle_P*(100+(10*RSW_Value));
MCR_Xavier 9:8ab0d3e46c3c 497 Angle_I = Angle_I + (Angle_P / (10*RSW_Value));
MCR_Xavier 9:8ab0d3e46c3c 498 if(Angle_I >= 10000) Angle_I = 10000;
MCR_Xavier 9:8ab0d3e46c3c 499 if(Angle_I <= (-10000)) Angle_I = (-10000);
MCR_Xavier 11:5ec2507020da 500 Angle_V = Angle_P + (Angle_I ) - (Angle_D * 0);
MCR_Xavier 11:5ec2507020da 501 Angle_V = Angle_V / 100;*/
MCR_Xavier 9:8ab0d3e46c3c 502
MCR_Xavier 9:8ab0d3e46c3c 503 if(Angle_V <= -1){
MCR_Xavier 9:8ab0d3e46c3c 504 Angle_V = Angle_V*(-1);
MCR_Xavier 9:8ab0d3e46c3c 505 ST_Rev=0;
MCR_Xavier 9:8ab0d3e46c3c 506 if(!Turn_Flg && Angle_V >= 10) Angle_I = 0;
MCR_Xavier 9:8ab0d3e46c3c 507 Turn_Flg=1;
MCR_Xavier 9:8ab0d3e46c3c 508 }
MCR_Xavier 9:8ab0d3e46c3c 509 else {
MCR_Xavier 9:8ab0d3e46c3c 510 ST_Rev=1;
MCR_Xavier 9:8ab0d3e46c3c 511 if(Turn_Flg && Angle_V >= 10) Angle_I = 0;
MCR_Xavier 9:8ab0d3e46c3c 512 Turn_Flg=0;
MCR_Xavier 9:8ab0d3e46c3c 513 }
MCR_Xavier 7:c99fe0eb544a 514
MCR_Xavier 9:8ab0d3e46c3c 515 if(Angle_V >= 100) Angle_V=100;
MCR_Xavier 9:8ab0d3e46c3c 516 ST_PWM=Angle_V;
MCR_Xavier 9:8ab0d3e46c3c 517 }
MCR_Xavier 5:20bb1e34b354 518 //----------ライントレース---------------
MCR_Xavier 5:20bb1e34b354 519 void LineTrace(){
MCR_Xavier 5:20bb1e34b354 520 int Trace_V;
MCR_Xavier 11:5ec2507020da 521 if(SteerMode == WTRACE) TraceI=((DSensData[4]+DSensData[5]+DSensData[6]+DSensData[7])-(DSensData[3]+DSensData[2]+DSensData[1]+DSensData[0]));
MCR_Xavier 11:5ec2507020da 522 else TraceI=((DSensData[4]+DSensData[5])-(DSensData[3]+DSensData[2]));
MCR_Xavier 8:352404951de8 523 /* if(TraceI <= -1){
MCR_Xavier 5:20bb1e34b354 524 TraceI = TraceI*(-1);
MCR_Xavier 5:20bb1e34b354 525 ST_Rev=1;
MCR_Xavier 5:20bb1e34b354 526 if(!Turn_Flg) Trace_I = 0;
MCR_Xavier 5:20bb1e34b354 527 Turn_Flg=1;
MCR_Xavier 5:20bb1e34b354 528 }
MCR_Xavier 5:20bb1e34b354 529 else {
MCR_Xavier 5:20bb1e34b354 530 ST_Rev=0;
MCR_Xavier 5:20bb1e34b354 531 if(Turn_Flg) Trace_I = 0;
MCR_Xavier 5:20bb1e34b354 532 Turn_Flg=0;
MCR_Xavier 8:352404951de8 533 }*/
MCR_Xavier 11:5ec2507020da 534 Trace_P = TraceI * (10);
MCR_Xavier 5:20bb1e34b354 535 Trace_I = Trace_I + (Trace_P / (400));
MCR_Xavier 8:352404951de8 536 if(Trace_I >= 10000) Trace_I = 10000;
MCR_Xavier 8:352404951de8 537 if(Trace_I <= (-10000)) Trace_I = (-10000);
MCR_Xavier 5:20bb1e34b354 538 Trace_D = Trace_B - TraceI;
MCR_Xavier 5:20bb1e34b354 539 Trace_B = TraceI;
MCR_Xavier 8:352404951de8 540 Trace_V = Trace_P + Trace_I - (Trace_D * 50);
MCR_Xavier 5:20bb1e34b354 541 Trace_V = Trace_V / 100;
MCR_Xavier 8:352404951de8 542
MCR_Xavier 8:352404951de8 543 if(Trace_V <= -1){
MCR_Xavier 8:352404951de8 544 Trace_V = Trace_V*(-1);
MCR_Xavier 8:352404951de8 545 ST_Rev=1;
MCR_Xavier 8:352404951de8 546 if(!Turn_Flg && Trace_V >= 10) Trace_I = 0;
MCR_Xavier 8:352404951de8 547 Turn_Flg=1;
MCR_Xavier 8:352404951de8 548 }
MCR_Xavier 8:352404951de8 549 else {
MCR_Xavier 8:352404951de8 550 ST_Rev=0;
MCR_Xavier 8:352404951de8 551 if(Turn_Flg && Trace_V >= 10) Trace_I = 0;
MCR_Xavier 8:352404951de8 552 Turn_Flg=0;
MCR_Xavier 8:352404951de8 553 }
MCR_Xavier 8:352404951de8 554
MCR_Xavier 5:20bb1e34b354 555 if(Trace_V >= 100) Trace_V=100;
MCR_Xavier 5:20bb1e34b354 556 ST_PWM=Trace_V;
MCR_Xavier 5:20bb1e34b354 557 }
MCR_Xavier 10:adefba925268 558 //----------コース上自己位置推定---------------
MCR_Xavier 10:adefba925268 559 void Localization(void){
MCR_Xavier 5:20bb1e34b354 560
MCR_Xavier 11:5ec2507020da 561 if(Count_R == 0 && W_WidsR >= linewide){ //右1
MCR_Xavier 10:adefba925268 562 WWIDCLR;
MCR_Xavier 10:adefba925268 563 EnCount_1R =0;
MCR_Xavier 10:adefba925268 564 Count_R++;}
MCR_Xavier 11:5ec2507020da 565 else if(!Count_R && EnCount_1R >= 10000) Count_R++;
MCR_Xavier 10:adefba925268 566
MCR_Xavier 10:adefba925268 567 if(Count_R == 1 && W_WidsR >= linewide && EnCount_1R >= 54000){ //右2
MCR_Xavier 10:adefba925268 568 WWIDCLR;
MCR_Xavier 10:adefba925268 569 EnCount_1R = 57000;
MCR_Xavier 10:adefba925268 570 Count_R++;}
MCR_Xavier 10:adefba925268 571 else if(Count_R == 1 && EnCount_1R >= 60000) Count_R++;
MCR_Xavier 10:adefba925268 572
MCR_Xavier 11:5ec2507020da 573 if(Count_R == 2 && EnCount_1R >= 80000 && Shortcut_Flg ){ //予備減速
MCR_Xavier 11:5ec2507020da 574 Count_R++;
MCR_Xavier 11:5ec2507020da 575 EnCount_1R = 130000;
MCR_Xavier 11:5ec2507020da 576 }
MCR_Xavier 11:5ec2507020da 577 else if(Count_R == 2 && EnCount_1R >= 130000)Count_R++; //予備減速
MCR_Xavier 10:adefba925268 578
MCR_Xavier 11:5ec2507020da 579 if(Count_R == 3 && EnCount_1R >= 150000 && ( W_WidsR >= linewide || Shortcut_Flg )){ //コーナ1IN
MCR_Xavier 10:adefba925268 580 WWIDCLR;
MCR_Xavier 10:adefba925268 581 EnCount_1R = 153000;
MCR_Xavier 10:adefba925268 582 Count_R++;}
MCR_Xavier 10:adefba925268 583 else if(Count_R == 3 && EnCount_1R >= 156000) Count_R++;
MCR_Xavier 10:adefba925268 584
MCR_Xavier 11:5ec2507020da 585 if(Count_R == 4 && EnCount_1R >= 205000 && W_WidsR >= linewide && !Shortcut_Flg){ //コーナ1OUT
MCR_Xavier 10:adefba925268 586 WWIDCLR;
MCR_Xavier 10:adefba925268 587 EnCount_1R = 208000;
MCR_Xavier 10:adefba925268 588 Count_R++;}
MCR_Xavier 11:5ec2507020da 589 else if(Count_R == 4 && EnCount_1R >= 221000 && !Shortcut_Flg ) Count_R++;
MCR_Xavier 11:5ec2507020da 590 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 ){
MCR_Xavier 11:5ec2507020da 591 SteerMode = WTRACE;
MCR_Xavier 11:5ec2507020da 592 WWIDCLR;
MCR_Xavier 11:5ec2507020da 593 EnCount_1R = 239000;
MCR_Xavier 11:5ec2507020da 594 Count_R++;}
MCR_Xavier 11:5ec2507020da 595 else if(Count_R == 4 && EnCount_1R >= 200000 && Shortcut_Flg ) {
MCR_Xavier 11:5ec2507020da 596 SteerMode = TRACE;
MCR_Xavier 11:5ec2507020da 597 WWIDCLR;
MCR_Xavier 11:5ec2507020da 598 ErrSt = 1000;
MCR_Xavier 11:5ec2507020da 599 EnCount_1R = 239000;
MCR_Xavier 11:5ec2507020da 600 Count_R++;}
MCR_Xavier 11:5ec2507020da 601
MCR_Xavier 10:adefba925268 602
MCR_Xavier 11:5ec2507020da 603 if(Count_R == 5 && EnCount_1R >= 239000 && ( W_WidsR >= linewide || Shortcut_Flg)){ //右3
MCR_Xavier 10:adefba925268 604 WWIDCLR;
MCR_Xavier 10:adefba925268 605 EnCount_1R = 242000;
MCR_Xavier 10:adefba925268 606 Count_R++;}
MCR_Xavier 10:adefba925268 607 else if(Count_R == 5 && EnCount_1R >= 245000) Count_R++;
MCR_Xavier 10:adefba925268 608
MCR_Xavier 11:5ec2507020da 609 if(Count_R == 6 && W_WidsL >= linewide && EnCount_1R >= 290000){ //左1
MCR_Xavier 10:adefba925268 610 WWIDCLR;
MCR_Xavier 10:adefba925268 611 EnCount_1R = 299000;
MCR_Xavier 10:adefba925268 612 Count_R++;}
MCR_Xavier 10:adefba925268 613 else if(Count_R == 6 && EnCount_1R >= 302000) Count_R++;
MCR_Xavier 10:adefba925268 614
MCR_Xavier 11:5ec2507020da 615 if(Count_R == 7 && W_WidsR >= linewide && EnCount_1R >= 330000){ //右3
MCR_Xavier 10:adefba925268 616 WWIDCLR;
MCR_Xavier 10:adefba925268 617 EnCount_1R = 342000;
MCR_Xavier 10:adefba925268 618 Count_R++;}
MCR_Xavier 11:5ec2507020da 619 else if(Count_R == 7 && EnCount_1R >= 350000) Count_R++;
MCR_Xavier 10:adefba925268 620
MCR_Xavier 11:5ec2507020da 621 if(Count_R == 8 && W_WidsL >= linewide && EnCount_1R >= 380000){ //左2
MCR_Xavier 10:adefba925268 622 WWIDCLR;
MCR_Xavier 10:adefba925268 623 EnCount_1R = 385000;
MCR_Xavier 10:adefba925268 624 Count_R++;}
MCR_Xavier 10:adefba925268 625 else if(Count_R == 8 && EnCount_1R >= 388000) Count_R++;
MCR_Xavier 10:adefba925268 626
MCR_Xavier 10:adefba925268 627 if(Count_R == 9 && EnCount_1R >= 400000)Count_R++; //予備減速
MCR_Xavier 10:adefba925268 628
MCR_Xavier 10:adefba925268 629 if(Count_R == 10 && W_WidsR >= linewide && EnCount_1R >= 422000){ //コーナ1IN
MCR_Xavier 10:adefba925268 630 WWIDCLR;
MCR_Xavier 10:adefba925268 631 EnCount_1R = 425000;
MCR_Xavier 10:adefba925268 632 Count_R++;}
MCR_Xavier 10:adefba925268 633 else if(Count_R == 10 && EnCount_1R >= 428000) Count_R++;
MCR_Xavier 10:adefba925268 634
MCR_Xavier 11:5ec2507020da 635 if(Count_R == 11 && W_WidsR >= linewide && EnCount_1R >= 470000 && R_No != R_Go){ //コーナ1OUT
MCR_Xavier 10:adefba925268 636 WWIDCLR;
MCR_Xavier 10:adefba925268 637 EnCount_1R = 480000;
MCR_Xavier 10:adefba925268 638 Count_R++;}
MCR_Xavier 11:5ec2507020da 639 else if(Count_R == 11 && EnCount_1R >= 483000 && R_No != R_Go) Count_R++;
MCR_Xavier 10:adefba925268 640
MCR_Xavier 11:5ec2507020da 641 if(Count_R >= 12 && W_WidsR >= linewide && EnCount_1R >= 500000){ //右1
MCR_Xavier 10:adefba925268 642 Count_C++;
MCR_Xavier 10:adefba925268 643 WWIDCLR;
MCR_Xavier 10:adefba925268 644 EnCount_1R =0;
MCR_Xavier 11:5ec2507020da 645 Count_R = 1;
MCR_Xavier 11:5ec2507020da 646 R_No ++;}
MCR_Xavier 11:5ec2507020da 647 else if(Count_R >= 12 && EnCount_1R >= 550000){
MCR_Xavier 10:adefba925268 648 Count_C++;
MCR_Xavier 10:adefba925268 649 EnCount_1R = 3000;
MCR_Xavier 11:5ec2507020da 650 Count_R = 1;
MCR_Xavier 11:5ec2507020da 651 R_No ++;}
MCR_Xavier 10:adefba925268 652
MCR_Xavier 10:adefba925268 653 }
MCR_Xavier 5:20bb1e34b354 654 //----------タイマー割り込み---------------
MCR_Xavier 5:20bb1e34b354 655 void flip(){
MCR_Xavier 5:20bb1e34b354 656 timer1++;
MCR_Xavier 11:5ec2507020da 657 timer2++;
MCR_Xavier 11:5ec2507020da 658 //Log_timer++;
MCR_Xavier 11:5ec2507020da 659 ErrChk();
MCR_Xavier 9:8ab0d3e46c3c 660 Get_Pot();
MCR_Xavier 5:20bb1e34b354 661 DSens();
MCR_Xavier 9:8ab0d3e46c3c 662 SteerCtrl(SteerMode);
MCR_Xavier 11:5ec2507020da 663 if(!TestDrive_Flg)SpeedCtrl(Speed);
MCR_Xavier 5:20bb1e34b354 664 MotorCtrl();
MCR_Xavier 5:20bb1e34b354 665 Line_Dec();
MCR_Xavier 6:32698402be64 666 LanceAng(Angle);
MCR_Xavier 11:5ec2507020da 667 //if(ModeSW()==0 && !SW_W) SD_Mount();
MCR_Xavier 11:5ec2507020da 668 //if(ModeSW()==1 && !SW_W) SD_Unmount();
MCR_Xavier 7:c99fe0eb544a 669 enCalc();
MCR_Xavier 11:5ec2507020da 670 //if(Log_timer >= 9 && writeflg && Tmpcnt <= 5999) TmpLog();
MCR_Xavier 7:c99fe0eb544a 671 //if(timer1 >= 1000 && !SW_W && !SW_B && !DebugFlg) Debug();
MCR_Xavier 5:20bb1e34b354 672 //ST_EN;
MCR_Xavier 5:20bb1e34b354 673 /*if(ModeSW()==2){
MCR_Xavier 5:20bb1e34b354 674 ST_EN;
MCR_Xavier 5:20bb1e34b354 675 //OTH_EN;
MCR_Xavier 5:20bb1e34b354 676 Angle = Pot_F + 90;
MCR_Xavier 5:20bb1e34b354 677 }
MCR_Xavier 5:20bb1e34b354 678 else if(ModeSW()==0) {
MCR_Xavier 5:20bb1e34b354 679 ST_DA;
MCR_Xavier 5:20bb1e34b354 680 OTH_DA;
MCR_Xavier 5:20bb1e34b354 681 }*/
MCR_Xavier 7:c99fe0eb544a 682
MCR_Xavier 7:c99fe0eb544a 683 }
MCR_Xavier 7:c99fe0eb544a 684 //----------Tmpログモード--------------------
MCR_Xavier 7:c99fe0eb544a 685 void TmpLog(){
MCR_Xavier 7:c99fe0eb544a 686
MCR_Xavier 11:5ec2507020da 687 LLog_tmp[Tmpcnt] = EnCount;
MCR_Xavier 11:5ec2507020da 688 /*Log_tmp[0][Tmpcnt] = Count_R;
MCR_Xavier 11:5ec2507020da 689 Log_tmp[1][Tmpcnt] = Count_R;
MCR_Xavier 11:5ec2507020da 690 Log_tmp[2][Tmpcnt] = W_WidsR;
MCR_Xavier 11:5ec2507020da 691 Log_tmp[3][Tmpcnt] = W_WidsL;
MCR_Xavier 11:5ec2507020da 692 Log_tmp[4][Tmpcnt] = DSensData[3];
MCR_Xavier 11:5ec2507020da 693 Log_tmp[5][Tmpcnt] = DSensData[4];
MCR_Xavier 11:5ec2507020da 694 Log_tmp[6][Tmpcnt] = Pot_R>>2;
MCR_Xavier 11:5ec2507020da 695 Log_tmp[7][Tmpcnt] = 0;
MCR_Xavier 11:5ec2507020da 696 Log_tmp[8][Tmpcnt] = 0;
MCR_Xavier 11:5ec2507020da 697 Log_tmp[9][Tmpcnt] = 0;*/
MCR_Xavier 7:c99fe0eb544a 698 Tmpcnt++;
MCR_Xavier 11:5ec2507020da 699 Log_timer = 0;
MCR_Xavier 7:c99fe0eb544a 700 }
MCR_Xavier 7:c99fe0eb544a 701 //----------SDログモード--------------------
MCR_Xavier 7:c99fe0eb544a 702 void SDLog(){
MCR_Xavier 7:c99fe0eb544a 703 //char i=0;
MCR_Xavier 7:c99fe0eb544a 704 if( SD_Flag & 0x02){
MCR_Xavier 7:c99fe0eb544a 705 //fprintf(fp, "%-10d%-10d\r\n",EnCount,EnCountI);
MCR_Xavier 7:c99fe0eb544a 706 //fprintf(fp, "%-4d%-4d%-4d%-4d\r\n",Log_table[0][0],Log_table[0][1],Log_table[0][6],Log_table[0][7]);
MCR_Xavier 7:c99fe0eb544a 707 //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]);
MCR_Xavier 7:c99fe0eb544a 708 /*for(i=0;i<=10;i++){
MCR_Xavier 7:c99fe0eb544a 709 fprintf(fp, "%-4d%-4d%-4d%-4d\r\n",Log_table[i][0],Log_table[i][1],Log_table[i][6],Log_table[i][7]);
MCR_Xavier 7:c99fe0eb544a 710 }*/
MCR_Xavier 7:c99fe0eb544a 711 }
MCR_Xavier 7:c99fe0eb544a 712 else Log_timer=0;
MCR_Xavier 7:c99fe0eb544a 713
MCR_Xavier 7:c99fe0eb544a 714 }
MCR_Xavier 7:c99fe0eb544a 715 //----------SD書き込みシーケンス--------------------
MCR_Xavier 7:c99fe0eb544a 716 void SD_WriteSq(){
MCR_Xavier 7:c99fe0eb544a 717 int i=0;
MCR_Xavier 7:c99fe0eb544a 718 if( SD_Flag & 0x02){
MCR_Xavier 7:c99fe0eb544a 719 //fprintf(fp, "\r\nLineWidsL\r\n");
MCR_Xavier 7:c99fe0eb544a 720 fprintf(fp, "EncountI time \r\n");
MCR_Xavier 11:5ec2507020da 721 fprintf(fp, "%-10d%-10d%-10d\r\n",EnCountI,timer1,Count_C);
MCR_Xavier 11:5ec2507020da 722 fprintf(fp, "Encount PWM1 PWM2 PWM3 PWM4 F_Pot R_Pot \r\n");
MCR_Xavier 11:5ec2507020da 723 for(i=0;i<=Tmpcnt;i++){
MCR_Xavier 11:5ec2507020da 724 //fprintf(fp, "%-10d%-4d%-2d\r\n",W_WidsR[i][0],Log_W_Wids[i][1],Log_W_WidsLR[i]);
MCR_Xavier 11:5ec2507020da 725 //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]);
MCR_Xavier 11:5ec2507020da 726 //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]);
MCR_Xavier 11:5ec2507020da 727 //fprintf(fp, "%-10d\r\n",LLog_tmp[i]);
MCR_Xavier 11:5ec2507020da 728 }
MCR_Xavier 11:5ec2507020da 729 fprintf(fp, "Wids EnCount_1R WidsLR \r\n");
MCR_Xavier 11:5ec2507020da 730 for(i=0;i<=WidsNo;i++){
MCR_Xavier 12:2cb9082e2d40 731 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]);
MCR_Xavier 7:c99fe0eb544a 732 }
MCR_Xavier 7:c99fe0eb544a 733 }
MCR_Xavier 7:c99fe0eb544a 734
MCR_Xavier 5:20bb1e34b354 735 }
MCR_Xavier 5:20bb1e34b354 736 //---------SDカードをマウント-------------
MCR_Xavier 5:20bb1e34b354 737 void SD_Mount(){
MCR_Xavier 5:20bb1e34b354 738 if(SD_Flag == 0x00){
MCR_Xavier 6:32698402be64 739 led_color(RED);
MCR_Xavier 7:c99fe0eb544a 740 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 741 lcd.printf(0,"Mounting");
MCR_Xavier 5:20bb1e34b354 742 fp = fopen("/sd/Log.txt", "w");
MCR_Xavier 6:32698402be64 743 led_color(BLU);
MCR_Xavier 5:20bb1e34b354 744 SD_Flag |= 0x01;
MCR_Xavier 7:c99fe0eb544a 745 if (fp != NULL){
MCR_Xavier 7:c99fe0eb544a 746 SD_Flag |= 0x02; //success
MCR_Xavier 7:c99fe0eb544a 747 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 748 lcd.printf(0,"Success");
MCR_Xavier 7:c99fe0eb544a 749 }
MCR_Xavier 5:20bb1e34b354 750 else SD_Flag |= 0x10; //fail
MCR_Xavier 5:20bb1e34b354 751 }
MCR_Xavier 6:32698402be64 752 led_color(GRE);
MCR_Xavier 5:20bb1e34b354 753 }
MCR_Xavier 5:20bb1e34b354 754
MCR_Xavier 5:20bb1e34b354 755 //---------SDカードをアンマウント-------------
MCR_Xavier 5:20bb1e34b354 756 void SD_Unmount(){
MCR_Xavier 5:20bb1e34b354 757 if(SD_Flag & 0x01){
MCR_Xavier 7:c99fe0eb544a 758 led_color(RED);
MCR_Xavier 7:c99fe0eb544a 759 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 760 lcd.printf(0,"UnMnt...");
MCR_Xavier 7:c99fe0eb544a 761 SD_WriteSq();
MCR_Xavier 5:20bb1e34b354 762 SD_Flag = 0;
screamer 0:525c842a3c89 763 fclose(fp);
MCR_Xavier 7:c99fe0eb544a 764 led_color(WHI);
MCR_Xavier 7:c99fe0eb544a 765 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 766 lcd.printf(0,"Success");
MCR_Xavier 7:c99fe0eb544a 767 wait(1);
MCR_Xavier 5:20bb1e34b354 768 }}
MCR_Xavier 5:20bb1e34b354 769 //----------マイコン初期設定---------------
MCR_Xavier 5:20bb1e34b354 770 void init(void){
MCR_Xavier 5:20bb1e34b354 771 //I/O設定
MCR_Xavier 5:20bb1e34b354 772 RS1.mode(PullDown); //ロータリスイッチbit1
MCR_Xavier 5:20bb1e34b354 773 RS2.mode(PullDown); //ロータリスイッチbit2
MCR_Xavier 5:20bb1e34b354 774 RS3.mode(PullDown); //ロータリスイッチbit3
MCR_Xavier 5:20bb1e34b354 775 RS4.mode(PullDown); //ロータリスイッチbit4
MCR_Xavier 5:20bb1e34b354 776 SW_W.mode(PullUp); //スイッチ白
MCR_Xavier 5:20bb1e34b354 777 SW_R.mode(PullUp); //スイッチ赤
MCR_Xavier 5:20bb1e34b354 778 SW_B.mode(PullUp); //スイッチ黒
MCR_Xavier 5:20bb1e34b354 779 s0signal.rise(&s0_trigger_rise); //エンコーダA相立ち上がり
MCR_Xavier 5:20bb1e34b354 780 s0signal.fall(&s0_trigger_fall); //エンコーダA相立ち下がり
MCR_Xavier 5:20bb1e34b354 781 s1signal.rise(&s1_trigger_rise); //エンコーダB相立ち上がり
MCR_Xavier 5:20bb1e34b354 782 s1signal.fall(&s1_trigger_fall); //エンコーダB相立ち下がり
MCR_Xavier 5:20bb1e34b354 783 //割り込み処理開始
MCR_Xavier 5:20bb1e34b354 784 flipper.attach_us(&flip,1000); //汎用タイマー割り込み
MCR_Xavier 5:20bb1e34b354 785 sensget.attach_us(&sensGet,100); //センサー用タイマー割り込み
MCR_Xavier 5:20bb1e34b354 786 //PWM周期設定
MCR_Xavier 5:20bb1e34b354 787 PWM_CH1.period(0.0001);
MCR_Xavier 5:20bb1e34b354 788 PWM_CH2.period(0.001);
MCR_Xavier 5:20bb1e34b354 789 PWM_CH3.period(0.001);
MCR_Xavier 5:20bb1e34b354 790 PWM_CH4.period(0.001);
MCR_Xavier 5:20bb1e34b354 791 PWM_CH5.period(0.001);
MCR_Xavier 5:20bb1e34b354 792 PWM_CH6.period(0.0001);
MCR_Xavier 5:20bb1e34b354 793 ST_DA;
MCR_Xavier 5:20bb1e34b354 794 OTH_DA;
screamer 0:525c842a3c89 795 }
MCR_Xavier 5:20bb1e34b354 796 //----------------メイン---------------------
MCR_Xavier 10:adefba925268 797 int main(){
MCR_Xavier 5:20bb1e34b354 798 init(); //マイコン初期設定
MCR_Xavier 6:32698402be64 799 wait(1);
MCR_Xavier 5:20bb1e34b354 800 ST_EN;
MCR_Xavier 11:5ec2507020da 801 SteerMode = TRACE;
MCR_Xavier 5:20bb1e34b354 802 lcd.contrast( 0x2E ); //LCDコントラスト設定
MCR_Xavier 5:20bb1e34b354 803 wait(0.5);
MCR_Xavier 11:5ec2507020da 804 if(ModeSelect_Flg == 0) ModeSelect();
MCR_Xavier 11:5ec2507020da 805 wait(1);
MCR_Xavier 5:20bb1e34b354 806 while(SW_R){
MCR_Xavier 5:20bb1e34b354 807 if(!SW_W && !SW_B && !DebugFlg) Debug();
MCR_Xavier 5:20bb1e34b354 808 lcd.clear();
MCR_Xavier 11:5ec2507020da 809 lcd.printf(0,"%dms",TripTime);
MCR_Xavier 11:5ec2507020da 810 lcd.printf(1,"%dmm/s",SPCom);
MCR_Xavier 6:32698402be64 811 //if(!SW_W) SD_Mount();
MCR_Xavier 10:adefba925268 812 wait(0.1);
MCR_Xavier 5:20bb1e34b354 813 }
MCR_Xavier 5:20bb1e34b354 814 lcd.clear();
MCR_Xavier 5:20bb1e34b354 815 lcd.printf(0,"Ready!");
MCR_Xavier 6:32698402be64 816 lcd.printf(1,"%x",SD_Flag);
MCR_Xavier 5:20bb1e34b354 817 wait(1);
MCR_Xavier 5:20bb1e34b354 818 lcd.clear();
MCR_Xavier 5:20bb1e34b354 819 lcd.printf(0,"Go!");
MCR_Xavier 5:20bb1e34b354 820 OTH_EN;
MCR_Xavier 5:20bb1e34b354 821 W_WidsL = W_WidsR = 0;
MCR_Xavier 10:adefba925268 822 //Speed = SPCom;
MCR_Xavier 11:5ec2507020da 823 Angle = 90;
MCR_Xavier 10:adefba925268 824 Speed = SPCom;
MCR_Xavier 10:adefba925268 825 EnCount_1R = -40000;
MCR_Xavier 11:5ec2507020da 826 timer1 =0;
MCR_Xavier 11:5ec2507020da 827 StartFlg = 1;
MCR_Xavier 11:5ec2507020da 828 //writeflg = 1;
MCR_Xavier 5:20bb1e34b354 829 while(1){
MCR_Xavier 10:adefba925268 830
MCR_Xavier 11:5ec2507020da 831 if(!SW_W && !SW_B && !DebugFlg && !EnCount) Debug();
MCR_Xavier 11:5ec2507020da 832 /*if(timer2 >= 100){
MCR_Xavier 11:5ec2507020da 833 timer2 = 0;
MCR_Xavier 11:5ec2507020da 834 lcd.clear();
MCR_Xavier 11:5ec2507020da 835 lcd.printf(0,"%d",timer1);
MCR_Xavier 11:5ec2507020da 836 lcd.printf(1,"%d",ErrSt);
MCR_Xavier 11:5ec2507020da 837 }*/
MCR_Xavier 10:adefba925268 838 Localization();
MCR_Xavier 11:5ec2507020da 839 //if(Log_timer >= 10 && writeflg && Tmpcnt <= 1000) TmpLog();
MCR_Xavier 6:32698402be64 840
MCR_Xavier 10:adefba925268 841 if(Count_R == 0 ){
MCR_Xavier 10:adefba925268 842 Speed = SPCom;
MCR_Xavier 11:5ec2507020da 843 led_color(WHI);
MCR_Xavier 11:5ec2507020da 844 Angle = 90;}
MCR_Xavier 10:adefba925268 845 if(Count_R == 1 ){
MCR_Xavier 10:adefba925268 846 Speed = SPCom;
MCR_Xavier 11:5ec2507020da 847 led_color(RED);
MCR_Xavier 11:5ec2507020da 848 if(EnCount_1R >= 30000) Angle = 150;
MCR_Xavier 11:5ec2507020da 849 else if(EnCount_1R >= 13000) Angle = 180;//11000
MCR_Xavier 11:5ec2507020da 850 }
MCR_Xavier 10:adefba925268 851 if(Count_R == 2 ){
MCR_Xavier 10:adefba925268 852 Speed = SPCom;
MCR_Xavier 11:5ec2507020da 853 led_color(GRE);
MCR_Xavier 11:5ec2507020da 854 if(EnCount_1R >= 75000) Angle = 180; //75000
MCR_Xavier 11:5ec2507020da 855 else Angle = 150;
MCR_Xavier 11:5ec2507020da 856 }
MCR_Xavier 10:adefba925268 857 if(Count_R == 3 ){
MCR_Xavier 10:adefba925268 858 Speed = SPYobi;
MCR_Xavier 11:5ec2507020da 859 Angle = 90;
MCR_Xavier 10:adefba925268 860 led_color(BLU);}
MCR_Xavier 10:adefba925268 861 if(Count_R == 4 ){
MCR_Xavier 10:adefba925268 862 led_color(YEL);
MCR_Xavier 11:5ec2507020da 863 if(EnCount_1R >= 200000 && Turbo_Flg) Speed = SPCom;
MCR_Xavier 11:5ec2507020da 864 else Speed = SPCor;
MCR_Xavier 11:5ec2507020da 865 Angle = 30;
MCR_Xavier 11:5ec2507020da 866 STAngle = -27;
MCR_Xavier 11:5ec2507020da 867 //if(Shortcut_Flg)SteerMode = ANGLE;
MCR_Xavier 11:5ec2507020da 868 }
MCR_Xavier 11:5ec2507020da 869 if(Count_R == 5 ){
MCR_Xavier 10:adefba925268 870 Speed = SPCom;
MCR_Xavier 11:5ec2507020da 871 Angle = 125;
MCR_Xavier 10:adefba925268 872 led_color(RED);}
MCR_Xavier 10:adefba925268 873 if(Count_R == 6 ){
MCR_Xavier 11:5ec2507020da 874 if(Shortcut_Flg)SteerMode = WTRACE;
MCR_Xavier 10:adefba925268 875 Speed = SPCom;
MCR_Xavier 6:32698402be64 876 led_color(GRE);
MCR_Xavier 11:5ec2507020da 877 if(EnCount_1R >= 270000){
MCR_Xavier 11:5ec2507020da 878 LA_EN;
MCR_Xavier 11:5ec2507020da 879 Angle = 52;}
MCR_Xavier 11:5ec2507020da 880 else if(EnCount_1R >= 240000) LA_DA;//Angle = 130;
MCR_Xavier 6:32698402be64 881 }
MCR_Xavier 10:adefba925268 882 if(Count_R == 7 ){
MCR_Xavier 11:5ec2507020da 883 if(Shortcut_Flg)SteerMode = TRACE;
MCR_Xavier 10:adefba925268 884 Speed = SPCom;
MCR_Xavier 10:adefba925268 885 led_color(PUR);
MCR_Xavier 11:5ec2507020da 886 if(EnCount_1R >= 320000){
MCR_Xavier 11:5ec2507020da 887 LA_EN;
MCR_Xavier 11:5ec2507020da 888 Angle = 135;}
MCR_Xavier 11:5ec2507020da 889 else LA_DA;//Angle = 52;
MCR_Xavier 6:32698402be64 890 }
MCR_Xavier 10:adefba925268 891 if(Count_R == 8 ){
MCR_Xavier 10:adefba925268 892 Speed = SPCom;
MCR_Xavier 6:32698402be64 893 led_color(RED);
MCR_Xavier 11:5ec2507020da 894 if(EnCount_1R >= 365000){
MCR_Xavier 11:5ec2507020da 895 LA_EN;
MCR_Xavier 11:5ec2507020da 896 Angle = 49;}
MCR_Xavier 11:5ec2507020da 897 else LA_DA;//Angle = 132;
MCR_Xavier 6:32698402be64 898 }
MCR_Xavier 10:adefba925268 899 if(Count_R == 9 ){
MCR_Xavier 11:5ec2507020da 900 LA_EN;
MCR_Xavier 10:adefba925268 901 Speed = SPCom;
MCR_Xavier 11:5ec2507020da 902 Angle = 49;
MCR_Xavier 10:adefba925268 903 led_color(GRE);}
MCR_Xavier 10:adefba925268 904 if(Count_R == 10){
MCR_Xavier 10:adefba925268 905 Speed = SPYobi;
MCR_Xavier 10:adefba925268 906 led_color(WHI);}
MCR_Xavier 10:adefba925268 907 if(Count_R == 11){
MCR_Xavier 10:adefba925268 908 led_color(BLU);
MCR_Xavier 10:adefba925268 909 Angle = 90;
MCR_Xavier 11:5ec2507020da 910 STAngle = -6;
MCR_Xavier 11:5ec2507020da 911 if(R_No == R_Go){
MCR_Xavier 11:5ec2507020da 912 if(EnCount_1R <= 470000){
MCR_Xavier 11:5ec2507020da 913 Angle = 400;
MCR_Xavier 11:5ec2507020da 914 SteerMode = ANGLE;}
MCR_Xavier 11:5ec2507020da 915 else {
MCR_Xavier 11:5ec2507020da 916 SteerMode = TRACE;
MCR_Xavier 11:5ec2507020da 917 ErrSt = AccelStopCnt;
MCR_Xavier 11:5ec2507020da 918 if(EnCount==0)Angle = 90;
MCR_Xavier 11:5ec2507020da 919 }
MCR_Xavier 11:5ec2507020da 920 }
MCR_Xavier 11:5ec2507020da 921 if(EnCount_1R >= 470000 && Turbo_Flg) Speed = SPCom;
MCR_Xavier 11:5ec2507020da 922 else Speed = SPCor;
MCR_Xavier 11:5ec2507020da 923 }
MCR_Xavier 10:adefba925268 924 if(Count_R == 12){
MCR_Xavier 11:5ec2507020da 925 SteerMode = TRACE;
MCR_Xavier 10:adefba925268 926 Speed = SPCom;
MCR_Xavier 10:adefba925268 927 led_color(YEL);}
MCR_Xavier 10:adefba925268 928
MCR_Xavier 10:adefba925268 929 }
MCR_Xavier 7:c99fe0eb544a 930 }
MCR_Xavier 11:5ec2507020da 931
MCR_Xavier 11:5ec2507020da 932 //----------モード選択--------------------
MCR_Xavier 11:5ec2507020da 933 void ModeSelect(void){
MCR_Xavier 11:5ec2507020da 934 int DebugModeSW;
MCR_Xavier 11:5ec2507020da 935 writeflg = 0;
MCR_Xavier 11:5ec2507020da 936 ModeSelect_Flg=1;
MCR_Xavier 11:5ec2507020da 937 lcd.clear();
MCR_Xavier 11:5ec2507020da 938 lcd.printf(0,"Select");
MCR_Xavier 11:5ec2507020da 939 lcd.printf(1,"TripTime");
MCR_Xavier 11:5ec2507020da 940 wait(1);
MCR_Xavier 11:5ec2507020da 941 lcd.clear();
MCR_Xavier 11:5ec2507020da 942 while( SW_R ){
MCR_Xavier 11:5ec2507020da 943 DebugModeSW = ModeSW();
MCR_Xavier 11:5ec2507020da 944 switch(DebugModeSW){
MCR_Xavier 11:5ec2507020da 945 case 0:
MCR_Xavier 11:5ec2507020da 946 lcd.clear();
MCR_Xavier 11:5ec2507020da 947 lcd.printf(0,"Turbo_ON");
MCR_Xavier 11:5ec2507020da 948 Turbo_Flg = 1;
MCR_Xavier 11:5ec2507020da 949 wait(0.1);
MCR_Xavier 11:5ec2507020da 950 break;
MCR_Xavier 11:5ec2507020da 951 case 1:
MCR_Xavier 11:5ec2507020da 952 lcd.clear();
MCR_Xavier 11:5ec2507020da 953 lcd.printf(0,"Round30");
MCR_Xavier 11:5ec2507020da 954 TripTime =30000;
MCR_Xavier 11:5ec2507020da 955 R_Go = 7;
MCR_Xavier 11:5ec2507020da 956 wait(0.1);
MCR_Xavier 11:5ec2507020da 957 break;
MCR_Xavier 11:5ec2507020da 958 case 2:
MCR_Xavier 11:5ec2507020da 959 lcd.clear();
MCR_Xavier 11:5ec2507020da 960 lcd.printf(0,"Round60");
MCR_Xavier 11:5ec2507020da 961 TripTime =60000;
MCR_Xavier 11:5ec2507020da 962 R_Go = 15;
MCR_Xavier 11:5ec2507020da 963 wait(0.1);
MCR_Xavier 11:5ec2507020da 964 break;
MCR_Xavier 11:5ec2507020da 965 case 3:
MCR_Xavier 11:5ec2507020da 966 lcd.clear();
MCR_Xavier 11:5ec2507020da 967 lcd.printf(0,"Round10");
MCR_Xavier 11:5ec2507020da 968 TripTime =10000;
MCR_Xavier 11:5ec2507020da 969 R_Go = 2;
MCR_Xavier 11:5ec2507020da 970 wait(0.1);
MCR_Xavier 11:5ec2507020da 971 break;
MCR_Xavier 11:5ec2507020da 972 case 4:
MCR_Xavier 11:5ec2507020da 973 lcd.clear();
MCR_Xavier 11:5ec2507020da 974 lcd.printf(0,"Round20");
MCR_Xavier 11:5ec2507020da 975 TripTime =20000;
MCR_Xavier 11:5ec2507020da 976 wait(0.1);
MCR_Xavier 11:5ec2507020da 977 break;
MCR_Xavier 11:5ec2507020da 978 case 5:
MCR_Xavier 11:5ec2507020da 979 lcd.clear();
MCR_Xavier 11:5ec2507020da 980 lcd.printf(0,"Shortcut60");
MCR_Xavier 11:5ec2507020da 981 TripTime =60000;
MCR_Xavier 11:5ec2507020da 982 Shortcut_Flg = 1;
MCR_Xavier 11:5ec2507020da 983 wait(0.1);
MCR_Xavier 11:5ec2507020da 984 break;
MCR_Xavier 11:5ec2507020da 985
MCR_Xavier 11:5ec2507020da 986 default:
MCR_Xavier 11:5ec2507020da 987 lcd.clear();
MCR_Xavier 11:5ec2507020da 988 lcd.printf(0,"Select");
MCR_Xavier 11:5ec2507020da 989 lcd.printf(1,"1-5");
MCR_Xavier 11:5ec2507020da 990 wait(0.1);
MCR_Xavier 11:5ec2507020da 991 break;
MCR_Xavier 11:5ec2507020da 992 }
MCR_Xavier 11:5ec2507020da 993 }
MCR_Xavier 11:5ec2507020da 994 wait(1);
MCR_Xavier 11:5ec2507020da 995 lcd.clear();
MCR_Xavier 11:5ec2507020da 996 while( SW_R){
MCR_Xavier 11:5ec2507020da 997 DebugModeSW = ModeSW();
MCR_Xavier 11:5ec2507020da 998 switch(DebugModeSW){
MCR_Xavier 11:5ec2507020da 999 case 1:
MCR_Xavier 11:5ec2507020da 1000 lcd.clear();
MCR_Xavier 11:5ec2507020da 1001 lcd.printf(0,"STSpeed");
MCR_Xavier 11:5ec2507020da 1002 lcd.printf(1,"5m/s");
MCR_Xavier 11:5ec2507020da 1003 SPCom=5000;
MCR_Xavier 11:5ec2507020da 1004 wait(0.1);
MCR_Xavier 11:5ec2507020da 1005 break;
MCR_Xavier 11:5ec2507020da 1006 case 4:
MCR_Xavier 11:5ec2507020da 1007 lcd.clear();
MCR_Xavier 11:5ec2507020da 1008 lcd.printf(0,"STSpeed");
MCR_Xavier 11:5ec2507020da 1009 lcd.printf(1,"4.5m/s");
MCR_Xavier 11:5ec2507020da 1010 wait(1);
MCR_Xavier 11:5ec2507020da 1011 SPCom=4500;
MCR_Xavier 11:5ec2507020da 1012 SPCor=2800;
MCR_Xavier 11:5ec2507020da 1013 break;
MCR_Xavier 11:5ec2507020da 1014 case 3:
MCR_Xavier 11:5ec2507020da 1015 lcd.clear();
MCR_Xavier 11:5ec2507020da 1016 lcd.printf(0,"STSpeed");
MCR_Xavier 12:2cb9082e2d40 1017 lcd.printf(1,"6m/s");
MCR_Xavier 12:2cb9082e2d40 1018 SPCom=6000;
MCR_Xavier 11:5ec2507020da 1019 wait(0.1);
MCR_Xavier 11:5ec2507020da 1020 break;
MCR_Xavier 11:5ec2507020da 1021 case 2:
MCR_Xavier 11:5ec2507020da 1022 lcd.clear();
MCR_Xavier 11:5ec2507020da 1023 lcd.printf(0,"STSpeed");
MCR_Xavier 11:5ec2507020da 1024 lcd.printf(1,"5.2m/s");
MCR_Xavier 11:5ec2507020da 1025 SPCom=5200;
MCR_Xavier 11:5ec2507020da 1026 wait(0.1);
MCR_Xavier 11:5ec2507020da 1027 break;
MCR_Xavier 11:5ec2507020da 1028 case 5:
MCR_Xavier 11:5ec2507020da 1029 lcd.clear();
MCR_Xavier 11:5ec2507020da 1030 lcd.printf(0,"AllSpeed");
MCR_Xavier 11:5ec2507020da 1031 lcd.printf(1,"3m/s");
MCR_Xavier 11:5ec2507020da 1032 wait(1);
MCR_Xavier 11:5ec2507020da 1033 SPCom=3000;
MCR_Xavier 11:5ec2507020da 1034 SPYobi=3000;
MCR_Xavier 11:5ec2507020da 1035 SPCor=3000;
MCR_Xavier 11:5ec2507020da 1036 wait(0.1);
MCR_Xavier 11:5ec2507020da 1037 break;
MCR_Xavier 11:5ec2507020da 1038
MCR_Xavier 11:5ec2507020da 1039 case 6:
MCR_Xavier 11:5ec2507020da 1040 lcd.clear();
MCR_Xavier 11:5ec2507020da 1041 lcd.printf(0,"FreeDrive");
MCR_Xavier 11:5ec2507020da 1042 TestDrive_Flg=1;
MCR_Xavier 11:5ec2507020da 1043 wait(0.1);
MCR_Xavier 11:5ec2507020da 1044 break;
MCR_Xavier 11:5ec2507020da 1045
MCR_Xavier 11:5ec2507020da 1046 default:
MCR_Xavier 11:5ec2507020da 1047 lcd.clear();
MCR_Xavier 11:5ec2507020da 1048 lcd.printf(0,"Select");
MCR_Xavier 11:5ec2507020da 1049 lcd.printf(1,"1-6");
MCR_Xavier 11:5ec2507020da 1050 wait(0.1);
MCR_Xavier 11:5ec2507020da 1051 break;
MCR_Xavier 11:5ec2507020da 1052 }
MCR_Xavier 11:5ec2507020da 1053 }
MCR_Xavier 11:5ec2507020da 1054 }
MCR_Xavier 11:5ec2507020da 1055
MCR_Xavier 7:c99fe0eb544a 1056 //----------デバッグモード--------------------
MCR_Xavier 7:c99fe0eb544a 1057 void Debug(void){
MCR_Xavier 7:c99fe0eb544a 1058 int DebugModeSW;
MCR_Xavier 11:5ec2507020da 1059 int PotTest1,PotTest2;
MCR_Xavier 11:5ec2507020da 1060 writeflg = 0;
MCR_Xavier 7:c99fe0eb544a 1061 DebugFlg=1;
MCR_Xavier 7:c99fe0eb544a 1062 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1063 lcd.printf(0,"DBGMode");
MCR_Xavier 7:c99fe0eb544a 1064 wait(2);
MCR_Xavier 7:c99fe0eb544a 1065 while( SW_R ){
MCR_Xavier 7:c99fe0eb544a 1066 DebugModeSW = ModeSW();
MCR_Xavier 7:c99fe0eb544a 1067 switch(DebugModeSW){
MCR_Xavier 7:c99fe0eb544a 1068 case 1:
MCR_Xavier 7:c99fe0eb544a 1069 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1070 lcd.printf(0,"SensT/F");
MCR_Xavier 7:c99fe0eb544a 1071 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1072 break;
MCR_Xavier 7:c99fe0eb544a 1073 case 2:
MCR_Xavier 7:c99fe0eb544a 1074 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1075 lcd.printf(0,"SensA/D");
MCR_Xavier 7:c99fe0eb544a 1076 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1077 break;
MCR_Xavier 7:c99fe0eb544a 1078 case 3:
MCR_Xavier 7:c99fe0eb544a 1079 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1080 lcd.printf(0,"Trip");
MCR_Xavier 7:c99fe0eb544a 1081 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1082 break;
MCR_Xavier 7:c99fe0eb544a 1083 case 4:
MCR_Xavier 7:c99fe0eb544a 1084 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1085 lcd.printf(0,"LinWidLR");
MCR_Xavier 7:c99fe0eb544a 1086 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1087 break;
MCR_Xavier 7:c99fe0eb544a 1088 case 5:
MCR_Xavier 7:c99fe0eb544a 1089 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1090 lcd.printf(0,"LineCnr");
MCR_Xavier 7:c99fe0eb544a 1091 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1092 break;
MCR_Xavier 7:c99fe0eb544a 1093 case 6:
MCR_Xavier 7:c99fe0eb544a 1094 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1095 lcd.printf(0,"LineA/D");
MCR_Xavier 7:c99fe0eb544a 1096 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1097 break;
MCR_Xavier 7:c99fe0eb544a 1098 case 7:
MCR_Xavier 7:c99fe0eb544a 1099 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1100 lcd.printf(0,"PotA/D");
MCR_Xavier 7:c99fe0eb544a 1101 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1102 break;
MCR_Xavier 7:c99fe0eb544a 1103 case 8:
MCR_Xavier 7:c99fe0eb544a 1104 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1105 lcd.printf(0,"Speed5");
MCR_Xavier 7:c99fe0eb544a 1106 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1107 break;
MCR_Xavier 7:c99fe0eb544a 1108 case 9:
MCR_Xavier 7:c99fe0eb544a 1109 lcd.clear();
MCR_Xavier 11:5ec2507020da 1110 lcd.printf(0,"Speed7");
MCR_Xavier 7:c99fe0eb544a 1111 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1112 break;
MCR_Xavier 7:c99fe0eb544a 1113 case 10:
MCR_Xavier 7:c99fe0eb544a 1114 lcd.clear();
MCR_Xavier 11:5ec2507020da 1115 lcd.printf(0,"Speed2");
MCR_Xavier 7:c99fe0eb544a 1116 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1117 break;
MCR_Xavier 7:c99fe0eb544a 1118 case 11:
MCR_Xavier 7:c99fe0eb544a 1119 lcd.clear();
MCR_Xavier 11:5ec2507020da 1120 lcd.printf(0,"Speed0");
MCR_Xavier 7:c99fe0eb544a 1121 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1122 break;
MCR_Xavier 7:c99fe0eb544a 1123 case 12:
MCR_Xavier 7:c99fe0eb544a 1124 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1125 lcd.printf(0,"linewide");
MCR_Xavier 7:c99fe0eb544a 1126 lcd.printf(1,"100");
MCR_Xavier 7:c99fe0eb544a 1127 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1128 break;
MCR_Xavier 7:c99fe0eb544a 1129 case 13:
MCR_Xavier 7:c99fe0eb544a 1130 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1131 lcd.printf(0,"SD_Mount?");
MCR_Xavier 7:c99fe0eb544a 1132 lcd.printf(1,"%x",SD_Flag);
MCR_Xavier 7:c99fe0eb544a 1133 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1134 if(!SW_W) SD_Mount();
MCR_Xavier 11:5ec2507020da 1135 writeflg = 1;
MCR_Xavier 7:c99fe0eb544a 1136 break;
MCR_Xavier 7:c99fe0eb544a 1137 case 14:
MCR_Xavier 7:c99fe0eb544a 1138 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1139 lcd.printf(0,"SD_UnMount?");
MCR_Xavier 7:c99fe0eb544a 1140 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1141 if(!SW_W) SD_Unmount();
MCR_Xavier 7:c99fe0eb544a 1142 break;
MCR_Xavier 7:c99fe0eb544a 1143 case 15:
MCR_Xavier 7:c99fe0eb544a 1144 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1145 lcd.printf(0,"DSPSens");
MCR_Xavier 7:c99fe0eb544a 1146 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1147 break;
MCR_Xavier 7:c99fe0eb544a 1148 default:
MCR_Xavier 7:c99fe0eb544a 1149 break;
MCR_Xavier 7:c99fe0eb544a 1150 }
MCR_Xavier 7:c99fe0eb544a 1151 }
MCR_Xavier 11:5ec2507020da 1152 wait(1);
MCR_Xavier 7:c99fe0eb544a 1153 while( SW_R){
MCR_Xavier 7:c99fe0eb544a 1154 DebugModeSW = ModeSW();
MCR_Xavier 7:c99fe0eb544a 1155 switch(DebugModeSW){
MCR_Xavier 7:c99fe0eb544a 1156 case 1:
MCR_Xavier 7:c99fe0eb544a 1157 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1158 lcd.printf(0,"0167RL");
MCR_Xavier 7:c99fe0eb544a 1159 lcd.printf(1,"%1d%1d%1d%1d%1d%1d",TFSensData[0],TFSensData[1],TFSensData[6],TFSensData[7],R_LineFlg ,L_LineFlg);
MCR_Xavier 7:c99fe0eb544a 1160 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1161 break;
MCR_Xavier 7:c99fe0eb544a 1162 case 2:
MCR_Xavier 7:c99fe0eb544a 1163 lcd.clear();
MCR_Xavier 11:5ec2507020da 1164 //lcd.printf(0,"SensA/D");
MCR_Xavier 11:5ec2507020da 1165 lcd.printf(0,"%3d,%3d",DSensData[3],DSensData[4]);
MCR_Xavier 11:5ec2507020da 1166 lcd.printf(1,"%3d,%3d",DSensData[2],DSensData[5]);
MCR_Xavier 7:c99fe0eb544a 1167 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1168 break;
MCR_Xavier 7:c99fe0eb544a 1169 case 3:
MCR_Xavier 7:c99fe0eb544a 1170 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1171 lcd.printf(0,"%d",EnCount );
MCR_Xavier 7:c99fe0eb544a 1172 lcd.printf(1,"%d",EnCountI);
MCR_Xavier 7:c99fe0eb544a 1173 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1174 break;
MCR_Xavier 7:c99fe0eb544a 1175 case 4:
MCR_Xavier 7:c99fe0eb544a 1176 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1177 lcd.printf(0,"%d",W_WidsL );
MCR_Xavier 7:c99fe0eb544a 1178 lcd.printf(1,"%d",W_WidsR );
MCR_Xavier 7:c99fe0eb544a 1179 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1180 break;
MCR_Xavier 7:c99fe0eb544a 1181 case 5:
MCR_Xavier 7:c99fe0eb544a 1182 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1183 lcd.printf(0,"%d",R_LineCnt );
MCR_Xavier 7:c99fe0eb544a 1184 lcd.printf(1,"%d",L_LineCnt );
MCR_Xavier 7:c99fe0eb544a 1185 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1186 break;
MCR_Xavier 7:c99fe0eb544a 1187 case 6:
MCR_Xavier 7:c99fe0eb544a 1188 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1189 lcd.printf(0,"%3d,%3d",DSensData[0],DSensData[1]);
MCR_Xavier 7:c99fe0eb544a 1190 lcd.printf(1,"%3d,%3d",DSensData[6],DSensData[7]);
MCR_Xavier 7:c99fe0eb544a 1191 wait(0.5);
MCR_Xavier 7:c99fe0eb544a 1192 break;
MCR_Xavier 7:c99fe0eb544a 1193 case 7:
MCR_Xavier 11:5ec2507020da 1194 PotTest1 = (DFP-(Steer_Ptm.read_u16()>>6))/3;
MCR_Xavier 11:5ec2507020da 1195 PotTest2 = ((Lance_Ptm.read_u16()>>6)-DRP)/3;
MCR_Xavier 7:c99fe0eb544a 1196 lcd.clear();
MCR_Xavier 11:5ec2507020da 1197 lcd.printf(0,"%3d,%3d",Pot_F,PotTest1);
MCR_Xavier 11:5ec2507020da 1198 lcd.printf(1,"%3d,%3d",Pot_R,PotTest2);
MCR_Xavier 11:5ec2507020da 1199 /*lcd.printf(0,"%3d,%3d",RearPot[1],RearPot[4]);
MCR_Xavier 11:5ec2507020da 1200 lcd.printf(1,"%3d,%3d",RearPot[5],RearPot[9]);*/
MCR_Xavier 7:c99fe0eb544a 1201 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1202 break;
MCR_Xavier 7:c99fe0eb544a 1203 case 8:
MCR_Xavier 11:5ec2507020da 1204 SPCom=5000;
MCR_Xavier 7:c99fe0eb544a 1205 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1206 lcd.printf(0,"Speed5");
MCR_Xavier 11:5ec2507020da 1207 lcd.printf(1,"%d",SPCom);
MCR_Xavier 7:c99fe0eb544a 1208 wait(0.1);
MCR_Xavier 8:352404951de8 1209 //SPYobi=3500;
MCR_Xavier 8:352404951de8 1210 //SPCor=3000;
MCR_Xavier 8:352404951de8 1211 //ParamS=2000;
MCR_Xavier 7:c99fe0eb544a 1212 break;
MCR_Xavier 7:c99fe0eb544a 1213 case 9:
MCR_Xavier 11:5ec2507020da 1214 SPCom=7000;
MCR_Xavier 7:c99fe0eb544a 1215 lcd.clear();
MCR_Xavier 8:352404951de8 1216 lcd.printf(0,"Speed7");
MCR_Xavier 11:5ec2507020da 1217 lcd.printf(1,"%d",SPCom);
MCR_Xavier 7:c99fe0eb544a 1218 wait(0.1);
MCR_Xavier 8:352404951de8 1219 //SPYobi=3000;
MCR_Xavier 8:352404951de8 1220 //SPCor=3000;
MCR_Xavier 7:c99fe0eb544a 1221 break;
MCR_Xavier 7:c99fe0eb544a 1222 case 10:
MCR_Xavier 11:5ec2507020da 1223 SPCom=2000;
MCR_Xavier 11:5ec2507020da 1224 SPYobi=2000;
MCR_Xavier 11:5ec2507020da 1225 SPCor=2000;
MCR_Xavier 7:c99fe0eb544a 1226 lcd.clear();
MCR_Xavier 11:5ec2507020da 1227 lcd.printf(0,"Speed2");
MCR_Xavier 11:5ec2507020da 1228 lcd.printf(1,"%d",SPCom);
MCR_Xavier 7:c99fe0eb544a 1229 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1230 break;
MCR_Xavier 7:c99fe0eb544a 1231 case 11:
MCR_Xavier 11:5ec2507020da 1232 SPCom=0;
MCR_Xavier 11:5ec2507020da 1233 SPYobi=0;
MCR_Xavier 11:5ec2507020da 1234 SPCor=0;
MCR_Xavier 7:c99fe0eb544a 1235 lcd.clear();
MCR_Xavier 11:5ec2507020da 1236 lcd.printf(0,"Speed0");
MCR_Xavier 11:5ec2507020da 1237 lcd.printf(1,"%d",SPCom);
MCR_Xavier 7:c99fe0eb544a 1238 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1239 break;
MCR_Xavier 7:c99fe0eb544a 1240 case 12:
MCR_Xavier 7:c99fe0eb544a 1241 lcd.clear();
MCR_Xavier 11:5ec2507020da 1242 lcd.printf(0,"R_No");
MCR_Xavier 11:5ec2507020da 1243 lcd.printf(1,"%d",R_No);
MCR_Xavier 7:c99fe0eb544a 1244 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1245 linewide=100;
MCR_Xavier 7:c99fe0eb544a 1246 break;
MCR_Xavier 11:5ec2507020da 1247 case 13:
MCR_Xavier 11:5ec2507020da 1248 lcd.clear();
MCR_Xavier 11:5ec2507020da 1249 lcd.printf(0,"ErrSt");
MCR_Xavier 11:5ec2507020da 1250 lcd.printf(1,"%d",ErrSt);
MCR_Xavier 11:5ec2507020da 1251 wait(0.1);
MCR_Xavier 11:5ec2507020da 1252 break;
MCR_Xavier 7:c99fe0eb544a 1253 case 15:
MCR_Xavier 7:c99fe0eb544a 1254 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1255 lcd.printf(0,"%d",DSensData[0]);
MCR_Xavier 7:c99fe0eb544a 1256 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1257 break;
MCR_Xavier 7:c99fe0eb544a 1258 default:
MCR_Xavier 7:c99fe0eb544a 1259 break;
MCR_Xavier 7:c99fe0eb544a 1260 }
MCR_Xavier 7:c99fe0eb544a 1261 }
MCR_Xavier 7:c99fe0eb544a 1262
MCR_Xavier 7:c99fe0eb544a 1263
MCR_Xavier 7:c99fe0eb544a 1264 while( SW_W ){
MCR_Xavier 7:c99fe0eb544a 1265 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1266 lcd.printf(0,"Exit?");
MCR_Xavier 7:c99fe0eb544a 1267 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1268 }
MCR_Xavier 7:c99fe0eb544a 1269 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1270 lcd.printf(0,"Bye-Bye");
MCR_Xavier 7:c99fe0eb544a 1271 wait(2);
MCR_Xavier 7:c99fe0eb544a 1272 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1273 DebugFlg=0;
MCR_Xavier 7:c99fe0eb544a 1274 }