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