Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

dev´s #261

Open
wants to merge 31 commits into
base: dev
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
31 commits
Select commit Hold shift + click to select a range
e7e92e7
dev´s
Juanduino Mar 21, 2023
a520bc1
Initial 8PWM
Juanduino Mar 21, 2023
44f2ecc
Update FOCMotor.h
Juanduino Mar 21, 2023
ef09b21
Update stm32_mcu.cpp
Juanduino Mar 21, 2023
160f199
Update StepperDriver8PWM.h
Juanduino Mar 21, 2023
ced13e3
Update foc_utils.cpp
Juanduino Mar 21, 2023
271de7d
Update StepperMotor.cpp
Juanduino Mar 22, 2023
d8d6158
Update stm32_mcu.h
Juanduino Mar 22, 2023
5dbf3d0
Update stm32_mcu.cpp
Juanduino Mar 22, 2023
417507e
Update stm32_mcu.cpp
Juanduino Mar 22, 2023
cf4bfec
Update stm32_mcu.cpp
Juanduino Mar 22, 2023
014ab05
Update stm32_mcu.cpp
Juanduino Mar 22, 2023
d4da3f5
Update stm32_mcu.cpp
Juanduino Mar 22, 2023
9da76da
Update stm32_mcu.cpp
Juanduino Mar 22, 2023
cb92a74
Update stm32_mcu.cpp
Juanduino Mar 23, 2023
d4b86b9
Update stm32_mcu.cpp
Juanduino Mar 23, 2023
e47ab30
Merge branch 'simplefoc:dev' into dev
Juanduino Mar 23, 2023
16f9fc2
Update stm32_mcu.cpp
Juanduino Mar 23, 2023
b8b4454
Merge branch 'dev' of https://github.com/Juanduino/Arduino-FOC into dev
Juanduino Mar 23, 2023
41834d8
Update stm32_mcu.cpp
Juanduino Mar 23, 2023
73bc24b
Update stm32_mcu.cpp
Juanduino Mar 23, 2023
1bf6033
Update stm32_mcu.cpp
Juanduino Mar 23, 2023
98e876c
Update stm32_mcu.cpp
Juanduino Mar 23, 2023
220a0ca
Update stm32_mcu.cpp
Juanduino Mar 23, 2023
4d1ee20
Complimentary outputs..
Juanduino Mar 23, 2023
aa2f9a8
Update stm32_mcu.cpp
Juanduino Mar 24, 2023
deba1dc
TIM1 and TIM8 INIT
Juanduino Mar 24, 2023
f39e628
Update stm32_mcu.cpp
Juanduino Mar 24, 2023
2456d3b
Set all outputs LOW
Juanduino Mar 24, 2023
fa0ddf3
Update stm32_mcu.cpp
Juanduino Mar 24, 2023
fe198f1
Dev. Changes
Juanduino Sep 30, 2023
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 4 additions & 3 deletions src/SimpleFOC.h
Original file line number Diff line number Diff line change
Expand Up @@ -108,10 +108,11 @@ void loop() {
#include "drivers/BLDCDriver3PWM.h"
#include "drivers/BLDCDriver6PWM.h"
#include "drivers/StepperDriver4PWM.h"
#include "drivers/StepperDriver8PWM.h"
#include "drivers/StepperDriver2PWM.h"
#include "current_sense/InlineCurrentSense.h"
#include "current_sense/LowsideCurrentSense.h"
#include "current_sense/GenericCurrentSense.h"
//#include "current_sense/InlineCurrentSense.h"
//#include "current_sense/LowsideCurrentSense.h"
//#include "current_sense/GenericCurrentSense.h"
#include "communication/Commander.h"
#include "communication/StepDirListener.h"
#include "communication/SimpleFOCDebug.h"
Expand Down
136 changes: 132 additions & 4 deletions src/StepperMotor.cpp
Original file line number Diff line number Diff line change
@@ -1,5 +1,7 @@
#include "StepperMotor.h"
#include "./communication/SimpleFOCDebug.h"
#include "arm_math.h"



// StepperMotor(int pp)
Expand Down Expand Up @@ -187,19 +189,19 @@ int StepperMotor::alignSensor() {
// align the electrical phases of the motor and sensor
// set angle -90(270 = 3PI/2) degrees
setPhaseVoltage(voltage_sensor_align, 0, _3PI_2);
_delay(700);
_delay(2000);
// read the sensor
sensor->update();
// get the current zero electric angle
zero_electric_angle = 0;
zero_electric_angle = electricalAngle();
_delay(20);
_delay(2000);
if(monitor_port){
SIMPLEFOC_DEBUG("MOT: Zero elec. angle: ", zero_electric_angle);
}
// stop everything
setPhaseVoltage(0, 0, 0);
_delay(200);
_delay(1000);
}else SIMPLEFOC_DEBUG("MOT: Skip offset calib.");
return exit_flag;
}
Expand Down Expand Up @@ -257,7 +259,7 @@ void StepperMotor::loopFOC() {
electrical_angle = electricalAngle();

// set the phase voltage - FOC heart function :)
setPhaseVoltage(voltage.q, voltage.d, electrical_angle);
setPhaseVoltageCORDIC_LL(voltage.q, voltage.d, electrical_angle);
}

// Iterative function running outer loop of the FOC algorithm
Expand Down Expand Up @@ -357,6 +359,8 @@ void StepperMotor::move(float new_target) {
void StepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
// Sinusoidal PWM modulation
// Inverse Park transformation
////SerialUSB.println("ange_el -> ");
//SerialUSB.println(angle_el);

// angle normalization in between 0 and 2pi
// only necessary if using _sin and _cos - approximation functions
Expand All @@ -367,10 +371,134 @@ void StepperMotor::setPhaseVoltage(float Uq, float Ud, float angle_el) {
Ualpha = _ca * Ud - _sa * Uq; // -sin(angle) * Uq;
Ubeta = _sa * Ud + _ca * Uq; // cos(angle) * Uq;

//set the voltages in hardware
driver->setPwm(Ualpha, Ubeta);
/*
Serial.print("Ualpha:");
Serial.print(Ualpha, 8);
Serial.print(",");
Serial.print("Ubeta:");
Serial.print(Ubeta, 8);
Serial.println();

*/

}

// Method using FOC to set Uq and Ud to the motor at the optimal angle
// Function implementing Sine PWM algorithms
// - space vector not implemented yet
//

int32_t float_to_q31(float input) {
int32_t q31 = (int32_t) (input * 2147483648.0f);
q31 = (q31 > 0) ? (q31 << 1) : (-q31 << 1);
return q31;
}

#define pi 3.1415926535897932384626433f

int32_t q31_value;
float value_f32_sine = 0;
float value_f32_cosine = 0;
q31_t cordic_cosine;
q31_t cordic_sine;

float wrap_to_1(float x) {
while (x > 1.0f) {
x -= 2.0f;
}
while (x < -1.0f) {
x += 2.0f;
}
return x;
}
/// @runger();
/// @brief
/// @param Uq
/// @param Ud
/// @param angle_el

#define PI32f 3.141592f

float cordic_sin_value;
float cordic_cos_value;

void StepperMotor::setPhaseVoltageCORDIC_LL(float Uq, float Ud, float angle_el) {


// convert angle flot to CORDICq31 format
uint32_t angle31 = (uint32_t)(angle_el * (1UL << 31) / (1.0f * PI));

/* Write angle and start CORDIC execution */

// WHO U GONNA CALL? CORDIC ->

CORDIC->WDATA = angle31;
q31_t cosOutput = (int32_t)CORDIC->RDATA;

// convert q31 result to float
cordic_cos_value = (float)cosOutput / (float)0x80000000;

/* Read sine */
q31_t sinOutput = (int32_t)CORDIC->RDATA;

// convert q31 results to float
cordic_sin_value = (float)sinOutput / (float)0x80000000;



//value_f32_sine = wrap_to_1(value_f32_sine);
//value_f32_cosine = wrap_to_1(value_f32_cosine);


// Inverse park transform
//Ualpha = value_f32_cosine * Ud - value_f32_sine * Uq; // -sin(angle) * Uq;
//Ubeta = value_f32_sine * Ud + value_f32_cosine * Uq; // cos(angle) * Uq;

arm_inv_park_f32(Ud, Uq, &Ualpha, &Ubeta, cordic_sin_value, cordic_cos_value);



// set the voltages in hardware
driver->setPwm(Ualpha, Ubeta);



}


void StepperMotor::setPhaseVoltageCORDIC(float Uq, float Ud, float angle_el) {


uint32_t q1_31_angle = (uint32_t)(angle_el * (1UL << 31) / (1.0f * PI));

// WHO U GONNA CALL? CORDIC ->
CORDIC->WDATA = q1_31_angle;
cordic_sine = CORDIC->RDATA;
cordic_cosine = CORDIC->RDATA;

value_f32_sine = (float)cordic_sine/(float)0x80000000;
value_f32_cosine = (float)cordic_cosine/(float)0x80000000;

value_f32_sine = wrap_to_1(value_f32_sine);
value_f32_cosine = wrap_to_1(value_f32_cosine);


// Inverse park transform
//Ualpha = value_f32_cosine * Ud - value_f32_sine * Uq; // -sin(angle) * Uq;
//Ubeta = value_f32_sine * Ud + value_f32_cosine * Uq; // cos(angle) * Uq;

arm_inv_park_f32(Ud, Uq, &Ualpha, &Ubeta, value_f32_sine, value_f32_cosine);

// set the voltages in hardware
driver->setPwm(Ualpha, Ubeta);



}


// Function (iterative) generating open loop movement for target velocity
// - target_velocity - rad/s
// it uses voltage_limit variable
Expand Down
17 changes: 15 additions & 2 deletions src/StepperMotor.h
Original file line number Diff line number Diff line change
Expand Up @@ -79,6 +79,8 @@ class StepperMotor: public FOCMotor

float Ualpha,Ubeta; //!< Phase voltages U alpha and U beta used for inverse Park and Clarke transform

uint16_t countervalue; //!< counter value for the open loop control TIM3->CNT;

/**
* Method using FOC to set Uq to the motor at the optimal angle
* Heart of the FOC algorithm
Expand All @@ -89,10 +91,19 @@ class StepperMotor: public FOCMotor
*/
void setPhaseVoltage(float Uq, float Ud, float angle_el) override;

private:
// CORDIC

void setPhaseVoltageCORDIC(float Uq, float Ud, float angle_el) override;


void setPhaseVoltageCORDIC_LL(float Uq, float Ud, float angle_el) override;

int alignSensor();



/** Sensor alignment to electrical 0 angle of the motor */
int alignSensor();

/** Motor and sensor alignment to the sensors absolute 0 angle */
int absoluteZeroSearch();

Expand All @@ -113,6 +124,8 @@ class StepperMotor: public FOCMotor
float angleOpenloop(float target_angle);
// open loop variables
long open_loop_timestamp;

private:
};


