Firmware: Motor and OmniBotController initialization in start.cpp

Firmware/OmniBot_RC/Application/start.cpp

@@ -16,18 +16,36 @@
 #include "UART_STM32.hpp"
 #include "Queue_STM32.hpp"
 #include "CDC_STM32.hpp"
+#include "MotorPID_STM32.hpp"
 
 #include "Components/ComputerComms_CDC/ComputerComms_CDC.hpp"
+#include "Components/OmniBotController/OmniBotController.hpp"
 
+// Leds
 GPIO_Pin_STM32 ledOrange(GPIOD, GPIO_PIN_13);
 GPIO_Pin_STM32 ledGreen(GPIOD, GPIO_PIN_12);
 GPIO_Pin_STM32 ledRed(GPIOD, GPIO_PIN_14);
 GPIO_Pin_STM32 ledBlue(GPIOD, GPIO_PIN_15);
 BinLeds discoveryLeds((GPIO_Pin*[]){&ledOrange, &ledGreen, &ledRed, &ledBlue}, 4);
 
+// Motors
+MotorPID_STM32::Configuration motorConfig = {
+		.wheelRadius = 40,
+		.reductionRatio = 1/72,
+		.encoderCountsPerRevolution = 52,
+};
+MotorPID_STM32 motor0(motorConfig);
+MotorPID_STM32 motor1(motorConfig);
+MotorPID_STM32 motor2(motorConfig);
+MotorPID_STM32 motor3(motorConfig);
+std::array<MotorPID*, 4> motors = {&motor0, &motor1, &motor2, &motor3};
+
+// Queues
 Queue_STM32<std::vector<uint8_t>, 4> computerPktQueue("computerPktQueue");
 
+// Components
 ComputerComms_CDC componentComputerComms(&computerPktQueue, CDC_STM32::getInstance());
+OmniBotController componentOmniBotController(motors);
 
 void executableDispatch(void* _executable){
 	Executable* executable = static_cast<Executable*>(_executable);
@@ -64,11 +82,15 @@ void start(){
 
 	//Init components
 	componentComputerComms.init();
+	componentOmniBotController.init();
 
 	//Init tasks
 	TaskHandle_t hTaskReceiveFromComputer;
 	xTaskCreate(executableDispatch, "ReceiveFromComputer", 256, &componentComputerComms, 20, &hTaskReceiveFromComputer);
 
+	TaskHandle_t hTaskOmniBotController;
+	xTaskCreate(executableDispatch, "OmniBotController", 256, &componentOmniBotController, 20, &hTaskOmniBotController);
+
 	discoveryLeds.set(3);
 	vTaskDelete(nullptr);
 }