#include "main.h"
-class Encoder {
- private:
- TIM_HandleTypeDef *timer_;
- uint32_t previous_millis_;
- uint32_t current_millis_;
- int32_t ticks_; //if negative the wheel is going backwards
- float wheel_circumference_;
- int ticks_per_revolution_;
-
- public:
- Encoder() {
- timer_ = NULL;
- wheel_circumference_ = 0;
- ticks_per_revolution_ = 0;
- }
-
- Encoder(TIM_HandleTypeDef *timer, float wheel_circ,
- int ticks_per_revolution) {
- timer_ = timer;
- wheel_circumference_ = wheel_circ;
- ticks_per_revolution_ = ticks_per_revolution;
-
- }
-
- int GetCount() {
- int count = ((int) __HAL_TIM_GET_COUNTER(this->timer_)
- - ((this->timer_->Init.Period) / 2));
- return count;
- }
-
- void ResetCount() {
- //set counter to half its maximum value
- __HAL_TIM_SET_COUNTER(timer_, (timer_->Init.Period / 2));
- }
-
- void Setup() {
- HAL_TIM_Encoder_Start(timer_, TIM_CHANNEL_ALL);
- this->ResetCount();
- this->previous_millis_ = 0;
- this->current_millis_ = HAL_GetTick();
- }
-
- void UpdateValues() {
- this->previous_millis_ = this->current_millis_;
- this->current_millis_ = HAL_GetTick();
- this->ticks_ = this->GetCount();
- this->ResetCount();
- }
-
- float GetMeters() {
- float meters = ((float) this->ticks_ * this->wheel_circumference_)
- / ticks_per_revolution_;
- return meters;
- }
+typedef struct Encoder {
+ TIM_HandleTypeDef *timer;
+ uint32_t previous_millis;
+ uint32_t current_millis;
+ int32_t ticks; //if negative the wheel is going backwards
+ float_t wheel_circumference;
+ int32_t ticks_per_revolution;
+};
- float GetLinearVelocity() {
- this->UpdateValues();
- float meters = this->GetMeters();
- float deltaTime = this->current_millis_ - this->previous_millis_;
- if (deltaTime == 0)
- return 0;
- float linear_velocity = (meters / (deltaTime / 1000));
- return linear_velocity;
- }
+void Setup() {
+ HAL_TIM_Encoder_Start(timer_, TIM_CHANNEL_ALL);
+ this->ResetCount();
+ this->previous_millis_ = 0;
+ this->current_millis_ = HAL_GetTick();
+}
+
+void ResetCount() {
+ //set counter to half its maximum value
+ __HAL_TIM_SET_COUNTER(timer_, (timer_->Init.Period / 2));
+}
+
+int GetCount() {
+ int count = ((int) __HAL_TIM_GET_COUNTER(this->timer_)
+ - ((this->timer_->Init.Period) / 2));
+ return count;
+}
+
+void UpdateValues() {
+ this->previous_millis_ = this->current_millis_;
+ this->current_millis_ = HAL_GetTick();
+ this->ticks_ = this->GetCount();
+ this->ResetCount();
+}
+
+float GetMeters() {
+ float meters = ((float) this->ticks_ * this->wheel_circumference_)
+ / ticks_per_revolution_;
+ return meters;
+}
+
+float GetLinearVelocity() {
+ this->UpdateValues();
+ float meters = this->GetMeters();
+ float deltaTime = this->current_millis_ - this->previous_millis_;
+ if (deltaTime == 0)
+ return 0;
+ float linear_velocity = (meters / (deltaTime / 1000));
+ return linear_velocity;
+}
-};
#endif
#include "main.h"
-class MotorController {
- private:
- GPIO_TypeDef *sleep_gpio_port_;
- uint16_t sleep_pin_;
- GPIO_TypeDef *dir_gpio_port_;
- uint16_t dir_pin_;
- TIM_HandleTypeDef *pwm_timer_;
- uint32_t pwm_channel_;
- int32_t max_dutycycle_;
-
- public:
- MotorController(GPIO_TypeDef *sleep_gpio_port, uint16_t sleep_pin,
- GPIO_TypeDef *dir_gpio_port, uint16_t dir_pin,
- TIM_HandleTypeDef *pwm_timer, uint32_t pwm_channel) {
- this->sleep_gpio_port_ = sleep_gpio_port;
- this->sleep_pin_ = sleep_pin;
- this->dir_gpio_port_ = dir_gpio_port;
- this->dir_pin_ = dir_pin;
- this->pwm_timer_ = pwm_timer;
- this->pwm_channel_ = pwm_channel;
- this->max_dutycycle_ = 0;
- }
-
- void Setup() {
- HAL_TIM_PWM_Start(pwm_timer_, pwm_channel_);
- this->max_dutycycle_ = pwm_timer_->Instance->ARR;
- }
-
- void SetSpeed(int duty_cycle) {
- if (duty_cycle >= 0) {
- //set direction to forward
- HAL_GPIO_WritePin(dir_gpio_port_, dir_pin_, GPIO_PIN_SET);
-
- //check if duty_cycle exceeds maximum
- if (duty_cycle > max_dutycycle_)
- __HAL_TIM_SET_COMPARE(pwm_timer_, pwm_channel_, max_dutycycle_);
- else
- __HAL_TIM_SET_COMPARE(pwm_timer_, pwm_channel_, duty_cycle);
-
- } else if (duty_cycle < 0){
- //set direction to backwards
- HAL_GPIO_WritePin(dir_gpio_port_, dir_pin_, GPIO_PIN_RESET);
-
- //check if duty_cycle is lower than minimum
- if (duty_cycle < -max_dutycycle_)
- __HAL_TIM_SET_COMPARE(pwm_timer_, pwm_channel_, max_dutycycle_);
- else
- //invert sign to make duty_cycle positive
- __HAL_TIM_SET_COMPARE(pwm_timer_, pwm_channel_, -duty_cycle);
- }
-
- HAL_GPIO_WritePin(sleep_gpio_port_, sleep_pin_, GPIO_PIN_SET);
-
- }
+// TODO(lb): reorder after C++ removal to avoid padding
+typedef struct MotorController {
+ GPIO_TypeDef *sleep_gpio_port;
+ uint16_t sleep_pin;
+ GPIO_TypeDef *dir_gpio_port;
+ uint16_t dir_pin;
+ TIM_HandleTypeDef *pwm_timer;
+ uint32_t pwm_channel;
+ int32_t max_dutycycle;
+} MotorController;
+
+typedef uint8_t MotorDirection;
+enum {
+ MotorDirection_Backward = 0,
+ MotorDirection_Forward,
+};
- void Brake() {
- HAL_GPIO_WritePin(sleep_gpio_port_, sleep_pin_, GPIO_PIN_SET);
- __HAL_TIM_SET_COMPARE(pwm_timer_, pwm_channel_, 0);
- }
+void motorcontroller_init(MotorController *motors);
+void motorcontroller_speed_set(MotorController *motor, int32_t duty_cycle);
+void motorcontroller_brake(MotorController *motor);
+void motorcontroller_coast(MotorController * motor);
- void Coast() {
- HAL_GPIO_WritePin(sleep_gpio_port_, sleep_pin_, GPIO_PIN_RESET);
- }
-};
#endif
\r
/* Exported macro ------------------------------------------------------------*/\r
/* USER CODE BEGIN EM */\r
-\r
+#define ABS(a) ((a) >= 0 ? (a) : (-(a)))\r
+#define MAX(a, b) ((a) >= (b) ? (a) : (b))\r
+#define MIN(a, b) ((a) <= (b) ? (a) : (b))\r
+#define CLAMP_TOP(a, b) MIN((a), (b))\r
+#define CLAMP_BOT(a, b) MAX((a), (b))\r
+#define CLAMP(v, min, max) CLAMP_BOT(CLAMP_TOP(v, max), min)\r
+\r
+#define ARRLENGHT(ARR) (sizeof((ARR)) / sizeof(*(ARR)))\r
/* USER CODE END EM */\r
\r
/* Exported functions prototypes ---------------------------------------------*/\r
--- /dev/null
+#include "control/motor_controller.h"
+
+// NOTE(lb): this assumes `motors` initialized and with 2 elements
+void motorcontroller_init(MotorController *motors) {
+ // TODO(lb): is `assert` a thing?
+ // assert(motors);
+ HAL_TIM_PWM_Start(motors[0].pwm_timer, motors[0].pwm_channel);
+ motors[0].max_dutycycle = motors[0].pwm_timer->Instance->ARR;
+
+ HAL_TIM_PWM_Start(motors[1].pwm_timer, motors[1].pwm_channel);
+ motors[1].max_dutycycle = motors[1].pwm_timer->Instance->ARR;
+}
+
+// TODO(lb): just pass the direction yourself and work with abs values
+void motorcontroller_speed_set(MotorController *motor, int32_t duty_cycle) {
+ HAL_GPIO_WritePin(motor->dir_gpio_port, motor->dir_pin,
+ (GPIO_PinState) (duty_cycle >= 0
+ ? MotorDirection_Forward
+ : MotorDirection_Backward));
+ duty_cycle = CLAMP_TOP(ABS(duty_cycle), motor->max_dutycycle);
+ __HAL_TIM_SET_COMPARE(motor->pwm_timer, motor->pwm_channel, duty_cycle);
+ HAL_GPIO_WritePin(motor->sleep_gpio_port, motor->sleep_pin, GPIO_PIN_SET);
+}
+
+void motorcontroller_brake(MotorController *motor) {
+ HAL_GPIO_WritePin(motor->sleep_gpio_port, motor->sleep_pin, GPIO_PIN_SET);
+ __HAL_TIM_SET_COMPARE(motor->pwm_timer, motor->pwm_channel, 0);
+}
+
+void motorcontroller_coast(MotorController * motor) {
+ HAL_GPIO_WritePin(motor->sleep_gpio_port, motor->sleep_pin, GPIO_PIN_RESET);
+}
\r
#include "communication/otto_messages.h"\r
\r
+// NOTE(lb): couldn't get it to link in the final executable\r
+#include "./control/motor_controller.c"\r
+\r
/* USER CODE END Includes */\r
\r
/* Private typedef -----------------------------------------------------------*/\r
/* USER CODE BEGIN PV */\r
\r
//Odometry\r
-Encoder right_encoder;\r
-Encoder left_encoder;\r
+Encoder encoder_left = {0};\r
+Encoder encoder_right = {0};\r
Odometry odom;\r
\r
//PID\r
Pid cross_pid;\r
\r
//MotorController\r
-MotorController right_motor(sleep1_GPIO_Port,\r
-sleep1_Pin,\r
- dir1_GPIO_Port,\r
- dir1_Pin,\r
- &htim4, TIM_CHANNEL_4);\r
-MotorController left_motor(sleep2_GPIO_Port,\r
-sleep2_Pin,\r
- dir2_GPIO_Port,\r
- dir2_Pin,\r
- &htim4, TIM_CHANNEL_3);\r
+static MotorController motors[] = {\r
+ {\r
+ // Right motor\r
+ .sleep_gpio_port = sleep1_GPIO_Port,\r
+ .sleep_pin = sleep1_Pin,\r
+ .dir_gpio_port = dir1_GPIO_Port,\r
+ .dir_pin = dir1_Pin,\r
+ .pwm_timer = &htim4,\r
+ .pwm_channel = TIM_CHANNEL_4,\r
+ },\r
+ {\r
+ // Left motor\r
+ .sleep_gpio_port = sleep2_GPIO_Port,\r
+ .sleep_pin = sleep2_Pin,\r
+ .dir_gpio_port = dir2_GPIO_Port,\r
+ .dir_pin = dir2_Pin,\r
+ .pwm_timer = &htim4,\r
+ .pwm_channel = TIM_CHANNEL_3,\r
+ },\r
+};\r
+MotorController *motor_right = &motors[0];\r
+MotorController *motor_left = &motors[1];\r
\r
//Communication\r
ConfigMessage config_msg;\r
}\r
}\r
\r
- left_encoder = Encoder(&htim2, config_msg.left_wheel_circumference,\r
- config_msg.ticks_per_revolution);\r
- right_encoder = Encoder(&htim5, config_msg.right_wheel_circumference,\r
- config_msg.ticks_per_revolution);\r
+ left_encoder.timer = &htim2;\r
+ left_encoder.wheel_circumference = config_msg.left_wheel_circumference;\r
+ left_encoder.ticks_per_revolution = config_msg.ticks_per_revolution;\r
+\r
+ right_encoder.timer = &htim5;\r
+ right_encoder.wheel_circumference = config_msg.right_wheel_circumference;\r
+ right_encoder.ticks_per_revolution = config_msg.ticks_per_revolution;\r
\r
odom = Odometry(config_msg.baseline);\r
\r
left_encoder.Setup();\r
right_encoder.Setup();\r
\r
- left_motor.Setup();\r
- right_motor.Setup();\r
+ motorcontroller_init(motors);\r
\r
//right and left motors have the same parameters\r
uint32_t max_dutycycle = *(&htim4.Instance->ARR);\r
right_pid.Config(config_msg.kp_right, config_msg.ki_right, config_msg.kd_right, pid_min, pid_max);\r
cross_pid.Config(config_msg.kp_cross, config_msg.ki_cross, config_msg.kd_cross, pid_min, pid_max);\r
\r
- left_motor.Coast();\r
- right_motor.Coast();\r
+ motorcontroller_brake(motor_left);\r
+ motorcontroller_brake(motor_right);\r
\r
//Enables TIM6 interrupt (used for PID control)\r
HAL_TIM_Base_Start_IT(&htim6);\r
left_dutycycle += cross_dutycycle;\r
right_dutycycle -= cross_dutycycle;\r
\r
- left_motor.SetSpeed(left_dutycycle);\r
- right_motor.SetSpeed(right_dutycycle);\r
-\r
+ motorcontroller_speed_set(motor_left, left_dutycycle);\r
+ motorcontroller_speed_set(motor_right, right_dutycycle);\r
}\r
\r
uint8_t porcoddio = 0;\r
if (GPIO_Pin == user_button_Pin) {\r
//TODO ci può servire il bottone blu?\r
} else if (GPIO_Pin == fault1_Pin) {\r
- left_motor.Brake();\r
- right_motor.Brake();\r
+ motorcontroller_brake(motor_left);\r
+ motorcontroller_brake(motor_right);\r
//stop TIM6 interrupt (used for PID control)\r
HAL_TIM_Base_Stop_IT(&htim6);\r
otto_status = 4;\r
} else if (GPIO_Pin == fault2_Pin) {\r
- left_motor.Brake();\r
- right_motor.Brake();\r
+ motorcontroller_brake(motor_left);\r
+ motorcontroller_brake(motor_right);\r
//stop TIM6 interrupt (used for PID control)\r
HAL_TIM_Base_Stop_IT(&htim6);\r
otto_status = 4;\r
**
** File : sysmem.c
**
-** Author : Auto-generated by STM32CubeIDE
+** Author : Auto-generated by STM32CubeIDE
**
** Abstract : STM32CubeIDE Minimal System Memory calls file
**
-** For more information about which c-functions
+** For more information about which c-functions
** need which of these lowlevel functions
** please consult the Newlib libc-manual
**
register char * stack_ptr asm("sp");
/* Functions */
+// NOTE(lb): definition was missing
+typedef void* caddr_t;
/**
_sbrk
**/
caddr_t _sbrk(int incr)
{
- extern char end asm("end");
- static char *heap_end;
- char *prev_heap_end;
+ extern char end asm("end");
+ static char *heap_end;
+ char *prev_heap_end;
- if (heap_end == 0)
- heap_end = &end;
+ if (heap_end == 0)
+ heap_end = &end;
- prev_heap_end = heap_end;
- if (heap_end + incr > stack_ptr)
- {
- errno = ENOMEM;
- return (caddr_t) -1;
- }
+ prev_heap_end = heap_end;
+ if (heap_end + incr > stack_ptr)
+ {
+ errno = ENOMEM;
+ return (caddr_t) -1;
+ }
- heap_end += incr;
+ heap_end += incr;
- return (caddr_t) prev_heap_end;
+ return (caddr_t) prev_heap_end;
}
-
projects / pioneer-stm32 / commitdiff