Expand Down
8 changes: 8 additions & 0 deletions src/common/base_classes/FOCMotor.h
Original file line number Diff line number Diff line change
Expand Up @@ -137,6 +137,13 @@ class FOCMotor
*/
virtual void setPhaseVoltage(float Uq, float Ud, float angle_el)=0;


// CORDIC

virtual void setPhaseVoltageCORDIC(float Uq, float Ud, float angle_el)=0;

virtual void setPhaseVoltageCORDIC_LL(float Uq, float Ud, float angle_el)=0;

// State calculation methods
/** Shaft angle calculation in radians [rad] */
float shaftAngle();
Expand All @@ -157,6 +164,7 @@ class FOCMotor
float target; //!< current target value - depends of the controller
float shaft_angle;//!< current motor angle
float electrical_angle;//!< current electrical angle
float electrical_angle2;//!< current electrical angle
float shaft_velocity;//!< current motor velocity
float current_sp;//!< target current ( q current )
float shaft_velocity_sp;//!< current target velocity
Expand Down
16 changes: 8 additions & 8 deletions src/common/base_classes/Sensor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -17,14 +17,14 @@ void Sensor::update() {
/** get current angular velocity (rad/s) */
float Sensor::getVelocity() {
// calculate sample time
float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6;
// TODO handle overflow - we do need to reset vel_angle_prev_ts
if (Ts < min_elapsed_time) return velocity; // don't update velocity if deltaT is too small

velocity = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts;
vel_angle_prev = angle_prev;
vel_full_rotations = full_rotations;
vel_angle_prev_ts = angle_prev_ts;
bool isCountingUpward = TIM3->CR1 & 0x0010 ? false : true;

uint32_t timer2 = TIM2 -> CCR1;
float velocity = (4 * PI / 65536) * 168000000 / ((float)timer2 - 1.0f);

if (!isCountingUpward) {
velocity = -velocity;}

return velocity;
}

Expand Down
52 changes: 30 additions & 22 deletions src/common/base_classes/StepperDriver.h
Original file line number Diff line number Diff line change
Expand Up @@ -5,28 +5,36 @@

class StepperDriver{
public:

/** Initialise hardware */
virtual int init() = 0;
/** Enable hardware */
virtual void enable() = 0;
/** Disable hardware */
virtual void disable() = 0;

long pwm_frequency; //!< pwm frequency value in hertz
float voltage_power_supply; //!< power supply voltage
float voltage_limit; //!< limiting voltage set to the motor

bool initialized = false; // true if driver was successfully initialized
void* params = 0; // pointer to hardware specific parameters of driver

/**
* Set phase voltages to the harware
*
* @param Ua phase A voltage
* @param Ub phase B voltage
*/
virtual void setPwm(float Ua, float Ub) = 0;
/** Initialise hardware */
virtual int init() = 0;
/** Enable hardware */
virtual void enable() = 0;
/** Disable hardware */
virtual void disable() = 0;

long pwm_frequency; //!< pwm frequency value in hertz
float voltage_power_supply; //!< power supply voltage
float voltage_limit; //!< limiting voltage set to the motor

bool initialized = false; // true if driver was successfully initialized
void* params = 0; // pointer to hardware specific parameters of driver

float dc_1a; //!< currently set duty cycle on phaseA
float dc_1b; //!< currently set duty cycle on phaseB
float dc_1c; //!< currently set duty cycle on phaseC
float dc_1d; //!< currently set duty cycle on phaseC


/**
* Set phase voltages to the hardware
*
* @param Ua phase A voltage
* @param Ub phase B voltage
*/
virtual void setPwm(float Ua, float Ub) = 0;



};

#endif
2 changes: 1 addition & 1 deletion src/common/foc_utils.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -69,4 +69,4 @@ __attribute__((weak)) float _sqrtApprox(float number) {//low in fat
y = * ( float * ) &i;
// y = y * ( f - ( x * y * y ) ); // better precision
return number * y;
}
}
1 change: 0 additions & 1 deletion src/current_sense/hardware_specific/rp2040/rp2040_mcu.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -241,7 +241,6 @@ void RP2040ADCEngine::start() {

void RP2040ADCEngine::stop() {
adc_run(false);
irq_set_enabled(DMA_IRQ_0, false);
dma_channel_abort(readDMAChannel);
// if (triggerPWMSlice>=0)
// dma_channel_abort(triggerDMAChannel);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -187,7 +187,7 @@ uint32_t _timerToRegularTRGO(HardwareTimer* timer){
#endif
#ifdef TIM8 // if defined timer 8
else if(timer->getHandle()->Instance == TIM8)
return ADC_EXTERNALTRIG_T8_TRGO;
return ADC_EXTERNALTRIG_T7_TRGO;
#endif
#ifdef TIM15 // if defined timer 15
else if(timer->getHandle()->Instance == TIM15)
Expand Down
8 changes: 7 additions & 1 deletion src/drivers/BLDCDriver6PWM.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,7 @@ int BLDCDriver6PWM::init() {
pinMode(pwmB_l, OUTPUT);
pinMode(pwmC_l, OUTPUT);
if(_isset(enable_pin)) pinMode(enable_pin, OUTPUT);

Serial.println("Setting Pins to Output!");

// sanity check for the voltage limit configuration
if( !_isset(voltage_limit) || voltage_limit > voltage_power_supply) voltage_limit = voltage_power_supply;
Expand All @@ -73,6 +73,12 @@ int BLDCDriver6PWM::init() {
params = _configure6PWM(pwm_frequency, dead_zone, pwmA_h,pwmA_l, pwmB_h,pwmB_l, pwmC_h,pwmC_l);
initialized = (params!=SIMPLEFOC_DRIVER_INIT_FAILED);
return params!=SIMPLEFOC_DRIVER_INIT_FAILED;

// set phase state to enabled - if driver was successfully initialized
phase_state[0] = PhaseState::PHASE_ON;
phase_state[1] = PhaseState::PHASE_ON;
phase_state[2] = PhaseState::PHASE_ON;

}


Expand Down
1 change: 1 addition & 0 deletions src/drivers/BLDCDriver6PWM.h
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@ class BLDCDriver6PWM: public BLDCDriver
int pwmA_h,pwmA_l; //!< phase A pwm pin number
int pwmB_h,pwmB_l; //!< phase B pwm pin number
int pwmC_h,pwmC_l; //!< phase C pwm pin number

int enable_pin; //!< enable pin number
bool enable_active_high = true;

Expand Down
Loading