Teruaki Motomiya / Nucleo-motor-control2

NUCLEOのキットを使ったルーレット

Dependencies:   mbed-src

Fork of Nucleo_motor_control2 by Teruaki Motomiya

main.cpp@0:b8098a4ce012, 2016-03-22 (annotated)

Committer:
motomiya
Date:
Tue Mar 22 22:53:00 2016 +0000
Revision:
0:b8098a4ce012
????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
motomiya 0:b8098a4ce012 1 /*************************************************
motomiya 0:b8098a4ce012 2 "Nucleo_motor_control2"
motomiya 0:b8098a4ce012 3 Nucleo Pack
motomiya 0:b8098a4ce012 4 マイコンボード　NUCLEO-F302R8
motomiya 0:b8098a4ce012 5 拡張ボード　X-NUCLEO-IHM07M1
motomiya 0:b8098a4ce012 6 モーター　BR2804-1700KV-1
motomiya 0:b8098a4ce012 7 を使ったモーター制御の実験
motomiya 0:b8098a4ce012 8
motomiya 0:b8098a4ce012 9 アナログPWM版 ルーレット仕様
motomiya 0:b8098a4ce012 10 Copyright(C) 2016 Columbus-seiki
motomiya 0:b8098a4ce012 11
motomiya 0:b8098a4ce012 12 2016-02-22 Rev0
motomiya 0:b8098a4ce012 13
motomiya 0:b8098a4ce012 14 **************************************************/
motomiya 0:b8098a4ce012 15 #include "mbed.h"
motomiya 0:b8098a4ce012 16 #include "main.h"
motomiya 0:b8098a4ce012 17
motomiya 0:b8098a4ce012 18 #define POLE_PAIR 7//モーターの磁極対数
motomiya 0:b8098a4ce012 19 #define PERIOD_TIMER 0.005 //タイマー周期(S)
motomiya 0:b8098a4ce012 20 #define SINTBL_LENGTH 512 //sinTbl[]要素数
motomiya 0:b8098a4ce012 21 #define PERIOD_PWM 0.0002 //PWM周期(S)　これ以下では　ヒゲ　がでる
motomiya 0:b8098a4ce012 22 #define HALF_PWM (PERIOD_PWM / 2) //PWM周期の真ん中　PERIOD_PWM÷2
motomiya 0:b8098a4ce012 23 #define DEG360 1024 //360°相当
motomiya 0:b8098a4ce012 24 #define DEG120 341 //120°
motomiya 0:b8098a4ce012 25 #define DEG240 682 //240°
motomiya 0:b8098a4ce012 26
motomiya 0:b8098a4ce012 27
motomiya 0:b8098a4ce012 28 //入出力ピンを定義
motomiya 0:b8098a4ce012 29 DigitalIn mybutton(USER_BUTTON);
motomiya 0:b8098a4ce012 30 Serial pc(PA_2, PA_3); //USB/COM パソコンの仮想シリアルポートへ。ここでは使いません
motomiya 0:b8098a4ce012 31 DigitalOut ld2(PB_13); //LD2 グリーンLED
motomiya 0:b8098a4ce012 32 DigitalOut d11(PB_2); //D11 パワボード赤LED
motomiya 0:b8098a4ce012 33
motomiya 0:b8098a4ce012 34 //pwm出力の定義
motomiya 0:b8098a4ce012 35 PwmOut pwmU(PA_8), pwmV(PA_9), pwmW(PA_10);//それぞれUH_PWM, VH_PWM, WH_PWM
motomiya 0:b8098a4ce012 36
motomiya 0:b8098a4ce012 37 //IOの定義
motomiya 0:b8098a4ce012 38 DigitalOut enableU(PC_10), enableV(PC_11), enableW(PC_12);//それぞれEnable_CH1-L6230, _CH2-, _CH3-
motomiya 0:b8098a4ce012 39
motomiya 0:b8098a4ce012 40 //アナログ入力の定義
motomiya 0:b8098a4ce012 41 AnalogIn ain(PB_1);//R42　POTENTIOPOTENTIOMETER
motomiya 0:b8098a4ce012 42 AnalogIn ainThrm(PC_2);// Temperature feedback
motomiya 0:b8098a4ce012 43
motomiya 0:b8098a4ce012 44 //タイマーの定義
motomiya 0:b8098a4ce012 45 Ticker myTimer;//繰返しタイマー
motomiya 0:b8098a4ce012 46
motomiya 0:b8098a4ce012 47 //変数
motomiya 0:b8098a4ce012 48 int preMybutton; //記憶
motomiya 0:b8098a4ce012 49 int enablePWM=0;//PWM開始と停止フラグ
motomiya 0:b8098a4ce012 50 //int scope[100];//モニター用バッファ
motomiya 0:b8098a4ce012 51 int count, loopCount=0; //カウンター
motomiya 0:b8098a4ce012 52 double outDuty=0 ,outU, outV, outW; //PWM出力デューティー
motomiya 0:b8098a4ce012 53 float theta, omega; //角度と角速度
motomiya 0:b8098a4ce012 54 int timePerRound=60; //1回転時間(S)
motomiya 0:b8098a4ce012 55
motomiya 0:b8098a4ce012 56 int sumThrm; //積算値
motomiya 0:b8098a4ce012 57 bool runRoulette=false;
motomiya 0:b8098a4ce012 58 int thetaMech=0; //機械(物理的な)モータ角度
motomiya 0:b8098a4ce012 59
motomiya 0:b8098a4ce012 60 /**********************************
motomiya 0:b8098a4ce012 61 sin関数
motomiya 0:b8098a4ce012 62 引数:最大1024=360°
motomiya 0:b8098a4ce012 63 出力:最大512=1.0
motomiya 0:b8098a4ce012 64 ***********************************/
motomiya 0:b8098a4ce012 65 int mySin(int angleNum)
motomiya 0:b8098a4ce012 66 {
motomiya 0:b8098a4ce012 67
motomiya 0:b8098a4ce012 68 angleNum &= 0x3ff; //0x3ff=1023 でマスク
motomiya 0:b8098a4ce012 69 if(angleNum >= 512) //180～360°
motomiya 0:b8098a4ce012 70 return -sinTbl[angleNum - 512];
motomiya 0:b8098a4ce012 71 else //0～180°
motomiya 0:b8098a4ce012 72 return sinTbl[angleNum];
motomiya 0:b8098a4ce012 73 }
motomiya 0:b8098a4ce012 74
motomiya 0:b8098a4ce012 75
motomiya 0:b8098a4ce012 76 /**********************************
motomiya 0:b8098a4ce012 77 タイマー割込み関数 5mS
motomiya 0:b8098a4ce012 78 ***********************************/
motomiya 0:b8098a4ce012 79 void myTimerCallBack() //全相をONにする
motomiya 0:b8098a4ce012 80 {
motomiya 0:b8098a4ce012 81 outDuty = 0.2; //最大1.0　出力電圧を調整します
motomiya 0:b8098a4ce012 82
motomiya 0:b8098a4ce012 83 theta += omega; //増加分
motomiya 0:b8098a4ce012 84 if(theta >= 1024) //360°オーバーフローを防止
motomiya 0:b8098a4ce012 85 theta -= 1024;
motomiya 0:b8098a4ce012 86 else if(theta <= -1024)
motomiya 0:b8098a4ce012 87 theta += 1024;
motomiya 0:b8098a4ce012 88
motomiya 0:b8098a4ce012 89 pwmU.pulsewidth(HALF_PWM + outDuty * mySin((int)theta & 0x3ff) / 512.0 * HALF_PWM);
motomiya 0:b8098a4ce012 90 pwmV.pulsewidth(HALF_PWM + outDuty * mySin(((int)theta - DEG120) & 0x3ff) / 512.0 * HALF_PWM);
motomiya 0:b8098a4ce012 91 pwmW.pulsewidth(HALF_PWM + outDuty * mySin(((int)theta - DEG240) & 0x3ff) / 512.0 * HALF_PWM);
motomiya 0:b8098a4ce012 92
motomiya 0:b8098a4ce012 93 thetaMech += omega; //増加分
motomiya 0:b8098a4ce012 94
motomiya 0:b8098a4ce012 95 //サイクルカウンター処理（同期）
motomiya 0:b8098a4ce012 96 count++;
motomiya 0:b8098a4ce012 97 if( count == 100) {
motomiya 0:b8098a4ce012 98 count = 0;
motomiya 0:b8098a4ce012 99 loopCount = 1; //loopの同期用 5mS X 100 = 0.5S
motomiya 0:b8098a4ce012 100 }
motomiya 0:b8098a4ce012 101
motomiya 0:b8098a4ce012 102 }
motomiya 0:b8098a4ce012 103 /***********************************
motomiya 0:b8098a4ce012 104
motomiya 0:b8098a4ce012 105 ************************************/
motomiya 0:b8098a4ce012 106 int main()
motomiya 0:b8098a4ce012 107 {
motomiya 0:b8098a4ce012 108
motomiya 0:b8098a4ce012 109 //pwmデューティー　初期値
motomiya 0:b8098a4ce012 110 outU = 0;
motomiya 0:b8098a4ce012 111 outV = 0;
motomiya 0:b8098a4ce012 112 outW = 0;
motomiya 0:b8098a4ce012 113
motomiya 0:b8098a4ce012 114 //PWM設定
motomiya 0:b8098a4ce012 115 enableU = 0;//PWM出力OFF. L6230PのENable端子です
motomiya 0:b8098a4ce012 116 enableV = 0;
motomiya 0:b8098a4ce012 117 enableW = 0;
motomiya 0:b8098a4ce012 118
motomiya 0:b8098a4ce012 119 pwmU.period(PERIOD_PWM);//0.2mS 5KHz
motomiya 0:b8098a4ce012 120 pwmV.period(PERIOD_PWM);
motomiya 0:b8098a4ce012 121 pwmW.period(PERIOD_PWM);
motomiya 0:b8098a4ce012 122
motomiya 0:b8098a4ce012 123 //タイマー設定
motomiya 0:b8098a4ce012 124 myTimer.attach(&myTimerCallBack, PERIOD_TIMER);//5ｍS毎にmyTimerCallBack()がコールされます
motomiya 0:b8098a4ce012 125
motomiya 0:b8098a4ce012 126
motomiya 0:b8098a4ce012 127 while(1) {
motomiya 0:b8098a4ce012 128 //------ およそ一定周期の処理 -------
motomiya 0:b8098a4ce012 129 if(loopCount != 0) {//5x100=0.5S
motomiya 0:b8098a4ce012 130 loopCount = 0;
motomiya 0:b8098a4ce012 131
motomiya 0:b8098a4ce012 132 ld2 = !ld2; //点滅
motomiya 0:b8098a4ce012 133 d11 = !d11; //パワボード
motomiya 0:b8098a4ce012 134
motomiya 0:b8098a4ce012 135 //ルーレット目標位置の作成用データ
motomiya 0:b8098a4ce012 136 sumThrm += (float)ainThrm * 1000000; //積算
motomiya 0:b8098a4ce012 137
motomiya 0:b8098a4ce012 138 //pc.printf("%f", ((float)(sumThrm & 0x1f) / 32.0 + 10) * 1024 * POLE_PAIR);
motomiya 0:b8098a4ce012 139 }
motomiya 0:b8098a4ce012 140
motomiya 0:b8098a4ce012 141 //-------- 時間に関係しない処理 --------
motomiya 0:b8098a4ce012 142 //PWM開始と停止
motomiya 0:b8098a4ce012 143 if((int)mybutton != preMybutton) {//ボタンが押された。チャタリングを考慮していません
motomiya 0:b8098a4ce012 144 preMybutton = (int)mybutton;//次に押されたときのために記憶
motomiya 0:b8098a4ce012 145
motomiya 0:b8098a4ce012 146 if(mybutton == 0) {//押されたときだけ。　
motomiya 0:b8098a4ce012 147 runRoulette = true; //enablePWM = ~enablePWM;//オルタネート
motomiya 0:b8098a4ce012 148 thetaMech = 0;//機械角リセット
motomiya 0:b8098a4ce012 149 //PWM開始
motomiya 0:b8098a4ce012 150 enableU = 1;//PWM出力ON. L6230PのENable端子です
motomiya 0:b8098a4ce012 151 enableV = 1;
motomiya 0:b8098a4ce012 152 enableW = 1;
motomiya 0:b8098a4ce012 153 }
motomiya 0:b8098a4ce012 154 }
motomiya 0:b8098a4ce012 155
motomiya 0:b8098a4ce012 156 //ルーレット停止位置
motomiya 0:b8098a4ce012 157 if( thetaMech > ((float)(sumThrm & 0x1f) / 32.0F + 5) * 1024 * POLE_PAIR)//
motomiya 0:b8098a4ce012 158 runRoulette = false;
motomiya 0:b8098a4ce012 159 //thetaMechが1024＊POLE_PAIRで一回転. 5回転を追加. sumThrmの0～31を抽出
motomiya 0:b8098a4ce012 160
motomiya 0:b8098a4ce012 161
motomiya 0:b8098a4ce012 162 //角速度に変換　ルーレットのスタートと停止
motomiya 0:b8098a4ce012 163 if(runRoulette) { //回転
motomiya 0:b8098a4ce012 164 timePerRound = 2;//速度指定(S/r)
motomiya 0:b8098a4ce012 165 omega = PERIOD_TIMER * DEG360 * POLE_PAIR / timePerRound;
motomiya 0:b8098a4ce012 166 } else //停止
motomiya 0:b8098a4ce012 167 omega = 0;
motomiya 0:b8098a4ce012 168
motomiya 0:b8098a4ce012 169 }//END while(1)
motomiya 0:b8098a4ce012 170
motomiya 0:b8098a4ce012 171
motomiya 0:b8098a4ce012 172 }
motomiya 0:b8098a4ce012 173