Add 006

content/posts/006-stm32begin-005-motors.md

@@ -1,7 +1,7 @@
 ---
 title: "STM32 For Beginners [5]: Motors"
 date: 2024-10-10T00:36:00+03:00
-draft: true
+draft: false
 summary: "..."
 author: "Rusted Skull"
 series: ["STM32 For Beginners"]
@@ -19,14 +19,254 @@ we are given a [Pololu DRV8835 carrier board](https://www.pololu.com/product/213
 chip. "Dual" in this case means that one board can drive two motors, perfect for a simple differential-drive
 folkrace robot.
 
-{{< figure src="https://a.pololu-files.com/picture/0J4056.1200.jpg" caption="Source: https://wwww.pololu.com" >}}
+{{< figure src="https://a.pololu-files.com/picture/0J4056.1200.jpg" caption="Source: https://wwww.pololu.com/" >}}
 
 If we look at the "Using the motor driver" section in the "Description" of the carrier board's page,
 we notice the following diagram:
 
-{{< figure src="https://a.pololu-files.com/picture/0J4058.600.png" caption="Source: https://www.pololu.com" >}}
+{{< figure src="https://a.pololu-files.com/picture/0J4058.600.png" caption="Source: https://www.pololu.com/" >}}
 
 The right side is dedicated to the motors, and the left side specifies the input signals expected from the microcontroller.
 We immediately notice that for this control scheme, the "phase-enabled mode", we need to generate two PWM
-signals, alongside a few GPIO output signals. Let's look a little closer as PWM now, since that's the missing
+signals, alongside a few GPIO output signals.
-piece for us.
+
+## The PWM Inputs
+
+The PWM inputs of the driver control the speeds of the motors. For differential drive, we need 2 separate PWMs with their
+own duty cycle setting. This lets us set the speed of each motor individually. These PWM channels go into the **AENBL** and
+**BENBL** pins of the driver respectively.
+
+## The GPIO Inputs
+
+The GPIO inputs control the direction of each motor. A simplified table is here:
+
+| xPHASE | Direction |
+|--------|-----------|
+| LOW    | FORWARD   |
+| HIGH   | BACKWARD  |
+
+Note: the specific direction will depend on which way the motor is connected, but the idea is illustrated clearly: put the phase
+high, motor goes one way; flip it low, motor goes the other way. This is all while keeping the PWM constant.
+
+Breaking happens when the PWM duty cycle is set to 0, regardless of the phase direction.
+
+## The Mode Input
+
+The mode input determines which control mode the driver uses. Since we use the phase-enabled mode, we need to pull the pin high.
+Do this by connecting the MODE input through a resistor (say 5 kOhm one) to the 3.3 V output of the STM32 Nucleo board.
+
+# Wiring
+
+The driver has 5 inputs alongside various power and ground lines. We first need to find a good timer to generate 2 PWMs with.
+Timer 1 has 2 channel outputs right next to each other (PA8 and PA9) and doesn't occupy the LED, so that's fine. We can also choose
+the 2 GPIO right next to those two pins for the phase GPIOs: PA10 and PA11.
+
+This gives us the final wiring table as follows:
+| Driver | Nucleo              | Other                     |
+|--------|---------------------|---------------------------|
+| GND    | GND                 | -                         |
+| VCC    | 3V3                 | -                         |
+| BENBL  | TIM1 CH2 (PA8)      | -                         |
+| BPHASE | OUT_M1_DIR (PA10)   | -                         |
+| AENBL  | TIM1 CH1 (PA9)      | -                         |
+| APHASE | OUT_M2_DIR (PA11)   | -                         |
+| MODE   | 3V3 via 5k resistor | -                         |
+| GND    | -                   | Battery negative terminal |
+| VIN    | -                   | Battery positive terminal |
+| BOUT2  | -                   | Motor 2 terminal          |
+| BOUT1  | -                   | Motor 2 terminal          |
+| AOUT2  | -                   | Motor 1 terminal          |
+| AOUT1  | -                   | Motor 1 terminal          |
+
+**NB:** Take extra care when powering the motor side. Use a lab power supply until you're sure your connections are valid. Always
+check your wiring for shorts with a multimeter **before** you power things on.
+
+# Configuration
+
+We have to configure 2 things:
+* Timer 1 for PWM generation,
+* GPIOs for controlling the direction.
+
+Let's start from the bottom and do GPIO configuration first:
+
+{{< video src="/media/stm32begin-005-001.mp4" type="video/mp4" preload="auto" >}}
+
+And then the PWM generation. For the numbers, we'll be going with a 40 kHz PWM signal
+again, primarily to minimize any kind of motor whine we might hear. So prescaler of 1,
+period of 99.
+
+{{< video src="/media/stm32begin-005-002.mp4" type="video/mp4" preload="auto" >}}
+
+This is. Really it. Generate the code and let's go into main.c!
+
+# The Code
+
+So we'll need a few functionalities with our code:
+* Starting the motors.
+* Setting the direction and speed of both motors.
+* Stopping motors (optional).
+
+The startup procedure is simple enough, if we remember the previous class: we start the timer, and we start both the PWM channels.
+In the code begin 2 section, we want to write this:
+
+```cpp
+  /* USER CODE BEGIN 2 */
+  HAL_TIM_Base_Start(&htim1);
+  HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
+  HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);
+  /* USER CODE END 2 */
+```
+
+With this done, we now have to consider how we want to drive the motors.
+
+Setting a single motor to drive in a given direction can be done as follows:
+```cpp
+  /* Infinite loop */
+  /* USER CODE BEGIN WHILE */
+  while (1)
+  {
+	int speed = 50;
+	__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, speed);
+	HAL_GPIO_WritePin(OUT_M1_DIR_GPIO_Port, OUT_M1_DIR_Pin, GPIO_PIN_SET);
+	HAL_Delay(2500);
+	HAL_GPIO_WritePin(OUT_M1_DIR_GPIO_Port, OUT_M1_DIR_Pin, GPIO_PIN_RESET);
+	HAL_Delay(2500);
+    /* USER CODE END WHILE */
+
+    /* USER CODE BEGIN 3 */
+  }
+  /* USER CODE END 3 */
+```
+
+The code will set the first motor to proceed at 50% speed, and will alter its direction
+every 2.5 seconds. A similar part of the code could be written to modify the second motor
+as well.
+
+However, our main loop is likely to be very busy with other things. So we'll want to abstract
+this logic a bit. In C, we can create our own functions. A classical function for a robot
+like this is the `motor_set(int speed_one, int speed_two)` function. Which lets you set the
+speed of both motors in one function call, but independently of one another.
+
+We'll write all of the following into user code begin 0, right before `int main(void)`.
+
+To define our own function, we first write the return type and the arguments of the function.
+We already know the arguments: two speed variables, but do we want the function to return anything?
+Not really, since it's a setter function, ergo, the return type is `void`.
+
+As such, we begin by writing this:
+```cpp
+/* USER CODE BEGIN 0 */
+void motor_set(int speed_one, int speed_two)
+{
+
+}
+/* USER CODE END 0 */
+```
+
+Now, inside the function we need to write the logic for controlling the motors. Let's focus on just one
+motor. We're passing in an `int`, which lets us use both positive and negative integers. It's not too
+hard to figure out what we can use this to our advantage: positive `speed_one` means motor goes forward
+(GPIO is high), negative `speed_one` means motor goes backwards (GPIO is low). And in either case, we'll
+be writing the absolute value of the speed argument into the compare register using `__HAL_TIM_SET_COMPARE()`.
+
+So what would it look like? This:
+
+```cpp
+/* USER CODE BEGIN 0 */
+void motor_set(int speed_one, int speed_two)
+{
+  if (speed_one > -1)
+  {
+    HAL_GPIO_WritePin(OUT_M1_DIR_GPIO_Port, OUT_M1_DIR_Pin, GPIO_PIN_SET);
+    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, speed_one);
+  }
+  else
+  {
+    HAL_GPIO_WritePin(OUT_M1_DIR_GPIO_Port, OUT_M1_DIR_Pin, GPIO_PIN_RESET);
+    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, -1 * speed_one); // (1)
+  }
+}
+/* USER CODE END 0 */
+```
+
+Notice the multiplication with -1 at comment (1). We can only write positive values into `__HAL_TIM_SET_COMPARE()`,
+ergo we have to multiply `speed_one` with -1 to get a positive value corresponding to the desired speed.
+
+Now, with this example written, we can simply add the same logic chain for motor 2:
+
+
+```cpp
+/* USER CODE BEGIN 0 */
+void motor_set(int speed_one, int speed_two)
+{
+  if (speed_one > -1)
+  {
+    HAL_GPIO_WritePin(OUT_M1_DIR_GPIO_Port, OUT_M1_DIR_Pin, GPIO_PIN_SET);
+    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, speed_one);
+  }
+  else
+  {
+    HAL_GPIO_WritePin(OUT_M1_DIR_GPIO_Port, OUT_M1_DIR_Pin, GPIO_PIN_RESET);
+    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, -1 * speed_one);
+  }
+
+  if (speed_two > -1)
+  {
+    HAL_GPIO_WritePin(OUT_M2_DIR_GPIO_Port, OUT_M2_DIR_Pin, GPIO_PIN_SET);
+    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, speed_two);
+  }
+  else
+  {
+    HAL_GPIO_WritePin(OUT_M2_DIR_GPIO_Port, OUT_M2_DIR_Pin, GPIO_PIN_RESET);
+    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, -1 * speed_two);
+  }
+}
+/* USER CODE END 0 */
+```
+
+Make sure the inputs and channels all match in both sets. But the function is now done.
+
+We can now use this in our main function like this:
+
+```cpp
+  /* Infinite loop */
+  /* USER CODE BEGIN WHILE */
+  while (1)
+  {
+    motor_set(50, -50);
+    HAL_Delay(1000);
+    motor_set(-50, 50);
+    HAL_Delay(1000);
+    /* USER CODE END WHILE */
+
+    /* USER CODE BEGIN 3 */
+  }
+  /* USER CODE END 3 */
+```
+
+And voila, we now have motor control!
+
+# Debugging
+
+A final note about debugging.
+
+If your motors are not moving at all, the first thing to do is to check the development board and driver side connections.
+Remove all motors and battery connectors from the motor side, and depower the entire system. Proceed to continuity check the pins
+on the driver to the nucleo development board, according to the connections table up to. A beep is good, no beep is bad.
+
+If the continuity checks out but issues persist, we can also use a multimeter to debug the signals a bit. Again, disconnect
+everything from the motor side before proceeding. Put your multimeter into voltage mode, connect one probe to ground and use
+the other to probe signals.
+
+Set your motors to drive at 50, 50 speed, for example. And then validate that:
+* Both APHASE and BPHASE get a high signal.
+* AENABLE and BENABLE should show somewhere about 3.3 / 2 volts, so 1.6 ~ 1.7 volts.
+
+If one of those is missing, then recheck your connections and your code alongside the configuration. One of the values is not
+being written into the right place.
+
+If that all works but the motors aren't running, contact an instructor.
+
+And finally, if your motors are going in the wrong direction, simply flip around their connectors.
+
+Now, onto sensors!