motors

Author: jhuang2006

SConscript

@@ -161,6 +161,9 @@ if os.getenv("FINAL_PROVISIONING"):
   flags += ["-DFINAL_PROVISIONING"]
 build_project("panda_jungle_h7", base_project_h7, "./board/jungle/main.c", flags)
 
+# body fw
+build_project("body_h7", base_project_h7, "./board/body/main.c", ["-DPANDA_BODY"])
+
 # test files
 if GetOption('extras'):
   SConscript('tests/libpanda/SConscript')

__init__.py

@@ -8,3 +8,6 @@
 
 # panda jungle
 from .board.jungle import PandaJungle, PandaJungleDFU # noqa: F401
+
+# panda body
+from .board.body import PandaBody  # noqa: F401

board/boards/board_declarations.h

@@ -52,6 +52,7 @@ struct board {
 #define HW_TYPE_RED_PANDA 7U
 #define HW_TYPE_TRES 9U
 #define HW_TYPE_CUATRO 10U
+#define HW_TYPE_BODY 0xB0U
 
 // CAN modes
 #define CAN_MODE_NORMAL 0U
@@ -60,3 +61,4 @@ struct board {
 extern struct board board_tres;
 extern struct board board_cuatro;
 extern struct board board_red;
+extern struct board board_body;

board/boards/body.h

@@ -0,0 +1,43 @@
+#pragma once
+
+#include "board_declarations.h"
+#include "unused_funcs.h"
+
+static void body_init(void) {
+}
+
+static void body_enable_can_transceiver(uint8_t transceiver, bool enabled) {
+  (void)transceiver;
+  (void)enabled;
+}
+
+static void body_set_can_mode(uint8_t mode) {
+  (void)mode;
+}
+
+static uint32_t body_read_voltage_mV(void) {
+  return 0U;
+}
+
+board board_body = {
+  .harness_config = 0,
+  .led_GPIO = {GPIOC, GPIOC, GPIOC},
+  .led_pin = {7U, 7U, 7U},
+  .led_pwm_channels = {0U, 0U, 0U},
+  .has_spi = false,
+  .has_fan = false,
+  .avdd_mV = 0U,
+  .fan_enable_cooldown_time = 0U,
+  .init = body_init,
+  .init_bootloader = unused_init_bootloader,
+  .enable_can_transceiver = body_enable_can_transceiver,
+  .set_can_mode = body_set_can_mode,
+  .read_voltage_mV = body_read_voltage_mV,
+  .read_current_mA = unused_read_current,
+  .set_ir_power = unused_set_ir_power,
+  .set_fan_enabled = unused_set_fan_enabled,
+  .set_siren = unused_set_siren,
+  .set_bootkick = unused_set_bootkick,
+  .read_som_gpio = unused_read_som_gpio,
+  .set_amp_enabled = unused_set_amp_enabled
+};

board/body/__init__.py

@@ -0,0 +1,42 @@
+# python helpers for the body panda
+import struct
+
+from panda import Panda
+
+
+class PandaBody(Panda):
+  SUPPORTED_DEVICES = (Panda.HW_TYPE_BODY,)
+
+  def __init__(self, *args, **kwargs):
+    super().__init__(*args, **kwargs)
+    if self.get_type() not in PandaBody.SUPPORTED_DEVICES:
+      raise ValueError("connected device is not a body panda")
+
+  # ****************** Motor Control *****************
+
+  def motor_set_speed(self, motor: int, speed: int) -> None:
+    assert motor in (1, 2), "motor must be 1 or 2"
+    assert -100 <= speed <= 100, "speed must be between -100 and 100"
+    speed_param = speed if speed >= 0 else (256 + speed)
+    self._handle.controlWrite(Panda.REQUEST_OUT, 0xe0, motor, speed_param, b'')
+
+  def motor_set_target_rpm(self, motor: int, rpm: float) -> None:
+    assert motor in (1, 2), "motor must be 1 or 2"
+    target_deci_rpm = int(round(rpm * 10.0))
+    assert -32768 <= target_deci_rpm <= 32767, "target rpm out of range (-3276.8 to 3276.7)"
+    target_param = target_deci_rpm if target_deci_rpm >= 0 else (target_deci_rpm + (1 << 16))
+    self._handle.controlWrite(Panda.REQUEST_OUT, 0xe4, motor, target_param, b'')
+
+  def motor_stop(self, motor: int) -> None:
+    assert motor in (1, 2), "motor must be 1 or 2"
+    self._handle.controlWrite(Panda.REQUEST_OUT, 0xe1, motor, 0, b'')
+
+  def motor_get_encoder_state(self, motor: int) -> tuple[int, float]:
+    assert motor in (1, 2), "motor must be 1 or 2"
+    dat = self._handle.controlRead(Panda.REQUEST_IN, 0xe2, motor, 0, 8)
+    position, rpm_milli = struct.unpack("<ii", dat)
+    return position, rpm_milli / 1000.0
+
+  def motor_reset_encoder(self, motor: int) -> None:
+    assert motor in (1, 2), "motor must be 1 or 2"
+    self._handle.controlWrite(Panda.REQUEST_OUT, 0xe3, motor, 0, b'')

board/body/boards/board_body.h

@@ -0,0 +1,52 @@
+
+#include "board/body/boards/motor_control.h"
+
+void board_body_init(void) {
+  motor_init();
+  motor_encoder_init();
+  motor_speed_controller_init();
+  motor_stop(1);
+  motor_stop(2);
+  motor_encoder_reset(1);
+  motor_encoder_reset(2);
+  
+}
+
+void board_body_init_bootloader(void) {
+}
+
+void board_body_tick(void) {
+}
+
+void board_body_enable_can_transceiver(uint8_t transceiver, bool enabled) {
+  UNUSED(transceiver);
+  set_gpio_output(GPIOD, 2U, enabled ? 1 : 0);
+}
+
+void board_body_set_can_mode(uint8_t mode) {
+  UNUSED(mode);
+}
+
+uint32_t board_body_read_voltage_mV(void) {
+  return 0U;
+}
+
+uint32_t board_body_read_current_mA(void) {
+  return 0U;
+}
+
+board board_body = {
+  .led_GPIO = {GPIOC, GPIOC, GPIOC},
+  .led_pin = {7, 7, 7},
+  .led_pwm_channels = {0, 0, 0},
+  .avdd_mV = 3300U,
+  .init = board_body_init,
+  .init_bootloader = board_body_init_bootloader,
+  .board_tick = board_body_tick,
+  .enable_can_transceiver = board_body_enable_can_transceiver,
+  .set_can_mode = board_body_set_can_mode,
+  .read_voltage_mV = board_body_read_voltage_mV,
+  .read_current_mA = board_body_read_current_mA,
+  .has_spi = false,
+  .has_fan = false,
+};

board/body/boards/board_declarations.h

@@ -0,0 +1,42 @@
+#pragma once
+
+#include <stdint.h>
+#include <stdbool.h>
+
+// ******************** Prototypes ********************
+typedef void (*board_init)(void);
+typedef void (*board_init_bootloader)(void);
+typedef void (*board_board_tick)(void);
+typedef void (*board_enable_can_transceiver)(uint8_t transceiver, bool enabled);
+typedef void (*board_set_can_mode)(uint8_t mode);
+typedef uint32_t (*board_read_voltage_mV)(void);
+typedef uint32_t (*board_read_current_mA)(void);
+
+struct board {
+  GPIO_TypeDef * const led_GPIO[3];
+  const uint8_t led_pin[3];
+  const uint8_t led_pwm_channels[3]; // leave at 0 to disable PWM
+  const uint16_t avdd_mV;
+  board_init init;
+  board_init_bootloader init_bootloader;
+  board_board_tick board_tick;
+  board_enable_can_transceiver enable_can_transceiver;
+  board_set_can_mode set_can_mode;
+  board_read_voltage_mV read_voltage_mV;
+  board_read_current_mA read_current_mA;
+  
+  // Body-specific features can be added here
+  bool has_spi;
+  bool has_fan;
+};
+
+// ******************* Definitions ********************
+#define HW_TYPE_UNKNOWN 0U
+#define HW_TYPE_BODY 0xB0U
+
+// CAN modes
+#define CAN_MODE_NORMAL 0U
+
+// ********************* Globals **********************
+uint8_t can_mode = CAN_MODE_NORMAL;
+