From 2db8a308472c0eac17cfe8d55341200d55c27c45 Mon Sep 17 00:00:00 2001 From: Federica Di Lauro Date: Fri, 7 Feb 2020 13:51:52 +0100 Subject: [PATCH] update pid_tuning classes --- .../otto_pid_tuning/Core/Inc/constants.h | 10 ----- .../otto_pid_tuning/Core/Inc/encoder.h | 43 ++++++++++++++++--- .../Core/Inc/motor_controller.h | 3 +- .../otto_pid_tuning/Core/Inc/odometry.h | 19 +++++--- .../otto_pid_tuning/Core/Src/encoder.cpp | 39 ----------------- .../otto_pid_tuning/Core/Src/main.cpp | 14 ++++-- 6 files changed, 61 insertions(+), 67 deletions(-) delete mode 100644 utils/pid_tuning/otto_pid_tuning/Core/Inc/constants.h delete mode 100644 utils/pid_tuning/otto_pid_tuning/Core/Src/encoder.cpp diff --git a/utils/pid_tuning/otto_pid_tuning/Core/Inc/constants.h b/utils/pid_tuning/otto_pid_tuning/Core/Inc/constants.h deleted file mode 100644 index 0d5d4d6..0000000 --- a/utils/pid_tuning/otto_pid_tuning/Core/Inc/constants.h +++ /dev/null @@ -1,10 +0,0 @@ -#ifndef CONSTANTS_H -#define CONSTANTS_H - -#define BASELINE 0.3 //distance between wheels in meters -#define MAX_DUTY_CYCLE 790 -#define TICKS_PER_REVOLUTION 148000 //x4 resolution -#define RIGHT_WHEEL_CIRCUMFERENCE 0.8 //in meters -#define LEFT_WHEEL_CIRCUMFERENCE 0.78 //in meters - -#endif diff --git a/utils/pid_tuning/otto_pid_tuning/Core/Inc/encoder.h b/utils/pid_tuning/otto_pid_tuning/Core/Inc/encoder.h index 5792c92..50004a6 100644 --- a/utils/pid_tuning/otto_pid_tuning/Core/Inc/encoder.h +++ b/utils/pid_tuning/otto_pid_tuning/Core/Inc/encoder.h @@ -2,7 +2,6 @@ #define ENCODER_H #include "main.h" -#include "constants.h" class Encoder { public: @@ -11,15 +10,21 @@ class Encoder { uint32_t current_millis_; int32_t ticks_; //if negative the wheel is going backwards float wheel_circumference_; + int ticks_per_revolution_; Encoder() { timer_ = NULL; wheel_circumference_ = 0; + ticks_per_revolution_ = 0; } - Encoder(TIM_HandleTypeDef *timer, float wheel_circ); + Encoder(TIM_HandleTypeDef *timer, float wheel_circ, + int ticks_per_revolution) { + timer_ = timer; + wheel_circumference_ = wheel_circ; + ticks_per_revolution_ = ticks_per_revolution; - void Setup(); + } int GetCount() { int count = ((int) __HAL_TIM_GET_COUNTER(this->timer_) @@ -29,14 +34,38 @@ class Encoder { void ResetCount() { //set counter to half its maximum value - __HAL_TIM_SET_COUNTER(timer_, (timer_->Init.Period) / 2); + __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(); + void UpdateValues() { + this->previous_millis_ = this->current_millis_; + this->current_millis_ = HAL_GetTick(); + this->ticks_ = this->GetCount(); + this->ResetCount(); + } - float GetMeters(); + float GetMeters() { + float meters = ((float) this->ticks_ * this->wheel_circumference_) + / ticks_per_revolution_; + return meters; + } - float GetLinearVelocity(); + 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 diff --git a/utils/pid_tuning/otto_pid_tuning/Core/Inc/motor_controller.h b/utils/pid_tuning/otto_pid_tuning/Core/Inc/motor_controller.h index 597ca0a..19d283b 100644 --- a/utils/pid_tuning/otto_pid_tuning/Core/Inc/motor_controller.h +++ b/utils/pid_tuning/otto_pid_tuning/Core/Inc/motor_controller.h @@ -22,11 +22,12 @@ class MotorController { this->dir_pin_ = dir_pin; this->pwm_timer_ = pwm_timer; this->pwm_channel_ = pwm_channel; - this->max_dutycycle_ = pwm_timer->Instance->ARR; + this->max_dutycycle_ = 0; } void setup() { HAL_TIM_PWM_Start(pwm_timer_, pwm_channel_); + this->max_dutycycle_ = pwm_timer_->Instance->ARR; } void set_speed(int duty_cycle) { diff --git a/utils/pid_tuning/otto_pid_tuning/Core/Inc/odometry.h b/utils/pid_tuning/otto_pid_tuning/Core/Inc/odometry.h index 2d13eeb..85f24f7 100644 --- a/utils/pid_tuning/otto_pid_tuning/Core/Inc/odometry.h +++ b/utils/pid_tuning/otto_pid_tuning/Core/Inc/odometry.h @@ -1,30 +1,35 @@ #ifndef ODOMETRY_H #define ODOMETRY_H -#include "constants.h" - class Odometry { private: float left_velocity_; float right_velocity_; - + float baseline_; public: Odometry() { left_velocity_ = 0; right_velocity_ = 0; + baseline_ = 0; + } + + Odometry(float baseline) { + left_velocity_ = 0; + right_velocity_ = 0; + baseline_ = baseline; } void UpdateValues(float linear_vel, float angular_vel) { - left_velocity_ = linear_vel - (BASELINE * angular_vel)/2; - right_velocity_ = 2 * linear_vel - left_velocity_; + left_velocity_ = linear_vel - (baseline_ * angular_vel) / 2; + right_velocity_ = linear_vel + (baseline_ * angular_vel) / 2; } - float GetLeftVelocity(){ + float GetLeftVelocity() { return left_velocity_; } - float GetRightVelocity(){ + float GetRightVelocity() { return right_velocity_; } diff --git a/utils/pid_tuning/otto_pid_tuning/Core/Src/encoder.cpp b/utils/pid_tuning/otto_pid_tuning/Core/Src/encoder.cpp deleted file mode 100644 index b826703..0000000 --- a/utils/pid_tuning/otto_pid_tuning/Core/Src/encoder.cpp +++ /dev/null @@ -1,39 +0,0 @@ -#include "encoder.h" - -Encoder::Encoder(TIM_HandleTypeDef *timer, float wheel_circ) { - timer_ = timer; - wheel_circumference_ = wheel_circ; -} - -void Encoder::Setup() { - HAL_TIM_Encoder_Start(timer_, TIM_CHANNEL_ALL); - this->ResetCount(); - this->previous_millis_ = 0; - this->current_millis_ = HAL_GetTick(); -} - -void Encoder::UpdateValues() { - this->previous_millis_ = this->current_millis_; - this->current_millis_ = HAL_GetTick(); - this->ticks_ = this->GetCount(); - this->ResetCount(); -} - -float Encoder::GetMeters() { - this->UpdateValues(); - float meters = ((float) this->ticks_ * this->wheel_circumference_) - / TICKS_PER_REVOLUTION; - return meters; -} - -float Encoder::GetLinearVelocity() { - this->UpdateValues(); - float meters = ((float) this->ticks_ * this->wheel_circumference_) - / TICKS_PER_REVOLUTION; - float deltaTime = this->current_millis_ - this->previous_millis_; - if (deltaTime == 0) - return 0; - float linear_velocity = (meters / (deltaTime / 1000)); - return linear_velocity; -} - diff --git a/utils/pid_tuning/otto_pid_tuning/Core/Src/main.cpp b/utils/pid_tuning/otto_pid_tuning/Core/Src/main.cpp index 84465c9..c6e27d9 100644 --- a/utils/pid_tuning/otto_pid_tuning/Core/Src/main.cpp +++ b/utils/pid_tuning/otto_pid_tuning/Core/Src/main.cpp @@ -58,10 +58,18 @@ UART_HandleTypeDef huart6; /* USER CODE BEGIN PV */ +//Parameters +float baseline = 0.3; +int ticks_per_revolution = 148000; //x4 resolution +float right_wheel_circumference = 0.783; //in meters +float left_wheel_circumference = 0.789; //in meters + //Odometry -Encoder right_encoder = Encoder(&htim5, RIGHT_WHEEL_CIRCUMFERENCE); -Encoder left_encoder = Encoder(&htim2, LEFT_WHEEL_CIRCUMFERENCE); -Odometry odom = Odometry(); +Encoder right_encoder = Encoder(&htim5, right_wheel_circumference, + ticks_per_revolution); +Encoder left_encoder = Encoder(&htim2, left_wheel_circumference, + ticks_per_revolution); +Odometry odom = Odometry(baseline); float left_velocity = 0; float right_velocity = 0; -- 2.52.0