matei/lemon-pepper-stepper
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

bringup changes

Browse Source
This commit is contained in:
matei jordache
2023-11-08 17:15:38 -05:00
parent 1bec8bc5b7
commit 7e7f196408
48 changed files with 13259 additions and 108525 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
firmware/lib/Arduino-FOC Submodule

Submodule firmware/lib/Arduino-FOC added at f9e9a2d29e

1
firmware/lib/Arduino-FOC-drivers Submodule

Submodule firmware/lib/Arduino-FOC-drivers added at c66b9db82a

40
firmware/lib/currentsense/utils.c
View File

@@ -1,11 +1,17 @@
#if
#include "adc.h"
#include "opamp.h"
#include "dma.h"
#include "../stm32_mcu.h"
#include "../../../../drivers/hardware_specific/stm32/stm32_mcu.h"
#include "Arduino-FOC/src/current_sense/hardware_api.h"
#include "Arduino-FOC/src/current_sense/hardware_specific/stm32_mcu.h"
#include "Arduino-FOC/src/drivers/hardware_specific/stm32/stm32_mcu.h"
#include "communication/SimpleFOCDebug.h"
volatile uint16_t adc1Result = {0};
float adcResolution = 4096.0f; // 12 bit ADC
float voltageScale = 3.3f; // full scale voltage range of ADC
float adcSens = adcResolution * voltageScale
volatile uint16_t adc1Result[2] = {0};
volatile uint16_t adc2Result[2] = {0};
float adcSens = 3.3f * 1.440f / 4096.0f;
@@ -15,24 +21,32 @@ float _readVoltageInline(const uint8_t pin, const void *cs_params)
switch (pin)
{
case PA3:
return adc1Result * adcSens;
return adc1Result[0]; // ADC1 CH13 -> Vopamp1 internal output
break;
case PB0:
return adc2Result[0] * adcSens;
return adc2Result[0]; //ADC2 CH16 -> Vopamp2 internal output
break;
case PA1:
return adc2Result[1] * adcSens;
return adc2Result[1]; //ADC2 CH18 -> Vopamp3 internal output
break;
case TS:
return adc1Result[1];
break;
default:
return 0.0f;
break;
}
}
void *_configureADCInline(const void *driver_params, const int pinA, const int pinB, const int pinC)
float _readVoltageLowSide(const int pinA, const void* cs_params){
return 0.0f;
}
void* _configureADCInline(const void *driver_params, const int pinA, const int pinB, const int pinC)
{
_UNUSED(driver_params);
@@ -48,11 +62,11 @@ void *_configureADCInline(const void *driver_params, const int pinA, const int p
if (HAL_ADC_Start_DMA(&hadc1, (uint32_t *)adc1Result, 1) != HAL_OK)
{
SIMPLEFOC_DEBUG("DMA read init failed");
SIMPLEFOC_DEBUG("DMA1 read init failed");
}
if (HAL_ADC_Start_DMA(&hadc2, (uint32_t *)adc2Result, 2) != HAL_OK)
{
SIMPLEFOC_DEBUG("DMA read init failed");
SIMPLEFOC_DEBUG("DMA2 read init failed");
}
HAL_OPAMP_Start(&hopamp1);
@@ -61,13 +75,13 @@ void *_configureADCInline(const void *driver_params, const int pinA, const int p
Stm32CurrentSenseParams *params = new Stm32CurrentSenseParams{
.pins = {pinA, pinB, pinC},
.adc_voltage_conv = (_ADC_VOLTAGE) / (_ADC_RESOLUTION),
.adc_voltage_conv = adcSens,
.timer_handle = (HardwareTimer *)(HardwareTimer_Handle[get_timer_index(TIM3)]->__this)};
return params;
}
void *_configureADCInline(const void *driver_params, const int pinA, const int pinB, const int pinC)
void* _configureADCLowSide(const void *driver_params, const int pinA, const int pinB, const int pinC)
{
_UNUSED(driver_params);
_UNUSED(pinA);
@@ -76,4 +90,8 @@ void *_configureADCInline(const void *driver_params, const int pinA, const int p
SIMPLEFOC_DEBUG("Lemon-Pepper does not use lowside sensing. Use inline current sense instead.");
return SIMPLEFOC_CURRENT_SENSE_INIT_FAILED;
}
void _driverSyncLowSide(void* driver_params, void* cs_params){
}

172
firmware/src/main.cpp
View File

@@ -14,19 +14,23 @@
#include "dfu.h"
#include "lemon-pepper.h"
#define USBD_MANUFACTURER_STRING "matei repair lab"
#define USBD_PRODUCT_STRING_FS "lemon-pepper-stepper"
#define USBD_MANUFACTURER_STRING "matei repair lab"
#define USBD_PRODUCT_STRING_FS "lemon-pepper-stepper"
// board specific data
typedef struct
{
uint16_t signature;
int8_t electricalDir;
Direction electricalDir;
float electricalZero;
uint16_t abzResolution;
uint8_t encoderCalibrated;
uint8_t canID;
}userData;
} userData;
userData boardData;
uint8_t updateData = 0;
const uint16_t magicWord = 0xAF0C;
// canbus things
@@ -37,32 +41,32 @@ extern uint8_t RxData[8];
#define POLEPAIRS 50
#define RPHASE 3
#define MOTORKV 40
#define ENC_PPR 16383 // max 16384
#define ENC_PPR 16383 // max 16383 (zero index) -> *4 for CPR, -1 is done in init to prevent rollover on 16 bit timer
/**
* SPI clockdiv of 16 gives ~10.5MHz clock. May still be stable with lower divisor.
* The HW encoder is configured using PPR, which is then *4 for CPR (full 12384 gives overflow on 16 bit timer.)
*/
SPISettings myMT6835SPISettings(168000000/16, MT6835_BITORDER, SPI_MODE3);
*/
SPISettings myMT6835SPISettings(168000000 / 16, MT6835_BITORDER, SPI_MODE3);
MagneticSensorMT6835 sensor = MagneticSensorMT6835(ENC_CS, myMT6835SPISettings);
STM32HWEncoder enc = STM32HWEncoder(ENC_PPR, ENC_A, ENC_B, ENC_Z);
/**
* The current sense amps have a gain of 90mA/V -> over 1.5A this is 135mA so we need gain of 24 to get full-scale.
* Actually we are limited to powers of 2 for gain. So it should be 16. This gives sensitivity of 1440mV/A.
* */
* */
InlineCurrentSense currentsense = InlineCurrentSense(1440, ISENSE_U, ISENSE_V, ISENSE_W);
StepperDriver4PWM driver = StepperDriver4PWM(MOT_A1, MOT_A2, MOT_B1, MOT_B2);
StepperMotor motor = StepperMotor(POLEPAIRS, RPHASE, MOTORKV);
StepperMotor motor = StepperMotor(POLEPAIRS);
Commander commander = Commander(SerialUSB);
uint16_t counter = 0;
// Prototypes
void configureFOC(void);
void configureCAN(void);
void calibrateEncoder(void);
uint8_t configureFOC(void);
uint8_t configureCAN(void);
uint8_t calibrateEncoder(void);
void setup()
{
@@ -73,50 +77,83 @@ void setup()
SerialUSB.begin(115200);
// EEPROM.get(0, boardData);
EEPROM.get(0, boardData);
digitalWrite(MOT_EN, HIGH);
digitalWrite(CAL_EN, LOW);
configureCAN();
configureFOC();
if (!configureCAN())
SIMPLEFOC_DEBUG("CAN init failed.");
if (!configureFOC())
SIMPLEFOC_DEBUG("FOC init failed.");
if(sensor.getABZResolution() != ENC_PPR){
if (sensor.getABZResolution() != ENC_PPR) // Check that PPR of the encoder matches our expectation.
{
digitalWrite(LED_FAULT, HIGH);
SIMPLEFOC_DEBUG("Encoder ABZ resolution unexpected.");
}
if(false){
calibrateEncoder();
}
// if(boardData.canID == 0x000)
// if (!boardData.encoderCalibrated) // If the encoder has not had self-calibration done, try.
// {
// // If the can ID is not initialized, then we'll look for a free ID.
// boardData.canID = FDCAN_FindUniqueID();
// SerialUSB.println(boardData.canID);
// uint8_t calibrationResult = calibrateEncoder();
// if (calibrationResult == 0x3)
// {
// boardData.encoderCalibrated = 1;
// updateData = 1;
// SIMPLEFOC_DEBUG("Encoder self calibration successful.");
// }
// else
// {
// boardData.encoderCalibrated = 0;
// digitalWrite(LED_FAULT, HIGH);
// SIMPLEFOC_DEBUG("Encoder self calibration failed! Result: %#02x", calibrationResult);
// }
// }
// if(boardData.signature != magicWord)
// if (boardData.canID == 0x000) // If the can ID is not set, then we'll look for a new, free ID.
// {
// uint8_t foundID = FDCAN_FindUniqueID();
// if (foundID != 0)
// {
// boardData.canID = foundID;
// updateData = 1;
// SIMPLEFOC_DEBUG("Unique CAN ID found: %i", foundID);
// } else {
// digitalWrite(LED_FAULT, HIGH);
// SIMPLEFOC_DEBUG("Failed to find a unique CAN ID!");
// }
// }
// if(updateData) // If the configuration data has changed at all, update the flash.
// {
// // If the EEPROM has not been initalized yet, save all the known data.
// EEPROM.put(0, boardData);
// }
}
void loop()
{
{
motor.loopFOC();
motor.move();
commander.run();
if(counter == 0){
motor.target = -motor.target;
if (counter == 0xFFF)
{
digitalToggle(LED_GOOD);
SerialUSB.print(sensor.getAngle());
SerialUSB.print("\t");
SerialUSB.print(enc.getAngle());
SerialUSB.print("\t");
SerialUSB.println(sensor.getABZResolution());
// SerialUSB.printf("%d\t%d\t%d\n", sensor.getAngle(), sensor.getABZResolution(), enc.getAngle());
counter = 0;
}
counter++;
#ifdef HAS_MONITOR
#ifdef HAS_MONITOR
motor.monitor();
#endif
#endif
}
void doMotor(char *cmd)
@@ -124,25 +161,46 @@ void doMotor(char *cmd)
commander.motor(&motor, cmd);
}
void configureFOC(void){
uint8_t configureFOC(void)
{
commander.add('M', doMotor, "motor");
commander.verbose = VerboseMode::machine_readable;
#ifdef SIMPLEFOC_STM32_DEBUG
#ifdef SIMPLEFOC_STM32_DEBUG
SimpleFOCDebug::enable(&SerialUSB);
#endif
#endif
// Encoder initialization.
// Ideally configuring the sensor over SPI then use STM32HWEncoder
sensor.init();
sensor.setABZResolution(ENC_PPR);
enc.init();
if(!enc.initialized)
digitalWrite(LED_FAULT, HIGH);
sensor.init();
// Check if the encoder has loaded the right PPR, if not, update and then write to EEPROM.
if(sensor.getABZResolution() != ENC_PPR){
delay(200);
sensor.setABZResolution(ENC_PPR);
sensor.writeEEPROM();
digitalWrite(LED_GOOD, HIGH);
digitalWrite(LED_FAULT, LOW);
for(uint8_t i=0; i < 60; i++){ // Datasheet says we need to wait 6 seconds after writing EEPROM.
digitalToggle(LED_GOOD);
digitalToggle(LED_FAULT);
delay(100);
}
digitalWrite(LED_GOOD, LOW);
digitalWrite(LED_FAULT, LOW);
}
// Driver initialization.
driver.pwm_frequency = 32000;
driver.voltage_power_supply = 12;
driver.voltage_limit = driver.voltage_power_supply/2;
driver.voltage_limit = driver.voltage_power_supply / 2;
driver.init();
// Motor PID parameters.
@@ -150,7 +208,7 @@ void configureFOC(void){
motor.PID_velocity.I = 24;
motor.PID_velocity.D = 0.01;
motor.PID_velocity.output_ramp = 750;
motor.PID_velocity.limit = 10;
motor.PID_velocity.limit = 500;
motor.LPF_velocity.Tf = 4;
motor.P_angle.P = 600;
@@ -161,16 +219,16 @@ void configureFOC(void){
// motor.voltage_sensor_align = 2;
motor.current_limit = 1;
motor.velocity_limit = 500;
motor.controller = MotionControlType::angle;
motor.controller = MotionControlType::velocity;
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
// Monitor initialization
#ifdef HAS_MONITOR
// Monitor initialization
#ifdef HAS_MONITOR
motor.useMonitoring(SerialUSB);
motor.monitor_start_char = 'M';
motor.monitor_end_char = 'M';
motor.monitor_downsample = 250;
#endif
#endif
motor.linkSensor(&sensor);
motor.linkDriver(&driver);
@@ -180,7 +238,7 @@ void configureFOC(void){
// motor.linkCurrentSense(&currentsense);
motor.target = 3;
motor.target = 10;
motor.zero_electric_angle = NOT_SET;
motor.sensor_direction = Direction::UNKNOWN;
@@ -196,6 +254,7 @@ void configureFOC(void){
// boardData.signature = magicWord;
// boardData.electricalZero = motor.zero_electric_angle;
// boardData.electricalDir = motor.sensor_direction;
// updateData = 1;
// }
// else{
// motor.zero_electric_angle = boardData.electricalZero;
@@ -203,39 +262,38 @@ void configureFOC(void){
// motor.init();
// motor.initFOC();
// }
return 1;
}
void configureCAN(void){
uint8_t configureCAN(void)
{
FDCAN_Start(0x000);
return 1;
}
void calibrateEncoder(void){
uint16_t calTime = micros();
uint8_t calibrateEncoder(void)
{
motor.target = 35; // roughly 2000rpm -> need to write 0x1 to Reg. AUTOCAL_FREQ
MT6835Options4 currentSettings = sensor.getOptions4();
currentSettings.autocal_freq = 0x1;
sensor.setOptions4(currentSettings);
uint16_t calTime = micros();
while (calTime - micros() < 2000000)
{
motor.loopFOC();
motor.move();
if(calTime - micros() > 2000){
if (calTime - micros() > 2000)
{
// after motor is spinning at constant speed, enable calibration.
digitalWrite(CAL_EN, HIGH);
}
}
digitalWrite(CAL_EN, LOW);
digitalWrite(CAL_EN, LOW);
uint8_t calibrationState = sensor.getCalibrationStatus();
if(calibrationState != 0x3){
digitalWrite(LED_FAULT, HIGH);
}
return sensor.getCalibrationStatus();
}