Takeshi Nishimura / Mbed 2 deprecated Lancer2016

ランサー

Dependencies:   SB1602E SDFileSystem mbed

Fork of Seeed_SDCard_Shield by Shields

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?

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"
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 //----------水平１--------------------
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 }