Takeshi Nishimura / Mbed 2 deprecated Lancer2016

ランサー

Dependencies:   SB1602E SDFileSystem mbed

Fork of Seeed_SDCard_Shield by Shields

main.cpp@10:adefba925268, 2017-04-18 (annotated)

Committer:
MCR_Xavier
Date:
Tue Apr 18 12:01:43 2017 +0000
Revision:
10:adefba925268
Parent:
9:8ab0d3e46c3c
Child:
11:5ec2507020da
?????

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 5:20bb1e34b354 10 #define CLR 0,0,0 //無 RGBパターン
MCR_Xavier 5:20bb1e34b354 11 #define RED 1,0,0 //赤
MCR_Xavier 5:20bb1e34b354 12 #define GRE 0,1,0 //緑
MCR_Xavier 5:20bb1e34b354 13 #define YEL 1,1,0 //黄
MCR_Xavier 5:20bb1e34b354 14 #define BLU 0,0,1 //青
MCR_Xavier 5:20bb1e34b354 15 #define PUR 1,0,1 //紫
MCR_Xavier 5:20bb1e34b354 16 #define CYA 0,1,1 //水
MCR_Xavier 5:20bb1e34b354 17 #define WHI 1,1,1 //白
MCR_Xavier 5:20bb1e34b354 18 #define DFP 508 //フロントポテンショメータ機械原点
MCR_Xavier 5:20bb1e34b354 19 #define DRP 234 //リヤポテンショメータ機械原点
MCR_Xavier 5:20bb1e34b354 20 #define FW 1 //駆動輪前進
MCR_Xavier 5:20bb1e34b354 21 #define BW 0 //駆動輪後進
MCR_Xavier 5:20bb1e34b354 22 #define ST_EN ST_CTR = 0
MCR_Xavier 5:20bb1e34b354 23 #define ST_DA ST_CTR = 1
MCR_Xavier 5:20bb1e34b354 24 #define OTH_EN OTH_CTR = 0
MCR_Xavier 5:20bb1e34b354 25 #define OTH_DA OTH_CTR = 1
MCR_Xavier 7:c99fe0eb544a 26 #define DSs 8 //LineセンサA/Dシフト数
MCR_Xavier 7:c99fe0eb544a 27 #define ASs 6 //トレース用LineセンサA/Dシフト数
MCR_Xavier 7:c99fe0eb544a 28 #define ThS 80 //Line白黒判別閾値
MCR_Xavier 7:c99fe0eb544a 29 #define DSPA0 0.00748 //ローパスフィルタ定数（200HzHaming窓N5）
MCR_Xavier 7:c99fe0eb544a 30 #define DSPA1 0.16347 //ローパスフィルタ定数（200HzHaming窓N5）
MCR_Xavier 7:c99fe0eb544a 31 #define DSPA2 0.4 //ローパスフィルタ定数（200HzHaming窓N5）
MCR_Xavier 7:c99fe0eb544a 32 #define DSPA3 0.16347 //ローパスフィルタ定数（200HzHaming窓N5）
MCR_Xavier 7:c99fe0eb544a 33 #define DSPA4 0.00748 //ローパスフィルタ定数（200HzHaming窓N5）
MCR_Xavier 9:8ab0d3e46c3c 34 #define TRACE 0 //トレースモード
MCR_Xavier 9:8ab0d3e46c3c 35 #define ANGLE 1 //角度制御モード
MCR_Xavier 10:adefba925268 36 #define WWIDCLR W_WidsR=W_WidsL=0
MCR_Xavier 10:adefba925268 37 #define NUM_DATA 10 //ソート用データ数
MCR_Xavier 5:20bb1e34b354 38
MCR_Xavier 5:20bb1e34b354 39 SB1602E lcd(PB_9,PB_8); //LCD_I2C設定(SDA, SCL)
MCR_Xavier 7:c99fe0eb544a 40 //SDFileSystem sd(D11, D12, D13, D10,"sd"); // MOSI, MISO, SCK, CS
MCR_Xavier 7:c99fe0eb544a 41 SDFileSystem sd(PA_7,PA_6,PA_5,PB_6,"sd"); // MOSI, MISO, SCK, CS
screamer 0:525c842a3c89 42 FILE *fp;
MCR_Xavier 5:20bb1e34b354 43 //センサ入力
MCR_Xavier 5:20bb1e34b354 44 DigitalIn RS1(PC_10); //ロータリースイッチbit1
MCR_Xavier 5:20bb1e34b354 45 DigitalIn RS2(PA_15); //ロータリースイッチbit2
MCR_Xavier 5:20bb1e34b354 46 DigitalIn RS3(PA_12); //ロータリースイッチbit3
MCR_Xavier 5:20bb1e34b354 47 DigitalIn RS4(PA_9); //ロータリースイッチbit4
MCR_Xavier 5:20bb1e34b354 48 AnalogIn AMS(PC_5); //アナログモードスイッチ
MCR_Xavier 5:20bb1e34b354 49 AnalogIn Line0(PC_0); //アナログラインセンサ
MCR_Xavier 5:20bb1e34b354 50 AnalogIn Line1(PC_1); //アナログラインセンサ
MCR_Xavier 5:20bb1e34b354 51 AnalogIn Line2(PC_2); //アナログラインセンサ
MCR_Xavier 5:20bb1e34b354 52 AnalogIn Line3(PC_3); //アナログラインセンサ
MCR_Xavier 5:20bb1e34b354 53 AnalogIn Line4(PA_0); //アナログラインセンサ
MCR_Xavier 5:20bb1e34b354 54 AnalogIn Line5(PA_1); //アナログラインセンサ
MCR_Xavier 5:20bb1e34b354 55 AnalogIn Line6(PA_2); //アナログラインセンサ
MCR_Xavier 5:20bb1e34b354 56 AnalogIn Line7(PA_3); //アナログラインセンサ
MCR_Xavier 5:20bb1e34b354 57 AnalogIn Steer_Ptm(PA_4); //ステアリングポテンショ
MCR_Xavier 5:20bb1e34b354 58 AnalogIn Lance_Ptm(PC_4); //槍ポテンショ
MCR_Xavier 5:20bb1e34b354 59 DigitalIn SW_W(PB_5); //スイッチ白
MCR_Xavier 5:20bb1e34b354 60 DigitalIn SW_R(PC_6); //スイッチ赤
MCR_Xavier 5:20bb1e34b354 61 DigitalIn SW_B(PC_12); //スイッチ黒
MCR_Xavier 5:20bb1e34b354 62 InterruptIn s0signal(D6); //ロータリエンコーダA相
MCR_Xavier 5:20bb1e34b354 63 InterruptIn s1signal(D7); //ロータリエンコーダB相
MCR_Xavier 5:20bb1e34b354 64 //モータ制御
MCR_Xavier 5:20bb1e34b354 65 PwmOut PWM_CH1(PC_7); //3/2槍
MCR_Xavier 5:20bb1e34b354 66 PwmOut PWM_CH2(PA_11); //1/4前左
MCR_Xavier 5:20bb1e34b354 67 PwmOut PWM_CH3(PB_2); //2/4前右
MCR_Xavier 5:20bb1e34b354 68 PwmOut PWM_CH4(PB_7); //4/2後右
MCR_Xavier 5:20bb1e34b354 69 PwmOut PWM_CH5(PA_10); //1/3後左
MCR_Xavier 5:20bb1e34b354 70 PwmOut PWM_CH6(PB_4); //3/1操舵
MCR_Xavier 5:20bb1e34b354 71 DigitalOut Rev_CH1(PB_12); //操舵モータDIR
MCR_Xavier 5:20bb1e34b354 72 DigitalOut Rev_CH2(PB_13); //前左モータDIR
MCR_Xavier 5:20bb1e34b354 73 DigitalOut Rev_CH3(PC_8); //前右モータDIR
MCR_Xavier 5:20bb1e34b354 74 DigitalOut Rev_CH4(PC_9); //後右モータDIR
MCR_Xavier 5:20bb1e34b354 75 DigitalOut Rev_CH5(PB_14); //後左モータDIR
MCR_Xavier 5:20bb1e34b354 76 DigitalOut Rev_CH6(PB_15); //槍モータDIR
MCR_Xavier 5:20bb1e34b354 77 DigitalOut SensLED(PC_11); //センサ用LED
MCR_Xavier 5:20bb1e34b354 78 DigitalOut ST_CTR(PB_0); //STEER_EnableCtrl
MCR_Xavier 5:20bb1e34b354 79 DigitalOut OTH_CTR(PB_1); //OTHER_EnableCtrl
MCR_Xavier 5:20bb1e34b354 80 //デバッグ
MCR_Xavier 5:20bb1e34b354 81 DigitalOut LED_R(PA_13); //LED_R
MCR_Xavier 5:20bb1e34b354 82 DigitalOut LED_G(PA_14); //LED_G
MCR_Xavier 5:20bb1e34b354 83 DigitalOut LED_B(PB_3); //LED_B
MCR_Xavier 5:20bb1e34b354 84 //割り込み定義
MCR_Xavier 7:c99fe0eb544a 85 Ticker flipper; //汎用タイマー
MCR_Xavier 7:c99fe0eb544a 86 Ticker sensget; //センサー用タイマー
MCR_Xavier 7:c99fe0eb544a 87 Ticker SDLogflip; //エンコーダ割り込み
MCR_Xavier 7:c99fe0eb544a 88 //Ticker encLerp; //エンコーダ割り込み
MCR_Xavier 5:20bb1e34b354 89
MCR_Xavier 5:20bb1e34b354 90 //プロトタイプ宣言
MCR_Xavier 5:20bb1e34b354 91 void init(void);
MCR_Xavier 5:20bb1e34b354 92 void MotorCtrl(void); //モータ総合管理
MCR_Xavier 5:20bb1e34b354 93 void enc(); //エンコーダ処理
MCR_Xavier 5:20bb1e34b354 94 int ModeSW(); //モードスイッチ
MCR_Xavier 5:20bb1e34b354 95 int Get_SW(); //スイッチ情報取得
MCR_Xavier 9:8ab0d3e46c3c 96 void Get_Pot(); //ポテンショメータA/D取得
MCR_Xavier 5:20bb1e34b354 97 void SpeedCtrl(int V); //速度制御
MCR_Xavier 5:20bb1e34b354 98 int PotTest(); //ポテンショテスト
MCR_Xavier 5:20bb1e34b354 99 float PwmTest(); //PWMテスト
MCR_Xavier 5:20bb1e34b354 100 void SD_Mount(); //SDカードをマウントする
MCR_Xavier 5:20bb1e34b354 101 void SD_Unmount(); //SDカードをアンマウントする
MCR_Xavier 5:20bb1e34b354 102 void DSens(); //センサ変調処理
MCR_Xavier 5:20bb1e34b354 103 void LineTrace(); //ライントレース
MCR_Xavier 5:20bb1e34b354 104 void led_color(char R , char G , char B ); //LED色制御
MCR_Xavier 5:20bb1e34b354 105 void ErrChk(void); //エラーチェック
MCR_Xavier 5:20bb1e34b354 106 void LanceAng(int Ang); //槍角度制御
MCR_Xavier 5:20bb1e34b354 107 void Line_Dec(void); //白線判定
MCR_Xavier 5:20bb1e34b354 108 void enCalc(void); //エンコーダ計算とか
MCR_Xavier 5:20bb1e34b354 109 void CornerSeq(void); //コーナーモード
MCR_Xavier 10:adefba925268 110 void Stop_Sq(void);
MCR_Xavier 5:20bb1e34b354 111 void L_LineSeq(void);
MCR_Xavier 5:20bb1e34b354 112 void R_LineSeq(void);
MCR_Xavier 6:32698402be64 113 void HolSeq1(void);
MCR_Xavier 6:32698402be64 114 void HolSeq2(void);
MCR_Xavier 5:20bb1e34b354 115 void Debug(void); //デバッグモード
MCR_Xavier 7:c99fe0eb544a 116 void SD_Log(void); //SDログ
MCR_Xavier 7:c99fe0eb544a 117 void SD_WriteSq(void); //SD書き込み
MCR_Xavier 7:c99fe0eb544a 118 void TmpLog(void); //内部メモリへのログ
MCR_Xavier 9:8ab0d3e46c3c 119 void SteerCtrl(char STMode); //ステアリングコントロール
MCR_Xavier 9:8ab0d3e46c3c 120 void AngleCtrl(int STAngle); //角度制御モード
MCR_Xavier 10:adefba925268 121 void Localization(void); // 位置推定
MCR_Xavier 10:adefba925268 122 void Hpsort(int a[ ], int n);
MCR_Xavier 10:adefba925268 123 void DownHeap(int a[ ], int top, int bottom);
MCR_Xavier 10:adefba925268 124 void Swap(int a[ ], int i, int j);
MCR_Xavier 5:20bb1e34b354 125
MCR_Xavier 5:20bb1e34b354 126 //グローバル変数の宣言
MCR_Xavier 5:20bb1e34b354 127 int EnCount=0,EnCountBuf=0,EnCountI=0; //エンコーダカウント
MCR_Xavier 10:adefba925268 128 int EnCount_1R=0;
MCR_Xavier 5:20bb1e34b354 129 int PPms=0,RPM_Test=0,Rev_Flag=0;
MCR_Xavier 5:20bb1e34b354 130 int ZCount=0; //エンコーダ停止判定カウンタ
MCR_Xavier 5:20bb1e34b354 131 int EncFlag=0;
MCR_Xavier 5:20bb1e34b354 132 unsigned char Rev=0; //エンコーダ回転方向検知
MCR_Xavier 5:20bb1e34b354 133 int encA=0,encB=0; //エンコーダ相状態確認
MCR_Xavier 5:20bb1e34b354 134 unsigned char SD_Flag=0;//SDカードの状態判定
MCR_Xavier 5:20bb1e34b354 135 unsigned char Line_Flg; //ライン情報フラグ
MCR_Xavier 7:c99fe0eb544a 136 int timer1=0,timer2=0,timer3=0,Tmpcnt=0,Log_timer=0; //タイマー
MCR_Xavier 5:20bb1e34b354 137 int Stime=0;
MCR_Xavier 5:20bb1e34b354 138 unsigned long long cntT=0;
MCR_Xavier 5:20bb1e34b354 139 int ONSensData[8],OFFSensData[8],DSensData[8],TFSensData[8];//ラインセンサ値
MCR_Xavier 10:adefba925268 140 int Pot_F=0,Pot_R=0,Pot_F2=0,StrPot_R[10]; //ポテンショ
MCR_Xavier 5:20bb1e34b354 141 char LA_Rev,FL_Rev,FR_Rev,RR_Rev,RL_Rev,ST_Rev; //モータ回転方向
MCR_Xavier 5:20bb1e34b354 142 int LA_PWM=0,FL_PWM=0,FR_PWM=0,RR_PWM=0,RL_PWM=0,ST_PWM=0; //モータPWMデューティ
MCR_Xavier 5:20bb1e34b354 143 char ErrSt=0;
MCR_Xavier 5:20bb1e34b354 144 int Speed=0,Angle=0;
MCR_Xavier 5:20bb1e34b354 145 int iV=0,iAng=0;
MCR_Xavier 5:20bb1e34b354 146 int TraceI,Trace_P=0,Trace_I=0,Trace_D=0,Trace_B=0;
MCR_Xavier 9:8ab0d3e46c3c 147 int Angle_P=0,Angle_I=0,Angle_D=0,Angle_B=0;
MCR_Xavier 5:20bb1e34b354 148 int RSW_Value;
MCR_Xavier 5:20bb1e34b354 149 char Turn_Flg=0;
MCR_Xavier 6:32698402be64 150 unsigned int R_LineCnt=0,L_LineCnt=0,R_LineCntF=0,L_LineCntF=0;
MCR_Xavier 5:20bb1e34b354 151 char R_LineFlg=0,L_LineFlg=0;
MCR_Xavier 5:20bb1e34b354 152 char DebugFlg=0;
MCR_Xavier 6:32698402be64 153 int W_WidsR=0,W_WidsL=0,W_WidsRCnt=0,W_WidsLCnt=0,dAng;
MCR_Xavier 6:32698402be64 154 unsigned char Count_C=0,Count_R=0,Count_L=0,Count_H=0;
MCR_Xavier 6:32698402be64 155 int ParamS=1;
MCR_Xavier 10:adefba925268 156 int SPCom=4500,SPYobi=4000,SPCor=3200,loopJump=1000000,linewide=100;
MCR_Xavier 9:8ab0d3e46c3c 157 char SteerAngle;
MCR_Xavier 9:8ab0d3e46c3c 158 char SteerMode=0;
MCR_Xavier 10:adefba925268 159 int a[NUM_DATA + 1];
MCR_Xavier 5:20bb1e34b354 160
MCR_Xavier 7:c99fe0eb544a 161 char Log_table[10][10];
MCR_Xavier 7:c99fe0eb544a 162 int Log_W_Wids[100][2];
MCR_Xavier 7:c99fe0eb544a 163 char Log_W_WidsLR[100];
MCR_Xavier 7:c99fe0eb544a 164 char Log_tmp[10][10000];
MCR_Xavier 7:c99fe0eb544a 165 int Sensecnt=0,writeflg=0;
MCR_Xavier 7:c99fe0eb544a 166
MCR_Xavier 7:c99fe0eb544a 167 unsigned char WidsNo=0;
MCR_Xavier 7:c99fe0eb544a 168 char Logcnt=0;
MCR_Xavier 5:20bb1e34b354 169 char Trace_table[]={
MCR_Xavier 5:20bb1e34b354 170 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 171 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 172 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 173 58,60,63,65,68,70,73,76,79,82,85,88,91,94,98,101,104,108,112,115,
MCR_Xavier 5:20bb1e34b354 174 119,123};
MCR_Xavier 5:20bb1e34b354 175 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 176 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 177 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 178 16,17,17,18,18,18,19,19,19,20,20};
MCR_Xavier 5:20bb1e34b354 179
MCR_Xavier 5:20bb1e34b354 180 float Motor_TableA[]={
MCR_Xavier 5:20bb1e34b354 181 1,0.99,0.98,0.98,0.97,0.96,0.95,0.95,0.94,0.93,0.93,0.92,0.92,0.91,
MCR_Xavier 5:20bb1e34b354 182 0.91,0.90,0.90,0.90,0.89,0.89,0.89,0.88,0.88,0.88,0.88,0.87,0.87,
MCR_Xavier 5:20bb1e34b354 183 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,
MCR_Xavier 5:20bb1e34b354 184 0.89,0.90,0.90,0.91,0.92,0.92,0.93,0.94,0.95,0.96,0.97};
MCR_Xavier 5:20bb1e34b354 185 float Motor_TableB[]={
MCR_Xavier 5:20bb1e34b354 186 1,1.01,1.02,1.03,1.04,1.05,1.06,1.07,1.08,1.09,1.10,1.11,1.13,1.14,
MCR_Xavier 5:20bb1e34b354 187 1.15,1.17,1.18,1.20,1.21,1.23,1.24,1.26,1.28,1.29,1.31,1.33,1.35,
MCR_Xavier 5:20bb1e34b354 188 1.37,1.39,1.42,1.44,1.46,1.49,1.51,1.54,1.56,1.59,1.62,1.65,1.68,
MCR_Xavier 5:20bb1e34b354 189 1.72,1.75,1.79,1.83,1.86,1.91,1.95,1.99,2.04,2.09,2.14};
MCR_Xavier 5:20bb1e34b354 190 float Motor_TableC[]={
MCR_Xavier 5:20bb1e34b354 191 1,0.99,0.98,0.97,0.97,0.96,0.95,0.94,0.93,0.92,0.91,0.90,0.90,0.89,
MCR_Xavier 5:20bb1e34b354 192 0.88,0.87,0.86,0.85,0.84,0.83,0.82,0.81,0.80,0.79,0.78,0.77,0.76,
MCR_Xavier 5:20bb1e34b354 193 0.75,0.74,0.73,0.72,0.70,0.69,0.68,0.67,0.66,0.64,0.63,0.62,0.60,
MCR_Xavier 5:20bb1e34b354 194 0.59,0.57,0.56,0.54,0.53,0.51,0.49,0.47,0.45,0.43,0.41};
MCR_Xavier 5:20bb1e34b354 195 float Motor_TableD[]={
MCR_Xavier 5:20bb1e34b354 196 1,1.01,1.02,1.03,1.03,1.04,1.05,1.06,1.07,1.08,1.09,1.10,1.10,1.11,
MCR_Xavier 5:20bb1e34b354 197 1.12,1.13,1.14,1.15,1.16,1.17,1.18,1.19,1.20,1.21,1.22,1.23,1.24,
MCR_Xavier 5:20bb1e34b354 198 1.25,1.26,1.27,1.28,1.30,1.31,1.32,1.33,1.34,1.36,1.37,1.38,1.40,
MCR_Xavier 5:20bb1e34b354 199 1.41,1.43,1.44,1.46,1.47,1.49,1.51,1.53,1.55,1.57,1.59};
MCR_Xavier 5:20bb1e34b354 200
MCR_Xavier 7:c99fe0eb544a 201 ////////////////////////////////////////////////
MCR_Xavier 7:c99fe0eb544a 202 //-------------ベースプログラム-----------------//
MCR_Xavier 7:c99fe0eb544a 203 ////////////////////////////////////////////////
MCR_Xavier 5:20bb1e34b354 204 //----------モードSW--------------------
MCR_Xavier 5:20bb1e34b354 205 int ModeSW(){
MCR_Xavier 5:20bb1e34b354 206 char RSW=0;
MCR_Xavier 5:20bb1e34b354 207 if(RS1)RSW += 1;
MCR_Xavier 5:20bb1e34b354 208 if(RS2)RSW += 2;
MCR_Xavier 5:20bb1e34b354 209 if(RS3)RSW += 4;
MCR_Xavier 5:20bb1e34b354 210 if(RS4)RSW += 8;
MCR_Xavier 5:20bb1e34b354 211 RSW_Value=int(RSW);
MCR_Xavier 5:20bb1e34b354 212 return RSW;}
MCR_Xavier 7:c99fe0eb544a 213 //----------SW情報取得--------------------
MCR_Xavier 7:c99fe0eb544a 214 int Get_SW(){
MCR_Xavier 7:c99fe0eb544a 215 char SW_Val=0;
MCR_Xavier 7:c99fe0eb544a 216 if(SW_W)SW_Val |= 0x01;
MCR_Xavier 7:c99fe0eb544a 217 if(SW_R)SW_Val |= 0x02;
MCR_Xavier 7:c99fe0eb544a 218 if(SW_B)SW_Val |= 0x04;
MCR_Xavier 7:c99fe0eb544a 219 return SW_Val;}
MCR_Xavier 5:20bb1e34b354 220 //----------エラーチェック--------------------
MCR_Xavier 5:20bb1e34b354 221 void ErrChk(void){
MCR_Xavier 5:20bb1e34b354 222 if(timer1 >= 30000) ErrSt = 100;
MCR_Xavier 5:20bb1e34b354 223 if(Pot_F >= 45 || Pot_F <= (-45)) ErrSt++;
MCR_Xavier 5:20bb1e34b354 224 if(ErrSt < 100 && Pot_F == 0 ) ErrSt=0;
MCR_Xavier 5:20bb1e34b354 225 if(ErrSt >= 120 ) ErrSt = 120;
MCR_Xavier 5:20bb1e34b354 226 }
MCR_Xavier 7:c99fe0eb544a 227 //-----------エンコーダカウント------------------
MCR_Xavier 7:c99fe0eb544a 228 void s0_trigger_rise() { // A相立上り変化割込み時の処理 (Low -> High)
MCR_Xavier 7:c99fe0eb544a 229 EncFlag=1; //外部割込み停止検出
MCR_Xavier 7:c99fe0eb544a 230 Rev |= 0x01;
MCR_Xavier 7:c99fe0eb544a 231 if(Rev&0x10) EnCount--;
MCR_Xavier 7:c99fe0eb544a 232 else EnCount++;
MCR_Xavier 7:c99fe0eb544a 233 }
MCR_Xavier 7:c99fe0eb544a 234 void s0_trigger_fall() { // A相立下り変化割込み時の処理 (High -> Low)
MCR_Xavier 7:c99fe0eb544a 235 Rev &= ~0x01;
MCR_Xavier 7:c99fe0eb544a 236 if(Rev&0x10) EnCount++;
MCR_Xavier 7:c99fe0eb544a 237 else EnCount--;
MCR_Xavier 7:c99fe0eb544a 238 }
MCR_Xavier 7:c99fe0eb544a 239 void s1_trigger_rise() { // B相立上り変化割込み時の処理 (Low -> High)
MCR_Xavier 7:c99fe0eb544a 240 Rev |= 0x10;
MCR_Xavier 7:c99fe0eb544a 241 if(Rev&0x01) EnCount++;
MCR_Xavier 7:c99fe0eb544a 242 else EnCount--;
MCR_Xavier 7:c99fe0eb544a 243 }
MCR_Xavier 7:c99fe0eb544a 244 void s1_trigger_fall() { // B相立下り変化割込み時の処理 (High -> Low)
MCR_Xavier 7:c99fe0eb544a 245 Rev &= ~0x10;
MCR_Xavier 7:c99fe0eb544a 246 if(Rev&0x01) EnCount--;
MCR_Xavier 7:c99fe0eb544a 247 else EnCount++;
MCR_Xavier 7:c99fe0eb544a 248 }
MCR_Xavier 7:c99fe0eb544a 249 //----------エンコーダ計算--------------------
MCR_Xavier 7:c99fe0eb544a 250 void enCalc(void){
MCR_Xavier 7:c99fe0eb544a 251 EnCountI = EnCountI + EnCount;
MCR_Xavier 10:adefba925268 252 EnCount_1R = EnCount_1R + EnCount;
MCR_Xavier 7:c99fe0eb544a 253 EnCount = 0;
MCR_Xavier 7:c99fe0eb544a 254 }
MCR_Xavier 7:c99fe0eb544a 255 //----------フルカラーLED制御--------------------
MCR_Xavier 7:c99fe0eb544a 256 void led_color(char R , char G , char B )
MCR_Xavier 7:c99fe0eb544a 257 {
MCR_Xavier 7:c99fe0eb544a 258 if ( R ) LED_R = 1;
MCR_Xavier 7:c99fe0eb544a 259 else LED_R = 0;
MCR_Xavier 7:c99fe0eb544a 260 if ( G ) LED_G = 1;
MCR_Xavier 7:c99fe0eb544a 261 else LED_G = 0;
MCR_Xavier 7:c99fe0eb544a 262 if ( B ) LED_B = 1;
MCR_Xavier 7:c99fe0eb544a 263 else LED_B = 0;
MCR_Xavier 7:c99fe0eb544a 264 }
MCR_Xavier 7:c99fe0eb544a 265 //----------センサー値の取得---------------
MCR_Xavier 7:c99fe0eb544a 266 void sensGet(){
MCR_Xavier 10:adefba925268 267 if(Stime >= 10) {
MCR_Xavier 10:adefba925268 268 Stime=0;
MCR_Xavier 10:adefba925268 269 Hpsort(a, NUM_DATA);
MCR_Xavier 10:adefba925268 270 }
MCR_Xavier 7:c99fe0eb544a 271 if(Stime == 0 ) SensLED=0;
MCR_Xavier 7:c99fe0eb544a 272 if(Stime == 5 ) SensLED=1;
MCR_Xavier 7:c99fe0eb544a 273 if(Stime == 4 ) {
MCR_Xavier 7:c99fe0eb544a 274 OFFSensData[0] = Line0.read_u16()>>DSs;
MCR_Xavier 7:c99fe0eb544a 275 OFFSensData[1] = Line1.read_u16()>>DSs;
MCR_Xavier 7:c99fe0eb544a 276 OFFSensData[2] = Line2.read_u16()>>ASs;
MCR_Xavier 7:c99fe0eb544a 277 OFFSensData[3] = Line3.read_u16()>>ASs;
MCR_Xavier 7:c99fe0eb544a 278 OFFSensData[4] = Line4.read_u16()>>ASs;
MCR_Xavier 7:c99fe0eb544a 279 OFFSensData[5] = Line5.read_u16()>>ASs;
MCR_Xavier 7:c99fe0eb544a 280 OFFSensData[6] = Line6.read_u16()>>DSs;
MCR_Xavier 7:c99fe0eb544a 281 OFFSensData[7] = Line7.read_u16()>>DSs;
MCR_Xavier 7:c99fe0eb544a 282 }
MCR_Xavier 7:c99fe0eb544a 283 if(Stime == 9 ) {
MCR_Xavier 7:c99fe0eb544a 284 ONSensData[0] = Line0.read_u16()>>DSs;
MCR_Xavier 7:c99fe0eb544a 285 ONSensData[1] = Line1.read_u16()>>DSs;
MCR_Xavier 7:c99fe0eb544a 286 ONSensData[2] = Line2.read_u16()>>ASs;
MCR_Xavier 7:c99fe0eb544a 287 ONSensData[3] = Line3.read_u16()>>ASs;
MCR_Xavier 7:c99fe0eb544a 288 ONSensData[4] = Line4.read_u16()>>ASs;
MCR_Xavier 7:c99fe0eb544a 289 ONSensData[5] = Line5.read_u16()>>ASs;
MCR_Xavier 7:c99fe0eb544a 290 ONSensData[6] = Line6.read_u16()>>DSs;
MCR_Xavier 7:c99fe0eb544a 291 ONSensData[7] = Line7.read_u16()>>DSs;
MCR_Xavier 7:c99fe0eb544a 292 }
MCR_Xavier 7:c99fe0eb544a 293 Stime++;
MCR_Xavier 10:adefba925268 294 a[Stime] = Lance_Ptm.read_u16()>>6;
MCR_Xavier 9:8ab0d3e46c3c 295 }
MCR_Xavier 9:8ab0d3e46c3c 296 //----------ポテンショ値の取得---------------
MCR_Xavier 9:8ab0d3e46c3c 297 void Get_Pot(){
MCR_Xavier 9:8ab0d3e46c3c 298
MCR_Xavier 9:8ab0d3e46c3c 299 Pot_F2 = (DFP-(Steer_Ptm.read_u16()>>6));
MCR_Xavier 9:8ab0d3e46c3c 300 Pot_F = Pot_F2/3;
MCR_Xavier 10:adefba925268 301 Pot_R = a[5];
MCR_Xavier 10:adefba925268 302 //Pot_R = Lance_Ptm.read_u16()>>6;
MCR_Xavier 9:8ab0d3e46c3c 303 }
MCR_Xavier 7:c99fe0eb544a 304 //----------センサー値差分の計算---------------
MCR_Xavier 7:c99fe0eb544a 305 void DSens(){
MCR_Xavier 7:c99fe0eb544a 306
MCR_Xavier 7:c99fe0eb544a 307 int sensorno;
MCR_Xavier 7:c99fe0eb544a 308 for(sensorno=0;sensorno<=7;sensorno++){
MCR_Xavier 7:c99fe0eb544a 309 DSensData[sensorno] = OFFSensData[sensorno] - ONSensData[sensorno];
MCR_Xavier 7:c99fe0eb544a 310 if(DSensData[sensorno] >= ThS ) TFSensData[sensorno] = 1;
MCR_Xavier 7:c99fe0eb544a 311 else TFSensData[sensorno] = 0;
MCR_Xavier 7:c99fe0eb544a 312 }
MCR_Xavier 7:c99fe0eb544a 313 }
MCR_Xavier 7:c99fe0eb544a 314
MCR_Xavier 10:adefba925268 315
MCR_Xavier 10:adefba925268 316 /* ヒープソートを行う */
MCR_Xavier 10:adefba925268 317 void Hpsort(int a[ ], int n)
MCR_Xavier 10:adefba925268 318 {
MCR_Xavier 10:adefba925268 319 int leaf, root;
MCR_Xavier 10:adefba925268 320
MCR_Xavier 10:adefba925268 321 leaf = n; /* 初期値は末尾の要素 */
MCR_Xavier 10:adefba925268 322 root = n/2; /* 初期値はその親 */
MCR_Xavier 10:adefba925268 323
MCR_Xavier 10:adefba925268 324 while (root > 0 ) { /* 半順序木を構成 */
MCR_Xavier 10:adefba925268 325 DownHeap(a, leaf, root);
MCR_Xavier 10:adefba925268 326 root--;
MCR_Xavier 10:adefba925268 327 }
MCR_Xavier 10:adefba925268 328
MCR_Xavier 10:adefba925268 329 while(leaf > 0) {
MCR_Xavier 10:adefba925268 330 Swap(a, 1, leaf); /* 半順序木の根と末尾の要素を交換 */
MCR_Xavier 10:adefba925268 331 leaf--; /* 末尾の要素を半順序木から外す */
MCR_Xavier 10:adefba925268 332 DownHeap(a, leaf, 1); /* 半順序木を再構成する */
MCR_Xavier 10:adefba925268 333 }
MCR_Xavier 10:adefba925268 334 }
MCR_Xavier 10:adefba925268 335
MCR_Xavier 10:adefba925268 336 /* 半順序木 ( ヒープ ) を構成する */
MCR_Xavier 10:adefba925268 337 void DownHeap(int a[ ], int leaf, int root)
MCR_Xavier 10:adefba925268 338 {
MCR_Xavier 10:adefba925268 339 int i;
MCR_Xavier 10:adefba925268 340
MCR_Xavier 10:adefba925268 341 i = root * 2;
MCR_Xavier 10:adefba925268 342 while (i <= leaf) {
MCR_Xavier 10:adefba925268 343 if (i < leaf && a[i + 1] > a[i]) /* a[i] と a[i + 1] の大きい方と */
MCR_Xavier 10:adefba925268 344 i++;
MCR_Xavier 10:adefba925268 345 if (a[root] >= a[i]) /* a[root] と比較 */
MCR_Xavier 10:adefba925268 346 break; /* 子の方が大きければ */
MCR_Xavier 10:adefba925268 347 Swap(a, root, i); /* 交換 */
MCR_Xavier 10:adefba925268 348
MCR_Xavier 10:adefba925268 349 root = i; /* 更にその子についても調べる */
MCR_Xavier 10:adefba925268 350 i = root * 2;
MCR_Xavier 10:adefba925268 351 }
MCR_Xavier 10:adefba925268 352 }
MCR_Xavier 10:adefba925268 353
MCR_Xavier 10:adefba925268 354 /* 配列の要素を交換する */
MCR_Xavier 10:adefba925268 355 void Swap(int a[ ], int i, int j)
MCR_Xavier 10:adefba925268 356 {
MCR_Xavier 10:adefba925268 357 int temp;
MCR_Xavier 10:adefba925268 358
MCR_Xavier 10:adefba925268 359 temp = a[i];
MCR_Xavier 10:adefba925268 360 a[i] = a[j];
MCR_Xavier 10:adefba925268 361 a[j] = temp;
MCR_Xavier 10:adefba925268 362 }
MCR_Xavier 10:adefba925268 363
MCR_Xavier 7:c99fe0eb544a 364 ////////////////////////////////////////////////
MCR_Xavier 7:c99fe0eb544a 365 //-------------行動計画------------------------//
MCR_Xavier 7:c99fe0eb544a 366 ////////////////////////////////////////////////
MCR_Xavier 5:20bb1e34b354 367 //----------コーナーモード--------------------
MCR_Xavier 5:20bb1e34b354 368 void CornerSeq(void){
MCR_Xavier 6:32698402be64 369 Count_C++;
MCR_Xavier 6:32698402be64 370 led_color(YEL);
MCR_Xavier 5:20bb1e34b354 371 Speed = 1000;
MCR_Xavier 6:32698402be64 372 Angle = 90;
MCR_Xavier 5:20bb1e34b354 373 }
MCR_Xavier 5:20bb1e34b354 374 //----------左ラインモード--------------------
MCR_Xavier 5:20bb1e34b354 375 void L_LineSeq(void){
MCR_Xavier 6:32698402be64 376 Count_L++;
MCR_Xavier 6:32698402be64 377 led_color(BLU);
MCR_Xavier 6:32698402be64 378 Speed = 2000;
MCR_Xavier 6:32698402be64 379 Angle = 122;
MCR_Xavier 5:20bb1e34b354 380 }
MCR_Xavier 5:20bb1e34b354 381 //----------右ラインモード--------------------
MCR_Xavier 5:20bb1e34b354 382 void R_LineSeq(void){
MCR_Xavier 6:32698402be64 383 Count_R++;
MCR_Xavier 6:32698402be64 384 led_color(RED);
MCR_Xavier 6:32698402be64 385 Speed = 2000;
MCR_Xavier 6:32698402be64 386 Angle = 213;
MCR_Xavier 6:32698402be64 387 }
MCR_Xavier 6:32698402be64 388 //----------水平１--------------------
MCR_Xavier 6:32698402be64 389 void HolSeq1(void){
MCR_Xavier 6:32698402be64 390 Count_R++;
MCR_Xavier 6:32698402be64 391 Count_H++;
MCR_Xavier 6:32698402be64 392 }
MCR_Xavier 6:32698402be64 393 //----------水平2--------------------
MCR_Xavier 6:32698402be64 394 void HolSeq2(void){
MCR_Xavier 10:adefba925268 395
MCR_Xavier 6:32698402be64 396 Count_R++;
MCR_Xavier 6:32698402be64 397 Count_H++;
MCR_Xavier 5:20bb1e34b354 398 }
MCR_Xavier 10:adefba925268 399 //----------停止モード--------------------
MCR_Xavier 10:adefba925268 400 void Stop_Sq(void){
MCR_Xavier 10:adefba925268 401 if(!SW_W && !SW_B && !DebugFlg) Debug();
MCR_Xavier 10:adefba925268 402 led_color(PUR);
MCR_Xavier 10:adefba925268 403 Speed = 0;
MCR_Xavier 10:adefba925268 404 lcd.clear();
MCR_Xavier 10:adefba925268 405 lcd.printf(0,"%d",EnCountI );
MCR_Xavier 10:adefba925268 406 lcd.printf(1,"%d,%d",Count_L,Count_R );
MCR_Xavier 10:adefba925268 407 wait(0.1);
MCR_Xavier 10:adefba925268 408 }
MCR_Xavier 7:c99fe0eb544a 409 ////////////////////////////////////////////////
MCR_Xavier 7:c99fe0eb544a 410 //-------------便利機能プログラム---------------//
MCR_Xavier 7:c99fe0eb544a 411 ////////////////////////////////////////////////
MCR_Xavier 5:20bb1e34b354 412 //----------白線判別--------------------
MCR_Xavier 5:20bb1e34b354 413 void Line_Dec(void){
MCR_Xavier 7:c99fe0eb544a 414 char LR_Flg=0;
MCR_Xavier 6:32698402be64 415 if(TFSensData[0] && TFSensData[1] ) {
MCR_Xavier 6:32698402be64 416 R_LineCntF = 0;
MCR_Xavier 6:32698402be64 417 R_LineCnt++;}
MCR_Xavier 6:32698402be64 418 else {
MCR_Xavier 6:32698402be64 419 R_LineCnt = 0;
MCR_Xavier 6:32698402be64 420 R_LineCntF++;}
MCR_Xavier 6:32698402be64 421 if(TFSensData[6] && TFSensData[7] ) {
MCR_Xavier 6:32698402be64 422 L_LineCntF = 0;
MCR_Xavier 6:32698402be64 423 L_LineCnt++;}
MCR_Xavier 6:32698402be64 424 else {
MCR_Xavier 6:32698402be64 425 L_LineCnt = 0;
MCR_Xavier 6:32698402be64 426 L_LineCntF++;}
MCR_Xavier 5:20bb1e34b354 427
MCR_Xavier 5:20bb1e34b354 428 if(R_LineCnt >= 2 && !R_LineFlg ){
MCR_Xavier 5:20bb1e34b354 429 R_LineFlg = 1;
MCR_Xavier 5:20bb1e34b354 430 W_WidsRCnt = EnCountI;
MCR_Xavier 5:20bb1e34b354 431 }
MCR_Xavier 6:32698402be64 432 else if(!R_LineCnt && R_LineFlg && R_LineCntF >= 4 ){
MCR_Xavier 5:20bb1e34b354 433 R_LineFlg = 0;
MCR_Xavier 7:c99fe0eb544a 434 LR_Flg = 1;
MCR_Xavier 6:32698402be64 435 W_WidsR = EnCountI - W_WidsRCnt;
MCR_Xavier 7:c99fe0eb544a 436 Log_W_WidsLR[WidsNo] = LR_Flg;
MCR_Xavier 7:c99fe0eb544a 437 Log_W_Wids[WidsNo][0] = W_WidsR;
MCR_Xavier 7:c99fe0eb544a 438 Log_W_Wids[WidsNo][1] = EnCountI;
MCR_Xavier 7:c99fe0eb544a 439 WidsNo++;
MCR_Xavier 5:20bb1e34b354 440 }
MCR_Xavier 5:20bb1e34b354 441 if(L_LineCnt >= 2 && !L_LineFlg ){
MCR_Xavier 5:20bb1e34b354 442 L_LineFlg = 1;
MCR_Xavier 5:20bb1e34b354 443 W_WidsLCnt = EnCountI;
MCR_Xavier 5:20bb1e34b354 444 }
MCR_Xavier 6:32698402be64 445 else if(!L_LineCnt && L_LineFlg && L_LineCntF >= 4 ){
MCR_Xavier 5:20bb1e34b354 446 L_LineFlg = 0;
MCR_Xavier 7:c99fe0eb544a 447 LR_Flg = 0;
MCR_Xavier 5:20bb1e34b354 448 W_WidsL = EnCountI - W_WidsLCnt;
MCR_Xavier 7:c99fe0eb544a 449 Log_W_WidsLR[WidsNo] = LR_Flg;
MCR_Xavier 7:c99fe0eb544a 450 Log_W_Wids[WidsNo][0] = W_WidsL;
MCR_Xavier 7:c99fe0eb544a 451 Log_W_Wids[WidsNo][1] = EnCountI;
MCR_Xavier 7:c99fe0eb544a 452 WidsNo++;
MCR_Xavier 5:20bb1e34b354 453 }
MCR_Xavier 5:20bb1e34b354 454 }
MCR_Xavier 7:c99fe0eb544a 455
MCR_Xavier 5:20bb1e34b354 456 //----------モータ総合管理--------------------
MCR_Xavier 5:20bb1e34b354 457 void MotorCtrl(){
MCR_Xavier 5:20bb1e34b354 458 if(ErrSt >= 100){ //異常判定
MCR_Xavier 5:20bb1e34b354 459 PWM_CH1.pulsewidth_us(0); //操舵PWM　(0~100)
MCR_Xavier 5:20bb1e34b354 460 PWM_CH2.pulsewidth_us(0); //前左PWM　(0~1000)
MCR_Xavier 5:20bb1e34b354 461 PWM_CH3.pulsewidth_us(0); //前右PWM　(0~1000)
MCR_Xavier 5:20bb1e34b354 462 PWM_CH4.pulsewidth_us(0); //後右PWM　(0~1000)
MCR_Xavier 5:20bb1e34b354 463 PWM_CH5.pulsewidth_us(0); //後左PWM　(0~1000)
MCR_Xavier 5:20bb1e34b354 464 PWM_CH6.pulsewidth_us(0); //槍舵PWM　(0~100)
MCR_Xavier 5:20bb1e34b354 465 OTH_DA;
MCR_Xavier 5:20bb1e34b354 466 }
MCR_Xavier 5:20bb1e34b354 467 else{
MCR_Xavier 5:20bb1e34b354 468 PWM_CH1.pulsewidth_us(ST_PWM); //槍PWM　　(0~100)
MCR_Xavier 5:20bb1e34b354 469 PWM_CH2.pulsewidth_us(FL_PWM); //前左PWM　(0~1000)
MCR_Xavier 5:20bb1e34b354 470 PWM_CH3.pulsewidth_us(FR_PWM); //前右PWM　(0~1000)
MCR_Xavier 5:20bb1e34b354 471 PWM_CH4.pulsewidth_us(RR_PWM); //後右PWM　(0~1000)
MCR_Xavier 5:20bb1e34b354 472 PWM_CH5.pulsewidth_us(RL_PWM); //後左PWM　(0~1000)
MCR_Xavier 5:20bb1e34b354 473 PWM_CH6.pulsewidth_us(LA_PWM); //操舵PWM　(0~100)
MCR_Xavier 5:20bb1e34b354 474 Rev_CH1 = ST_Rev; //槍回転方向　（H：CW）
MCR_Xavier 5:20bb1e34b354 475 Rev_CH2 = !FL_Rev; //前左回転方向（L：FW）
MCR_Xavier 5:20bb1e34b354 476 Rev_CH3 = FR_Rev; //前右回転方向（H：FW）
MCR_Xavier 5:20bb1e34b354 477 Rev_CH4 = RR_Rev; //後右回転方向（H：FW）
MCR_Xavier 5:20bb1e34b354 478 Rev_CH5 = !RL_Rev; //後左回転方向（L：FW）
MCR_Xavier 5:20bb1e34b354 479 Rev_CH6 = LA_Rev; //操舵回転方向（H：CW）
MCR_Xavier 5:20bb1e34b354 480 }
MCR_Xavier 5:20bb1e34b354 481 }
MCR_Xavier 5:20bb1e34b354 482 //----------速度制御--------------------
MCR_Xavier 5:20bb1e34b354 483 void SpeedCtrl(int V){
MCR_Xavier 5:20bb1e34b354 484 float dV;
MCR_Xavier 5:20bb1e34b354 485 char PT;
MCR_Xavier 5:20bb1e34b354 486 dV=EnCount*32;
MCR_Xavier 5:20bb1e34b354 487 dV = (V - dV);
MCR_Xavier 5:20bb1e34b354 488 if(dV <= 0){
MCR_Xavier 5:20bb1e34b354 489 iV = 0;
MCR_Xavier 5:20bb1e34b354 490 dV=dV*(-1);
MCR_Xavier 5:20bb1e34b354 491 FL_Rev=FR_Rev=RR_Rev=RL_Rev=BW;
MCR_Xavier 5:20bb1e34b354 492 }
MCR_Xavier 5:20bb1e34b354 493 else FL_Rev=FR_Rev=RR_Rev=RL_Rev=FW;
MCR_Xavier 5:20bb1e34b354 494 iV = iV + (dV/10);
MCR_Xavier 5:20bb1e34b354 495 dV = dV + iV;
MCR_Xavier 7:c99fe0eb544a 496 if(dV>=1000) dV=1000;
MCR_Xavier 5:20bb1e34b354 497 if(Pot_F < 0){
MCR_Xavier 5:20bb1e34b354 498 PT = Pot_F * (-1);
MCR_Xavier 8:352404951de8 499 PT = int (PT*2);
MCR_Xavier 8:352404951de8 500 if(PT >= 50 ) PT=50;
MCR_Xavier 5:20bb1e34b354 501 FL_PWM = int( dV * Motor_TableA[PT]);
MCR_Xavier 5:20bb1e34b354 502 FR_PWM = int( dV * Motor_TableB[PT]);
MCR_Xavier 5:20bb1e34b354 503 RL_PWM = int( dV * Motor_TableC[PT]);
MCR_Xavier 5:20bb1e34b354 504 RR_PWM = int( dV * Motor_TableD[PT]);
MCR_Xavier 5:20bb1e34b354 505 }
MCR_Xavier 5:20bb1e34b354 506 else{
MCR_Xavier 5:20bb1e34b354 507 PT = Pot_F;
MCR_Xavier 8:352404951de8 508 PT = int (PT*1.5);
MCR_Xavier 8:352404951de8 509 if(PT >= 50 ) PT=50;
MCR_Xavier 5:20bb1e34b354 510 FR_PWM = int( dV * Motor_TableA[PT]);
MCR_Xavier 5:20bb1e34b354 511 FL_PWM = int( dV * Motor_TableB[PT]);
MCR_Xavier 5:20bb1e34b354 512 RR_PWM = int( dV * Motor_TableC[PT]);
MCR_Xavier 5:20bb1e34b354 513 RL_PWM = int( dV * Motor_TableD[PT]);
MCR_Xavier 5:20bb1e34b354 514 }
screamer 0:525c842a3c89 515
MCR_Xavier 5:20bb1e34b354 516 }
MCR_Xavier 5:20bb1e34b354 517 //----------槍角度制御--------------------
MCR_Xavier 5:20bb1e34b354 518 void LanceAng(int Ang){
MCR_Xavier 6:32698402be64 519 //int dAng;
MCR_Xavier 5:20bb1e34b354 520 dAng = Pot_R - (DRP + (Ang * 3));
MCR_Xavier 5:20bb1e34b354 521 if(dAng <= 0){ //CW
MCR_Xavier 5:20bb1e34b354 522 iAng = 0;
MCR_Xavier 5:20bb1e34b354 523 LA_Rev = 1;
MCR_Xavier 5:20bb1e34b354 524 dAng = dAng * (-1);
MCR_Xavier 5:20bb1e34b354 525 }
MCR_Xavier 5:20bb1e34b354 526 else{ //CCW
MCR_Xavier 5:20bb1e34b354 527 LA_Rev = 0;
MCR_Xavier 5:20bb1e34b354 528 }
MCR_Xavier 5:20bb1e34b354 529 dAng = dAng * 2;
MCR_Xavier 5:20bb1e34b354 530 iAng = iAng + (dAng/100);
MCR_Xavier 5:20bb1e34b354 531 //dAng = dAng + iAng
MCR_Xavier 5:20bb1e34b354 532 if(dAng >= 60) dAng = 60;
MCR_Xavier 5:20bb1e34b354 533 LA_PWM = dAng;
MCR_Xavier 5:20bb1e34b354 534 }
MCR_Xavier 9:8ab0d3e46c3c 535 //------ステアリングコントロール-----------
MCR_Xavier 9:8ab0d3e46c3c 536 void SteerCtrl(char STMode){
MCR_Xavier 9:8ab0d3e46c3c 537 if(STMode == ANGLE) AngleCtrl(-35);
MCR_Xavier 9:8ab0d3e46c3c 538 else LineTrace();
MCR_Xavier 9:8ab0d3e46c3c 539 }
MCR_Xavier 9:8ab0d3e46c3c 540
MCR_Xavier 9:8ab0d3e46c3c 541 //-----------角度コントロール-------------
MCR_Xavier 9:8ab0d3e46c3c 542 void AngleCtrl(int STAngle){
MCR_Xavier 9:8ab0d3e46c3c 543 int Angle_V=0;
MCR_Xavier 9:8ab0d3e46c3c 544 Angle_P=((STAngle*3) - Pot_F2);
MCR_Xavier 9:8ab0d3e46c3c 545 Angle_D = Angle_B - Angle_P;
MCR_Xavier 9:8ab0d3e46c3c 546 Angle_B = Angle_P;
MCR_Xavier 9:8ab0d3e46c3c 547 Angle_P = Angle_P*(100+(10*RSW_Value));
MCR_Xavier 9:8ab0d3e46c3c 548 Angle_I = Angle_I + (Angle_P / (10*RSW_Value));
MCR_Xavier 9:8ab0d3e46c3c 549 if(Angle_I >= 10000) Angle_I = 10000;
MCR_Xavier 9:8ab0d3e46c3c 550 if(Angle_I <= (-10000)) Angle_I = (-10000);
MCR_Xavier 9:8ab0d3e46c3c 551 Angle_V = Angle_P + (Angle_I*0 ) - (Angle_D * 0);
MCR_Xavier 9:8ab0d3e46c3c 552 Angle_V = Angle_V / 100;
MCR_Xavier 9:8ab0d3e46c3c 553
MCR_Xavier 9:8ab0d3e46c3c 554 if(Angle_V <= -1){
MCR_Xavier 9:8ab0d3e46c3c 555 Angle_V = Angle_V*(-1);
MCR_Xavier 9:8ab0d3e46c3c 556 ST_Rev=0;
MCR_Xavier 9:8ab0d3e46c3c 557 if(!Turn_Flg && Angle_V >= 10) Angle_I = 0;
MCR_Xavier 9:8ab0d3e46c3c 558 Turn_Flg=1;
MCR_Xavier 9:8ab0d3e46c3c 559 }
MCR_Xavier 9:8ab0d3e46c3c 560 else {
MCR_Xavier 9:8ab0d3e46c3c 561 ST_Rev=1;
MCR_Xavier 9:8ab0d3e46c3c 562 if(Turn_Flg && Angle_V >= 10) Angle_I = 0;
MCR_Xavier 9:8ab0d3e46c3c 563 Turn_Flg=0;
MCR_Xavier 9:8ab0d3e46c3c 564 }
MCR_Xavier 7:c99fe0eb544a 565
MCR_Xavier 9:8ab0d3e46c3c 566 if(Angle_V >= 100) Angle_V=100;
MCR_Xavier 9:8ab0d3e46c3c 567 ST_PWM=Angle_V;
MCR_Xavier 9:8ab0d3e46c3c 568 }
MCR_Xavier 5:20bb1e34b354 569 //----------ライントレース---------------
MCR_Xavier 5:20bb1e34b354 570 void LineTrace(){
MCR_Xavier 5:20bb1e34b354 571 int Trace_V;
MCR_Xavier 5:20bb1e34b354 572 TraceI=(DSensData[4]-DSensData[3]);
MCR_Xavier 8:352404951de8 573 /* if(TraceI <= -1){
MCR_Xavier 5:20bb1e34b354 574 TraceI = TraceI*(-1);
MCR_Xavier 5:20bb1e34b354 575 ST_Rev=1;
MCR_Xavier 5:20bb1e34b354 576 if(!Turn_Flg) Trace_I = 0;
MCR_Xavier 5:20bb1e34b354 577 Turn_Flg=1;
MCR_Xavier 5:20bb1e34b354 578 }
MCR_Xavier 5:20bb1e34b354 579 else {
MCR_Xavier 5:20bb1e34b354 580 ST_Rev=0;
MCR_Xavier 5:20bb1e34b354 581 if(Turn_Flg) Trace_I = 0;
MCR_Xavier 5:20bb1e34b354 582 Turn_Flg=0;
MCR_Xavier 8:352404951de8 583 }*/
MCR_Xavier 8:352404951de8 584 Trace_P = TraceI * (12);
MCR_Xavier 5:20bb1e34b354 585 Trace_I = Trace_I + (Trace_P / (400));
MCR_Xavier 8:352404951de8 586 if(Trace_I >= 10000) Trace_I = 10000;
MCR_Xavier 8:352404951de8 587 if(Trace_I <= (-10000)) Trace_I = (-10000);
MCR_Xavier 5:20bb1e34b354 588 Trace_D = Trace_B - TraceI;
MCR_Xavier 5:20bb1e34b354 589 Trace_B = TraceI;
MCR_Xavier 8:352404951de8 590 Trace_V = Trace_P + Trace_I - (Trace_D * 50);
MCR_Xavier 5:20bb1e34b354 591 Trace_V = Trace_V / 100;
MCR_Xavier 8:352404951de8 592
MCR_Xavier 8:352404951de8 593 if(Trace_V <= -1){
MCR_Xavier 8:352404951de8 594 Trace_V = Trace_V*(-1);
MCR_Xavier 8:352404951de8 595 ST_Rev=1;
MCR_Xavier 8:352404951de8 596 if(!Turn_Flg && Trace_V >= 10) Trace_I = 0;
MCR_Xavier 8:352404951de8 597 Turn_Flg=1;
MCR_Xavier 8:352404951de8 598 }
MCR_Xavier 8:352404951de8 599 else {
MCR_Xavier 8:352404951de8 600 ST_Rev=0;
MCR_Xavier 8:352404951de8 601 if(Turn_Flg && Trace_V >= 10) Trace_I = 0;
MCR_Xavier 8:352404951de8 602 Turn_Flg=0;
MCR_Xavier 8:352404951de8 603 }
MCR_Xavier 8:352404951de8 604
MCR_Xavier 5:20bb1e34b354 605 if(Trace_V >= 100) Trace_V=100;
MCR_Xavier 5:20bb1e34b354 606 ST_PWM=Trace_V;
MCR_Xavier 5:20bb1e34b354 607 }
MCR_Xavier 10:adefba925268 608 //----------コース上自己位置推定---------------
MCR_Xavier 10:adefba925268 609 void Localization(void){
MCR_Xavier 5:20bb1e34b354 610
MCR_Xavier 10:adefba925268 611 if(!Count_R && W_WidsR >= linewide && EnCount_1R >= 0){ //右1
MCR_Xavier 10:adefba925268 612 WWIDCLR;
MCR_Xavier 10:adefba925268 613 EnCount_1R =0;
MCR_Xavier 10:adefba925268 614 Count_R++;}
MCR_Xavier 10:adefba925268 615 else if(!Count_R && EnCount_1R >= 3000) Count_R++;
MCR_Xavier 10:adefba925268 616
MCR_Xavier 10:adefba925268 617 if(Count_R == 1 && W_WidsR >= linewide && EnCount_1R >= 54000){ //右2
MCR_Xavier 10:adefba925268 618 WWIDCLR;
MCR_Xavier 10:adefba925268 619 EnCount_1R = 57000;
MCR_Xavier 10:adefba925268 620 Count_R++;}
MCR_Xavier 10:adefba925268 621 else if(Count_R == 1 && EnCount_1R >= 60000) Count_R++;
MCR_Xavier 10:adefba925268 622
MCR_Xavier 10:adefba925268 623 if(Count_R == 2 && EnCount_1R >= 130000)Count_R++; //予備減速
MCR_Xavier 10:adefba925268 624
MCR_Xavier 10:adefba925268 625 if(Count_R == 3 && W_WidsR >= linewide && EnCount_1R >= 150000){ //コーナ1IN
MCR_Xavier 10:adefba925268 626 WWIDCLR;
MCR_Xavier 10:adefba925268 627 EnCount_1R = 153000;
MCR_Xavier 10:adefba925268 628 Count_R++;}
MCR_Xavier 10:adefba925268 629 else if(Count_R == 3 && EnCount_1R >= 156000) Count_R++;
MCR_Xavier 10:adefba925268 630
MCR_Xavier 10:adefba925268 631 if(Count_R == 4 && W_WidsR >= linewide && EnCount_1R >= 205000){ //コーナ1OUT
MCR_Xavier 10:adefba925268 632 WWIDCLR;
MCR_Xavier 10:adefba925268 633 EnCount_1R = 208000;
MCR_Xavier 10:adefba925268 634 Count_R++;}
MCR_Xavier 10:adefba925268 635 else if(Count_R == 4 && EnCount_1R >= 211000) Count_R++;
MCR_Xavier 10:adefba925268 636
MCR_Xavier 10:adefba925268 637 if(Count_R == 5 && W_WidsR >= linewide && EnCount_1R >= 239000){ //右3
MCR_Xavier 10:adefba925268 638 WWIDCLR;
MCR_Xavier 10:adefba925268 639 EnCount_1R = 242000;
MCR_Xavier 10:adefba925268 640 Count_R++;}
MCR_Xavier 10:adefba925268 641 else if(Count_R == 5 && EnCount_1R >= 245000) Count_R++;
MCR_Xavier 10:adefba925268 642
MCR_Xavier 10:adefba925268 643 if(Count_R == 6 && W_WidsL >= linewide && EnCount_1R >= 296000){ //左1
MCR_Xavier 10:adefba925268 644 WWIDCLR;
MCR_Xavier 10:adefba925268 645 EnCount_1R = 299000;
MCR_Xavier 10:adefba925268 646 Count_R++;}
MCR_Xavier 10:adefba925268 647 else if(Count_R == 6 && EnCount_1R >= 302000) Count_R++;
MCR_Xavier 10:adefba925268 648
MCR_Xavier 10:adefba925268 649 if(Count_R == 7 && W_WidsR >= linewide && EnCount_1R >= 399000){ //右3
MCR_Xavier 10:adefba925268 650 WWIDCLR;
MCR_Xavier 10:adefba925268 651 EnCount_1R = 342000;
MCR_Xavier 10:adefba925268 652 Count_R++;}
MCR_Xavier 10:adefba925268 653 else if(Count_R == 7 && EnCount_1R >= 345000) Count_R++;
MCR_Xavier 10:adefba925268 654
MCR_Xavier 10:adefba925268 655 if(Count_R == 8 && W_WidsL >= linewide && EnCount_1R >= 382000){ //左2
MCR_Xavier 10:adefba925268 656 WWIDCLR;
MCR_Xavier 10:adefba925268 657 EnCount_1R = 385000;
MCR_Xavier 10:adefba925268 658 Count_R++;}
MCR_Xavier 10:adefba925268 659 else if(Count_R == 8 && EnCount_1R >= 388000) Count_R++;
MCR_Xavier 10:adefba925268 660
MCR_Xavier 10:adefba925268 661 if(Count_R == 9 && EnCount_1R >= 400000)Count_R++; //予備減速
MCR_Xavier 10:adefba925268 662
MCR_Xavier 10:adefba925268 663 if(Count_R == 10 && W_WidsR >= linewide && EnCount_1R >= 422000){ //コーナ1IN
MCR_Xavier 10:adefba925268 664 WWIDCLR;
MCR_Xavier 10:adefba925268 665 EnCount_1R = 425000;
MCR_Xavier 10:adefba925268 666 Count_R++;}
MCR_Xavier 10:adefba925268 667 else if(Count_R == 10 && EnCount_1R >= 428000) Count_R++;
MCR_Xavier 10:adefba925268 668
MCR_Xavier 10:adefba925268 669 if(Count_R == 11 && W_WidsR >= linewide && EnCount_1R >= 477000){ //コーナ1OUT
MCR_Xavier 10:adefba925268 670 WWIDCLR;
MCR_Xavier 10:adefba925268 671 EnCount_1R = 480000;
MCR_Xavier 10:adefba925268 672 Count_R++;}
MCR_Xavier 10:adefba925268 673 else if(Count_R == 11 && EnCount_1R >= 483000) Count_R++;
MCR_Xavier 10:adefba925268 674
MCR_Xavier 10:adefba925268 675 if(Count_R >= 12 && W_WidsR >= linewide && EnCount_1R >= 540000){ //右1
MCR_Xavier 10:adefba925268 676 Count_C++;
MCR_Xavier 10:adefba925268 677 WWIDCLR;
MCR_Xavier 10:adefba925268 678 EnCount_1R =0;
MCR_Xavier 10:adefba925268 679 Count_R = 1;}
MCR_Xavier 10:adefba925268 680 else if(Count_R >= 12 && EnCount_1R >= 543000){
MCR_Xavier 10:adefba925268 681 Count_C++;
MCR_Xavier 10:adefba925268 682 EnCount_1R = 3000;
MCR_Xavier 10:adefba925268 683 Count_R = 1;}
MCR_Xavier 10:adefba925268 684
MCR_Xavier 10:adefba925268 685 }
MCR_Xavier 5:20bb1e34b354 686 //----------タイマー割り込み---------------
MCR_Xavier 5:20bb1e34b354 687 void flip(){
MCR_Xavier 5:20bb1e34b354 688 timer1++;
MCR_Xavier 7:c99fe0eb544a 689 timer2++; //10msで1cnt
MCR_Xavier 5:20bb1e34b354 690 timer3++;
MCR_Xavier 7:c99fe0eb544a 691 Log_timer++;
MCR_Xavier 6:32698402be64 692 //ErrChk();
MCR_Xavier 9:8ab0d3e46c3c 693 Get_Pot();
MCR_Xavier 5:20bb1e34b354 694 DSens();
MCR_Xavier 9:8ab0d3e46c3c 695 //LineTrace();
MCR_Xavier 9:8ab0d3e46c3c 696 SteerCtrl(SteerMode);
MCR_Xavier 5:20bb1e34b354 697 SpeedCtrl(Speed);
MCR_Xavier 5:20bb1e34b354 698 MotorCtrl();
MCR_Xavier 5:20bb1e34b354 699 Line_Dec();
MCR_Xavier 6:32698402be64 700 LanceAng(Angle);
MCR_Xavier 7:c99fe0eb544a 701 if(ModeSW()==0 && !SW_W) SD_Mount();
MCR_Xavier 7:c99fe0eb544a 702 if(ModeSW()==1 && !SW_W) SD_Unmount();
MCR_Xavier 7:c99fe0eb544a 703 if(writeflg && Tmpcnt <= 9999) TmpLog();
MCR_Xavier 7:c99fe0eb544a 704 enCalc();
MCR_Xavier 7:c99fe0eb544a 705 //if(timer1 >= 1000 && !SW_W && !SW_B && !DebugFlg) Debug();
MCR_Xavier 5:20bb1e34b354 706 //ST_EN;
MCR_Xavier 5:20bb1e34b354 707 /*if(ModeSW()==2){
MCR_Xavier 5:20bb1e34b354 708 ST_EN;
MCR_Xavier 5:20bb1e34b354 709 //OTH_EN;
MCR_Xavier 5:20bb1e34b354 710 Angle = Pot_F + 90;
MCR_Xavier 5:20bb1e34b354 711 }
MCR_Xavier 5:20bb1e34b354 712 else if(ModeSW()==0) {
MCR_Xavier 5:20bb1e34b354 713 ST_DA;
MCR_Xavier 5:20bb1e34b354 714 OTH_DA;
MCR_Xavier 5:20bb1e34b354 715 }*/
MCR_Xavier 7:c99fe0eb544a 716 if(timer3 >= 9){
MCR_Xavier 5:20bb1e34b354 717 //lcd.clear();
MCR_Xavier 5:20bb1e34b354 718 //lcd.printf(0,"Mode:%d,%d",ModeSW(),Pot_R);
MCR_Xavier 5:20bb1e34b354 719 //lcd.printf(0,"%d",timer1);
MCR_Xavier 5:20bb1e34b354 720 //lcd.printf(1,"%d,%d",FL_PWM,EnCountI);
MCR_Xavier 7:c99fe0eb544a 721 timer2++;
MCR_Xavier 5:20bb1e34b354 722 timer3=0;
MCR_Xavier 5:20bb1e34b354 723 }
MCR_Xavier 7:c99fe0eb544a 724 //if( SD_Flag & 0x02)fprintf(fp, "%-10d%-10d%-10d\r\n",EnCount,ZCount,EnCountBuf);
MCR_Xavier 7:c99fe0eb544a 725
MCR_Xavier 7:c99fe0eb544a 726 }
MCR_Xavier 7:c99fe0eb544a 727 //----------Tmpログモード--------------------
MCR_Xavier 7:c99fe0eb544a 728 void TmpLog(){
MCR_Xavier 7:c99fe0eb544a 729
MCR_Xavier 7:c99fe0eb544a 730 Log_tmp[0][Tmpcnt] = EnCount;
MCR_Xavier 7:c99fe0eb544a 731 Log_tmp[1][Tmpcnt] = DSensData[0]>>1;
MCR_Xavier 7:c99fe0eb544a 732 Log_tmp[2][Tmpcnt] = DSensData[1]>>1;
MCR_Xavier 7:c99fe0eb544a 733 Log_tmp[3][Tmpcnt] = DSensData[6]>>1;
MCR_Xavier 7:c99fe0eb544a 734 Log_tmp[4][Tmpcnt] = DSensData[7]>>1;
MCR_Xavier 7:c99fe0eb544a 735 Log_tmp[5][Tmpcnt] = Pot_F;
MCR_Xavier 7:c99fe0eb544a 736 Log_tmp[6][Tmpcnt] = Pot_R >> 2;
MCR_Xavier 7:c99fe0eb544a 737 Log_tmp[7][Tmpcnt] = EnCount;
MCR_Xavier 7:c99fe0eb544a 738 Log_tmp[8][Tmpcnt] = EnCount;
MCR_Xavier 7:c99fe0eb544a 739 Log_tmp[9][Tmpcnt] = EnCount;
MCR_Xavier 7:c99fe0eb544a 740 Tmpcnt++;
MCR_Xavier 7:c99fe0eb544a 741 }
MCR_Xavier 7:c99fe0eb544a 742 //----------SDログモード--------------------
MCR_Xavier 7:c99fe0eb544a 743 void SDLog(){
MCR_Xavier 7:c99fe0eb544a 744 //char i=0;
MCR_Xavier 7:c99fe0eb544a 745 if( SD_Flag & 0x02){
MCR_Xavier 7:c99fe0eb544a 746 //fprintf(fp, "%-10d%-10d\r\n",EnCount,EnCountI);
MCR_Xavier 7:c99fe0eb544a 747 //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 748 //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 749 /*for(i=0;i<=10;i++){
MCR_Xavier 7:c99fe0eb544a 750 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 751 }*/
MCR_Xavier 7:c99fe0eb544a 752 }
MCR_Xavier 7:c99fe0eb544a 753 else Log_timer=0;
MCR_Xavier 7:c99fe0eb544a 754
MCR_Xavier 7:c99fe0eb544a 755 }
MCR_Xavier 7:c99fe0eb544a 756 //----------SD書き込みシーケンス--------------------
MCR_Xavier 7:c99fe0eb544a 757 void SD_WriteSq(){
MCR_Xavier 7:c99fe0eb544a 758 int i=0;
MCR_Xavier 7:c99fe0eb544a 759 if( SD_Flag & 0x02){
MCR_Xavier 7:c99fe0eb544a 760 //fprintf(fp, "\r\nLineWidsL\r\n");
MCR_Xavier 7:c99fe0eb544a 761 fprintf(fp, "EncountI time \r\n");
MCR_Xavier 7:c99fe0eb544a 762 fprintf(fp, "%-10d%-10d\r\n",EnCountI,timer1);
MCR_Xavier 10:adefba925268 763 /*fprintf(fp, "Encount Sens0 Sens1 Sens6 Sens7 Pot_F Pot_R \r\n");
MCR_Xavier 7:c99fe0eb544a 764 for(i=0;i<=9999;i++){
MCR_Xavier 7:c99fe0eb544a 765 //fprintf(fp, "%-10d%-4d%-2d\r\n",Log_W_Wids[i][0],Log_W_Wids[i][1],Log_W_WidsLR[i]);
MCR_Xavier 7:c99fe0eb544a 766 fprintf(fp, "%-4d%-4d%-4d%-4d%-4d%-4d%-4d%-4d%-4d%-4d\r\n",Log_tmp[0][i],Log_tmp[1][i],Log_tmp[2][i],Log_tmp[3][i],Log_tmp[4][i],Log_tmp[5][i],Log_tmp[6][i],Log_tmp[7][i],Log_tmp[8][i],Log_tmp[9][i]);
MCR_Xavier 10:adefba925268 767 }*/
MCR_Xavier 7:c99fe0eb544a 768 fprintf(fp, "EncountI Wids WidsLR \r\n");
MCR_Xavier 7:c99fe0eb544a 769 for(i=0;i<=99;i++){
MCR_Xavier 7:c99fe0eb544a 770 fprintf(fp, "%-10d%-10d%-4d\r\n",Log_W_Wids[i][0],Log_W_Wids[i][1],Log_W_WidsLR[i]);
MCR_Xavier 7:c99fe0eb544a 771 }
MCR_Xavier 7:c99fe0eb544a 772 }
MCR_Xavier 7:c99fe0eb544a 773
MCR_Xavier 5:20bb1e34b354 774 }
MCR_Xavier 5:20bb1e34b354 775 //---------SDカードをマウント-------------
MCR_Xavier 5:20bb1e34b354 776 void SD_Mount(){
MCR_Xavier 5:20bb1e34b354 777 if(SD_Flag == 0x00){
MCR_Xavier 6:32698402be64 778 led_color(RED);
MCR_Xavier 7:c99fe0eb544a 779 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 780 lcd.printf(0,"Mounting");
MCR_Xavier 5:20bb1e34b354 781 fp = fopen("/sd/Log.txt", "w");
MCR_Xavier 6:32698402be64 782 led_color(BLU);
MCR_Xavier 5:20bb1e34b354 783 SD_Flag |= 0x01;
MCR_Xavier 7:c99fe0eb544a 784 if (fp != NULL){
MCR_Xavier 7:c99fe0eb544a 785 SD_Flag |= 0x02; //success
MCR_Xavier 7:c99fe0eb544a 786 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 787 lcd.printf(0,"Success");
MCR_Xavier 7:c99fe0eb544a 788 }
MCR_Xavier 5:20bb1e34b354 789 else SD_Flag |= 0x10; //fail
MCR_Xavier 5:20bb1e34b354 790 }
MCR_Xavier 6:32698402be64 791 led_color(GRE);
MCR_Xavier 5:20bb1e34b354 792 }
MCR_Xavier 5:20bb1e34b354 793
MCR_Xavier 5:20bb1e34b354 794 //---------SDカードをアンマウント-------------
MCR_Xavier 5:20bb1e34b354 795 void SD_Unmount(){
MCR_Xavier 5:20bb1e34b354 796 if(SD_Flag & 0x01){
MCR_Xavier 7:c99fe0eb544a 797 led_color(RED);
MCR_Xavier 7:c99fe0eb544a 798 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 799 lcd.printf(0,"UnMnt...");
MCR_Xavier 7:c99fe0eb544a 800 SD_WriteSq();
MCR_Xavier 5:20bb1e34b354 801 SD_Flag = 0;
screamer 0:525c842a3c89 802 fclose(fp);
MCR_Xavier 7:c99fe0eb544a 803 led_color(WHI);
MCR_Xavier 7:c99fe0eb544a 804 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 805 lcd.printf(0,"Success");
MCR_Xavier 7:c99fe0eb544a 806 wait(1);
MCR_Xavier 5:20bb1e34b354 807 }}
MCR_Xavier 5:20bb1e34b354 808 //----------マイコン初期設定---------------
MCR_Xavier 5:20bb1e34b354 809 void init(void){
MCR_Xavier 5:20bb1e34b354 810 //I/O設定
MCR_Xavier 5:20bb1e34b354 811 RS1.mode(PullDown); //ロータリスイッチbit1
MCR_Xavier 5:20bb1e34b354 812 RS2.mode(PullDown); //ロータリスイッチbit2
MCR_Xavier 5:20bb1e34b354 813 RS3.mode(PullDown); //ロータリスイッチbit3
MCR_Xavier 5:20bb1e34b354 814 RS4.mode(PullDown); //ロータリスイッチbit4
MCR_Xavier 5:20bb1e34b354 815 SW_W.mode(PullUp); //スイッチ白
MCR_Xavier 5:20bb1e34b354 816 SW_R.mode(PullUp); //スイッチ赤
MCR_Xavier 5:20bb1e34b354 817 SW_B.mode(PullUp); //スイッチ黒
MCR_Xavier 5:20bb1e34b354 818 s0signal.rise(&s0_trigger_rise); //エンコーダA相立ち上がり
MCR_Xavier 5:20bb1e34b354 819 s0signal.fall(&s0_trigger_fall); //エンコーダA相立ち下がり
MCR_Xavier 5:20bb1e34b354 820 s1signal.rise(&s1_trigger_rise); //エンコーダB相立ち上がり
MCR_Xavier 5:20bb1e34b354 821 s1signal.fall(&s1_trigger_fall); //エンコーダB相立ち下がり
MCR_Xavier 5:20bb1e34b354 822 //割り込み処理開始
MCR_Xavier 5:20bb1e34b354 823 flipper.attach_us(&flip,1000); //汎用タイマー割り込み
MCR_Xavier 7:c99fe0eb544a 824 SDLogflip.attach_us(&SDLog,10000); //ログ取り割り込み
MCR_Xavier 5:20bb1e34b354 825 sensget.attach_us(&sensGet,100); //センサー用タイマー割り込み
MCR_Xavier 5:20bb1e34b354 826 //PWM周期設定
MCR_Xavier 5:20bb1e34b354 827 PWM_CH1.period(0.0001);
MCR_Xavier 5:20bb1e34b354 828 PWM_CH2.period(0.001);
MCR_Xavier 5:20bb1e34b354 829 PWM_CH3.period(0.001);
MCR_Xavier 5:20bb1e34b354 830 PWM_CH4.period(0.001);
MCR_Xavier 5:20bb1e34b354 831 PWM_CH5.period(0.001);
MCR_Xavier 5:20bb1e34b354 832 PWM_CH6.period(0.0001);
MCR_Xavier 5:20bb1e34b354 833 ST_DA;
MCR_Xavier 5:20bb1e34b354 834 OTH_DA;
screamer 0:525c842a3c89 835 }
MCR_Xavier 5:20bb1e34b354 836 //----------------メイン---------------------
MCR_Xavier 10:adefba925268 837 int main(){
MCR_Xavier 5:20bb1e34b354 838 init(); //マイコン初期設定
MCR_Xavier 6:32698402be64 839 wait(1);
MCR_Xavier 5:20bb1e34b354 840 ST_EN;
MCR_Xavier 5:20bb1e34b354 841 lcd.contrast( 0x2E ); //LCDコントラスト設定
MCR_Xavier 5:20bb1e34b354 842 wait(0.5);
MCR_Xavier 5:20bb1e34b354 843 while(SW_R){
MCR_Xavier 5:20bb1e34b354 844 if(!SW_W && !SW_B && !DebugFlg) Debug();
MCR_Xavier 5:20bb1e34b354 845 lcd.clear();
MCR_Xavier 5:20bb1e34b354 846 lcd.printf(0,"Wait...");
MCR_Xavier 6:32698402be64 847 lcd.printf(1,"%x",SD_Flag);
MCR_Xavier 6:32698402be64 848 //if(!SW_W) SD_Mount();
MCR_Xavier 10:adefba925268 849 wait(0.1);
MCR_Xavier 5:20bb1e34b354 850 }
MCR_Xavier 5:20bb1e34b354 851 lcd.clear();
MCR_Xavier 5:20bb1e34b354 852 lcd.printf(0,"Ready!");
MCR_Xavier 6:32698402be64 853 lcd.printf(1,"%x",SD_Flag);
MCR_Xavier 7:c99fe0eb544a 854 writeflg = 1;
MCR_Xavier 5:20bb1e34b354 855 wait(1);
MCR_Xavier 5:20bb1e34b354 856 lcd.clear();
MCR_Xavier 5:20bb1e34b354 857 lcd.printf(0,"Go!");
MCR_Xavier 5:20bb1e34b354 858 OTH_EN;
MCR_Xavier 5:20bb1e34b354 859 W_WidsL = W_WidsR = 0;
MCR_Xavier 10:adefba925268 860 //Speed = SPCom;
MCR_Xavier 6:32698402be64 861 Angle = 90;
MCR_Xavier 9:8ab0d3e46c3c 862 SteerMode = TRACE;
MCR_Xavier 10:adefba925268 863 Speed = SPCom;
MCR_Xavier 10:adefba925268 864 EnCount_1R = -40000;
MCR_Xavier 5:20bb1e34b354 865 while(1){
MCR_Xavier 10:adefba925268 866
MCR_Xavier 10:adefba925268 867 if(!SW_W && !SW_B && !DebugFlg) Debug();
MCR_Xavier 10:adefba925268 868
MCR_Xavier 10:adefba925268 869 Localization();
MCR_Xavier 10:adefba925268 870 if(Count_C >= 2 ) Stop_Sq();
MCR_Xavier 6:32698402be64 871
MCR_Xavier 10:adefba925268 872 if(Count_R == 0 ){
MCR_Xavier 10:adefba925268 873 Speed = SPCom;
MCR_Xavier 10:adefba925268 874 led_color(WHI);}
MCR_Xavier 10:adefba925268 875 if(Count_R == 1 ){
MCR_Xavier 10:adefba925268 876 Speed = SPCom;
MCR_Xavier 10:adefba925268 877 led_color(RED);}
MCR_Xavier 10:adefba925268 878 if(Count_R == 2 ){
MCR_Xavier 10:adefba925268 879 Speed = SPCom;
MCR_Xavier 10:adefba925268 880 led_color(GRE);}
MCR_Xavier 10:adefba925268 881 if(Count_R == 3 ){
MCR_Xavier 10:adefba925268 882 Speed = SPYobi;
MCR_Xavier 10:adefba925268 883 Angle = 90;
MCR_Xavier 10:adefba925268 884 led_color(BLU);}
MCR_Xavier 10:adefba925268 885 if(Count_R == 4 ){
MCR_Xavier 10:adefba925268 886 led_color(YEL);
MCR_Xavier 10:adefba925268 887 Speed = SPCor;}
MCR_Xavier 10:adefba925268 888 if(Count_R == 5 ){
MCR_Xavier 10:adefba925268 889 Speed = SPCom;
MCR_Xavier 10:adefba925268 890 Angle = 122;
MCR_Xavier 10:adefba925268 891 led_color(RED);}
MCR_Xavier 10:adefba925268 892 if(Count_R == 6 ){
MCR_Xavier 10:adefba925268 893 Speed = SPCom;
MCR_Xavier 6:32698402be64 894 led_color(GRE);
MCR_Xavier 10:adefba925268 895 if(EnCount_1R >= 270000) Angle = 55;
MCR_Xavier 10:adefba925268 896 else Angle = 122;
MCR_Xavier 6:32698402be64 897 }
MCR_Xavier 10:adefba925268 898 if(Count_R == 7 ){
MCR_Xavier 10:adefba925268 899 Speed = SPCom;
MCR_Xavier 10:adefba925268 900 led_color(PUR);
MCR_Xavier 10:adefba925268 901 if(EnCount_1R >= 320000) Angle = 122;
MCR_Xavier 10:adefba925268 902 else Angle = 55;
MCR_Xavier 6:32698402be64 903 }
MCR_Xavier 10:adefba925268 904 if(Count_R == 8 ){
MCR_Xavier 10:adefba925268 905 Speed = SPCom;
MCR_Xavier 6:32698402be64 906 led_color(RED);
MCR_Xavier 10:adefba925268 907 if(EnCount_1R >= 360000) Angle = 55;
MCR_Xavier 10:adefba925268 908 else Angle = 122;
MCR_Xavier 6:32698402be64 909 }
MCR_Xavier 10:adefba925268 910 if(Count_R == 9 ){
MCR_Xavier 10:adefba925268 911 Speed = SPCom;
MCR_Xavier 10:adefba925268 912 Angle = 55;
MCR_Xavier 10:adefba925268 913 led_color(GRE);}
MCR_Xavier 10:adefba925268 914 if(Count_R == 10){
MCR_Xavier 10:adefba925268 915 Speed = SPYobi;
MCR_Xavier 10:adefba925268 916 led_color(WHI);}
MCR_Xavier 10:adefba925268 917 if(Count_R == 11){
MCR_Xavier 10:adefba925268 918 led_color(BLU);
MCR_Xavier 10:adefba925268 919 Angle = 90;
MCR_Xavier 10:adefba925268 920 Speed = SPCor;}
MCR_Xavier 10:adefba925268 921 if(Count_R == 12){
MCR_Xavier 10:adefba925268 922 Speed = SPCom;
MCR_Xavier 10:adefba925268 923 led_color(YEL);}
MCR_Xavier 10:adefba925268 924
MCR_Xavier 10:adefba925268 925 }
MCR_Xavier 7:c99fe0eb544a 926 }
MCR_Xavier 7:c99fe0eb544a 927 //----------デバッグモード--------------------
MCR_Xavier 7:c99fe0eb544a 928 void Debug(void){
MCR_Xavier 7:c99fe0eb544a 929 int DebugModeSW;
MCR_Xavier 7:c99fe0eb544a 930 DebugFlg=1;
MCR_Xavier 7:c99fe0eb544a 931 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 932 lcd.printf(0,"DBGMode");
MCR_Xavier 7:c99fe0eb544a 933 wait(2);
MCR_Xavier 7:c99fe0eb544a 934 while( SW_R ){
MCR_Xavier 7:c99fe0eb544a 935 DebugModeSW = ModeSW();
MCR_Xavier 7:c99fe0eb544a 936 switch(DebugModeSW){
MCR_Xavier 7:c99fe0eb544a 937 case 1:
MCR_Xavier 7:c99fe0eb544a 938 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 939 lcd.printf(0,"SensT/F");
MCR_Xavier 7:c99fe0eb544a 940 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 941 break;
MCR_Xavier 7:c99fe0eb544a 942 case 2:
MCR_Xavier 7:c99fe0eb544a 943 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 944 lcd.printf(0,"SensA/D");
MCR_Xavier 7:c99fe0eb544a 945 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 946 break;
MCR_Xavier 7:c99fe0eb544a 947 case 3:
MCR_Xavier 7:c99fe0eb544a 948 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 949 lcd.printf(0,"Trip");
MCR_Xavier 7:c99fe0eb544a 950 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 951 break;
MCR_Xavier 7:c99fe0eb544a 952 case 4:
MCR_Xavier 7:c99fe0eb544a 953 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 954 lcd.printf(0,"LinWidLR");
MCR_Xavier 7:c99fe0eb544a 955 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 956 break;
MCR_Xavier 7:c99fe0eb544a 957 case 5:
MCR_Xavier 7:c99fe0eb544a 958 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 959 lcd.printf(0,"LineCnr");
MCR_Xavier 7:c99fe0eb544a 960 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 961 break;
MCR_Xavier 7:c99fe0eb544a 962 case 6:
MCR_Xavier 7:c99fe0eb544a 963 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 964 lcd.printf(0,"LineA/D");
MCR_Xavier 7:c99fe0eb544a 965 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 966 break;
MCR_Xavier 7:c99fe0eb544a 967 case 7:
MCR_Xavier 7:c99fe0eb544a 968 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 969 lcd.printf(0,"PotA/D");
MCR_Xavier 7:c99fe0eb544a 970 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 971 break;
MCR_Xavier 7:c99fe0eb544a 972 case 8:
MCR_Xavier 7:c99fe0eb544a 973 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 974 lcd.printf(0,"Speed5");
MCR_Xavier 7:c99fe0eb544a 975 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 976 break;
MCR_Xavier 7:c99fe0eb544a 977 case 9:
MCR_Xavier 7:c99fe0eb544a 978 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 979 lcd.printf(0,"Speed6");
MCR_Xavier 7:c99fe0eb544a 980 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 981 break;
MCR_Xavier 7:c99fe0eb544a 982 case 10:
MCR_Xavier 7:c99fe0eb544a 983 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 984 lcd.printf(0,"LoopJump");
MCR_Xavier 7:c99fe0eb544a 985 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 986 break;
MCR_Xavier 7:c99fe0eb544a 987 case 11:
MCR_Xavier 7:c99fe0eb544a 988 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 989 lcd.printf(0,"linewide");
MCR_Xavier 7:c99fe0eb544a 990 lcd.printf(1,"400");
MCR_Xavier 7:c99fe0eb544a 991 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 992 break;
MCR_Xavier 7:c99fe0eb544a 993 case 12:
MCR_Xavier 7:c99fe0eb544a 994 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 995 lcd.printf(0,"linewide");
MCR_Xavier 7:c99fe0eb544a 996 lcd.printf(1,"100");
MCR_Xavier 7:c99fe0eb544a 997 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 998 break;
MCR_Xavier 7:c99fe0eb544a 999 case 13:
MCR_Xavier 7:c99fe0eb544a 1000 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1001 lcd.printf(0,"SD_Mount?");
MCR_Xavier 7:c99fe0eb544a 1002 lcd.printf(1,"%x",SD_Flag);
MCR_Xavier 7:c99fe0eb544a 1003 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1004 if(!SW_W) SD_Mount();
MCR_Xavier 7:c99fe0eb544a 1005 break;
MCR_Xavier 7:c99fe0eb544a 1006 case 14:
MCR_Xavier 7:c99fe0eb544a 1007 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1008 lcd.printf(0,"SD_UnMount?");
MCR_Xavier 7:c99fe0eb544a 1009 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1010 if(!SW_W) SD_Unmount();
MCR_Xavier 7:c99fe0eb544a 1011 break;
MCR_Xavier 7:c99fe0eb544a 1012 case 15:
MCR_Xavier 7:c99fe0eb544a 1013 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1014 lcd.printf(0,"DSPSens");
MCR_Xavier 7:c99fe0eb544a 1015 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1016 break;
MCR_Xavier 7:c99fe0eb544a 1017 default:
MCR_Xavier 7:c99fe0eb544a 1018 break;
MCR_Xavier 7:c99fe0eb544a 1019 }
MCR_Xavier 7:c99fe0eb544a 1020 }
MCR_Xavier 7:c99fe0eb544a 1021 wait(2);
MCR_Xavier 7:c99fe0eb544a 1022 while( SW_R){
MCR_Xavier 7:c99fe0eb544a 1023 DebugModeSW = ModeSW();
MCR_Xavier 7:c99fe0eb544a 1024 switch(DebugModeSW){
MCR_Xavier 7:c99fe0eb544a 1025 case 1:
MCR_Xavier 7:c99fe0eb544a 1026 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1027 lcd.printf(0,"0167RL");
MCR_Xavier 7:c99fe0eb544a 1028 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 1029 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1030 break;
MCR_Xavier 7:c99fe0eb544a 1031 case 2:
MCR_Xavier 7:c99fe0eb544a 1032 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1033 lcd.printf(0,"SensA/D");
MCR_Xavier 7:c99fe0eb544a 1034 lcd.printf(1,"%3d,%3d",DSensData[3],DSensData[4]);
MCR_Xavier 7:c99fe0eb544a 1035 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1036 break;
MCR_Xavier 7:c99fe0eb544a 1037 case 3:
MCR_Xavier 7:c99fe0eb544a 1038 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1039 lcd.printf(0,"%d",EnCount );
MCR_Xavier 7:c99fe0eb544a 1040 lcd.printf(1,"%d",EnCountI);
MCR_Xavier 7:c99fe0eb544a 1041 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1042 break;
MCR_Xavier 7:c99fe0eb544a 1043 case 4:
MCR_Xavier 7:c99fe0eb544a 1044 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1045 lcd.printf(0,"%d",W_WidsL );
MCR_Xavier 7:c99fe0eb544a 1046 lcd.printf(1,"%d",W_WidsR );
MCR_Xavier 7:c99fe0eb544a 1047 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1048 break;
MCR_Xavier 7:c99fe0eb544a 1049 case 5:
MCR_Xavier 7:c99fe0eb544a 1050 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1051 lcd.printf(0,"%d",R_LineCnt );
MCR_Xavier 7:c99fe0eb544a 1052 lcd.printf(1,"%d",L_LineCnt );
MCR_Xavier 7:c99fe0eb544a 1053 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1054 break;
MCR_Xavier 7:c99fe0eb544a 1055 case 6:
MCR_Xavier 7:c99fe0eb544a 1056 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1057 lcd.printf(0,"%3d,%3d",DSensData[0],DSensData[1]);
MCR_Xavier 7:c99fe0eb544a 1058 lcd.printf(1,"%3d,%3d",DSensData[6],DSensData[7]);
MCR_Xavier 7:c99fe0eb544a 1059 wait(0.5);
MCR_Xavier 7:c99fe0eb544a 1060 break;
MCR_Xavier 7:c99fe0eb544a 1061 case 7:
MCR_Xavier 7:c99fe0eb544a 1062 lcd.clear();
MCR_Xavier 10:adefba925268 1063 /*lcd.printf(0,"%d",Pot_F2);
MCR_Xavier 10:adefba925268 1064 lcd.printf(1,"%d",Pot_R);*/
MCR_Xavier 10:adefba925268 1065 lcd.printf(0,"%3d,%3d",a[1],a[5]);
MCR_Xavier 10:adefba925268 1066 lcd.printf(1,"%3d,%3d",a[6],a[10]);
MCR_Xavier 7:c99fe0eb544a 1067 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1068 break;
MCR_Xavier 7:c99fe0eb544a 1069 case 8:
MCR_Xavier 7:c99fe0eb544a 1070 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1071 lcd.printf(0,"Speed5");
MCR_Xavier 7:c99fe0eb544a 1072 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1073 SPCom=5000;
MCR_Xavier 8:352404951de8 1074 //SPYobi=3500;
MCR_Xavier 8:352404951de8 1075 //SPCor=3000;
MCR_Xavier 8:352404951de8 1076 //ParamS=2000;
MCR_Xavier 7:c99fe0eb544a 1077 break;
MCR_Xavier 7:c99fe0eb544a 1078 case 9:
MCR_Xavier 7:c99fe0eb544a 1079 lcd.clear();
MCR_Xavier 8:352404951de8 1080 lcd.printf(0,"Speed7");
MCR_Xavier 7:c99fe0eb544a 1081 wait(0.1);
MCR_Xavier 8:352404951de8 1082 SPCom=7000;
MCR_Xavier 8:352404951de8 1083 //SPYobi=3000;
MCR_Xavier 8:352404951de8 1084 //SPCor=3000;
MCR_Xavier 7:c99fe0eb544a 1085 break;
MCR_Xavier 7:c99fe0eb544a 1086 case 10:
MCR_Xavier 7:c99fe0eb544a 1087 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1088 lcd.printf(0,"LoopJump");
MCR_Xavier 7:c99fe0eb544a 1089 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1090 loopJump = 1000000;
MCR_Xavier 7:c99fe0eb544a 1091 break;
MCR_Xavier 7:c99fe0eb544a 1092 case 11:
MCR_Xavier 7:c99fe0eb544a 1093 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1094 lcd.printf(0,"linewide");
MCR_Xavier 7:c99fe0eb544a 1095 lcd.printf(1,"400");
MCR_Xavier 7:c99fe0eb544a 1096 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1097 linewide=400;
MCR_Xavier 7:c99fe0eb544a 1098 break;
MCR_Xavier 7:c99fe0eb544a 1099 case 12:
MCR_Xavier 7:c99fe0eb544a 1100 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1101 lcd.printf(0,"linewide");
MCR_Xavier 7:c99fe0eb544a 1102 lcd.printf(1,"100");
MCR_Xavier 7:c99fe0eb544a 1103 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1104 linewide=100;
MCR_Xavier 7:c99fe0eb544a 1105 break;
MCR_Xavier 7:c99fe0eb544a 1106 case 15:
MCR_Xavier 7:c99fe0eb544a 1107 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1108 lcd.printf(0,"%d",DSensData[0]);
MCR_Xavier 7:c99fe0eb544a 1109 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1110 break;
MCR_Xavier 7:c99fe0eb544a 1111 default:
MCR_Xavier 7:c99fe0eb544a 1112 break;
MCR_Xavier 7:c99fe0eb544a 1113 }
MCR_Xavier 7:c99fe0eb544a 1114 }
MCR_Xavier 7:c99fe0eb544a 1115
MCR_Xavier 7:c99fe0eb544a 1116
MCR_Xavier 7:c99fe0eb544a 1117 while( SW_W ){
MCR_Xavier 7:c99fe0eb544a 1118 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1119 lcd.printf(0,"Exit?");
MCR_Xavier 7:c99fe0eb544a 1120 wait(0.1);
MCR_Xavier 7:c99fe0eb544a 1121 }
MCR_Xavier 7:c99fe0eb544a 1122 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1123 lcd.printf(0,"Bye-Bye");
MCR_Xavier 7:c99fe0eb544a 1124 wait(2);
MCR_Xavier 7:c99fe0eb544a 1125 lcd.clear();
MCR_Xavier 7:c99fe0eb544a 1126 DebugFlg=0;
MCR_Xavier 7:c99fe0eb544a 1127 }