From concept to hardware.

1// arm-controller.cpp | STM32F405 @ 168 MHz
2// External TMC5160 axes on the SPI expansion.
3#include "stm32f4xx_hal.h"
4#include "main.h"
5#include "spi.h"
6#include "tim.h"
7#include "iwdg.h"
8#include <array>
9#include <atomic>
10#include <cstdint>
11// CubeMX: SysTick HAL timebase; TIM6 IRQ.
12// TIM6: APB1 timer = 84 MHz, PSC=83, ARR=999.
13// SPI1: mode 3, 8-bit, MSB first, 1.3125 MHz.
14// EN has a pull-up; e-stop also gates power.
15using Steps = std::array<int32_t, 4>;
16using Atomic = std::atomic<uint32_t>;
17static_assert(Atomic::is_always_lock_free);
18// BSP verifies homing and configures TMC5160
19// current, ramps, limits and clears GSTAT.
20extern bool Board_ReadHomed(Steps& position);
21constexpr Steps travel{
22  16000, 8000, 12000, 6000};
23// All four chip selects share the same port.
24constexpr uint16_t cs[]{
25  AXIS0_CS_Pin, AXIS1_CS_Pin,
26  AXIS2_CS_Pin, AXIS3_CS_Pin};
27static Atomic pending{0}, fault{0};
28static Steps target{};
29static uint32_t lastCycle{}, lastCommand{};
30static bool ready = false, armed = false;
31 
32static void disableDrives() {
33  HAL_GPIO_WritePin(MOTOR_EN_GPIO_Port,
34    MOTOR_EN_Pin, GPIO_PIN_SET);
35}
36static void trip() {
37  disableDrives();
38  fault.store(1); // No automatic re-arm.
39}
40static bool valid(const Steps& q) {
41  for (unsigned i = 0; i < 4; ++i) {
42    if (q[i] < -travel[i] || q[i] > travel[i]) {
43      return false;
44    }
45  }
46  return true;
47}
48 
49// TMC5160 XTARGET: write bit + register 0x2D.
50static bool writeTarget(unsigned axis,
51                        int32_t position) {
52  const auto bits = uint32_t(position);
53  uint8_t tx[]{0xAD, uint8_t(bits >> 24),
54    uint8_t(bits >> 16), uint8_t(bits >> 8),
55    uint8_t(bits)};
56  uint8_t rx[5]{};
57  HAL_GPIO_WritePin(AXIS0_CS_GPIO_Port,
58    cs[axis], GPIO_PIN_RESET);
59  const auto status = HAL_SPI_TransmitReceive(
60    &hspi1, tx, rx, sizeof(tx), 1);
61  HAL_GPIO_WritePin(AXIS0_CS_GPIO_Port,
62    cs[axis], GPIO_PIN_SET);
63  // Status bit 1: driver error; bit 0: reset.
64  return status == HAL_OK && !(rx[0] & 0x03);
65}
66extern "C" void HAL_TIM_PeriodElapsedCallback(
67    TIM_HandleTypeDef* timer) {
68  if (timer->Instance == TIM6) {
69    pending.fetch_add(1); // Never SPI in ISR.
70  }
71}
72extern "C" void HAL_GPIO_EXTI_Callback(
73    uint16_t pin) {
74  if (pin == ESTOP_Pin) { trip(); }
75}
76 
77// Call once after all CubeMX MX_*_Init calls.
78extern "C" void Arm_Init() {
79  disableDrives();
80  lastCycle = HAL_GetTick();
81  if (HAL_TIM_Base_Start_IT(&htim6) != HAL_OK) {
82    trip(); return;
83  }
84  ready = true;
85}
86 
87// These APIs run only in the foreground task.
88extern "C" bool Arm_Arm() {
89  if (!ready || armed || fault.load()) {
90    return false;
91  }
92  if (!Board_ReadHomed(target) ||
93      !valid(target)) {
94    trip(); return false;
95  }
96  for (unsigned i = 0; i < 4; ++i) {
97    if (!writeTarget(i, target[i])) {
98      trip(); return false;
99    }
100  }
101  // Serialize enable against the e-stop ISR.
102  const auto mask = __get_PRIMASK();
103  __disable_irq();
104  const bool released = HAL_GPIO_ReadPin(
105    ESTOP_GPIO_Port, ESTOP_Pin) == GPIO_PIN_SET;
106  armed = released && !fault.load();
107  if (armed) {
108    HAL_GPIO_WritePin(MOTOR_EN_GPIO_Port,
109      MOTOR_EN_Pin, GPIO_PIN_RESET);
110    lastCommand = HAL_GetTick();
111  }
112  __set_PRIMASK(mask);
113  return armed;
114}
115bool Arm_Command(const Steps& next) {
116  if (!armed || fault.load()) { return false; }
117  if (!valid(next)) { trip(); return false; }
118  target = next; // Fresh planner output only.
119  lastCommand = HAL_GetTick();
120  return true;
121}
122// Call continuously from the main superloop.
123extern "C" void Arm_Poll() {
124  if (!ready || fault.load()) { return; }
125  const auto ticks = pending.exchange(0);
126  if (ticks == 0) { return; }
127  const auto started = HAL_GetTick();
128  const uint32_t dt = started - lastCycle;
129  if (ticks > 1 || dt > 2) { trip(); return; }
130  lastCycle = started; // Unsigned wrap-safe dt.
131  if (armed) {
132    if (uint32_t(started-lastCommand) > 100) {
133      trip(); return;
134    }
135    for (unsigned i = 0; i < 4; ++i) {
136      if (fault.load()) { return; }
137      if (!writeTarget(i, target[i])) {
138        trip(); return;
139      }
140    }
141  }
142  if (uint32_t(HAL_GetTick()-started) > 1) {
143    trip(); return;
144  }
145  if (!fault.load()) {
146    if (HAL_IWDG_Refresh(&hiwdg) != HAL_OK) {
147      trip();
148    }
149  }
150}