Koichi Kikkawa
20200821_motacon_ver4
Diff: BLDCmotorDriver.cpp
- Revision:
- 6:51643d078474
- Parent:
- 5:cf8183ae80ef
--- a/BLDCmotorDriver.cpp Fri Mar 13 01:07:30 2020 +0000
+++ b/BLDCmotorDriver.cpp Fri Aug 21 08:29:01 2020 +0000
@@ -4,181 +4,132 @@
PinName pH1, PinName pH2, PinName pH3, PinName pFault) :
GH_A(pGH_A), GH_B(pGH_B), GH_C(pGH_C), GL_A(pGL_A), GL_B(pGL_B), GL_C(pGL_C),
H1(pH1), H2(pH2), H3(pH3), Fault(LED1){
-
- sampleTime = 1e-3;
- switchingPeriod = 1.0 / 20e3;
+ HS_usec = 1e9; //速度表示を0にするため、最初にHS_usecを非常に大きい値にする。HS_usecが0だとprintfにinfと表示される。(main関数での計算上そうなる)
+ sampleTime = 1e-4;
+ switchingPeriod = 1.0 / 20e3; //PWMの周波数を20kHzにしている
dutyCycle = tempDutyCycle = 0;
- GH_A.period(switchingPeriod); // applies to all PwmOut instances
-// rl.setLimits(0.1, -0.5, 0, sampleTime); // initial 10 second ramp
- //H1.mode(PullNone);
- //H2.mode(PullNone);
- //H3.mode(PullNone);
- H1.rise(this, &BLDCmotorDriver::commutation);
+ GL_A.period(switchingPeriod); // applies to all PwmOut instances
+ H1.rise(this, &BLDCmotorDriver::H1rise); //U相ホールセンサの立上りだけ回転数計測処理してからcommutation処理するようにしている
H2.rise(this, &BLDCmotorDriver::commutation);
H3.rise(this, &BLDCmotorDriver::commutation);
H1.fall(this, &BLDCmotorDriver::commutation);
H2.fall(this, &BLDCmotorDriver::commutation);
H3.fall(this, &BLDCmotorDriver::commutation);
-}
-void BLDCmotorDriver::configure(float sampleTime, float switchingFrequency, float rampUpSlope, float rampDownSlope) {
- if (sampleTime < 1e-6)
- sampleTime = 1e-3;
- if (switchingFrequency < 100)
- switchingFrequency = 20e3;
- if (rampUpSlope < 0 || rampUpSlope > 1)
- rampUpSlope = 0.1;
- if (rampDownSlope > 0 || rampDownSlope < -1)
- rampDownSlope = -0.1;
- this->sampleTime = sampleTime;
- switchingPeriod = 1.0 / switchingFrequency;
- rl.setLimits(rampUpSlope, rampDownSlope, 0, sampleTime);
-}
-int BLDCmotorDriver::getSector(){ // hall 120°
-
+}
+
+int BLDCmotorDriver::getSector(){ //ホールセンサ信号読み取り モータ回転角検出
h1 = H1.read();
h2 = H2.read();
h3 = H3.read();
+ if(h1 == 0 && h2 == 0 && h3 == 1){ _currentSector = 0; }
+ else if(h1 == 0 && h2 == 1 && h3 == 1){ _currentSector = 1; }
+ else if(h1 == 0 && h2 == 1 && h3 == 0){ _currentSector = 2; }
+ else if(h1 == 1 && h2 == 1 && h3 == 0){ _currentSector = 3; }
+ else if(h1 == 1 && h2 == 0 && h3 == 0){ _currentSector = 4; }
+ else if(h1 == 1 && h2 == 0 && h3 == 1){ _currentSector = 5; }
+ currentSector = _currentSector;
+ Fault = 0;
+ return currentSector;
+}
+void BLDCmotorDriver::H1rise() { //H1 (U相ホールセンサ)
+ /*回転数計測処理開始*/
+/* 計測方法1 */
+/*
+ HScnt++;
+ t.stop();
+ rpm_a = t.read_us();
+ rpm_b = rpm_b - HS_usec;
+ rpm_b = rpm_b + rpm_a;
+ HS_usec = rpm_b / 20;
+ t.reset();
+ t.start();
+*/
+/* 計測方法2 */ //こちらの方が安定している。
+///*
+ HS_check = 0; //ホールセンサ割込みが起きていることを表すため0を代入
+ HS_cnt++;
+ t.stop();
+ if( t.read_ms() >= 100 ){
+ HS_usec = t.read_us() / HS_cnt;
+ HS_cnt = 0;
+ t.reset();
+ }
+ t.start();
+//*/
+ /*回転数計測処理終了*/
+ commutation(); //駆動信号出力処理に飛ぶ
+}
+void BLDCmotorDriver::commutation() {
+ if( t.read_ms() >= 120 && HS_check ){ //ホールセンサ(U相立上り)割り込みが一定時間発生していない(モータが回転していない場合)の処理
+ HS_usec = 1e9; //速度表示を0にするためHS_usecを非常に大きい値にする
+ }
+ HS_check = 1; //ホールセンサ割込み関数で0に戻される。割込みが起きない場合(1のまま一定時間過ぎると)HS_usecに1e9代入する処理に繋がる。
-
- if(h1 == 0 && h2 == 0 && h3 == 1){
- _currentSector = 0;
- }
- else if(h1 == 0 && h2 == 1 && h3 == 1){
- _currentSector = 1;
- }
- else if(h1 == 0 && h2 == 1 && h3 == 0){
- _currentSector = 2;
- }
- else if(h1 == 1 && h2 == 1 && h3 == 0){
- _currentSector = 3;
- }
- else if(h1 == 1 && h2 == 0 && h3 == 0){
- _currentSector = 4;
- }
- else if(h1 == 1 && h2 == 0 && h3 == 1){
- _currentSector = 5;
- }
- previousSector = _currentSector - 1;
- difference = _currentSector - previousSector;
- if (difference == 1){
- currentSector = _currentSector;
- Fault = 0;
- }
- else Fault = 1;
-return currentSector;
-}
-
-void BLDCmotorDriver::commutation() {
- dutyCycle = rl.out(tempDutyCycle);
- currentSector = getSector();
+ dutyCycle = tempDutyCycle;
+ currentSector = getSector();
if (dutyCycle > 0) {
- currentSector++;
- if(currentSector > 5)currentSector = 0;
- switch(currentSector) {
-
- case 0: //001
- GL_C = 0;
- GL_B = 0;
- GL_A = 1;
- GH_C = 0;
- GH_B = dutyCycle;
- GH_A = 0;
- break;
- case 1:
- GL_C = 1;
- GL_B = 0;
- GL_A = 0;
- GH_C = 0;
- GH_B = dutyCycle;
- GH_A = 0;
- break;
- case 2:
- GL_C = 1;
- GL_B = 0;
- GL_A = 0;
- GH_C = 0;
- GH_B = 0;
- GH_A = dutyCycle;
- break;
- case 3:
- GL_C = 0;
- GL_B = 1;
- GL_A = 0;
- GH_C = 0;
- GH_B = 0;
- GH_A = dutyCycle;
- break;
- case 4:
- GL_C = 0;
- GL_B = 1;
- GL_A = 0;
- GH_C = dutyCycle;
- GH_B = 0;
- GH_A = 0;
- break;
- case 5:
- GL_C = 0;
- GL_B = 0;
- GL_A = 1;
- GH_C = dutyCycle;
- GH_B = 0;
- GH_A = 0;
- break;
+ if( !direction ){
+ currentSector++;
+ if(currentSector > 5){
+ currentSector = 0;
+ }
+ switch(currentSector) { /*正転*/
+ case 0: //001
+ GL_C = 0; GL_B = 0; GL_A = dutyCycle; GH_C = 0; GH_B = 1; GH_A = 0;
+ break;
+ case 1:
+ GL_C = dutyCycle; GL_B = 0; GL_A = 0; GH_C = 0; GH_B = 1; GH_A = 0;
+ break;
+ case 2:
+ GL_C = dutyCycle; GL_B = 0; GL_A = 0; GH_C = 0; GH_B = 0; GH_A = 1;
+ break;
+ case 3:
+ GL_C = 0; GL_B = dutyCycle; GL_A = 0; GH_C = 0; GH_B = 0; GH_A = 1;
+ break;
+ case 4:
+ GL_C = 0; GL_B = dutyCycle; GL_A = 0; GH_C = 1; GH_B = 0; GH_A = 0;
+ break;
+ case 5:
+ GL_C = 0; GL_B = 0; GL_A = dutyCycle; GH_C = 1; GH_B = 0; GH_A = 0;
+ break;
+ }
+ }
+ else if ( direction ) { //dutyがマイナスかつ進行方向が後退になっている場合
+ GL_C = dutyCycle; GL_B = dutyCycle; GL_A = dutyCycle; GH_C = 0; GH_B = 0; GH_A = 0; //回生動作する
}
- } else if (dutyCycle < 0) {
- currentSector--;
- if(currentSector < 0)currentSector = 5;
- switch(currentSector) {
- case 0:
- GL_C = 0;
- GL_B = 0;
- GL_A = 1;
- GH_C = -dutyCycle;
- GH_B = 0;
- GH_A = 0;
- break;
- case 1:
- GL_C = 0;
- GL_B = 0;
- GL_A = 1;
- GH_C = 0;
- GH_B = -dutyCycle;
- GH_A = 0;
- break;
- case 2:
- GL_C = 1;
- GL_B = 0;
- GL_A = 0;
- GH_C = 0;
- GH_B = -dutyCycle;
- GH_A = 0;
- break;
- case 3:
- GL_C = 1;
- GL_B = 0;
- GL_A = 0;
- GH_C = 0;
- GH_B = 0;
- GH_A = -dutyCycle;
- break;
- case 4:
- GL_C = 0;
- GL_B = 1;
- GL_A = 0;
- GH_C = 0;
- GH_B = 0;
- GH_A = -dutyCycle;
- break;
- case 5:
- GL_C = 0;
- GL_B = 1;
- GL_A = 0;
- GH_C = -dutyCycle;
- GH_B = 0;
- GH_A = 0;
- break;
- }
- }else {
+ }
+ else if( dutyCycle < 0 ){
+ if( direction ){ /*逆転*/
+ currentSector--;
+ if(currentSector < 0){
+ currentSector = 5;
+ }
+ switch(currentSector) {
+ case 0:
+ GL_C = 0; GL_B = 0; GL_A = -dutyCycle; GH_C = 1; GH_B = 0; GH_A = 0;
+ break;
+ case 1:
+ GL_C = 0; GL_B = 0; GL_A = -dutyCycle; GH_C = 0; GH_B = 1; GH_A = 0;
+ break;
+ case 2:
+ GL_C = -dutyCycle; GL_B = 0; GL_A = 0; GH_C = 0; GH_B = 1; GH_A = 0;
+ break;
+ case 3:
+ GL_C = -dutyCycle; GL_B = 0; GL_A = 0; GH_C = 0; GH_B = 0; GH_A = 1;
+ break;
+ case 4:
+ GL_C = 0; GL_B = -dutyCycle; GL_A = 0; GH_C = 0; GH_B = 0; GH_A = 1;
+ break;
+ case 5:
+ GL_C = 0; GL_B = -dutyCycle; GL_A = 0; GH_C = 1; GH_B = 0; GH_A = 0;
+ break;
+ }
+ }
+ else if ( !direction ) { //dutyがマイナスかつ進行方向が後退になっている場合
+ GL_C = -dutyCycle; GL_B = -dutyCycle; GL_A = -dutyCycle; GH_C = 0; GH_B = 0; GH_A = 0; //回生動作する
+ }
+ }
+ else {
coast();
}
}
@@ -190,21 +141,17 @@
coast();
}
}
-/*void BLDCmotorDriver::adjustDutyCycle() {
- dutyCycle = rl.out(tempDutyCycle);
- float diff = tempDutyCycle - dutyCycle;
- if (diff < 0.01 || diff > -0.01)
- ticker.detach();
-}*/
+void BLDCmotorDriver::setDirection(float DS) {
+ if (!DS) {
+ direction = 0;
+ } else {
+ direction = 1;
+ }
+}
+
void BLDCmotorDriver::coast() {
- GH_A = 0;
- GL_A = 0;
- GH_B = 0;
- GL_B = 0;
- GH_C = 0;
- GL_C = 0;
+ GH_A = 0; GL_A = 0; GH_B = 0; GL_B = 0; GH_C = 0; GL_C = 0;
dutyCycle = tempDutyCycle = 0;
- rl.reset();
ticker.detach();
}
float BLDCmotorDriver::getDutyCycle() {