Commit 3c0f0425 authored by Jacob Odgaard Hausted's avatar Jacob Odgaard Hausted
Browse files

Gitignore removed drobotti drivers. Made quick fix renaming Core/Drobotti/Drivers to Drivers_.

parent 5294835a
......@@ -241,9 +241,9 @@
<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Core"/>
<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Middlewares"/>
<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Drivers"/>
<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Middlewares"/>
</sourceEntries>
......@@ -445,9 +445,9 @@
<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Core"/>
<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Middlewares"/>
<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Drivers"/>
<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Middlewares"/>
</sourceEntries>
......@@ -679,11 +679,11 @@
<sourceEntries>
<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Middlewares"/>
<entry excluding="Src/main.cpp|Src/main.c" flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Core"/>
<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Drivers"/>
<entry flags="VALUE_WORKSPACE_PATH|RESOLVED" kind="sourcePath" name="Middlewares"/>
</sourceEntries>
......
/*
* VNH5019.h
*
* Created on: May 11, 2021
* Author: Jacob Hausted
*/
#ifndef INC_DROBOTTI_DRIVERS__VNH5019_HPP_
#define INC_DROBOTTI_DRIVERS__VNH5019_HPP_
#include <DRobotti/Utils/PeripheralTypes.hpp>
#include <array>
class VNH5019 {
public:
enum MOTOR_FORWARD_ORIENTATION{
CLOCKWISE = 0,
COUNTERCLOCKWISE = 1
};
enum DRIVING_DIRECTION {
FORWARD = 0,
BACKWARD = 1
};
VNH5019() = delete;
VNH5019(MOTOR_FORWARD_ORIENTATION); // Using this, user must call init function before use, otherwise it will result in a HardFault being
// triggered on the MCU. Can be used if the peripherals cannot be guaranteed to be initialized upon
// instantiating this object.
VNH5019(MOTOR_FORWARD_ORIENTATION, PWMChannel& pwm_ch, GPIOPin& in_a, GPIOPin& in_b, GPIOPin& en); // Using this, the object is ready for use
// upon instantiation.
virtual ~VNH5019();
void Init(PWMChannel& pwm_ch, GPIOPin& in_a, GPIOPin& in_b, GPIOPin& en);
void SetPulseWidth(uint32_t dutycycle);
void SetDrivingDirection(DRIVING_DIRECTION);
void Start();
void Stop();
void Brake();
private:
PWMChannel* pwm_channel_;
uint32_t pwm_period_; // us
GPIOPin* in_a_pin_;
GPIOPin* in_b_pin_;
GPIOPin* en_pin_;
bool motor_stopped_;
std::array<std::array<bool, 2>, 2> motor_driver_direction_truthtable_;
};
#endif /* INC_DROBOTTI_DRIVERS__VNH5019_HPP_ */
/*
* WheelEncoder.hpp
*
* Created on: May 11, 2021
* Author: Jacob Hausted
*
* Acts as a wrapper for ST's HAL functions to utilize a timer in "encoder mode", that is,
* the timer is clocked externally, effectively acting as a counter while also setting a
* direction bit depending on the order of pulses between two input channels.
*/
#ifndef INC_DROBOTTI_DRIVERS__WHEELENCODER_HPP_
#define INC_DROBOTTI_DRIVERS__WHEELENCODER_HPP_
#include "stm32f4xx_hal.h"
class WheelEncoder {
public:
WheelEncoder() = delete;
WheelEncoder(TIM_HandleTypeDef& timer_handle);
uint32_t GetAbsoluteTicks();
int32_t GetTicks();
uint32_t GetAbsoluteTicksAndReset();
int32_t GetTicksAndReset();
void ResetTicks();
float GetRPM(float elapsed_time);
bool GetCurrentDirection();
virtual ~WheelEncoder();
private:
TIM_HandleTypeDef timer_handle_; // Handle for a timer configured in "Encoder mode"
//static uint16_t ticks_pr_revolution = 6533;
};
#endif /* INC_DROBOTTI_DRIVERS__WHEELENCODER_HPP_ */
......@@ -8,8 +8,8 @@
#ifndef INC_DROBOTTI_SPEEDREGULATIONCONTROLLER_HPP_
#define INC_DROBOTTI_SPEEDREGULATIONCONTROLLER_HPP_
#include <DRobotti/Drivers/VNH5019.hpp>
#include <DRobotti/Drivers/WheelEncoder.hpp>
#include <DRobotti/Drivers_/VNH5019.hpp>
#include <DRobotti/Drivers_/WheelEncoder.hpp>
#include <DRobotti/Regulator.hpp>
class SpeedRegulationController {
......
/*
* VNH5019.cpp
*
* Created on: May 11, 2021
* Author: Jacob Hausted
*/
#include <DRobotti/Drivers_/VNH5019.hpp>
VNH5019::VNH5019(MOTOR_FORWARD_ORIENTATION orientation_mask) :
motor_stopped_(false),
motor_driver_direction_truthtable_({{{1, 0}, {0, 1}}}) // Initializer list of array of arrays - defaults to clockwise rotation
{
/* From VNH5019, page 14, the direction truthtable is listed
* IN_A IN_B
* [ 1 , 0 ] <-- Clockwise
* [ 0 , 1 ] <-- Counter clockwise
*
* To streamline the rotational direction of the motors, the table can be flipped (by specyfying the bitmask) depending on how to motor
* is mounted on the robot - i.e rear wheel motors are mounted opposite of each other, flipping the table for
* one allows to use static defined variables for clockwise and counterclokwise rotation.
*/
for(auto &row : motor_driver_direction_truthtable_){
for(auto &element : row){
element ^= orientation_mask;
}
}
}
VNH5019::VNH5019(MOTOR_FORWARD_ORIENTATION orientation_mask, PWMChannel& pwm_ch, GPIOPin& in_a, GPIOPin& in_b, GPIOPin& en) :
motor_stopped_(false),
motor_driver_direction_truthtable_({{{1, 0}, {0, 1}}}) // Initializer list of array of arrays - defaults to clockwise rotation
{
/* From VNH5019, page 14, the direction truthtable is listed
* IN_A IN_B
* [ 1 , 0 ] <-- Clockwise
* [ 0 , 1 ] <-- Counter clockwise
*
* To streamline the rotational direction of the motors, the table can be flipped (by specifying the bitmask) depending on how the motor
* is mounted on the robot - i.e rear wheel motors are mounted opposite of each other, flipping the table for
* one allows to use static defined variables for clockwise and counterclockwise rotation.
*/
for(auto &row : motor_driver_direction_truthtable_){
for(auto &element : row){
element ^= orientation_mask;
}
}
// Initialize & set the HW peripherals
Init(pwm_ch, in_a, in_b, en);
SetPulseWidth(0);
}
VNH5019::~VNH5019() {
SetPulseWidth(0);
// Disable motors
en_pin_->write_state(0);
}
void VNH5019::Init(PWMChannel& pwm_ch, GPIOPin& in_a, GPIOPin& in_b, GPIOPin& en){
// HW Periphs
pwm_channel_= &pwm_ch;
pwm_period_ = pwm_channel_->timer_handle.Init.Period;
in_a_pin_ = &in_a;
in_b_pin_ = &in_b;
en_pin_ = &en;
// Enable motors and set an initial driving direction
en_pin_->write_state(1);
SetDrivingDirection(DRIVING_DIRECTION::FORWARD);
}
void VNH5019::SetPulseWidth(uint32_t dutycycle){
*pwm_channel_->pulsewidth_register = (pwm_period_ * dutycycle) / 100; // Map pwm_register period to a value between 0 and 100
}
void VNH5019::SetDrivingDirection(DRIVING_DIRECTION driving_dir){
if(!motor_stopped_){
switch(driving_dir){
case DRIVING_DIRECTION::FORWARD:
/* From VNH5019 datasheet, page 14, the direction truthtable is listed
* Set variables marked with "{}"
* IN_A IN_B
* [ {x} , {x} ]
* [ x , x ]
*/
in_a_pin_->write_state(motor_driver_direction_truthtable_[0][0]);
in_b_pin_->write_state(motor_driver_direction_truthtable_[0][1]);
break;
case DRIVING_DIRECTION::BACKWARD:
/* From VNH5019 datasheet, page 14, the direction truthtable is listed
* Set variables marked with "{}"
* IN_A IN_B
* [ x , x ]
* [ {x} , {x} ]
*/
in_a_pin_->write_state(motor_driver_direction_truthtable_[1][0]);
in_b_pin_->write_state(motor_driver_direction_truthtable_[1][1]);
break;
default:
break;
}
}
}
void VNH5019::Start(){
motor_stopped_ = false;
en_pin_->write_state(1);
}
void VNH5019::Stop(){
motor_stopped_ = true;
/* From VNH5019 datasheet, page 14,
* IN_A = IN_B = 0 brakes the motors to GND
*/
SetPulseWidth(0);
in_a_pin_->write_state(0);
in_b_pin_->write_state(0);
en_pin_->write_state(0);
}
void VNH5019::Brake(){
motor_stopped_ = true;
/* From VNH5019 datasheet, page 14,
* IN_A = IN_B = 1 brakes the motors to V_cc
*/
in_a_pin_->write_state(1);
in_b_pin_->write_state(1);
}
/*
* WheelEncoder.cpp
*
* Created on: May 11, 2021
* Author: Jacob Hausted
*/
#include <DRobotti/Drivers_/WheelEncoder.hpp>
/* CTOR:
* Takes a timer periph reference. Currently it is assumed that this timer is configured beforehand
* and is running in "encoder mode", that is, it is set to use an external clock a (encoder) without
* any prescaling. Currently the class does not perform any checks as to whether the timer passed
* is configured correctly.
*
* To avoid timer underflow/overflow, the counter value is set to begin at half the maximum value of
* the counter register within the timer.
*
* The timer is assumed instantiated - that is, if using the STM32 autogenerated code,
* the object has been passed through its approipiate init functions. Autogenerated code does this
* in the start of main().
*
* @Params:
* - timer_handle : Instantiated timer object reference, running in "encoder mode".
* @Ret:
*
*/
WheelEncoder::WheelEncoder(TIM_HandleTypeDef& timer_handle) :
timer_handle_(timer_handle)
{
HAL_TIM_Encoder_Start(&timer_handle_, TIM_CHANNEL_ALL);
timer_handle_.Instance->CNT = (timer_handle_.Instance->ARR / 2); // Initialize the counter to half of it's maximum
}
/* DTOR:
* Upon destruction, the underlying timer object is "stopped", meaning both the peripheral itself and the input channels are disabled.
*
* @Params:
*
* @Ret:
*
*/
WheelEncoder::~WheelEncoder() {
HAL_TIM_Encoder_Stop(&timer_handle_, TIM_CHANNEL_ALL);
}
/* GetAbsoluteTicks:
* Returns the counter register value without substraction of its initial value.
*
* @Params:
*
* @Ret:
* - Unsigned counter register value.
*/
uint32_t WheelEncoder::GetAbsoluteTicks(){
return timer_handle_.Instance->CNT;
}
/* GetTicks:
* Returns the counter register value relative to its initial value, that is, it
* returns the difference from its initial value.
*
* @Params:
*
* @Ret:
* - Signed difference between counter initial value and current.
*/
int32_t WheelEncoder::GetTicks(){
return (timer_handle_.Instance->ARR / 2) - timer_handle_.Instance->CNT;
}
/* GetAbsoluteTicksAndReset:
* As AbsoluteTics, however, the counter register value is reset after reading to its inital value.
*
* @Params:
*
* @Ret:
* - Unsigned counter register value.
*/
uint32_t WheelEncoder::GetAbsoluteTicksAndReset(){
uint32_t ticks = timer_handle_.Instance->CNT;
ResetTicks();
return ticks;
}
/* GetTicksAndReset:
* As GetTicks, however, the counter register value is reset after reading to its inital value.
*
* @Params:
*
* @Ret:
* - Signed difference between counter initial value and current.
*/
int32_t WheelEncoder::GetTicksAndReset(){
int32_t ticks = (timer_handle_.Instance->ARR / 2) - timer_handle_.Instance->CNT;
ResetTicks();
return ticks;
}
/* ResetTicks:
* Returns the counter register value relative to its initial value, that is, it
* returns the difference from its initial value.
* @Params:
*
* @Ret:
*
*/
void WheelEncoder::ResetTicks(){
timer_handle_.Instance->CNT = (timer_handle_.Instance->ARR / 2);
}
/* GetRPM:
* Reads the counter register value relative to its initial value and converts this to an RPM based on
* the time elapsed. The counter register is reset upon completion.
*
* @Params:
* - Time elapsed in seconds to compute the RPM from
* @Ret:
* - Current RPM (rounds pr minute) of the motors
*/
float WheelEncoder::GetRPM(float elapsed_time){
float ticks = static_cast<float>(GetTicksAndReset());
if(ticks == 0){
return 0;
}
return ((ticks / 6533) / elapsed_time) * 60; // 6533 comes from the encoder datasheet
// TODO: Link datasheet
}
bool WheelEncoder::GetCurrentDirection(){
// TODO
}
......@@ -36,8 +36,8 @@
// Message includes
#include <ackermann_msgs/msg/ackermann_drive.h>
#include <ackermann_msgs/msg/ackermann_drive_stamped.h>
#include <DRobotti/Drivers/VNH5019.hpp>
#include <DRobotti/Drivers/WheelEncoder.hpp>
#include <DRobotti/Drivers_/VNH5019.hpp>
#include <DRobotti/Drivers_/WheelEncoder.hpp>
#include <DRobotti/PI.hpp>
#include <DRobotti/SharedResources.hpp>
#include <DRobotti/Utils/PeripheralTypes.hpp>
......@@ -238,7 +238,7 @@ void StartDefaultTask(void *argument)
executor = rclc_executor_get_zero_initialized_executor();
// total number of handles = n_subscriptions + n_timers
unsigned int num_handles = 3;
unsigned int num_handles = 3; // TODO: Set variable, not pure number
rclc_executor_init(&executor, &support.context, num_handles, &allocator);
rclc_executor_add_subscription(&executor, &move_cmd_subscriber, &move_cmd_message, &onMovCmd_cb, ON_NEW_DATA);
......
......@@ -59,7 +59,7 @@
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
void ResumeMotorControlTask(void);
void ResumeMotorControlTask(void); // TODO: Fix include errors/hard to understnad
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment