5125602875
change setPwm function in FOCDriver to accept 3 args, with a default value for the 3rd Change stepper driver classes to use 3 arg setPwm, ignoring 3rd arg. created InlineCurrentSenseSync class in lib/currentsense. fixed timer bug in calibrate encoder, in main defined a label for serialusb, to make it easier to swap out.
108 lines
3.3 KiB
C++
108 lines
3.3 KiB
C++
#include "StepperDriver2PWM.h"
|
|
|
|
StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int* _in1, int _pwm2, int* _in2, int en1, int en2){
|
|
// Pin initialization
|
|
pwm1 = _pwm1; // phase 1 pwm pin number
|
|
dir1a = _in1[0]; // phase 1 INA pin number
|
|
dir1b = _in1[1]; // phase 1 INB pin number
|
|
pwm2 = _pwm2; // phase 2 pwm pin number
|
|
dir2a = _in2[0]; // phase 2 INA pin number
|
|
dir2b = _in2[1]; // 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;
|
|
pwm_frequency = NOT_SET;
|
|
|
|
}
|
|
|
|
StepperDriver2PWM::StepperDriver2PWM(int _pwm1, int _dir1, int _pwm2, int _dir2, int en1, int en2){
|
|
// Pin initialization
|
|
pwm1 = _pwm1; // phase 1 pwm pin number
|
|
dir1a = _dir1; // phase 1 direction pin
|
|
pwm2 = _pwm2; // phase 2 pwm pin number
|
|
dir2a = _dir2; // phase 2 direction pin
|
|
// these pins are not used
|
|
dir1b = NOT_SET;
|
|
dir2b = NOT_SET;
|
|
|
|
// enable_pin pin
|
|
enable_pin1 = en1;
|
|
enable_pin2 = en2;
|
|
|
|
// default power-supply value
|
|
voltage_power_supply = DEF_POWER_SUPPLY;
|
|
voltage_limit = NOT_SET;
|
|
pwm_frequency = NOT_SET;
|
|
|
|
}
|
|
|
|
// enable motor driver
|
|
void StepperDriver2PWM::enable(){
|
|
// enable_pin the driver - if enable_pin pin available
|
|
if ( _isset(enable_pin1) ) digitalWrite(enable_pin1, HIGH);
|
|
if ( _isset(enable_pin2) ) 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 ( _isset(enable_pin1) ) digitalWrite(enable_pin1, LOW);
|
|
if ( _isset(enable_pin2) ) digitalWrite(enable_pin2, LOW);
|
|
|
|
}
|
|
|
|
// init hardware pins
|
|
int StepperDriver2PWM::init() {
|
|
// PWM pins
|
|
pinMode(pwm1, OUTPUT);
|
|
pinMode(pwm2, OUTPUT);
|
|
pinMode(dir1a, OUTPUT);
|
|
pinMode(dir2a, OUTPUT);
|
|
if( _isset(dir1b) ) pinMode(dir1b, OUTPUT);
|
|
if( _isset(dir2b) ) pinMode(dir2b, OUTPUT);
|
|
|
|
if( _isset(enable_pin1) ) pinMode(enable_pin1, OUTPUT);
|
|
if( _isset(enable_pin2) ) pinMode(enable_pin2, OUTPUT);
|
|
|
|
// sanity check for the voltage limit configuration
|
|
if( !_isset(voltage_limit) || 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
|
|
params = _configure2PWM(pwm_frequency, pwm1, pwm2);
|
|
initialized = (params!=SIMPLEFOC_DRIVER_INIT_FAILED);
|
|
return params!=SIMPLEFOC_DRIVER_INIT_FAILED;
|
|
}
|
|
|
|
|
|
// Set voltage to the pwm pin
|
|
void StepperDriver2PWM::setPwm(float Ua, float Ub, float Uc) {
|
|
float duty_cycle1(0.0f),duty_cycle2(0.0f);
|
|
// limit the voltage in driver
|
|
Ua = _constrain(Ua, -voltage_limit, voltage_limit);
|
|
Ub = _constrain(Ub, -voltage_limit, voltage_limit);
|
|
// hardware specific writing
|
|
duty_cycle1 = _constrain(abs(Ua)/voltage_power_supply,0.0f,1.0f);
|
|
duty_cycle2 = _constrain(abs(Ub)/voltage_power_supply,0.0f,1.0f);
|
|
|
|
// phase 1 direction
|
|
digitalWrite(dir1a, Ua >= 0 ? LOW : HIGH);
|
|
if( _isset(dir1b) ) digitalWrite(dir1b, Ua >= 0 ? HIGH : LOW );
|
|
// phase 2 direction
|
|
digitalWrite(dir2a, Ub >= 0 ? LOW : HIGH);
|
|
if( _isset(dir2b) ) digitalWrite(dir2b, Ub >= 0 ? HIGH : LOW );
|
|
|
|
// write to hardware
|
|
_writeDutyCycle2PWM(duty_cycle1, duty_cycle2, params);
|
|
}
|