driver/servo/servo.c
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /******************************************************************************* | ||
| 2 | * | ||
| 3 | * FILE: | ||
| 4 | * servo.c | ||
| 5 | * | ||
| 6 | * DESCRIPTION: | ||
| 7 | * Contains API functions to get access to the servo motors driver | ||
| 8 | * | ||
| 9 | * COPYRIGHT: | ||
| 10 | * Copyright (c) 2025 Sun Devil Rocketry. | ||
| 11 | * All rights reserved. | ||
| 12 | * | ||
| 13 | * This software is licensed under terms that can be found in the LICENSE | ||
| 14 | * file in the root directory of this software component. | ||
| 15 | * If no LICENSE file comes with this software, it is covered under the | ||
| 16 | * BSD-3-Clause. | ||
| 17 | * | ||
| 18 | * https://opensource.org/license/bsd-3-clause | ||
| 19 | * | ||
| 20 | *******************************************************************************/ | ||
| 21 | |||
| 22 | /*------------------------------------------------------------------------------ | ||
| 23 | Standard Includes | ||
| 24 | ------------------------------------------------------------------------------*/ | ||
| 25 | |||
| 26 | |||
| 27 | /*------------------------------------------------------------------------------ | ||
| 28 | Project Includes | ||
| 29 | ------------------------------------------------------------------------------*/ | ||
| 30 | #include "main.h" | ||
| 31 | #include "sdr_pin_defines_A0002.h" | ||
| 32 | #include "servo.h" | ||
| 33 | #include "led.h" | ||
| 34 | #include "init.h" | ||
| 35 | #include "usb.h" | ||
| 36 | |||
| 37 | /*------------------------------------------------------------------------------ | ||
| 38 | Literal Constants | ||
| 39 | ------------------------------------------------------------------------------*/ | ||
| 40 | #define SER_PER 0.55555 | ||
| 41 | |||
| 42 | /*------------------------------------------------------------------------------ | ||
| 43 | Global Variables | ||
| 44 | ------------------------------------------------------------------------------*/ | ||
| 45 | #ifdef CANARD | ||
| 46 | extern SERVO_PRESET servo_preset; | ||
| 47 | #endif | ||
| 48 | |||
| 49 | #ifdef APPA | ||
| 50 | extern PRESET_DATA preset_data; | ||
| 51 | #endif | ||
| 52 | |||
| 53 | /*------------------------------------------------------------------------------ | ||
| 54 | Procedures | ||
| 55 | ------------------------------------------------------------------------------*/ | ||
| 56 | |||
| 57 | /******************************************************************************* | ||
| 58 | * * | ||
| 59 | * PROCEDURE: * | ||
| 60 | * servo_init * | ||
| 61 | * * | ||
| 62 | * DESCRIPTION: * | ||
| 63 | * Initialize/Reset servo * | ||
| 64 | * * | ||
| 65 | *******************************************************************************/ | ||
| 66 | 3 | SERVO_STATUS servo_init | |
| 67 | ( | ||
| 68 | void | ||
| 69 | ) | ||
| 70 | { | ||
| 71 | /* GPIO Initialization */ | ||
| 72 | 3 | HAL_GPIO_WritePin(MOTOR1_EN_PORT, MOTOR1_EN, GPIO_PIN_SET); | |
| 73 | 3 | HAL_GPIO_WritePin(MOTOR2_EN_PORT, MOTOR2_EN, GPIO_PIN_SET); | |
| 74 | 3 | HAL_GPIO_WritePin(MOTOR3_EN_PORT, MOTOR3_EN, GPIO_PIN_SET); | |
| 75 | 3 | HAL_GPIO_WritePin(MOTOR4_EN_PORT, MOTOR4_EN, GPIO_PIN_SET); | |
| 76 | |||
| 77 | /* Timer intialization */ | ||
| 78 | 3 | HAL_StatusTypeDef hal_status1, hal_status2, hal_status3, hal_status4; | |
| 79 | |||
| 80 | 3 | hal_status1 = HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_4); | |
| 81 | 3 | hal_status2 = HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_3); | |
| 82 | 3 | hal_status3 = HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1); | |
| 83 | 3 | hal_status4 = HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1); | |
| 84 | |||
| 85 | 3 | if ( hal_status1 == HAL_OK && | |
| 86 | hal_status2 == HAL_OK && | ||
| 87 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
|
3 | hal_status3 == HAL_OK && |
| 88 | hal_status4 == HAL_OK ) | ||
| 89 | { | ||
| 90 | return SERVO_OK; | ||
| 91 | } | ||
| 92 | else | ||
| 93 | { | ||
| 94 | ✗ | return SERVO_FAIL; | |
| 95 | } | ||
| 96 | |||
| 97 | } /* servo_init */ | ||
| 98 | |||
| 99 | |||
| 100 | /******************************************************************************* | ||
| 101 | * * | ||
| 102 | * PROCEDURE: * | ||
| 103 | * motor_drive * | ||
| 104 | * * | ||
| 105 | * DESCRIPTION: * | ||
| 106 | * Drives the specified servo motor to the desired angle (0-180). * | ||
| 107 | * * | ||
| 108 | *******************************************************************************/ | ||
| 109 | 12 | void motor_drive | |
| 110 | ( | ||
| 111 | SERVO_ID servo, | ||
| 112 | uint8_t angle | ||
| 113 | ) | ||
| 114 | { | ||
| 115 | 12 | uint8_t pulse = angle_to_pulse(motor_snap_to_bound(angle, 180, 0)); | |
| 116 | |||
| 117 |
4/5✓ Branch 0 taken 3 times.
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 3 times.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
|
12 | switch (servo) |
| 118 | { | ||
| 119 | 3 | case SERVO_1: | |
| 120 | 3 | htim3.Instance->CCR4 = pulse; | |
| 121 | 3 | break; | |
| 122 | 3 | case SERVO_2: | |
| 123 | 3 | htim3.Instance->CCR3 = pulse; | |
| 124 | 3 | break; | |
| 125 | 3 | case SERVO_3: | |
| 126 | 3 | htim3.Instance->CCR1 = pulse; | |
| 127 | 3 | break; | |
| 128 | 3 | case SERVO_4: | |
| 129 | 3 | htim2.Instance->CCR1 = pulse; | |
| 130 | 3 | break; | |
| 131 | default: | ||
| 132 | /* Optionally: handle error */ | ||
| 133 | break; | ||
| 134 | } | ||
| 135 | |||
| 136 | 12 | } /* motor_drive */ | |
| 137 | |||
| 138 | |||
| 139 | /******************************************************************************* | ||
| 140 | * * | ||
| 141 | * PROCEDURE: * | ||
| 142 | * servo_reset * | ||
| 143 | * * | ||
| 144 | * DESCRIPTION: * | ||
| 145 | * Resets all servos to their preset positions * | ||
| 146 | * * | ||
| 147 | *******************************************************************************/ | ||
| 148 | ✗ | void servo_reset | |
| 149 | ( | ||
| 150 | void | ||
| 151 | ) | ||
| 152 | { | ||
| 153 | #ifdef CANARD | ||
| 154 | motor_drive(SERVO_1, servo_preset.rp_servo1); | ||
| 155 | motor_drive(SERVO_2, servo_preset.rp_servo2); | ||
| 156 | motor_drive(SERVO_3, servo_preset.rp_servo3); | ||
| 157 | motor_drive(SERVO_4, servo_preset.rp_servo4); | ||
| 158 | #endif | ||
| 159 | |||
| 160 | #ifdef APPA | ||
| 161 | motor_drive(SERVO_1, preset_data.servo_preset.rp_servo1); | ||
| 162 | motor_drive(SERVO_2, preset_data.servo_preset.rp_servo2); | ||
| 163 | motor_drive(SERVO_3, preset_data.servo_preset.rp_servo3); | ||
| 164 | motor_drive(SERVO_4, preset_data.servo_preset.rp_servo4); | ||
| 165 | #endif | ||
| 166 | |||
| 167 | ✗ | } /* servo_reset */ | |
| 168 | |||
| 169 | |||
| 170 | /******************************************************************************* | ||
| 171 | * * | ||
| 172 | * PROCEDURE: * | ||
| 173 | * motors_drive * | ||
| 174 | * * | ||
| 175 | * DESCRIPTION: * | ||
| 176 | * A complete function that drives all servos in this board * | ||
| 177 | * Uses motor_drive for each servo. * | ||
| 178 | * * | ||
| 179 | *******************************************************************************/ | ||
| 180 | ✗ | void motors_drive | |
| 181 | ( | ||
| 182 | uint8_t angle | ||
| 183 | ) | ||
| 184 | { | ||
| 185 | ✗ | motor_drive(SERVO_1, angle); | |
| 186 | ✗ | motor_drive(SERVO_2, angle); | |
| 187 | ✗ | motor_drive(SERVO_3, angle); | |
| 188 | ✗ | motor_drive(SERVO_4, angle); | |
| 189 | |||
| 190 | ✗ | } /* motors_drive */ | |
| 191 | |||
| 192 | |||
| 193 | /******************************************************************************* | ||
| 194 | * * | ||
| 195 | * PROCEDURE: * | ||
| 196 | * servo_cmd_execute * | ||
| 197 | * * | ||
| 198 | * DESCRIPTION: * | ||
| 199 | * Executes a servo command given from the terminal. * | ||
| 200 | * * | ||
| 201 | *******************************************************************************/ | ||
| 202 | ✗ | SERVO_STATUS servo_cmd_execute | |
| 203 | ( | ||
| 204 | uint8_t subcommand | ||
| 205 | ) | ||
| 206 | { | ||
| 207 | ✗ | USB_STATUS usb_status; | |
| 208 | |||
| 209 | ✗ | switch ( subcommand ) | |
| 210 | { | ||
| 211 | ✗ | case SERVO_SWEEP: | |
| 212 | { | ||
| 213 | ✗ | uint8_t degree; | |
| 214 | ✗ | usb_status = usb_receive(°ree, sizeof(uint8_t), 1000); | |
| 215 | ✗ | motors_drive(degree); | |
| 216 | ✗ | if (usb_status == USB_OK) | |
| 217 | { | ||
| 218 | return SERVO_OK; | ||
| 219 | } | ||
| 220 | else | ||
| 221 | { | ||
| 222 | ✗ | led_set_color( LED_YELLOW ); | |
| 223 | ✗ | HAL_Delay( 5000 ); | |
| 224 | ✗ | return SERVO_FAIL; | |
| 225 | } | ||
| 226 | } | ||
| 227 | ✗ | case SERVO_RESET: | |
| 228 | { | ||
| 229 | ✗ | servo_reset(); | |
| 230 | ✗ | return SERVO_OK; | |
| 231 | } | ||
| 232 | default: | ||
| 233 | { | ||
| 234 | return SERVO_FAIL; | ||
| 235 | } | ||
| 236 | } | ||
| 237 | |||
| 238 | } /* servo_cmd_execute */ | ||
| 239 | |||
| 240 | |||
| 241 | /******************************************************************************* | ||
| 242 | * * | ||
| 243 | * PROCEDURE: * | ||
| 244 | * angle_to_pulse * | ||
| 245 | * * | ||
| 246 | * DESCRIPTION: * | ||
| 247 | * Convert turn angle degree into PWM pulse * | ||
| 248 | * * | ||
| 249 | *******************************************************************************/ | ||
| 250 | 12 | uint8_t angle_to_pulse | |
| 251 | ( | ||
| 252 | uint8_t angle | ||
| 253 | ) | ||
| 254 | { | ||
| 255 | 12 | return 25 + (angle*SER_PER); | |
| 256 | |||
| 257 | } /* angle_to_pulse */ | ||
| 258 | |||
| 259 | |||
| 260 | /******************************************************************************* | ||
| 261 | * * | ||
| 262 | * PROCEDURE: * | ||
| 263 | * motor_snap_to_bound * | ||
| 264 | * * | ||
| 265 | * DESCRIPTION: * | ||
| 266 | * Given an angle, upper bound, and lower bound, snap the angle to upper * | ||
| 267 | * or lower if the angle is outside of the bounds. * | ||
| 268 | * * | ||
| 269 | *******************************************************************************/ | ||
| 270 | 12 | uint8_t motor_snap_to_bound | |
| 271 | ( | ||
| 272 | uint8_t angle, | ||
| 273 | uint8_t upper, | ||
| 274 | uint8_t lower | ||
| 275 | ) | ||
| 276 | { | ||
| 277 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 12 times.
|
12 | if (angle >= lower && angle <= upper) |
| 278 | { | ||
| 279 | return angle; | ||
| 280 | } | ||
| 281 | ✗ | else if ( angle > upper && angle <= ( upper + ( ( 255 - upper ) / 2 ) ) ) | |
| 282 | { | ||
| 283 | return upper; | ||
| 284 | } | ||
| 285 | else | ||
| 286 | { | ||
| 287 | ✗ | return lower; | |
| 288 | } | ||
| 289 | |||
| 290 | } /* motor_snap_to_bound */ | ||
| 291 |