get motor driver task up

Author: ryanthecoder

stm32-modules/flex-stacker/firmware/motor_control/freertos_motor_task.cpp

@@ -26,12 +26,12 @@ auto run(tasks::FirmwareTasks::QueueAggregator* aggregator) -> void {
     aggregator->register_queue(_queue);
     _top_task.provide_aggregator(aggregator);
 
-    //        motor_hardware_init();
+    motor_hardware_init();
 
-    //    auto policy = motor_policy::MotorPolicy();
+    auto policy = motor_policy::MotorPolicy();
     while (true) {
-        vTaskDelay(1000);
-        //        _top_task.run_once(policy);
+        //vTaskDelay(1000);
+        _top_task.run_once(policy);
     }
 }
 

stm32-modules/flex-stacker/firmware/motor_control/motor_hardware.c

@@ -9,8 +9,8 @@
 #define Z_DIR_PORT (GPIOC)
 #define Z_EN_PIN (GPIO_PIN_3)
 #define Z_EN_PORT (GPIOA)
-#define Z_N_BRAKE_PIN(GPIO_PIN_7)
-#define Z_N_BRAKE_PORT(GPIOB)
+#define Z_N_BRAKE_PIN (GPIO_PIN_7)
+#define Z_N_BRAKE_PORT (GPIOB)
 
 /** Limit switches **/
 /* Note: Photointerrupters limit switches */
@@ -29,8 +29,8 @@
 #define X_DIR_PORT (GPIOA)
 #define X_EN_PIN (GPIO_PIN_4)
 #define X_EN_PORT (GPIOA)
-#define X_N_BRAKE_PIN(GPIO_PIN_9)
-#define X_N_BRAKE_PORT(GPIOB)
+#define X_N_BRAKE_PIN (GPIO_PIN_9)
+#define X_N_BRAKE_PORT (GPIOB)
 
 
 /** Limit switches **/
@@ -48,8 +48,8 @@
 #define L_STEP_PORT (GPIOB)
 #define L_DIR_PIN (GPIO_PIN_0)
 #define L_DIR_PORT (GPIOB)
-#define L_EN_PIN (GPIO_PIN_4)
-#define L_EN_PORT (GPIOA)
+#define L_EN_PIN (GPIO_PIN_5)
+#define L_EN_PORT (GPIOC)
 
 
 /** Limit switches **/
@@ -60,3 +60,64 @@
 #define L_N_RELEASED_PORT (GPIO_PIN_C)
 
 
+void motor_hardware_init(void){
+
+  GPIO_InitTypeDef GPIO_InitStruct = {0};
+/* USER CODE BEGIN MX_GPIO_Init_1 */
+/* USER CODE END MX_GPIO_Init_1 */
+
+  /* GPIO Ports Clock Enable */
+  __HAL_RCC_GPIOC_CLK_ENABLE();
+  __HAL_RCC_GPIOF_CLK_ENABLE();
+  __HAL_RCC_GPIOA_CLK_ENABLE();
+  __HAL_RCC_GPIOB_CLK_ENABLE();
+
+  /*Configure GPIO pin Output Level */
+  HAL_GPIO_WritePin(GPIOC, Z_DIR_PIN|L_N_HELD_PIN, GPIO_PIN_RESET);
+
+  /*Configure GPIO pin Output Level */
+  HAL_GPIO_WritePin(GPIOA, Z_EN_PIN|X_EN_PIN|X_DIR_PIN, GPIO_PIN_RESET);
+
+  /*Configure GPIO pin Output Level */
+  HAL_GPIO_WritePin(GPIOB, L_DIR_PIN, GPIO_PIN_RESET);
+
+  /*Configure GPIO pins :  Z_MINUS_LIMIT_PIN */
+  GPIO_InitStruct.Pin = Z_MINUS_LIMIT_PIN;
+  GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
+
+  /*Configure GPIO pins : Z_DIR_PIN L_EN_PIN */
+  GPIO_InitStruct.Pin = Z_DIR_PIN|L_EN_PIN;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+  HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
+
+  /*Configure GPIO pins : Z_PLUS_LIMIT_PIN LIMIT_X__Pin LIMIT_X_A2_Pin */
+  GPIO_InitStruct.Pin = Z_PLUS_LIMIT_PIN|X_MINUS_LIMIT_PIN|X_PLUS_LIMIT_PIN;
+  GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+
+  /*Configure GPIO pins : Z_EN_PIN X_EN_PIN X_DIR_PIN */
+  GPIO_InitStruct.Pin = Z_EN_PIN|X_EN_PIN|X_DIR_PIN;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+
+  /*Configure GPIO pins : L_DIR_PIN Z_N_BRAKE_PIN X_N_BRAKE_PIN*/
+  GPIO_InitStruct.Pin = L_DIR_PIN | Z_N_BRAKE_PIN | X_N_BRAKE_PIN;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
+
+  /*Configure GPIO pins : L_N_HELD_PIN */
+  GPIO_InitStruct.Pin = L_N_HELD_PIN;
+  GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
+}
+

stm32-modules/include/flex-stacker/firmware/motor_policy.hpp

@@ -6,6 +6,7 @@
 
 namespace motor_policy {
 
+
 class MotorPolicy {
   public:
     auto enable_motor(MotorID motor_id) -> void;

stm32-modules/include/flex-stacker/flex-stacker/motor_task.hpp

@@ -10,11 +10,25 @@
 #include "core/version.hpp"
 #include "flex-stacker/messages.hpp"
 #include "flex-stacker/tasks.hpp"
+#include "firmware/motor_policy.hpp"
 #include "flex-stacker/tmc2160_registers.hpp"
 #include "hal/message_queue.hpp"
 
 namespace motor_task {
 
+template <typename P>
+concept MotorControlPolicy = requires(P p, MotorID motor_id) {
+    {
+        p.enable_motor(motor_id)
+        } -> std::same_as<void>;
+    {
+        p.disable_motor(motor_id)
+        } ->  std::same_as<void>;
+    {
+        p.set_motor_speed(motor_id, double{0.0})
+        } -> std::same_as<bool>;
+};
+
 using Message = messages::MotorMessage;
 
 template <template <class> class QueueImpl>
@@ -38,7 +52,9 @@ class MotorTask {
         _task_registry = aggregator;
     }
 
-    auto run_once() -> void {
+
+    template <MotorControlPolicy Policy>
+    auto run_once(Policy& policy) -> void {
         if (!_task_registry) {
             return;
         }