Takeshi Nishimura / Mbed 2 deprecated MiniTracer2018_V1

MiniTracerのテストプログラムです

Dependencies:   mbed

main.cpp@0:21afd0549d07, 2018-04-28 (annotated)

Committer:
MCR_Xavier
Date:
Sat Apr 28 06:26:41 2018 +0000
Revision:
0:21afd0549d07
MiniTracer???????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MCR_Xavier 0:21afd0549d07 1 #include "mbed.h"
MCR_Xavier 0:21afd0549d07 2
MCR_Xavier 0:21afd0549d07 3 //シンボル定義
MCR_Xavier 0:21afd0549d07 4 #define FW 0 //前進
MCR_Xavier 0:21afd0549d07 5 #define BW 1 //後進
MCR_Xavier 0:21afd0549d07 6
MCR_Xavier 0:21afd0549d07 7 AnalogIn LineL(PA_0); //アナログラインセンサ
MCR_Xavier 0:21afd0549d07 8 AnalogIn LineR(PA_1); //アナログラインセンサ
MCR_Xavier 0:21afd0549d07 9 AnalogIn Volume(PA_4); //ボリューム入力
MCR_Xavier 0:21afd0549d07 10 DigitalOut led1(PB_7); //LED_1
MCR_Xavier 0:21afd0549d07 11 DigitalOut led2(PB_6); //LED_2
MCR_Xavier 0:21afd0549d07 12 DigitalOut led3(PB_5); //LED_3
MCR_Xavier 0:21afd0549d07 13 DigitalOut led4(PB_4); //LED_4
MCR_Xavier 0:21afd0549d07 14 DigitalOut L_Dir(PA_12); //左モータ回転方向
MCR_Xavier 0:21afd0549d07 15 DigitalOut R_Dir(PA_9); //右モータ回転方向
MCR_Xavier 0:21afd0549d07 16 DigitalOut MotorDA(PA_11); //モーター出力EN
MCR_Xavier 0:21afd0549d07 17 DigitalIn SW_IN(PB_3); //スイッチ入力
MCR_Xavier 0:21afd0549d07 18 PwmOut PWM_L(PA_8); //左モータPWM
MCR_Xavier 0:21afd0549d07 19 PwmOut PWM_R(PA_10); //右モータPWM
MCR_Xavier 0:21afd0549d07 20 //AQM0802A lcd(PB_7,PB_6);
MCR_Xavier 0:21afd0549d07 21 Serial pc(USBTX, USBRX); // tx, rx
MCR_Xavier 0:21afd0549d07 22
MCR_Xavier 0:21afd0549d07 23
MCR_Xavier 0:21afd0549d07 24 //割り込み定義
MCR_Xavier 0:21afd0549d07 25 Ticker flipper; //汎用タイマー
MCR_Xavier 0:21afd0549d07 26
MCR_Xavier 0:21afd0549d07 27 //プロトタイプ宣言
MCR_Xavier 0:21afd0549d07 28 void init(void); //マイコン初期設定
MCR_Xavier 0:21afd0549d07 29 void led_out(void); //LED出力
MCR_Xavier 0:21afd0549d07 30 void MotorCtrl(void); //モータ管理
MCR_Xavier 0:21afd0549d07 31 void LineTrace(void); //ライントレース
MCR_Xavier 0:21afd0549d07 32 void SensUp(void); //センサー値更新
MCR_Xavier 0:21afd0549d07 33 int GetVol(void); //ボリューム値取得
MCR_Xavier 0:21afd0549d07 34
MCR_Xavier 0:21afd0549d07 35 //グローバル変数の宣言
MCR_Xavier 0:21afd0549d07 36 int timer1=0; //汎用タイマー
MCR_Xavier 0:21afd0549d07 37 int ledval=1; //LED出力値
MCR_Xavier 0:21afd0549d07 38 int MotorL_Rev=0,MotorR_Rev=0; //モータの回転方向
MCR_Xavier 0:21afd0549d07 39 int MotorL=0,MotorR=0; //モータPWMデューティ比
MCR_Xavier 0:21afd0549d07 40 int ErrFlg=0; //エラー判定フラグ
MCR_Xavier 0:21afd0549d07 41 int SensValBuf=0; //センサ値のバッファ
MCR_Xavier 0:21afd0549d07 42 int SensorR,SensorL; //ラインセンサ
MCR_Xavier 0:21afd0549d07 43 //----------ボリューム値取得-----------------
MCR_Xavier 0:21afd0549d07 44 int GetVol(void){
MCR_Xavier 0:21afd0549d07 45 int Val;
MCR_Xavier 0:21afd0549d07 46 Val = Volume.read_u16()>>12;
MCR_Xavier 0:21afd0549d07 47 return Val;
MCR_Xavier 0:21afd0549d07 48 }
MCR_Xavier 0:21afd0549d07 49 //----------センサ値更新-----------------
MCR_Xavier 0:21afd0549d07 50 void SensUp(void){
MCR_Xavier 0:21afd0549d07 51 SensorR = LineR.read_u16()>>8;
MCR_Xavier 0:21afd0549d07 52 SensorL = LineL.read_u16()>>8;
MCR_Xavier 0:21afd0549d07 53 }
MCR_Xavier 0:21afd0549d07 54 //------------モータ管理--------------------
MCR_Xavier 0:21afd0549d07 55 void MotorCtrl(void){
MCR_Xavier 0:21afd0549d07 56 int RMotorVal,LMotorVal;
MCR_Xavier 0:21afd0549d07 57 if(ErrFlg){ //異常判定
MCR_Xavier 0:21afd0549d07 58 PWM_L.pulsewidth_us(0); //左PWM出力0
MCR_Xavier 0:21afd0549d07 59 PWM_R.pulsewidth_us(0); //右PWM出力0
MCR_Xavier 0:21afd0549d07 60 }
MCR_Xavier 0:21afd0549d07 61 else{ //通常時
MCR_Xavier 0:21afd0549d07 62 if(MotorL < 0 ){
MCR_Xavier 0:21afd0549d07 63 LMotorVal = MotorL * (-1);
MCR_Xavier 0:21afd0549d07 64 MotorL_Rev = BW;}
MCR_Xavier 0:21afd0549d07 65 else{
MCR_Xavier 0:21afd0549d07 66 LMotorVal = MotorL;
MCR_Xavier 0:21afd0549d07 67 MotorL_Rev = FW;}
MCR_Xavier 0:21afd0549d07 68
MCR_Xavier 0:21afd0549d07 69 if(MotorR < 0 ){
MCR_Xavier 0:21afd0549d07 70 RMotorVal = MotorR * (-1);
MCR_Xavier 0:21afd0549d07 71 MotorR_Rev = BW;}
MCR_Xavier 0:21afd0549d07 72 else{
MCR_Xavier 0:21afd0549d07 73 RMotorVal = MotorR;
MCR_Xavier 0:21afd0549d07 74 MotorR_Rev = FW;}
MCR_Xavier 0:21afd0549d07 75
MCR_Xavier 0:21afd0549d07 76 if(LMotorVal >= 1000) MotorL = 1000;
MCR_Xavier 0:21afd0549d07 77 if(RMotorVal >= 1000) MotorR = 1000;
MCR_Xavier 0:21afd0549d07 78 PWM_L.pulsewidth_us(LMotorVal); //左PWM (0~1000)
MCR_Xavier 0:21afd0549d07 79 PWM_R.pulsewidth_us(RMotorVal); //右PWM (0~1000)
MCR_Xavier 0:21afd0549d07 80 }
MCR_Xavier 0:21afd0549d07 81
MCR_Xavier 0:21afd0549d07 82 L_Dir = MotorL_Rev; //右モータ回転方向（H：CW）
MCR_Xavier 0:21afd0549d07 83 R_Dir = !MotorR_Rev; //左モータ回転方向（L：FW）
MCR_Xavier 0:21afd0549d07 84 }
MCR_Xavier 0:21afd0549d07 85 //------------ライントレース--------------------
MCR_Xavier 0:21afd0549d07 86 void LineTrace(void){
MCR_Xavier 0:21afd0549d07 87 int SensVal,CommSpeed=100,PGain=1;
MCR_Xavier 0:21afd0549d07 88
MCR_Xavier 0:21afd0549d07 89 SensVal = SensorR - SensorL;
MCR_Xavier 0:21afd0549d07 90 MotorR = int(CommSpeed - (SensVal * PGain));
MCR_Xavier 0:21afd0549d07 91 MotorL = int(CommSpeed + (SensVal * PGain));
MCR_Xavier 0:21afd0549d07 92 }
MCR_Xavier 0:21afd0549d07 93 //-------------LED出力------------------
MCR_Xavier 0:21afd0549d07 94 void led_out(void){
MCR_Xavier 0:21afd0549d07 95 if(ledval & 0x01) led1 = 1;
MCR_Xavier 0:21afd0549d07 96 else led1 = 0;
MCR_Xavier 0:21afd0549d07 97 if(ledval & 0x02) led2 = 1;
MCR_Xavier 0:21afd0549d07 98 else led2 = 0;
MCR_Xavier 0:21afd0549d07 99 if(ledval & 0x04) led3 = 1;
MCR_Xavier 0:21afd0549d07 100 else led3 = 0;
MCR_Xavier 0:21afd0549d07 101 if(ledval & 0x08) led4 = 1;
MCR_Xavier 0:21afd0549d07 102 else led4 = 0;
MCR_Xavier 0:21afd0549d07 103 }
MCR_Xavier 0:21afd0549d07 104
MCR_Xavier 0:21afd0549d07 105 //----------タイマー割り込み---------------
MCR_Xavier 0:21afd0549d07 106 void flip(){
MCR_Xavier 0:21afd0549d07 107 timer1++;
MCR_Xavier 0:21afd0549d07 108 MotorCtrl();
MCR_Xavier 0:21afd0549d07 109 led_out();
MCR_Xavier 0:21afd0549d07 110 SensUp();
MCR_Xavier 0:21afd0549d07 111 LineTrace();
MCR_Xavier 0:21afd0549d07 112 }
MCR_Xavier 0:21afd0549d07 113
MCR_Xavier 0:21afd0549d07 114 //----------マイコン初期設定---------------
MCR_Xavier 0:21afd0549d07 115 void init(void){
MCR_Xavier 0:21afd0549d07 116 //I/O設定
MCR_Xavier 0:21afd0549d07 117 /*
MCR_Xavier 0:21afd0549d07 118 RS1.mode(PullDown); //ロータリスイッチbit1
MCR_Xavier 0:21afd0549d07 119 RS2.mode(PullDown); //ロータリスイッチbit2
MCR_Xavier 0:21afd0549d07 120 RS3.mode(PullDown); //ロータリスイッチbit3
MCR_Xavier 0:21afd0549d07 121 RS4.mode(PullDown); //ロータリスイッチbit4*/
MCR_Xavier 0:21afd0549d07 122 SW_IN.mode(PullUp); //スイッチ入力ピンプルアップ
MCR_Xavier 0:21afd0549d07 123 MotorDA = 1;
MCR_Xavier 0:21afd0549d07 124
MCR_Xavier 0:21afd0549d07 125 //割り込み処理開始
MCR_Xavier 0:21afd0549d07 126 flipper.attach_us(&flip,1000); //汎用タイマー割り込み
MCR_Xavier 0:21afd0549d07 127 //PWM周期設定
MCR_Xavier 0:21afd0549d07 128 PWM_L.period(0.001);
MCR_Xavier 0:21afd0549d07 129 PWM_R.period(0.001);
MCR_Xavier 0:21afd0549d07 130 }
MCR_Xavier 0:21afd0549d07 131
MCR_Xavier 0:21afd0549d07 132 //---------------メイン--------------------
MCR_Xavier 0:21afd0549d07 133 int main() {
MCR_Xavier 0:21afd0549d07 134 init();
MCR_Xavier 0:21afd0549d07 135 int sensV;
MCR_Xavier 0:21afd0549d07 136 wait(1);
MCR_Xavier 0:21afd0549d07 137 while(SW_IN);
MCR_Xavier 0:21afd0549d07 138 MotorDA = 0;
MCR_Xavier 0:21afd0549d07 139 while(1) {
MCR_Xavier 0:21afd0549d07 140 if(!SW_IN) pc.printf("%5d %5d %5d %5d \r\n",SensorL,SensorR,MotorL,MotorR);
MCR_Xavier 0:21afd0549d07 141 ledval = sensV;
MCR_Xavier 0:21afd0549d07 142 wait(0.02);
MCR_Xavier 0:21afd0549d07 143
MCR_Xavier 0:21afd0549d07 144 }
MCR_Xavier 0:21afd0549d07 145 }