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

DRobotti_SpeedControl/.cproject

@@ -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>
                     				

DRobotti_SpeedControl/Core/Inc/DRobotti/Drivers_/VNH5019.hpp

+/*
+ * 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_ */

DRobotti_SpeedControl/Core/Inc/DRobotti/Drivers_/WheelEncoder.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_ */

DRobotti_SpeedControl/Core/Inc/DRobotti/SpeedRegulationController.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 {

DRobotti_SpeedControl/Core/Src/DRobotti/Drivers_/VNH5019.cpp

+/*
+ * 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);
+}
+

DRobotti_SpeedControl/Core/Src/DRobotti/Drivers_/WheelEncoder.cpp

+/*
+ * 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
+}

DRobotti_SpeedControl/Core/Src/freertos.cpp

@@ -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);

DRobotti_SpeedControl/Core/Src/main.cpp

@@ -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 -----------------------------------------------*/