omnitiles/drivers/
gim6010.rs

1// SPDX-License-Identifier: MIT
2// © 2025–2026 Christopher Liu
3
4//! Motor control over CAN for SteadyWin GIM6010-48 with a built-in GDZ468 driver.
5//!
6//! This module wraps the driver's custom CAN protocol for a single motor.
7
8use crate::hw::CanBus;
9
10use bxcan::{Frame, Id, OverrunError, StandardId};
11use core::convert::TryInto;
12use core::f32::consts::PI;
13use micromath::F32Ext;
14
15/// Error type for `CanMotor` operations.
16#[derive(Debug)]
17pub enum Error {
18    /// Payload too long for a single CAN data frame (max 8 bytes total).
19    PayloadTooLong,
20    /// TX mailbox or transmit failure.
21    TxMailbox,
22    /// Receive-side CAN error.
23    Rx(OverrunError),
24    /// Response frame contained no data bytes.
25    NoData,
26    /// Response had a different command code than expected.
27    UnexpectedCommand(u8),
28}
29
30impl From<OverrunError> for Error {
31    fn from(e: OverrunError) -> Self {
32        Error::Rx(e)
33    }
34}
35
36/// High-level CAN motor for a single driver instance, parameterized by logical device address.
37///
38/// `DEV_ADDR` is the protocol device address (`Dev_addr`), in the range 1 to 254 inclusive. The
39/// default `DEV_ADDR` is 0x01.
40///
41/// The driver will:
42///   - transmit commands with `StdID = 0x100 | DEV_ADDR`
43///   - expect responses from `StdID = DEV_ADDR`
44pub struct Gim6010<const DEV_ADDR: u16>;
45
46impl<const DEV_ADDR: u16> Gim6010<DEV_ADDR> {
47    /// Create a new handle for this motor address.
48    ///
49    /// This is a zero-sized type; all state lives on the driver itself.
50    #[inline]
51    pub fn new() -> Self {
52        Self
53    }
54
55    /// Host -> motor StdID (11-bit) used for commands.
56    #[inline]
57    fn host_id() -> StandardId {
58        StandardId::new((0x100 | (DEV_ADDR & 0x7FF)) as u16).unwrap()
59    }
60
61    /// Motor -> host StdID (11-bit) used for responses.
62    #[inline]
63    fn dev_id() -> StandardId {
64        StandardId::new(DEV_ADDR & 0x7FF).unwrap()
65    }
66
67    /// Send a command an optionally wait for its response.
68    ///
69    /// - `cmd` is the command code (e.g., 0xA2 for read speed).
70    /// - `payload` is any extra bytes following the command code.
71    /// - If `wait_reply` is true, this will block until a matching response frame (same device ID
72    ///   and command code) is received.
73    ///
74    /// Returns:
75    ///   - `Ok(Some(data))` when `wait_reply` is true and a response was received.
76    ///   - `Ok(None)` when `wait_reply` is false.
77    fn request_response<I>(
78        &mut self,
79        bus: &mut CanBus<I>,
80        cmd: u8,
81        payload: &[u8],
82        wait_reply: bool,
83    ) -> Result<Option<[u8; 8]>, Error>
84    where
85        stm32f7xx_hal::can::Can<I>: bxcan::Instance,
86    {
87        // Total payload including command must be <= 8 bytes
88        if payload.len() > 7 {
89            return Err(Error::PayloadTooLong);
90        }
91
92        // Build TX buffer
93        let mut buf = [0u8; 8];
94        buf[0] = cmd;
95        let dlc = 1 + payload.len();
96        buf[1..dlc].copy_from_slice(payload);
97
98        // Transmit
99        let tx_id = Self::host_id();
100        let tx_result = bus
101            .transmit_data(tx_id, &buf[..dlc])
102            .ok_or(Error::PayloadTooLong)?;
103
104        match tx_result {
105            Ok(_status) => {}
106            Err(_) => return Err(Error::TxMailbox),
107        }
108
109        if !wait_reply {
110            return Ok(None);
111        }
112
113        // Wait for matching response
114        loop {
115            let frame: Frame = bus.receive()?;
116
117            // Standard frame only
118            let id = match frame.id() {
119                Id::Standard(id) => id,
120                Id::Extended(_) => continue,
121            };
122
123            // Only accept DEV_ADDR responses
124            if id != Self::dev_id() {
125                continue;
126            }
127
128            let data = match frame.data() {
129                Some(d) if d.len() > 0 => d,
130                _ => return Err(Error::NoData),
131            };
132
133            let resp_cmd = data[0];
134            if resp_cmd != cmd {
135                continue; // Ignore if different command
136            }
137
138            let mut out = [0u8; 8];
139            let len = data.len().min(8);
140            out[..len].copy_from_slice(&data[..len]);
141            return Ok(Some(out));
142        }
143    }
144
145    /// Clear any latched faults.
146    pub fn clear_faults<I>(&mut self, bus: &mut CanBus<I>) -> Result<(), Error>
147    where
148        stm32f7xx_hal::can::Can<I>: bxcan::Instance,
149    {
150        let _ = self.request_response(bus, 0xAF, &[], true)?;
151        Ok(())
152    }
153
154    /// Turn off motor output. The motor enters a free state and becomes uncontrollable (this is the
155    /// state the motor enters after power-on).
156    pub fn disable_output<I>(&mut self, bus: &mut CanBus<I>) -> Result<(), Error>
157    where
158        stm32f7xx_hal::can::Can<I>: bxcan::Instance,
159    {
160        let _ = self.request_response(bus, 0xCF, &[], false)?;
161        Ok(())
162    }
163
164    /// Command the motor in speed control mode with a setpoint in rpm.
165    ///
166    /// - `rpm` is signed (negative values indicate reverse direction).
167    /// - Resolution is 0.01 rpm
168    pub fn set_speed_rpm<I>(&mut self, bus: &mut CanBus<I>, rpm: f32) -> Result<(), Error>
169    where
170        stm32f7xx_hal::can::Can<I>: bxcan::Instance,
171    {
172        // Convert to protocol units: little-endian signed 32-bit int, 0.01 rpm
173        let scaled: i32 = (rpm * 100.0).round() as i32;
174        let bytes = scaled.to_le_bytes();
175
176        let _ = self.request_response(bus, 0xC1, &bytes, false)?;
177        Ok(())
178    }
179
180    /// Read back the real-time motor speed in rpm.
181    pub fn read_speed_rpm<I>(&mut self, bus: &mut CanBus<I>) -> Result<f32, Error>
182    where
183        stm32f7xx_hal::can::Can<I>: bxcan::Instance,
184    {
185        let resp = self
186            .request_response(bus, 0xA2, &[], true)?
187            .ok_or(Error::NoData)?;
188
189        if resp[0] != 0xA2 {
190            return Err(Error::UnexpectedCommand(resp[0]));
191        }
192
193        let speed_bytes: [u8; 4] = resp[1..5].try_into().expect("slice with exact length");
194        let raw = i32::from_le_bytes(speed_bytes);
195        let rpm = raw as f32 / 100.0;
196        Ok(rpm)
197    }
198
199    // Read the raw status frame.
200    pub fn read_status_frame<I>(&mut self, bus: &mut CanBus<I>) -> Result<[u8; 8], Error>
201    where
202        stm32f7xx_hal::can::Can<I>: bxcan::Instance,
203    {
204        let resp = self
205            .request_response(bus, 0xAE, &[], true)?
206            .ok_or(Error::NoData)?;
207
208        if resp[0] != 0xAE {
209            return Err(Error::UnexpectedCommand(resp[0]));
210        }
211
212        Ok(resp)
213    }
214}
215
216impl<const DEV_ADDR: u16> Gim6010<DEV_ADDR> {
217    /// Default Pos_Max from the driver documentation, in units of 0.1 rad.
218    ///
219    /// The encoder range [0..65535] is mapped to [-Pos_Max, +Pos_Max], with:
220    ///   `Pos_Max = 955` → ±95.5 rad at the motor shaft.
221    pub const POS_MAX_0P1_RAD: i16 = 955;
222
223    /// Maximum mechanical shaft angle in radians that the raw encoder value represents.
224    #[inline]
225    pub fn shaft_pos_max_rad() -> f32 {
226        (Self::POS_MAX_0P1_RAD as f32) * 0.1
227    }
228
229    /// Convert a raw encoder value [0..65535] to a motor shaft angle in radians.
230    ///
231    /// Mapping assumed:
232    ///   raw = 0       -> -Pos_Max
233    ///   raw = 32767.5 -> 0
234    ///   raw = 65535   -> +Pos_Max
235    #[inline]
236    pub fn raw_angle_to_rad(raw: u16) -> f32 {
237        let max = Self::shaft_pos_max_rad(); // ~95.5 rad
238        let norm = (raw as f32) / 65535.0 * 2.0 - 1.0; // [-1, +1]
239        norm * max
240    }
241
242    /// Convert a raw encoder value [0..65535] to a motor shaft angle in degrees.
243    #[inline]
244    pub fn raw_angle_to_deg(raw: u16) -> f32 {
245        let rad = Self::raw_angle_to_rad(raw);
246        rad * 180.0 / PI
247    }
248
249    /// Convert a desired motor shaft angle in radians to a raw encoder value [0..65535].
250    ///
251    /// Input is automatically clamped to [-Pos_Max, +Pos_Max].
252    #[inline]
253    pub fn angle_rad_to_raw(angle_rad: f32) -> u16 {
254        let max = Self::shaft_pos_max_rad();
255        let a = if angle_rad > max {
256            max
257        } else if angle_rad < -max {
258            -max
259        } else {
260            angle_rad
261        };
262
263        let norm = a / max; // [-1, +1]
264        let raw_f = (norm + 1.0) * 0.5 * 65535.0;
265        let raw_f = if raw_f < 0.0 {
266            0.0
267        } else if raw_f > 65535.0 {
268            65535.0
269        } else {
270            raw_f
271        };
272        raw_f.round() as u16
273    }
274
275    /// Convert a desired motor shaft angle in degrees to a raw encoder value [0..65535].
276    #[inline]
277    pub fn angle_deg_to_raw(angle_deg: f32) -> u16 {
278        let rad = angle_deg * PI / 180.0;
279        Self::angle_rad_to_raw(rad)
280    }
281}
omnitiles/drivers/gim6010.rs

omnitiles/drivers/
gim6010.rs
