robossembler/servo
7
0
Fork 0
Code Issues 27 Pull requests 2 Projects 1 Releases 1 Packages Wiki Activity Actions

ADD: dummy foc

Browse source 
SIMPLE

CLEAN: cleanup code and add printf handler

FIX: counters

ADD: motor

ENH: temporary disable can

ADD: dummy foc, but creepy work
This commit is contained in:
vanyabeat 2024-02-06 11:32:46 +03:00 • committed by Igor Brylyov
parent 3af1d1a710
commit bd7f049b75
20 changed files with 629 additions and 567 deletions
Download patch file Download diff file Expand all files Collapse all files

123
firmware/Src/main.c
View file

@ -19,7 +19,6 @@
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "adc.h"
#include "can.h"
#include "spi.h"
#include "tim.h"
#include "usart.h"
@ -27,7 +26,14 @@
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include <errno.h>
#include <helpers.h>
#include <math.h>
#include <stdio.h>
#include <sys/unistd.h>
#include <motor.h>
#include <foc.h>
#include <pid.h>
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
@ -37,6 +43,7 @@
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
@ -59,50 +66,7 @@ static void MX_NVIC_Init(void);
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
#define PI 3.14159265359f
#define TWO_PI (2 * PI)
#define MAX_CURRENT 3.0f // Max phase current
#define POLE_PAIRS 3 // Motor's number of pole pairs
#define TARGET_ANGLE 120.0f // Target angle in degrees
#define TARGET_ANGLE_RAD (TARGET_ANGLE * PI / 180.0f) //
void StartMotorControl(void) {
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);
}
void StopMotorControl(void) {
HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_1);
HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_2);
HAL_TIM_PWM_Stop(&htim1, TIM_CHANNEL_3);
}
void UpdateMotor(float a, float b, float c) {
uint32_t pwm_value_a = (uint32_t)((a / MAX_CURRENT) * ((float)htim1.Init.Period));
uint32_t pwm_value_b = (uint32_t)((b / MAX_CURRENT) * ((float)htim1.Init.Period));
uint32_t pwm_value_c = (uint32_t)((c / MAX_CURRENT) * ((float)htim1.Init.Period));
__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, pwm_value_a);
__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, pwm_value_b);
__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, pwm_value_c);
}
void RunMotorToAngle(float rotorAngle) {
float phaseA, phaseB, phaseC;
float rotorRadians = fmod(rotorAngle, 360.0f) * (PI / 180.0f);
if (rotorAngle >= TARGET_ANGLE) {
// StopMotor(); // Stop the motor
}
phaseA = MAX_CURRENT * sinf(rotorRadians);
phaseB = MAX_CURRENT * sinf(rotorRadians - TWO_PI / 3.0f);
phaseC = MAX_CURRENT * sinf(rotorRadians + TWO_PI / 3.0f);
// char buf[100];
// /// print all phases currents
// sprintf(buf, "Phase A B C: %f %f %f\n", phaseA, phaseB, phaseC);
// USART1_PutString(buf);
UpdateMotor(phaseA, phaseB, phaseC);
}
/* USER CODE END 0 */
/**
@ -135,96 +99,35 @@ int main(void)
MX_GPIO_Init();
MX_TIM1_Init();
MX_USART1_UART_Init();
MX_USART2_UART_Init();
MX_TIM2_Init();
MX_SPI2_Init();
MX_CAN1_Init();
MX_CAN2_Init();
MX_TIM3_Init();
MX_ADC2_Init();
MX_TIM5_Init();
/* Initialize interrupts */
MX_NVIC_Init();
/* USER CODE BEGIN 2 */
AS5045_CS_Init();
/// INIT DRV8313
/// INIT DRV8313
HAL_GPIO_WritePin(DRV_RESET_GPIO_Port, DRV_RESET_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(DRV_SLEEP_GPIO_Port, DRV_SLEEP_Pin, GPIO_PIN_SET);
HAL_Delay(100);
HAL_GPIO_WritePin(EN_U_GPIO_Port, EN_U_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(EN_V_GPIO_Port, EN_V_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(EN_W_GPIO_Port, EN_W_Pin, GPIO_PIN_SET);
/* USER CODE END 2 */
start_motor_control(&htim1, TIM_CHANNEL_1, TIM_CHANNEL_2, TIM_CHANNEL_3);
foc_init(&Motor_6208, 12, 0.0, 24.0, set_pwm, AS5045_ReadAngle);
/* Infinite loop */
/* USER CODE BEGIN WHILE */
float angle_values[200];
uint8_t angle_index = 0;
float ADC123_ANGLE[4] = {0, 0, 0, 0};
StartMotorControl();
while (1)
{
// Ready to MERGE
RunMotorToAngle(ADC123_ANGLE[3] + 10.0f);
if (flag_10kHz == SET)
{
foc_run(&Motor_6208, &htim1, &hspi2);
flag_10kHz = RESET;
// Do something every 10 kHz
// BLINK LED1
HAL_GPIO_TogglePin(LED1_GPIO_Port, LED1_Pin);
uint8_t status = 0;
uint16_t angle = AS5045_ReadAngle(&hspi2, &status);
if (angle_index == 200)
{
angle_index = 0;
float average = 0;
for (int i = 0; i < 200; i++)
{
average += angle_values[i];
}
average /= 200.0;
ADC123_ANGLE[0] = GetCurrentFromADC(ReadADCValue(&hadc2, ADC_CHANNEL_15)); // Read ADC
ADC123_ANGLE[1] = GetCurrentFromADC(ReadADCValue(&hadc2, ADC_CHANNEL_8)); // Read ADC
ADC123_ANGLE[2] = GetCurrentFromADC(ReadADCValue(&hadc2, ADC_CHANNEL_9)); // Read ADC
ADC123_ANGLE[3] = average;
USART1_PutString("\xDE\xAD");
SendFloatsWithLL((uint8_t *)ADC123_ANGLE);
USART1_PutString("\xBE\xAF");
// break;
}
if (status == 0)
{
float degrees = NormalizeToDegrees(angle);
angle_values[angle_index] = degrees;
angle_index++;
}
else
{
// Handle the error
if (status & AS5040_DIAG_OC_FAULT)
{
// Offset Compensation not finished
}
if (status & AS5040_DIAG_CO_FAULT)
{
// Cordic Overflow error
}
if (status & AS5040_DIAG_LIN_FAULT)
{
// Linearity Alarm error
}
// StopMotorControl();
}
// sprintf(buf, "Average: %f\r\n", degrees);
// USART1_PutString(buf);
// HAL_Delay(100);
}
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */