void encoder_count_reset(Encoder *encoder);
int encoder_count_get(Encoder *encoder);
-inline float meters_from_ticks(float encoder_ticks,
- float wheel_circumference,
- float ticks_per_revolution);
+float meters_from_ticks(float encoder_ticks,
+ float wheel_circumference,
+ float ticks_per_revolution);
#endif
#include "main.h"
// TODO(lb): reorder after C++ removal to avoid padding
-typedef struct MotorController {
+typedef struct {
GPIO_TypeDef *sleep_gpio_port;
uint16_t sleep_pin;
GPIO_TypeDef *dir_gpio_port;
} Odometry;
void odometry_setpoint_from_cmdvel(Odometry *odom, float linear_vel,
- float angular_vel) {
- odom->setpoint.left = linear_vel - (odom->baseline * angular_vel) / 2;
- odom->setpoint.right = linear_vel + (odom->baseline * angular_vel) / 2;
-}
+ float angular_vel);
#if 0
class Odometry {
#ifndef PID_H
#define PID_H
-class Pid {
- private:
+typedef struct {
//PID constants
- float kp_;
- float ki_;
- float kd_;
+ float kp;
+ float ki;
+ float kd;
- float error_;
- float setpoint_;
+ float error;
+ float setpoint;
//needed for integrative term
- float error_sum_;
+ float error_sum;
//needed for derivative term
- float previous_error_;
+ float previous_error;
- int min_;
- int max_;
+ int32_t min;
+ int32_t max;
+} Pid;
- public:
- Pid() {
- this->kp_ = 0;
- this->ki_ = 0;
- this->kd_ = 0;
+int32_t pid_update(Pid *pid, float measure);
- this->error_ = 0;
- this->setpoint_ = 0;
-
- this->previous_error_ = 0;
- this->error_sum_ = 0;
-
- this->min_ = 0;
- this->max_ = 0;
- }
- Pid(float kp, float ki, float kd, int min, int max) {
- this->kp_ = kp;
- this->ki_ = ki;
- this->kd_ = kd;
-
- this->error_ = 0;
- this->setpoint_ = 0;
-
- this->previous_error_ = 0;
- this->error_sum_ = 0;
-
- this->min_ = min;
- this->max_ = max;
-
- }
-
- void Config(float kp, float ki, float kd, int min, int max) {
- this->kp_ = kp;
- this->ki_ = ki;
- this->kd_ = kd;
-
- this->error_ = 0;
- this->setpoint_ = 0;
-
- this->previous_error_ = 0;
- this->error_sum_ = 0;
-
- this->min_ = min;
- this->max_ = max;
-
- }
-
- void Set(float setpoint) {
- this->setpoint_ = setpoint;
- }
-
- int Update(float measure) {
-
- this->error_ = this->setpoint_ - measure;
-
- //proportional term
- float output = this->error_ * this->kp_;
-
- //integral term without windup
- error_sum_ += this->error_;
- output += error_sum_ * this->ki_;
-
- //derivative term
- output += (this->error_ - this->previous_error_) * kd_;
- this->previous_error_ = this->error_;
-
- int integer_output = static_cast<int>(output);
-
- //anti windup
- if (integer_output > this->max_) {
- integer_output = this->max_;
- this->error_sum_ -= this->error_;
- } else if (integer_output < this->min_) {
- integer_output = this->min_;
- this->error_sum_ -= this->error_;
- }
-
- return integer_output;
-
- }
-};
#endif
--- /dev/null
+#include "control/encoder.h"
+#include "control/odometry.h"
+#include "control/motor_controller.h"
+#include "control/pid.h"
+
+// NOTE(lb): this assumes `motors` initialized and with 2 elements
+void motorcontroller_init(MotorController *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);
+}
+
+// NOTE(lb): this assumes `encoders` initialized and with 2 elements
+void encoder_init(Encoder *encoders) {
+ HAL_TIM_Encoder_Start(encoders[0].timer, TIM_CHANNEL_ALL);
+ encoder_count_reset(&encoders[0]);
+ encoders[0].previous_millis = 0;
+ encoders[0].current_millis = HAL_GetTick();
+
+ HAL_TIM_Encoder_Start(encoders[1].timer, TIM_CHANNEL_ALL);
+ encoder_count_reset(&encoders[1]);
+ encoders[1].previous_millis = 0;
+ encoders[1].current_millis = HAL_GetTick();
+}
+
+void encoder_count_reset(Encoder *encoder) {
+ //set counter to half its maximum value
+ __HAL_TIM_SET_COUNTER(encoder->timer, (encoder->timer->Init.Period / 2));
+}
+
+int encoder_count_get(Encoder *encoder) {
+ int count = (int)__HAL_TIM_GET_COUNTER(encoder->timer) -
+ (encoder->timer->Init.Period / 2);
+ return count;
+}
+
+void encoder_update(Encoder *encoder) {
+ encoder->previous_millis = encoder->current_millis;
+ encoder->current_millis = HAL_GetTick();
+ encoder->ticks = encoder_count_get(encoder);
+ encoder_count_reset(encoder);
+}
+
+float encoder_linear_velocity(Encoder *encoder) {
+ float meters = meters_from_ticks(encoder->ticks,
+ encoder->wheel_circumference,
+ encoder->ticks_per_revolution);
+ float deltatime = encoder->current_millis - encoder->previous_millis;
+ float linear_velocity = deltatime ? (meters / (deltatime / 1000.f)) : 0.f;
+ return linear_velocity;
+}
+
+void odometry_setpoint_from_cmdvel(Odometry *odom, float linear_vel,
+ float angular_vel) {
+ odom->setpoint.left = linear_vel - (odom->baseline * angular_vel) / 2;
+ odom->setpoint.right = linear_vel + (odom->baseline * angular_vel) / 2;
+}
+
+int32_t pid_update(Pid *pid, float measure) {
+ pid->error = pid->setpoint - measure;
+
+ //proportional term
+ float output = pid->error * pid->kp;
+
+ //integral term without windup
+ pid->error_sum += pid->error;
+ output += pid->error_sum * pid->ki;
+
+ //derivative term
+ output += (pid->error - pid->previous_error) * pid->kd;
+ pid->previous_error = pid->error;
+
+ //anti windup
+ pid->error_sum -= pid->error;
+ int32_t integer_output = CLAMP(((int32_t)output), pid->min, pid->max);
+ return integer_output;
+}
+
+
+float meters_from_ticks(float encoder_ticks,
+ float wheel_circumference,
+ float ticks_per_revolution) {
+ float meters = (encoder_ticks * wheel_circumference) / ticks_per_revolution;
+ return meters;
+}
+++ /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
-#include "./control/encoder.c"\r
-\r
/* USER CODE END Includes */\r
\r
/* Private typedef -----------------------------------------------------------*/\r
\r
/* USER CODE BEGIN PV */\r
\r
-//Odometry\r
static Encoder encoders[2] = {{0}, {0}};\r
Encoder *encoder_right = &encoders[0];\r
Encoder *encoder_left = &encoders[1];\r
\r
Odometry odom = {0};\r
\r
-//PID\r
-Pid left_pid;\r
-Pid right_pid;\r
-Pid cross_pid;\r
+Pid pid_left = {0};\r
+Pid pid_right = {0};\r
+Pid pid_cross = {0};\r
\r
-//MotorController\r
static MotorController motors[2] = {\r
{\r
// Right motor\r
pid_min = -(int) max_dutycycle;\r
pid_max = (int) max_dutycycle;\r
\r
- left_pid.Config(config_msg.kp_left, config_msg.ki_left, config_msg.kd_left, pid_min, pid_max);\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
+ pid_left.kp = config_msg.kp_left;\r
+ pid_left.ki = config_msg.ki_left;\r
+ pid_left.kd = config_msg.kd_left;\r
+ pid_left.min = pid_min;\r
+ pid_left.max = pid_max;\r
+\r
+ pid_right.kp = config_msg.kp_right;\r
+ pid_right.ki = config_msg.ki_right;\r
+ pid_right.kd = config_msg.kd_right;\r
+ pid_right.min = pid_min;\r
+ pid_right.max = pid_max;\r
+\r
+ pid_cross.kp = config_msg.kp_cross;\r
+ pid_cross.ki = config_msg.ki_cross;\r
+ pid_cross.kd = config_msg.kd_cross;\r
+ pid_cross.min = pid_min;\r
+ pid_cross.max = pid_max;\r
+\r
\r
motorcontroller_brake(motor_left);\r
motorcontroller_brake(motor_right);\r
//PID control\r
encoder_update(encoder_left);\r
float left_velocity = encoder_linear_velocity(encoder_left);\r
- int left_dutycycle = left_pid.Update(left_velocity);\r
+ int left_dutycycle = pid_update(&pid_left, left_velocity);\r
\r
encoder_update(encoder_right);\r
float right_velocity = encoder_linear_velocity(encoder_right);\r
- int right_dutycycle = right_pid.Update(right_velocity);\r
+ int right_dutycycle = pid_update(&pid_right, right_velocity);\r
\r
float difference = left_velocity - right_velocity;\r
- int cross_dutycycle = cross_pid.Update(difference);\r
+ int cross_dutycycle = pid_update(&pid_cross, difference);\r
\r
left_dutycycle += cross_dutycycle;\r
right_dutycycle -= cross_dutycycle;\r
float left_setpoint = odom.velocity.left;\r
float right_setpoint = odom.velocity.right;\r
\r
- left_pid.Set(left_setpoint);\r
- right_pid.Set(right_setpoint);\r
+ pid_left.setpoint = left_setpoint;\r
+ pid_right.setpoint = right_setpoint;\r
\r
float cross_setpoint = left_setpoint - right_setpoint;\r
- cross_pid.Set(cross_setpoint);\r
+ pid_cross.setpoint = cross_setpoint;\r
\r
/*\r
* Manage new transmission\r
projects / pioneer-stm32 / commitdiff