v2.0.1 verison added

Author: askuric

README.md

@@ -29,7 +29,7 @@ void BLDCMotor::linkDriver(BLDCDriver* _driver) {
   driver = _driver;
 }
 
-// init hardware pins   
+// init hardware pins
 void BLDCMotor::init() {
   if(monitor_port) monitor_port->println("MOT: Initialise variables.");
   // sanity check for the voltage limit configuration
@@ -53,13 +53,13 @@ void BLDCMotor::disable()
 {
   // set zero to PWM
   driver->setPwm(0, 0, 0);
-  // disable the driver 
+  // disable the driver
   driver->disable();
 }
 // enable motor driver
 void BLDCMotor::enable()
 {
-  // enable the driver 
+  // enable the driver
   driver->enable();
   // set zero to PWM
   driver->setPwm(0, 0, 0);
@@ -93,7 +93,7 @@ int  BLDCMotor::initFOC( float zero_electric_offset, Direction sensor_direction
 int BLDCMotor::alignSensor() {
   if(monitor_port) monitor_port->println("MOT: Align sensor.");
   // align the electrical phases of the motor and sensor
-  // set angle -90 degrees 
+  // set angle -90 degrees
 
   float start_angle = shaftAngle();
   for (int i = 0; i <=5; i++ ) {
@@ -112,6 +112,8 @@ int BLDCMotor::alignSensor() {
     sensor->natural_direction = Direction::CCW;
   } else if (mid_angle == start_angle) {
     if(monitor_port) monitor_port->println("MOT: Sensor failed to notice movement");
+  } else{
+    if(monitor_port) monitor_port->println("MOT: natural_direction==CW");
   }
 
   // let the motor stabilize for 2 sec
@@ -134,19 +136,19 @@ int BLDCMotor::alignSensor() {
 }
 
 
-// Encoder alignment the absolute zero angle 
+// Encoder alignment the absolute zero angle
 // - to the index
 int BLDCMotor::absoluteZeroAlign() {
 
   if(monitor_port) monitor_port->println("MOT: Absolute zero align.");
     // if no absolute zero return
   if(!sensor->hasAbsoluteZero()) return 0;
-  
+
 
   if(monitor_port && sensor->needsAbsoluteZeroSearch()) monitor_port->println("MOT: Searching...");
   // search the absolute zero with small velocity
   while(sensor->needsAbsoluteZeroSearch() && shaft_angle < _2PI){
-    loopFOC();   
+    loopFOC();
     voltage_q = PID_velocity(velocity_index_search - shaftVelocity());
   }
   voltage_q = 0;
@@ -167,9 +169,9 @@ int BLDCMotor::absoluteZeroAlign() {
 // Iterative function looping FOC algorithm, setting Uq on the Motor
 // The faster it can be run the better
 void BLDCMotor::loopFOC() {
-  // shaft angle 
+  // shaft angle
   shaft_angle = shaftAngle();
-  // set the phase voltage - FOC heart function :) 
+  // set the phase voltage - FOC heart function :)
   setPhaseVoltage(voltage_q, voltage_d, _electricalAngle(shaft_angle,pole_pairs));
 }
 
@@ -219,7 +221,7 @@ void BLDCMotor::move(float new_target) {
 
 // Method using FOC to set Uq and Ud to the motor at the optimal angle
 // Function implementing Space Vector PWM and Sine PWM algorithms
-// 
+//
 // Function using sine approximation
 // regular sin + cos ~300us    (no memory usaage)
 // approx  _sin + _cos ~110us  (400Byte ~ 20% of memory)
@@ -257,7 +259,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
       };
       // static int trap_150_state = 0;
       sector = 12 * (_normalizeAngle(angle_el + PI/6.0 + zero_electric_angle) / _2PI); // adding PI/6 to align with other modes
-      
+
       Ua = Uq + trap_150_map[sector][0] * Uq;
       Ub = Uq + trap_150_map[sector][1] * Uq;
       Uc = Uq + trap_150_map[sector][2] * Uq;
@@ -272,7 +274,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
     break;
 
     case FOCModulationType::SinePWM :
-      // Sinusoidal PWM modulation 
+      // Sinusoidal PWM modulation
       // Inverse Park + Clarke transformation
 
       // angle normalization in between 0 and 2pi
@@ -299,10 +301,10 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
       break;
 
     case FOCModulationType::SpaceVectorPWM :
-      // Nice video explaining the SpaceVectorModulation (SVPWM) algorithm 
+      // Nice video explaining the SpaceVectorModulation (SVPWM) algorithm
       // https://www.youtube.com/watch?v=QMSWUMEAejg
 
-      // if negative voltages change inverse the phase 
+      // if negative voltages change inverse the phase
       // angle + 180degrees
       if(Uq < 0) angle_el += _PI;
       Uq = abs(Uq);
@@ -316,14 +318,14 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
       // calculate the duty cycles
       float T1 = _SQRT3*_sin(sector*_PI_3 - angle_el) * Uq/driver->voltage_limit;
       float T2 = _SQRT3*_sin(angle_el - (sector-1.0)*_PI_3) * Uq/driver->voltage_limit;
-      // two versions possible 
+      // two versions possible
       float T0 = 0; // pulled to 0 - better for low power supply voltage
-      if (centered) { 
+      if (centered) {
         T0 = 1 - T1 - T2; //centered around driver->voltage_limit/2
-      } 
+      }
 
       // calculate the duty cycles(times)
-      float Ta,Tb,Tc; 
+      float Ta,Tb,Tc;
       switch(sector){
         case 1:
           Ta = T1 + T2 + T0/2;
@@ -369,7 +371,7 @@ void BLDCMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
       break;
 
   }
-  
+
   // set the voltages in driver
   driver->setPwm(Ua, Ub, Uc);
 }
@@ -386,7 +388,7 @@ void BLDCMotor::velocityOpenloop(float target_velocity){
   float Ts = (now_us - open_loop_timestamp) * 1e-6;
 
   // calculate the necessary angle to achieve target velocity
-  shaft_angle += target_velocity*Ts; 
+  shaft_angle += target_velocity*Ts;
 
   // set the maximal allowed voltage (voltage_limit) with the necessary angle
   setPhaseVoltage(voltage_limit,  0, _electricalAngle(shaft_angle, pole_pairs));
@@ -403,17 +405,17 @@ void BLDCMotor::angleOpenloop(float target_angle){
   unsigned long now_us = _micros();
   // calculate the sample time from last call
   float Ts = (now_us - open_loop_timestamp) * 1e-6;
-  
+
   // calculate the necessary angle to move from current position towards target angle
   // with maximal velocity (velocity_limit)
   if(abs( target_angle - shaft_angle ) > abs(velocity_limit*Ts))
-    shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts; 
+    shaft_angle += _sign(target_angle - shaft_angle) * abs( velocity_limit )*Ts;
   else
     shaft_angle = target_angle;
-  
+
   // set the maximal allowed voltage (voltage_limit) with the necessary angle
   setPhaseVoltage(voltage_limit,  0, _electricalAngle(shaft_angle, pole_pairs));
 
   // save timestamp for next call
   open_loop_timestamp = now_us;
-}
+}

library_source/BLDCMotor.cpp

@@ -106,5 +106,6 @@ void loop() {
 #include "drivers/BLDCDriver3PWM.h"
 #include "drivers/BLDCDriver6PWM.h"
 #include "drivers/StepperDriver4PWM.h"
+#include "drivers/StepperDriver2PWM.h"
 
 #endif

library_source/SimpleFOC.h

@@ -105,6 +105,8 @@ int StepperMotor::alignSensor() {
     sensor->natural_direction = Direction::CCW;
   } else if (mid_angle == start_angle) {
     if(monitor_port) monitor_port->println("MOT: Sensor failed to notice movement");
+  } else{
+    if(monitor_port) monitor_port->println("MOT: natural_direction==CW");
   }
 
   // let the motor stabilize for 2 sec
@@ -246,7 +248,7 @@ void StepperMotor::velocityOpenloop(float target_velocity){
 
   // calculate the necessary angle to achieve target velocity
   shaft_angle += target_velocity*Ts; 
-
+    
   // set the maximal allowed voltage (voltage_limit) with the necessary angle
   setPhaseVoltage(voltage_limit, 0, _electricalAngle(shaft_angle,pole_pairs));
 

library_source/StepperMotor.cpp

@@ -3,7 +3,7 @@
 // function buffering delay() 
 // arduino uno function doesn't work well with interrupts
 void _delay(unsigned long ms){
-#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__)
+#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega2560__)
   // if arduino uno and other atmega328p chips
   // use while instad of delay, 
   // due to wrong measurement based on changed timer0
@@ -19,7 +19,7 @@ void _delay(unsigned long ms){
 // function buffering _micros() 
 // arduino function doesn't work well with interrupts
 unsigned long _micros(){
-#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__)
+#if defined(__AVR_ATmega328P__) || defined(__AVR_ATmega168__) || defined(__AVR_ATmega2560__)
 // if arduino uno and other atmega328p chips
     //return the value based on the prescaler
     if((TCCR0B & 0b00000111) == 0x01) return (micros()/32);

library_source/common/time_utils.cpp

@@ -35,6 +35,8 @@ void BLDCDriver3PWM::disable()
 
 // init hardware pins   
 int BLDCDriver3PWM::init() {
+  // a bit of separation
+  _delay(1000);
 
   // PWM pins
   pinMode(pwmA, OUTPUT);
@@ -56,14 +58,14 @@ int BLDCDriver3PWM::init() {
 // Set voltage to the pwm pin
 void BLDCDriver3PWM::setPwm(float Ua, float Ub, float Uc) {  
   // limit the voltage in driver
-  Ua = _constrain(Ua, -voltage_limit, voltage_limit);
-  Ub = _constrain(Ub, -voltage_limit, voltage_limit);
-  Uc = _constrain(Uc, -voltage_limit, voltage_limit);    
+  Ua = _constrain(Ua, 0.0, voltage_limit);
+  Ub = _constrain(Ub, 0.0, voltage_limit);
+  Uc = _constrain(Uc, 0.0, voltage_limit);    
   // calculate duty cycle
   // limited in [0,1]
-  float dc_a = _constrain(Ua / voltage_power_supply, 0 , 1 );
-  float dc_b = _constrain(Ub / voltage_power_supply, 0 , 1 );
-  float dc_c = _constrain(Uc / voltage_power_supply, 0 , 1 );
+  float dc_a = _constrain(Ua / voltage_power_supply, 0.0 , 1.0 );
+  float dc_b = _constrain(Ub / voltage_power_supply, 0.0 , 1.0 );
+  float dc_c = _constrain(Uc / voltage_power_supply, 0.0 , 1.0 );
   // hardware specific writing
   // hardware specific function - depending on driver and mcu
   _writeDutyCycle3PWM(dc_a, dc_b, dc_c, pwmA, pwmB, pwmC);

library_source/drivers/BLDCDriver3PWM.cpp

@@ -41,6 +41,8 @@ void BLDCDriver6PWM::disable()
 
 // init hardware pins   
 int BLDCDriver6PWM::init() {
+  // a bit of separation
+  _delay(1000);
 
   // PWM pins
   pinMode(pwmA_l, OUTPUT);
@@ -64,9 +66,9 @@ int BLDCDriver6PWM::init() {
 // Set voltage to the pwm pin
 void BLDCDriver6PWM::setPwm(float Ua, float Ub, float Uc) {  
   // limit the voltage in driver
-  Ua = _constrain(Ua, -voltage_limit, voltage_limit);
-  Ub = _constrain(Ub, -voltage_limit, voltage_limit);
-  Uc = _constrain(Uc, -voltage_limit, voltage_limit);    
+  Ua = _constrain(Ua, 0, voltage_limit);
+  Ub = _constrain(Ub, 0, voltage_limit);
+  Uc = _constrain(Uc, 0, voltage_limit);    
   // calculate duty cycle
   // limited in [0,1]
   float dc_a = _constrain(Ua / voltage_power_supply, 0.0 , 1.0 );

library_source/drivers/BLDCDriver6PWM.cpp

@@ -0,0 +1,98 @@
+#include "StepperDriver2PWM.h"
+
+StepperDriver2PWM::StepperDriver2PWM(int ph1PWM, int ph1INA, int ph1INB, int ph2PWM, int ph2INA, int ph2INB, int en1, int en2){
+  // Pin initialization
+  pwm1 = ph1PWM; //!< phase 1 pwm pin number
+  ina1 = ph1INA; //!< phase 1 INA pin number
+  inb1 = ph1INB; //!< phase 1 INB pin number
+  pwm2 = ph2PWM; //!< phase 2 pwm pin number
+  ina2 = ph2INA; //!< phase 2 INA pin number
+  inb2 = ph2INB; //!< phase 2 INB pin number
+
+  // enable_pin pin
+  enable_pin1 = en1;
+  enable_pin2 = en2;
+
+  // default power-supply value
+  voltage_power_supply = DEF_POWER_SUPPLY;
+  voltage_limit = NOT_SET;
+
+}
+
+// enable motor driver
+void  StepperDriver2PWM::enable(){
+    // enable_pin the driver - if enable_pin pin available
+    if ( enable_pin1 != NOT_SET ) digitalWrite(enable_pin1, HIGH);
+    if ( enable_pin2 != NOT_SET ) digitalWrite(enable_pin2, HIGH);
+    // set zero to PWM
+    setPwm(0,0);
+}
+
+// disable motor driver
+void StepperDriver2PWM::disable()
+{
+  // set zero to PWM
+  setPwm(0, 0);
+  // disable the driver - if enable_pin pin available
+  if ( enable_pin1 != NOT_SET ) digitalWrite(enable_pin1, LOW);
+  if ( enable_pin2 != NOT_SET ) digitalWrite(enable_pin2, LOW);
+
+}
+
+// init hardware pins   
+int StepperDriver2PWM::init() {
+  // a bit of separation
+  _delay(1000);
+
+  // PWM pins
+  pinMode(pwm1, OUTPUT);
+  pinMode(pwm2, OUTPUT);
+  pinMode(ina1, OUTPUT);
+  pinMode(ina2, OUTPUT);
+  pinMode(inb1, OUTPUT);
+  pinMode(inb2, OUTPUT);
+
+  if(enable_pin1 != NOT_SET) pinMode(enable_pin1, OUTPUT);
+  if(enable_pin2 != NOT_SET) pinMode(enable_pin2, OUTPUT);
+
+  // sanity check for the voltage limit configuration
+  if(voltage_limit == NOT_SET || voltage_limit > voltage_power_supply) voltage_limit =  voltage_power_supply;
+
+  // Set the pwm frequency to the pins
+  // hardware specific function - depending on driver and mcu
+  _configure2PWM(pwm_frequency, pwm1, pwm2);
+  return 0;
+}
+
+
+// Set voltage to the pwm pin
+void StepperDriver2PWM::setPwm(float Ualpha, float Ubeta) {  
+  float duty_cycle1(0.0),duty_cycle2(0.0);
+  // limit the voltage in driver
+  Ualpha = _constrain(Ualpha, -voltage_limit, voltage_limit);
+  Ubeta = _constrain(Ubeta, -voltage_limit, voltage_limit);
+  // hardware specific writing
+  duty_cycle1 = _constrain(abs(Ualpha)/voltage_power_supply,0.0,1.0);
+  duty_cycle2 = _constrain(abs(Ubeta)/voltage_power_supply,0.0,1.0);
+  // set direction
+  if( Ualpha > 0 ){
+    digitalWrite(inb1, LOW);
+    digitalWrite(ina1, HIGH);
+  }
+  else{
+    digitalWrite(ina1, LOW);
+    digitalWrite(inb1, HIGH);
+  }
+    
+  if( Ubeta > 0 ){
+    digitalWrite(ina2, LOW);
+    digitalWrite(inb2, HIGH);
+  }
+  else{
+    digitalWrite(inb2, LOW);
+    digitalWrite(ina2, HIGH);
+  }
+
+  // write to hardware
+  _writeDutyCycle2PWM(duty_cycle1, duty_cycle2, pwm1, pwm2);
+}