diff --git a/CANSERVO.c b/CANSERVO.c new file mode 100644 index 0000000..83a11a4 --- /dev/null +++ b/CANSERVO.c @@ -0,0 +1,824 @@ +#include +#include +#include +#include + +#include "CANSERVO.h" +#include "CANSPI.h" + +uint16_t servo_midpoint = 8192; + + +/** + * @file CANSERVO.c + * @brief CAN-servo driver: CAN message helpers + high-level servo commands. + * + * General notes: + * - High-level setters/readers frequently call SERVO_OK() and reset the MCU if false. + * That means ANY detected servo fault (or CAN comms failure interpreted as fault) + * can immediately reboot the STM32. If you later move policy to main.c, you can + * keep this file purely as a driver layer. + * + * - Many functions accept float but ultimately send uint16 values over CAN. + * This means values are truncated when converted to integer (implementation-defined + * rounding in C casts is truncation toward zero). Keep this in mind when documenting + * min/max and units. + */ + +// ============================================================================ +// CAN Handling +// ============================================================================ + +/** + * @brief Helper function to calculate write checksums when sending CAN messages. + * + * Checksum scheme used by your protocol implementation: + * checksum = (ID + address + reg_length + lo_byte + hi_byte) & 0xFF + * + * @param ID Servo node ID (SERVO_ID). + * @param address Servo register address. + * @param reg_length Register length in bytes (SERVO_REG_LENGTH, typically 2). + * @param lo_byte Low byte of payload. + * @param hi_byte High byte of payload. + * @return 8-bit checksum. + */ +uint8_t calculate_write_checksum (uint8_t ID, uint8_t address, uint8_t reg_length, uint8_t lo_byte, uint8_t hi_byte) { + uint8_t write_checksum = (uint8_t) ((ID+address+reg_length+lo_byte+hi_byte) & 0xFF); + + return write_checksum; +} + +/** + * @brief Helper function to calculate read checksums when receiving CAN messages. + * + * Checksum scheme used by your protocol implementation: + * checksum = (ID + address + reg_length) & 0xFF + * + * @param ID Servo node ID (SERVO_ID). + * @param address Servo register address. + * @param reg_length Register length in bytes (SERVO_REG_LENGTH, typically 2). + * @return 8-bit checksum. + */ +uint8_t calculate_read_checksum (uint8_t ID, uint8_t address, uint8_t reg_length) { + uint8_t read_checksum = (uint8_t) ((ID+address+reg_length) & 0xFF); + + return read_checksum; +} + +/** + * @brief Handles sending CAN messages (16-bit register write). + * + * Message format (as implemented): + * - Extended CAN ID (frame.id): SERVO_CAN_ID + * - DLC: SERVO_WRITE_DLC + * - data0: 0x96 (command header) + * - data1: SERVO_ID (servo device ID) + * - data2: reg (register address) + * - data3: SERVO_REG_LENGTH (register length in bytes) + * - data4: payload LSB + * - data5: payload MSB + * - data6: write checksum + * + * @param val 16-bit raw register value. + * @param reg Register address to write. + * + * @note Uses CANSPI_Transmit() and prints "Values sent..." on success. + * @note HAL_Delay(10) is used after transmit (protocol settle time). + */ +void send_can_msg(uint16_t val, uint8_t reg) { + + uCAN_MSG txMessage; + uint8_t lo_byte = val & 0xFF; + uint8_t hi_byte = val >> 8; + + txMessage.frame.idType = dEXTENDED_CAN_MSG_ID_2_0B; + txMessage.frame.id = SERVO_CAN_ID; + txMessage.frame.dlc = SERVO_WRITE_DLC; + txMessage.frame.data0 = 0x96; + txMessage.frame.data1 = SERVO_ID; + txMessage.frame.data2 = reg; + txMessage.frame.data3 = SERVO_REG_LENGTH; + txMessage.frame.data4 = lo_byte; + txMessage.frame.data5 = hi_byte; + txMessage.frame.data6 = calculate_write_checksum (SERVO_ID, reg, SERVO_REG_LENGTH, lo_byte, hi_byte); + + uint8_t can_response = CANSPI_Transmit(&txMessage); + + if (can_response == 0){ + return; + } + + HAL_Delay(10); + return; + +} + +/** + * @brief Handles receiving CAN messages (16-bit register read). + * + * Request format (as implemented): + * - Extended CAN ID (frame.id): SERVO_CAN_ID + * - DLC: SERVO_READ_DLC + * - data0: 0x96 + * - data1: SERVO_ID + * - data2: reg + * - data3: 0 + * - data4: read checksum (ID + reg + reg_length) & 0xFF + * + * Response handling (as implemented): + * - CANSPI_Receive() must succeed + * - rxMessage.frame.data0 must equal 0x69 (expected response header) + * - returned value is little-endian: data4 = LSB, data5 = MSB + * + * @param reg Register address to read. + * @return Raw 16-bit register value. + * + * @warning On failure paths this function returns -1 or -2, but the return type is uint16_t. + * That means callers will receive 0xFFFF or 0xFFFE, which can look like "all bits set". + * This is especially risky for status registers like REG_EMERGENCY_STOP. + */ +uint16_t receive_can_msg(uint8_t reg) { + + uCAN_MSG txMessage; + uCAN_MSG rxMessage; + uint16_t reg_data = 0; + + txMessage.frame.idType = dEXTENDED_CAN_MSG_ID_2_0B; + txMessage.frame.id = SERVO_CAN_ID; + txMessage.frame.dlc = SERVO_READ_DLC; + txMessage.frame.data0 = 0x96; + txMessage.frame.data1 = SERVO_ID; + txMessage.frame.data2 = reg; + txMessage.frame.data3 = 0; + txMessage.frame.data4 = calculate_read_checksum (SERVO_ID, reg, SERVO_REG_LENGTH); + + + CANSPI_Transmit(&txMessage); + HAL_Delay(10); + + if(CANSPI_Receive(&rxMessage)) { + + if(rxMessage.frame.data0 == 0x69){ + reg_data = (((uint16_t)rxMessage.frame.data5) << 8) | rxMessage.frame.data4; + + return reg_data; + + } else { + + // NOTE: returns 0xFFFF due to uint16_t return type. + return -1; + } + + + } else { + + // NOTE: returns 0xFFFE due to uint16_t return type. + return -2; + } + +} + + + +// ============================================================================ +// Servo Initialization +// ============================================================================ + +/** + * @brief Sets initial values for the CAN SERVO upon start-up. + * + * Writes (via set_* wrappers): + * - SERVO_MAX_POSITION_LIMIT -> REG_POSITION_MAX_LIMIT (soft max limit) + * - SERVO_MIN_POSITION_LIMIT -> REG_POSITION_MIN_LIMIT (soft min limit) + * - SERVO_MAX_CURRENT -> REG_CURRENT_MAX (max current limit) + * + * @note Ensure SERVO_* macros match safe limits from the servo datasheet. + */ +void servo_init(void) { + + set_servo_max_position_limit(SERVO_MAX_POSITION_LIMIT); + set_servo_min_position_limit(SERVO_MIN_POSITION_LIMIT); + set_servo_max_voltage(SERVO_MAX_VOLTAGE); + set_servo_min_voltage(SERVO_MIN_VOLTAGE); + set_servo_max_current(SERVO_MAX_CURRENT); + + servo_midpoint = 8192; + + + return; +} + + + +// ============================================================================ +// Servo Positioning +// ============================================================================ + +/** + * @brief Sets the position of the servo's arm (angle command). + * + * Register written: REG_POSITION_NEW + * + * Encoding used in this code: + * - servo_position = 0x2000 + angle * 4096 / 90 + * This implies: + * - 4096 counts = 90 degrees + * - 0x2000 corresponds to "0 degrees" reference + * + * @param angle Angle in degrees. + * @note Comment says valid input is [-180, +180] degrees, but there is no clamp here. + * @warning If SERVO_OK() is false, this function resets the MCU. + */ +void set_servo_angle (float angle) { + + if (!SERVO_OK()) { + servo_error_handler(); + return; + } + + uint16_t servo_position = servo_midpoint + (angle * 4096 / 90); + send_can_msg(servo_position, REG_POSITION_NEW); + + return; +} + + +/** + * @brief Returns the servo arm's angle position (angle readback). + * + * Register read: REG_POSITION_NEW (same register used for writing command here) + * Conversion used: + * - angle_deg = (raw - 0x2000) * (90 / 4096) + * + * @return Angle in degrees. + * @warning If SERVO_OK() is false, this function resets the MCU. + */ +float check_servo_angle (void) { + + if (!SERVO_OK()) { + servo_error_handler(); + return -1; + } + + uint16_t servo_position = receive_can_msg(REG_POSITION_NEW); + float servo_angle = (servo_position - servo_midpoint) * (90.0/4096.0); + + return servo_angle; +} + +/** + * @brief Set CAN servo mid position. + * + * Register written: REG_POSITION_MID + * + * Data range/encoding note (from your comment): + * - Data = 0..16383 + * - Resolution: 4096 = 90 degrees + * + * @param servo_midpoint Midpoint value (raw units per servo firmware). + * @warning Marked TODO / "does not actually work as intended" in your notes. + * @warning If SERVO_OK() is false, this function resets the MCU. + */ + +void set_servo_midpoint (void) +{ + if (!SERVO_OK()) { + servo_error_handler(); + return; + } + + // Read current raw position (counts) + uint16_t raw_pos = receive_can_msg(REG_POSITION_NEW); + + // Set new "zero" reference + servo_midpoint = raw_pos; + + // Shift endpoints with wrap-around (16-bit overflow allowed as requested) + uint16_t new_max = (uint16_t)(servo_midpoint + 4096u); + uint16_t new_min = (uint16_t)(servo_midpoint - 4096u); + + // Apply new soft limits + set_servo_max_position_limit((float)new_max); + set_servo_min_position_limit((float)new_min); + + return; +} + + +/** + * @brief Returns the servo's mid point. + * + * Register read: REG_POSITION_MID + * + * @return Midpoint value converted by SERVO_U16_TO_ANGLE(). + * @warning Marked DO NOT USE / "does not actually work as intended" in your notes. + * @warning If SERVO_OK() is false, this function resets the MCU. + */ +float check_servo_midpoint (void) { + + if (!SERVO_OK()) { + servo_error_handler(); + return -1; + } + + uint16_t raw_servo_midpoint = receive_can_msg(REG_POSITION_MID); + + return SERVO_U16_TO_ANGLE(raw_servo_midpoint); +} + + + +// ============================================================================ +// Velocity & Acceleration +// ============================================================================ + +/** + * @brief Set the acceleration time of the servo (speed-up ramp time). + * + * Register written: REG_SPEED_UP + * Units (per your comment): milliseconds. + * + * @param servo_speed_up_time Time in ms (sent as raw 16-bit value by send_can_msg()). + * @note Function accepts float but sends uint16_t; values will be truncated. + * @warning If SERVO_OK() is false, this function resets the MCU. + */ +void set_servo_speed_up (float servo_speed_up_time) { + + if (!SERVO_OK()) { + servo_error_handler(); + return; + } + + send_can_msg(servo_speed_up_time, REG_SPEED_UP); + + return; +} + +/** + * @brief Set the deceleration time of the servo (speed-down ramp time). + * + * Register written: REG_SPEED_DN + * Units (per your comment): milliseconds. + * + * @param servo_speed_down_time Time in ms (sent as raw 16-bit value by send_can_msg()). + * @note Function accepts float but sends uint16_t; values will be truncated. + * @warning If SERVO_OK() is false, this function resets the MCU. + */ +void set_servo_speed_down (float servo_speed_down_time) { + + if (!SERVO_OK()) { + servo_error_handler(); + return; + } + + send_can_msg(servo_speed_down_time, REG_SPEED_DN); + + return; +} + +/** + * @brief Set the maximum speed value that operates in a normal state. + * + * Register written: REG_VELOCITY_MAX + * + * @param servo_max_velocity Raw max-velocity value. + * @note Your comment says valid range is 0..4095, but no clamp is applied. + * @note Function accepts float but sends uint16_t; values will be truncated. + * @warning If SERVO_OK() is false, this function resets the MCU. + */ +void set_servo_max_velocity (float servo_max_velocity) { + + if (!SERVO_OK()) { + servo_error_handler(); + return; + } + + send_can_msg(servo_max_velocity, REG_VELOCITY_MAX); + + return; +} + + + +// ============================================================================ +// Servo temperature handling +// ============================================================================ + +/** + * @brief Returns the temperature of the servo's internals. + * + * Register read: REG_TEMP + * + * Conversion used in this code: + * - temp_degC = 175.72 * raw/65536 - 46.85 + * + * @return Temperature in degrees Celsius. + * @warning If SERVO_OK() is false, this function resets the MCU. + */ +float check_servo_temp (void) { + + if (!SERVO_OK()) { + servo_error_handler(); + return -1; + } + + uint16_t raw_servo_temperature = receive_can_msg(REG_TEMP); + + uint16_t servo_temperature = 175.72 * raw_servo_temperature / 65536 - 46.85; // in degrees + + return servo_temperature; +} + +/** + * @brief Set a temperature max limit for the MCU temperature. + * + * Register written: REG_TEMPER_MAX + * + * @param servo_max_temperature Max temperature limit (raw units per servo firmware). + * @note Your comment: "Servo goes into emergency mode" when exceeded. + * @note Function accepts float but sends uint16_t; values will be truncated. + * @warning If SERVO_OK() is false, this function resets the MCU. + */ +void set_servo_max_temperature (float servo_max_temperature) { + + if (!SERVO_OK()) { + servo_error_handler(); + return; + } + + send_can_msg(servo_max_temperature, REG_TEMPER_MAX); + + return; +} + +/** + * @brief Set a temperature min limit for the MCU temperature. + * + * Register written: REG_TEMPER_MIN + * + * @param servo_min_temperature Min temperature limit (raw units per servo firmware). + * @note Your comment: "Servo goes into emergency mode" when undercut. + * @note Function accepts float but sends uint16_t; values will be truncated. + * @warning If SERVO_OK() is false, this function resets the MCU. + */ +void set_servo_min_temperature (float servo_min_temperature) { + + if (!SERVO_OK()) { + servo_error_handler(); + return; + } + + send_can_msg(servo_min_temperature, REG_TEMPER_MIN); + + return; +} + + + +// ============================================================================ +// Servo current and voltage handling +// ============================================================================ + +/** + * @brief Set the servo max current draw. + * + * Register written: REG_CURRENT_MAX + * + * @param max_servo_current Max current limit (mA per your comment). + * @note Your comment suggests range: 1mA..6000mA (stall current ~6000mA). + * @note Function accepts float but sends uint16_t; values will be truncated. + * @warning If SERVO_OK() is false, this function resets the MCU. + */ +void set_servo_max_current (float max_servo_current) { + + if (!SERVO_OK()) { + servo_error_handler(); + return; + } + + send_can_msg(max_servo_current, REG_CURRENT_MAX); + + return; +} + +/** + * @brief Returns the current draw of the servo. + * + * Register read: REG_CURRENT + * + * @return Raw current value (units depend on servo firmware/datasheet). + * @warning If SERVO_OK() is false, this function resets the MCU. + */ +float check_servo_current (void) { + + if (!SERVO_OK()) { + servo_error_handler(); + return -1; + } + + uint16_t servo_current = receive_can_msg(REG_CURRENT); + + + return servo_current; +} + +/** + * @brief Returns the configured max-rated stall current of the servo. + * + * Register read: REG_CURRENT_MAX + * + * @return Raw max current value (units depend on servo firmware/datasheet). + * @warning If SERVO_OK() is false, this function resets the MCU. + */ +float check_servo_max_current (void) { + + if (!SERVO_OK()) { + servo_error_handler(); + return -1; + } + + uint16_t max_servo_current = receive_can_msg(REG_CURRENT_MAX); + + + return max_servo_current; +} + +/** + * @brief Set the maximum supply voltage limit of the servo. + * + * Register written: REG_VOLTAGE_MAX + * + * @param servo_max_voltage Max voltage limit (raw units per servo firmware/datasheet). + * @note Function accepts float but sends uint16_t; values will be truncated. + * @warning If SERVO_OK() is false, this function resets the MCU. + */ +void set_servo_max_voltage (float servo_max_voltage) { + + if (!SERVO_OK()) { + servo_error_handler(); + return; + } + + send_can_msg(servo_max_voltage, REG_VOLTAGE_MAX); + + return; +} + + +/** + * @brief Set the minimum supply voltage limit of the servo. + * + * Register written: REG_VOLTAGE_MIN + * + * @param servo_min_voltage Min voltage limit (raw units per servo firmware/datasheet). + * @note Function accepts float but sends uint16_t; values will be truncated. + * @warning If SERVO_OK() is false, this function resets the MCU. + */ +void set_servo_min_voltage (float servo_min_voltage) { + + if (!SERVO_OK()) { + servo_error_handler(); + return; + } + + send_can_msg(servo_min_voltage, REG_VOLTAGE_MIN); + + return; +} + + + +// ============================================================================ +// Servo End Limits +// ============================================================================ + +/** + * @brief Set a soft limit for the servo's max position. + * + * Register written: REG_POSITION_MAX_LIMIT + * @param servo_position_max_limit Soft max limit (raw units per servo encoding). + * @note Function accepts float but sends uint16_t; values will be truncated. + * @warning If SERVO_OK() is false, this function resets the MCU. + */ +void set_servo_max_position_limit (float servo_position_max_limit) { + + if (!SERVO_OK()) { + servo_error_handler(); + return; + } + + send_can_msg(servo_position_max_limit, REG_POSITION_MAX_LIMIT); + + return; +} + +/** + * @brief Set a soft limit for the servo's min position. + * + * Register written: REG_POSITION_MIN_LIMIT + * @param servo_position_min_limit Soft min limit (raw units per servo encoding). + * @note Function accepts float but sends uint16_t; values will be truncated. + * @warning If SERVO_OK() is false, this function resets the MCU. + */ +void set_servo_min_position_limit (float servo_position_min_limit) { + + if (!SERVO_OK()) { + servo_error_handler(); + return; + } + + send_can_msg(servo_position_min_limit, REG_POSITION_MIN_LIMIT); + + return; +} + +/** + * @brief Set a hard limit for servo's max position. + * + * Register written: REG_POS_MAX + * + * @param servo_position_max Hard max position (raw units per servo encoding). + * + * @warning Your comment: this puts the servo into emergency stop mode and may be hard to exit. + * @warning If SERVO_OK() is false, this function resets the MCU. + */ +void set_servo_max_position (float servo_position_max) { + + if (!SERVO_OK()) { + servo_error_handler(); + return; + } + + send_can_msg(servo_position_max, REG_POS_MAX); + + return; +} + +/** + * @brief Set a hard limit for servo's min position. + * + * Register written: REG_POS_MIN + * + * @param servo_position_min Hard min position (raw units per servo encoding). + * + * @warning Your comment: this puts the servo into emergency stop mode and may be hard to exit. + * @warning If SERVO_OK() is false, this function resets the MCU. + */ +void set_servo_min_position (float servo_position_min) { + + if (!SERVO_OK()) { + servo_error_handler(); + return; + } + + send_can_msg(servo_position_min, REG_POS_MIN); + + return; +} + + + +// ============================================================================ +// Servo Status Handling +// ============================================================================ + +/** + * @brief Check whether the servo is in a normal (non-emergency) state. + * + * Register read: REG_EMERGENCY_STOP (your note indicates address ~0x48). + * + * This function checks servo_status bitmasks: + * - ERR_POS_MIN / ERR_POS_MAX + * - ERR_MCU_TEMP_UND / ERR_MCU_TEMP_OVR + * - ERR_VOLT_UND / ERR_VOLT_OVR + * + * @return true if no checked emergency bits are set, false otherwise. + * + * @warning If receive_can_msg() fails, it can return 0xFFFF/0xFFFE (because it returns uint16_t). + * That may cause this function to report many errors and return false. + */ +bool SERVO_OK (void) { + + uint16_t servo_status = receive_can_msg(REG_EMERGENCY_STOP); // <-- should be 0x48 + bool servo_condition = true; + + if (servo_status & ERR_POS_MIN) { + servo_condition = false; + } + + if (servo_status & ERR_POS_MAX) { + servo_condition = false; + } + + if (servo_status & ERR_MCU_TEMP_UND) { + servo_condition = false; + } + + if (servo_status & ERR_MCU_TEMP_OVR) { + servo_condition = false; + } + + if (servo_status & ERR_VOLT_UND) { + servo_condition = false; + } + + if (servo_status & ERR_VOLT_OVR) { + servo_condition = false; + } + + return servo_condition; // no emergency bits set +} + +/** + * @brief Perform servo fault recovery sequence. + * + * This function attempts to restore normal operation after + * SERVO_OK() reports a fault condition. + * + * Recovery actions: + * - Enable forced emergency speed-down mode via REG_POWER_CONFIG. + * - Issue a software reset request via REG_POWER_CONFIG. + * - Clear forced emergency-stop mode via REG_POWER_CONFIG. + * - Reapply default configuration via servo_init(). + * + * Registers affected: + * - REG_POWER_CONFIG (forced ES mode, software reset) + * - REG_POSITION_MAX_LIMIT / REG_POSITION_MIN_LIMIT + * - REG_VOLTAGE_MAX / REG_VOLTAGE_MIN + * - REG_CURRENT_MAX + * + * @note This function assumes the underlying fault condition + * (e.g., voltage or temperature) has been resolved. + */ +void servo_error_handler (void) { + + // Stay safe first + servo_set_speed_down_emergeny_mode(); + + // Try to recover + servo_soft_reset_only(); + HAL_Delay(100); // give servo time to reboot + + // If still faulted, DO NOT clear emergency mode. + servo_init(); + + if (!SERVO_OK()) { + return; // try again next time + } + + // Now it is safe to leave emergency mode + servo_clear_forced_es(); + HAL_Delay(50); + + return; +} + + + +/** + * @brief Force the servo into "Speed_Down" behavior in forced emergency mode. + * + * Register written: REG_POWER_CONFIG + * Field: Forced ES mode (typically bits [10:9] per your earlier screenshot/notes) + * Value: PWR_FORCED_ES_SPEED_DOWN + * + * @note This function writes only the forced ES mode bits (as provided by the macro), + * leaving other reserved bits as 0. + * @warning If SERVO_OK() is false, this function resets the MCU. + */ +void servo_set_speed_down_emergeny_mode(void) { + + if (!SERVO_OK()) { + return; + } + + // Only touch allowed bits; keep reserved as 0 + send_can_msg((uint16_t)PWR_FORCED_ES_SPEED_DOWN, REG_POWER_CONFIG); +} + +/** + * @brief Clear forced emergency-stop mode (set forced ES mode to OFF). + * + * Register written: REG_POWER_CONFIG + * Value: PWR_FORCED_ES_OFF + * + * @warning If SERVO_OK() is false, this function resets the MCU. + */ +void servo_clear_forced_es(void) { + + send_can_msg((uint16_t)PWR_FORCED_ES_OFF, REG_POWER_CONFIG); +} + +/** + * @brief Reset the servo over CAN (software reset request). + * + * Register written: REG_POWER_CONFIG + * Value: PWR_SW_RESET_BIT (typically bit0) + * + * @note Your comment: this may implicitly clear forced ES mode since REG_POWER_CONFIG is overwritten. + * @warning No delay/retry logic is implemented here (caller should throttle). + */ +void servo_soft_reset_only(void) { + // This may implicitly clear forced ES mode since you're overwriting the reg + send_can_msg((uint16_t)PWR_SW_RESET_BIT, REG_POWER_CONFIG); +} diff --git a/CANSERVO.h b/CANSERVO.h new file mode 100644 index 0000000..7178a36 --- /dev/null +++ b/CANSERVO.h @@ -0,0 +1,395 @@ +#ifndef CANSERVO_H +#define CANSERVO_H + +#include +#include + +/** + * @file CANSERVO.h + * @brief Public API + register/encoding definitions for CAN-controlled servo. + * + * Notes: + * - Many functions accept float but ultimately send 16-bit values over CAN. + * - Register addresses, ranges, and units should be validated against the datasheet. + * - REG_POWER_CONFIG is treated as write-only in your design (forced ES mode + SW reset). + */ + +// ============================================================================ +// Register definitions (servo register map) +// ============================================================================ + +/** @name Position / limits */ +//@{ +#define REG_POSITION_NEW 0x1E /**< Command / position reference (raw position encoding). */ +#define REG_POSITION_MID 0xC2 /**< Midpoint register (marked "does not work as intended" in notes). */ + +#define REG_POSITION_MAX_LIMIT 0xB0 /**< Soft max position limit (0..16383, 4096=90deg per notes). */ +#define REG_POSITION_MIN_LIMIT 0xB2 /**< Soft min position limit (0..16383, 4096=90deg per notes). */ + +#define REG_POS_MAX 0x50 /**< Hard max limit (may trigger emergency stop mode). */ +#define REG_POS_MIN 0x52 /**< Hard min limit (may trigger emergency stop mode). */ +//@} + +/** @name Velocity / acceleration */ +//@{ +#define REG_VELOCITY_MAX 0x54 /**< Max velocity in normal state (notes: 0..4095). */ +#define REG_SPEED_UP 0xDC /**< Acceleration ramp time (ms per notes). */ +#define REG_SPEED_DN 0xDE /**< Deceleration ramp time (ms per notes). */ +#define REG_SPEED_ES 0xE0 /**< Emergency-mode speed (datasheet-specific; you use forced ES mode via power config). */ +//@} + +/** @name Temperature */ +//@{ +#define REG_TEMP 0xD2 /**< Temperature readback (raw -> degC conversion in CANSERVO.c). */ +#define REG_TEMPER_MAX 0x5C /**< Max temperature limit (triggers emergency mode). */ +#define REG_TEMPER_MIN 0x6C /**< Min temperature limit (triggers emergency mode). */ +//@} + +/** @name Current / voltage */ +//@{ +#define REG_CURRENT 0x16 /**< Current readback (units depend on datasheet). */ +#define REG_CURRENT_MAX 0xD8 /**< Max current limit (mA per notes). */ + +#define REG_VOLTAGE_MAX 0x58 /**< Max voltage limit (units depend on datasheet). */ +#define REG_VOLTAGE_MIN 0x5A /**< Min voltage limit (units depend on datasheet). */ +//@} + +/** @name Status / control */ +//@{ +#define REG_EMERGENCY_STOP 0x48 /**< Emergency status flags (bitmask). */ +#define REG_POWER_CONFIG 0x46 /**< Power config (forced ES mode + SW reset bit). */ +//@} + +// ============================================================================ +// Servo protocol / addressing definitions +// ============================================================================ + +/** + * @name CAN protocol fields + * These constants define how messages are built for your servo protocol. + */ +//@{ +#define SERVO_CAN_ID 0x000 /**< CAN frame ID used for the servo (extended ID in your code). */ +#define SERVO_ID 0 /**< Servo node/device ID placed in data1. */ + +#define SERVO_WRITE_DLC 7 /**< DLC used for register write frames. */ +#define SERVO_READ_DLC 5 /**< DLC used for register read frames. */ + +#define SERVO_REG_LENGTH 2 /**< Register length in bytes (16-bit registers). */ +//@} + +// ============================================================================ +// Servo configuration definitions (project-level defaults) +// ============================================================================ + +/** + * Soft servo limits for -90deg to +90deg behavior (per your notes). + * Encoding note from your code: + * - 0x2000 is "center" + * - 4096 counts corresponds to 90 degrees + */ +#define SERVO_MAX_POSITION_LIMIT 12288 /**< Soft max limit (check against datasheet). */ +#define SERVO_MIN_POSITION_LIMIT 4096 /**< Soft min limit (check against datasheet). */ +#define SERVO_INITIAL_MIDPOINT 8192 +extern uint16_t servo_midpoint; + +/** + * Hard servo limits used if the servo goes beyond soft range. + * @warning Comments indicate hard limits may trigger emergency-stop mode and be hard to recover from. + */ +#define SERVO_MAX_POSITION 12288+0.1*4096 /**< +10% error margin around +90deg region (float expression). */ +#define SERVO_MIN_POSITION 4096-0.1*4096 /**< -10% error margin around -90deg region (float expression). */ + +#define SERVO_MAX_CURRENT 6800 /**< Max stall current (mA per notes). */ +#define SERVO_MAX_VOLTAGE 1200+1200*0.01 /**< 12V +1% (units depend on datasheet encoding). */ +#define SERVO_MIN_VOLTAGE 1200-1200*0.01 /**< 12V -1% (units depend on datasheet encoding). */ + +// ============================================================================ +// Encoding helper macros +// ============================================================================ + +/** + * @brief Convert degrees to raw 16-bit servo position encoding. + * @param angle_deg Angle in degrees. + * @return Raw position (uint16_t) using: raw = angle*(4096/90) + 0x2000. + */ +#define ANGLE_TO_SERVO_U16(angle_deg) \ + ((uint16_t)((angle_deg) * (4096.0f / 90.0f) + 0x2000)) + +/** + * @brief Convert raw 16-bit servo position encoding to degrees. + * @param servo_u16 Raw position encoding. + * @return Angle in degrees using: deg = (raw-0x2000)*(90/4096). + */ +#define SERVO_U16_TO_ANGLE(servo_u16) \ + (((float)(servo_u16) - 0x2000f) * (90.0f / 4096.0f)) + +// ============================================================================ +// Emergency status flags (REG_EMERGENCY_STOP bitmasks) +// ============================================================================ + +/** + * @name Emergency status bits + * These match how SERVO_OK() tests servo_status in your CANSERVO.c. + */ +//@{ +#define ERR_POS_MIN (1 << 8) /**< Position min limit exceeded. */ +#define ERR_POS_MAX (1 << 9) /**< Position max limit exceeded. */ +#define ERR_MCU_TEMP_UND (1 << 10) /**< MCU temperature too low. */ +#define ERR_MCU_TEMP_OVR (1 << 11) /**< MCU temperature too high. */ +#define ERR_VOLT_UND (1 << 13) /**< Voltage too low. */ +#define ERR_VOLT_OVR (1 << 14) /**< Voltage too high. */ +//@} + +/** @deprecated Prefer PWR_SW_RESET_BIT for REG_POWER_CONFIG SW reset. */ +#define POWER_CFG_SW_RESET (1U << 0) + +// ============================================================================ +// Power config modes (REG_POWER_CONFIG) +// ============================================================================ + +/** + * @name Forced emergency-stop (forced ES) field + * Typically bits [10:9] based on your earlier notes/screenshot. + * + * 00 = OFF + * 01 = Motor_Free + * 10 = Speed_Down + * 11 = Motor_Hold + */ +//@{ +#define PWR_FORCED_ES_SHIFT 9 +#define PWR_FORCED_ES_MASK (0x3u << PWR_FORCED_ES_SHIFT) + +#define PWR_FORCED_ES_OFF (0u << PWR_FORCED_ES_SHIFT) +#define PWR_FORCED_ES_MOTOR_FREE (1u << PWR_FORCED_ES_SHIFT) +#define PWR_FORCED_ES_SPEED_DOWN (2u << PWR_FORCED_ES_SHIFT) /**< Speed_Down (0x0400). */ +#define PWR_FORCED_ES_MOTOR_HOLD (3u << PWR_FORCED_ES_SHIFT) +//@} + +/** @brief Software reset request bit written via REG_POWER_CONFIG. */ +#define PWR_SW_RESET_BIT (1u << 0) /**< 0x0001 */ + +// ============================================================================ +// Timer / step token used by your Servo_Handler throttling +// ============================================================================ + +/** + * @brief Step token set by a periodic timer ISR to rate-limit servo recovery actions. + * + * Typical pattern: + * - ISR sets g_servo_step = 1 when an action is allowed. + * - Servo handler consumes token (sets back to 0). + */ +extern volatile uint8_t g_servo_step; + +// ============================================================================ +// Function prototypes +// ============================================================================ + +/** + * @name CAN helpers + * Low-level CAN message construction and register read/write primitives. + */ +//@{ + +/** + * @brief Calculate write checksum used in send_can_msg(). + * @param ID Servo device ID. + * @param address Register address. + * @param reg_length Register length in bytes (typically 2). + * @param lo_byte Payload low byte. + * @param hi_byte Payload high byte. + * @return 8-bit checksum. + */ +uint8_t calculate_write_checksum(uint8_t ID, uint8_t address, uint8_t reg_length, + uint8_t lo_byte, uint8_t hi_byte); + +/** + * @brief Calculate read checksum used in receive_can_msg(). + * @param ID Servo device ID. + * @param address Register address. + * @param reg_length Register length in bytes (typically 2). + * @return 8-bit checksum. + */ +uint8_t calculate_read_checksum(uint8_t ID, uint8_t address, uint8_t reg_length); + +/** + * @brief Write a 16-bit value to a servo register over CAN. + * @param val 16-bit raw value to write. + * @param reg Register address. + */ +void send_can_msg(uint16_t val, uint8_t reg); + +/** + * @brief Read a 16-bit value from a servo register over CAN. + * @param reg Register address. + * @return 16-bit raw register value. + */ +uint16_t receive_can_msg(uint8_t reg); + +//@} + +// ---------------------------------------------------------------------------- + +/** + * @name Servo high-level functionality + * Wrapper functions that convert application-level units/ideas into register writes/reads. + */ +//@{ + +/** + * @brief Initialize servo configuration at startup using project macros. + * Writes soft position limits and max current limit. + */ +void servo_init(void); + +/** + * @brief Command a servo angle. + * @param angle Angle in degrees (notes suggest -180..+180; no clamp is applied in implementation). + */ +void set_servo_angle(float angle); + +/** + * @brief Read back servo angle. + * @return Angle in degrees using SERVO_U16_TO_ANGLE conversion. + */ +float check_servo_angle(void); + +/** + * @brief Set servo midpoint (marked as not working as intended in your notes). + * @param servo_midpoint Raw midpoint value (encoding per datasheet/firmware). + */ +void set_servo_midpoint(void); + +/** + * @brief Read servo midpoint (marked DO NOT USE in your notes). + * @return Midpoint converted using SERVO_U16_TO_ANGLE. + */ +float check_servo_midpoint(void); + +/** + * @brief Set acceleration ramp-up time. + * @param servo_speed_up_time Time in ms (sent as raw 16-bit value). + */ +void set_servo_speed_up(float servo_speed_up_time); + +/** + * @brief Set deceleration ramp-down time. + * @param servo_speed_down_time Time in ms (sent as raw 16-bit value). + */ +void set_servo_speed_down(float servo_speed_down_time); + +/** + * @brief Set maximum velocity in normal mode. + * @param servo_max_velocity Raw value (notes suggest 0..4095; no clamp is applied). + */ +void set_servo_max_velocity(float servo_max_velocity); + +/** + * @brief Read servo internal temperature. + * @return Temperature in degrees C using conversion in implementation. + */ +float check_servo_temp(void); + +/** + * @brief Set maximum temperature limit (triggers emergency mode if exceeded). + * @param servo_max_temperature Raw limit value (encoding per datasheet). + */ +void set_servo_max_temperature(float servo_max_temperature); + +/** + * @brief Set minimum temperature limit (triggers emergency mode if undercut). + * @param servo_min_temperature Raw limit value (encoding per datasheet). + */ +void set_servo_min_temperature(float servo_min_temperature); + +/** + * @brief Set maximum current limit. + * @param max_servo_current Current limit (mA per notes; encoding per datasheet). + */ +void set_servo_max_current(float max_servo_current); + +/** + * @brief Read current draw. + * @return Raw current reading (units per datasheet). + */ +float check_servo_current(void); + +/** + * @brief Read configured max current limit. + * @return Raw max current limit (units per datasheet). + */ +float check_servo_max_current(void); + +/** + * @brief Set maximum voltage limit. + * @param servo_max_voltage Raw max voltage value (units per datasheet). + */ +void set_servo_max_voltage(float servo_max_voltage); + +/** + * @brief Set minimum voltage limit. + * @param servo_min_voltage Raw min voltage value (units per datasheet). + */ +void set_servo_min_voltage(float servo_min_voltage); + +/** + * @brief Set soft maximum position limit. + * @param servo_position_max_limit Raw position limit value (encoding per datasheet). + */ +void set_servo_max_position_limit(float servo_position_max_limit); + +/** + * @brief Set soft minimum position limit. + * @param servo_position_min_limit Raw position limit value (encoding per datasheet). + */ +void set_servo_min_position_limit(float servo_position_min_limit); + +/** + * @brief Set hard maximum position limit (may trigger emergency stop; use with caution). + * @param servo_position_max Raw hard limit value (encoding per datasheet). + */ +void set_servo_max_position(float servo_position_max); + +/** + * @brief Set hard minimum position limit (may trigger emergency stop; use with caution). + * @param servo_position_min Raw hard limit value (encoding per datasheet). + */ +void set_servo_min_position(float servo_position_min); + +/** + * @brief Check if servo has no emergency status flags set (based on REG_EMERGENCY_STOP). + * @return true if no checked error bits are set, false otherwise. + */ +bool SERVO_OK(void); + +/** + * @brief Attempt recovery from a detected servo fault. + * + * Performs forced speed-down, software reset, clears emergency mode, + * and reapplies initial configuration. + */ +void servo_error_handler (void); + +/** + * @brief Force servo into "Speed_Down" mode via REG_POWER_CONFIG forced-ES field. + * @note Writes PWR_FORCED_ES_SPEED_DOWN to REG_POWER_CONFIG. + */ +void servo_set_speed_down_emergeny_mode(void); + +/** + * @brief Clear forced-ES mode via REG_POWER_CONFIG (forced-ES field set to OFF). + * @note Writes PWR_FORCED_ES_OFF to REG_POWER_CONFIG. + */ +void servo_clear_forced_es(void); + +/** + * @brief Request a servo software reset via REG_POWER_CONFIG. + * @note Writes PWR_SW_RESET_BIT to REG_POWER_CONFIG. + */ +void servo_soft_reset_only(void); + +//@} + +#endif diff --git a/CANSPI.c b/CANSPI.c new file mode 100644 index 0000000..c01984a --- /dev/null +++ b/CANSPI.c @@ -0,0 +1,387 @@ +/* + (c) 2016 Microchip Technology Inc. and its subsidiaries. You may use this + software and any derivatives exclusively with Microchip products. + + THIS SOFTWARE IS SUPPLIED BY MICROCHIP "AS IS". NO WARRANTIES, WHETHER + EXPRESS, IMPLIED OR STATUTORY, APPLY TO THIS SOFTWARE, INCLUDING ANY IMPLIED + WARRANTIES OF NON-INFRINGEMENT, MERCHANTABILITY, AND FITNESS FOR A + PARTICULAR PURPOSE, OR ITS INTERACTION WITH MICROCHIP PRODUCTS, COMBINATION + WITH ANY OTHER PRODUCTS, OR USE IN ANY APPLICATION. + + IN NO EVENT WILL MICROCHIP BE LIABLE FOR ANY INDIRECT, SPECIAL, PUNITIVE, + INCIDENTAL OR CONSEQUENTIAL LOSS, DAMAGE, COST OR EXPENSE OF ANY KIND + WHATSOEVER RELATED TO THE SOFTWARE, HOWEVER CAUSED, EVEN IF MICROCHIP HAS + BEEN ADVISED OF THE POSSIBILITY OR THE DAMAGES ARE FORESEEABLE. TO THE + FULLEST EXTENT ALLOWED BY LAW, MICROCHIP'S TOTAL LIABILITY ON ALL CLAIMS IN + ANY WAY RELATED TO THIS SOFTWARE WILL NOT EXCEED THE AMOUNT OF FEES, IF ANY, + THAT YOU HAVE PAID DIRECTLY TO MICROCHIP FOR THIS SOFTWARE. + + MICROCHIP PROVIDES THIS SOFTWARE CONDITIONALLY UPON YOUR ACCEPTANCE OF THESE + TERMS. +*/ + +#include "CANSPI.h" +#include "MCP2515.h" + +/** Local Function Prototypes */ +static uint32_t convertReg2ExtendedCANid(uint8_t tempRXBn_EIDH, uint8_t tempRXBn_EIDL, uint8_t tempRXBn_SIDH, uint8_t tempRXBn_SIDL); +static uint32_t convertReg2StandardCANid(uint8_t tempRXBn_SIDH, uint8_t tempRXBn_SIDL) ; +static void convertCANid2Reg(uint32_t tempPassedInID, uint8_t canIdType, id_reg_t *passedIdReg); + +/** Local Variables */ +ctrl_status_t ctrlStatus; +ctrl_error_status_t errorStatus; +id_reg_t idReg; + +/** CAN SPI APIs */ + +/* Entering Sleep Mode */ +void CANSPI_Sleep(void) +{ + /* Clear CAN bus wakeup interrupt */ + MCP2515_BitModify(MCP2515_CANINTF, 0x40, 0x00); + + /* Enable CAN bus activity wakeup */ + MCP2515_BitModify(MCP2515_CANINTE, 0x40, 0x40); + + MCP2515_SetSleepMode(); +} + +/* Initialize CAN */ +bool CANSPI_Initialize(void) +{ + RXF0 RXF0reg; + RXF1 RXF1reg; + RXF2 RXF2reg; + RXF3 RXF3reg; + RXF4 RXF4reg; + RXF5 RXF5reg; + RXM0 RXM0reg; + RXM1 RXM1reg; + + /* Intialize Rx Mask values */ + RXM0reg.RXM0SIDH = 0x00; + RXM0reg.RXM0SIDL = 0x00; + RXM0reg.RXM0EID8 = 0x00; + RXM0reg.RXM0EID0 = 0x00; + + RXM1reg.RXM1SIDH = 0x00; + RXM1reg.RXM1SIDL = 0x00; + RXM1reg.RXM1EID8 = 0x00; + RXM1reg.RXM1EID0 = 0x00; + + /* Intialize Rx Filter values */ + RXF0reg.RXF0SIDH = 0x00; + RXF0reg.RXF0SIDL = 0x00; //Starndard Filter + RXF0reg.RXF0EID8 = 0x00; + RXF0reg.RXF0EID0 = 0x00; + + RXF1reg.RXF1SIDH = 0x00; + RXF1reg.RXF1SIDL = 0x08; //Exntended Filter + RXF1reg.RXF1EID8 = 0x00; + RXF1reg.RXF1EID0 = 0x00; + + RXF2reg.RXF2SIDH = 0x00; + RXF2reg.RXF2SIDL = 0x00; + RXF2reg.RXF2EID8 = 0x00; + RXF2reg.RXF2EID0 = 0x00; + + RXF3reg.RXF3SIDH = 0x00; + RXF3reg.RXF3SIDL = 0x00; + RXF3reg.RXF3EID8 = 0x00; + RXF3reg.RXF3EID0 = 0x00; + + RXF4reg.RXF4SIDH = 0x00; + RXF4reg.RXF4SIDL = 0x00; + RXF4reg.RXF4EID8 = 0x00; + RXF4reg.RXF4EID0 = 0x00; + + RXF5reg.RXF5SIDH = 0x00; + RXF5reg.RXF5SIDL = 0x08; + RXF5reg.RXF5EID8 = 0x00; + RXF5reg.RXF5EID0 = 0x00; + + /* Intialize MCP2515, check SPI */ + if(!MCP2515_Initialize()) + { + return false; + } + + /* Change mode as configuration mode */ + if(!MCP2515_SetConfigMode()) + { + return false; + } + + /* Configure filter & mask */ + MCP2515_WriteByteSequence(MCP2515_RXM0SIDH, MCP2515_RXM0EID0, &(RXM0reg.RXM0SIDH)); + MCP2515_WriteByteSequence(MCP2515_RXM1SIDH, MCP2515_RXM1EID0, &(RXM1reg.RXM1SIDH)); + MCP2515_WriteByteSequence(MCP2515_RXF0SIDH, MCP2515_RXF0EID0, &(RXF0reg.RXF0SIDH)); + MCP2515_WriteByteSequence(MCP2515_RXF1SIDH, MCP2515_RXF1EID0, &(RXF1reg.RXF1SIDH)); + MCP2515_WriteByteSequence(MCP2515_RXF2SIDH, MCP2515_RXF2EID0, &(RXF2reg.RXF2SIDH)); + MCP2515_WriteByteSequence(MCP2515_RXF3SIDH, MCP2515_RXF3EID0, &(RXF3reg.RXF3SIDH)); + MCP2515_WriteByteSequence(MCP2515_RXF4SIDH, MCP2515_RXF4EID0, &(RXF4reg.RXF4SIDH)); + MCP2515_WriteByteSequence(MCP2515_RXF5SIDH, MCP2515_RXF5EID0, &(RXF5reg.RXF5SIDH)); + + /* Accept All (Standard + Extended) */ + MCP2515_WriteByte(MCP2515_RXB0CTRL, 0x04); //Enable BUKT, Accept Filter 0 + MCP2515_WriteByte(MCP2515_RXB1CTRL, 0x01); //Accept Filter 1 + + /* + * tq = 2 * (brp(0) + 1) / 16000000 = 0.125us + * tbit = (SYNC_SEG(1 fixed) + PROP_SEG + PS1 + PS2) + * tbit = 1tq + 5tq + 6tq + 4tq = 16tq + * 16tq = 2us = 500kbps + */ + + /* 00(SJW 1tq) 000000 */ + MCP2515_WriteByte(MCP2515_CNF1, 0x00); + + /* 1 1 100(5tq) 101(6tq) */ + MCP2515_WriteByte(MCP2515_CNF2, 0x90); // Edit to change + + /* 1 0 000 011(4tq) */ + MCP2515_WriteByte(MCP2515_CNF3, 0x02); // Edit to change + + /* Normal 모드로 설정 */ + if(!MCP2515_SetNormalMode()) + return false; + + return true; +} + +/* Transmit CAN message */ +uint8_t CANSPI_Transmit(uCAN_MSG *tempCanMsg) +{ + uint8_t returnValue = 0; + + idReg.tempSIDH = 0; + idReg.tempSIDL = 0; + idReg.tempEID8 = 0; + idReg.tempEID0 = 0; + + ctrlStatus.ctrl_status = MCP2515_ReadStatus(); + + /* Finding empty buffer */ + if (ctrlStatus.TXB0REQ != 1) + { + /* convert CAN ID for register */ + convertCANid2Reg(tempCanMsg->frame.id, tempCanMsg->frame.idType, &idReg); + + /* Load data to Tx Buffer */ + MCP2515_LoadTxSequence(MCP2515_LOAD_TXB0SIDH, &(idReg.tempSIDH), tempCanMsg->frame.dlc, &(tempCanMsg->frame.data0)); + + /* Request to transmit */ + MCP2515_RequestToSend(MCP2515_RTS_TX0); + + returnValue = 1; + } + else if (ctrlStatus.TXB1REQ != 1) + { + convertCANid2Reg(tempCanMsg->frame.id, tempCanMsg->frame.idType, &idReg); + + MCP2515_LoadTxSequence(MCP2515_LOAD_TXB1SIDH, &(idReg.tempSIDH), tempCanMsg->frame.dlc, &(tempCanMsg->frame.data0)); + MCP2515_RequestToSend(MCP2515_RTS_TX1); + + returnValue = 1; + } + else if (ctrlStatus.TXB2REQ != 1) + { + convertCANid2Reg(tempCanMsg->frame.id, tempCanMsg->frame.idType, &idReg); + + MCP2515_LoadTxSequence(MCP2515_LOAD_TXB2SIDH, &(idReg.tempSIDH), tempCanMsg->frame.dlc, &(tempCanMsg->frame.data0)); + MCP2515_RequestToSend(MCP2515_RTS_TX2); + + returnValue = 1; + } + + return (returnValue); +} + +/* Receive CAN message */ +uint8_t CANSPI_Receive(uCAN_MSG *tempCanMsg) +{ + uint8_t returnValue = 0; + rx_reg_t rxReg; + ctrl_rx_status_t rxStatus; + + rxStatus.ctrl_rx_status = MCP2515_GetRxStatus(); + + /* Check receive buffer */ + if (rxStatus.rxBuffer != 0) + { + /* finding buffer which has a message */ + if ((rxStatus.rxBuffer == MSG_IN_RXB0)|(rxStatus.rxBuffer == MSG_IN_BOTH_BUFFERS)) + { + MCP2515_ReadRxSequence(MCP2515_READ_RXB0SIDH, rxReg.rx_reg_array, sizeof(rxReg.rx_reg_array)); + } + else if (rxStatus.rxBuffer == MSG_IN_RXB1) + { + MCP2515_ReadRxSequence(MCP2515_READ_RXB1SIDH, rxReg.rx_reg_array, sizeof(rxReg.rx_reg_array)); + } + + /* if the message is extended CAN type */ + if (rxStatus.msgType == dEXTENDED_CAN_MSG_ID_2_0B) + { + tempCanMsg->frame.idType = (uint8_t) dEXTENDED_CAN_MSG_ID_2_0B; + tempCanMsg->frame.id = convertReg2ExtendedCANid(rxReg.RXBnEID8, rxReg.RXBnEID0, rxReg.RXBnSIDH, rxReg.RXBnSIDL); + } + else + { + /* Standard type */ + tempCanMsg->frame.idType = (uint8_t) dSTANDARD_CAN_MSG_ID_2_0B; + tempCanMsg->frame.id = convertReg2StandardCANid(rxReg.RXBnSIDH, rxReg.RXBnSIDL); + } + + tempCanMsg->frame.dlc = rxReg.RXBnDLC; + tempCanMsg->frame.data0 = rxReg.RXBnD0; + tempCanMsg->frame.data1 = rxReg.RXBnD1; + tempCanMsg->frame.data2 = rxReg.RXBnD2; + tempCanMsg->frame.data3 = rxReg.RXBnD3; + tempCanMsg->frame.data4 = rxReg.RXBnD4; + tempCanMsg->frame.data5 = rxReg.RXBnD5; + tempCanMsg->frame.data6 = rxReg.RXBnD6; + tempCanMsg->frame.data7 = rxReg.RXBnD7; + + returnValue = 1; + } + + return (returnValue); +} + +/* check message buffer and return count */ +uint8_t CANSPI_messagesInBuffer(void) +{ + uint8_t messageCount = 0; + + ctrlStatus.ctrl_status = MCP2515_ReadStatus(); + + if(ctrlStatus.RX0IF != 0) + { + messageCount++; + } + + if(ctrlStatus.RX1IF != 0) + { + messageCount++; + } + + return (messageCount); +} + +/* check BUS off */ +uint8_t CANSPI_isBussOff(void) +{ + uint8_t returnValue = 0; + + errorStatus.error_flag_reg = MCP2515_ReadByte(MCP2515_EFLG); + + if(errorStatus.TXBO == 1) + { + returnValue = 1; + } + + return (returnValue); +} + +/* check Rx Passive Error */ +uint8_t CANSPI_isRxErrorPassive(void) +{ + uint8_t returnValue = 0; + + errorStatus.error_flag_reg = MCP2515_ReadByte(MCP2515_EFLG); + + if(errorStatus.RXEP == 1) + { + returnValue = 1; + } + + return (returnValue); +} + +/* check Tx Passive Error */ +uint8_t CANSPI_isTxErrorPassive(void) +{ + uint8_t returnValue = 0; + + errorStatus.error_flag_reg = MCP2515_ReadByte(MCP2515_EFLG); + + if(errorStatus.TXEP == 1) + { + returnValue = 1; + } + + return (returnValue); +} + +/* convert register value to extended CAN ID */ +static uint32_t convertReg2ExtendedCANid(uint8_t tempRXBn_EIDH, uint8_t tempRXBn_EIDL, uint8_t tempRXBn_SIDH, uint8_t tempRXBn_SIDL) +{ + uint32_t returnValue = 0; + uint32_t ConvertedID = 0; + uint8_t CAN_standardLo_ID_lo2bits; + uint8_t CAN_standardLo_ID_hi3bits; + + CAN_standardLo_ID_lo2bits = (tempRXBn_SIDL & 0x03); + CAN_standardLo_ID_hi3bits = (tempRXBn_SIDL >> 5); + ConvertedID = (tempRXBn_SIDH << 3); + ConvertedID = ConvertedID + CAN_standardLo_ID_hi3bits; + ConvertedID = (ConvertedID << 2); + ConvertedID = ConvertedID + CAN_standardLo_ID_lo2bits; + ConvertedID = (ConvertedID << 8); + ConvertedID = ConvertedID + tempRXBn_EIDH; + ConvertedID = (ConvertedID << 8); + ConvertedID = ConvertedID + tempRXBn_EIDL; + returnValue = ConvertedID; + return (returnValue); +} + +/* convert register value to standard CAN ID */ +static uint32_t convertReg2StandardCANid(uint8_t tempRXBn_SIDH, uint8_t tempRXBn_SIDL) +{ + uint32_t returnValue = 0; + uint32_t ConvertedID; + + ConvertedID = (tempRXBn_SIDH << 3); + ConvertedID = ConvertedID + (tempRXBn_SIDL >> 5); + returnValue = ConvertedID; + + return (returnValue); +} + +/* convert CAN ID to register value */ +static void convertCANid2Reg(uint32_t tempPassedInID, uint8_t canIdType, id_reg_t *passedIdReg) +{ + uint8_t wipSIDL = 0; + + if (canIdType == dEXTENDED_CAN_MSG_ID_2_0B) + { + //EID0 + passedIdReg->tempEID0 = 0xFF & tempPassedInID; + tempPassedInID = tempPassedInID >> 8; + + //EID8 + passedIdReg->tempEID8 = 0xFF & tempPassedInID; + tempPassedInID = tempPassedInID >> 8; + + //SIDL + wipSIDL = 0x03 & tempPassedInID; + tempPassedInID = tempPassedInID << 3; + wipSIDL = (0xE0 & tempPassedInID) + wipSIDL; + wipSIDL = wipSIDL + 0x08; + passedIdReg->tempSIDL = 0xEB & wipSIDL; + + //SIDH + tempPassedInID = tempPassedInID >> 8; + passedIdReg->tempSIDH = 0xFF & tempPassedInID; + } + else + { + passedIdReg->tempEID8 = 0; + passedIdReg->tempEID0 = 0; + tempPassedInID = tempPassedInID << 5; + passedIdReg->tempSIDL = 0xFF & tempPassedInID; + tempPassedInID = tempPassedInID >> 8; + passedIdReg->tempSIDH = 0xFF & tempPassedInID; + } +} diff --git a/CANSPI.h b/CANSPI.h new file mode 100644 index 0000000..5ecfee7 --- /dev/null +++ b/CANSPI.h @@ -0,0 +1,58 @@ +/* + (c) 2016 Microchip Technology Inc. and its subsidiaries. You may use this + software and any derivatives exclusively with Microchip products. + + THIS SOFTWARE IS SUPPLIED BY MICROCHIP "AS IS". NO WARRANTIES, WHETHER + EXPRESS, IMPLIED OR STATUTORY, APPLY TO THIS SOFTWARE, INCLUDING ANY IMPLIED + WARRANTIES OF NON-INFRINGEMENT, MERCHANTABILITY, AND FITNESS FOR A + PARTICULAR PURPOSE, OR ITS INTERACTION WITH MICROCHIP PRODUCTS, COMBINATION + WITH ANY OTHER PRODUCTS, OR USE IN ANY APPLICATION. + + IN NO EVENT WILL MICROCHIP BE LIABLE FOR ANY INDIRECT, SPECIAL, PUNITIVE, + INCIDENTAL OR CONSEQUENTIAL LOSS, DAMAGE, COST OR EXPENSE OF ANY KIND + WHATSOEVER RELATED TO THE SOFTWARE, HOWEVER CAUSED, EVEN IF MICROCHIP HAS + BEEN ADVISED OF THE POSSIBILITY OR THE DAMAGES ARE FORESEEABLE. TO THE + FULLEST EXTENT ALLOWED BY LAW, MICROCHIP'S TOTAL LIABILITY ON ALL CLAIMS IN + ANY WAY RELATED TO THIS SOFTWARE WILL NOT EXCEED THE AMOUNT OF FEES, IF ANY, + THAT YOU HAVE PAID DIRECTLY TO MICROCHIP FOR THIS SOFTWARE. + + MICROCHIP PROVIDES THIS SOFTWARE CONDITIONALLY UPON YOUR ACCEPTANCE OF THESE + TERMS. +*/ + +#ifndef __CAN_SPI_H +#define __CAN_SPI_H + +#include +#include "main.h" + +typedef union { + struct { + uint8_t idType; + uint32_t id; + uint8_t dlc; + uint8_t data0; + uint8_t data1; + uint8_t data2; + uint8_t data3; + uint8_t data4; + uint8_t data5; + uint8_t data6; + uint8_t data7; + } frame; + uint8_t array[14]; +} uCAN_MSG; + +#define dSTANDARD_CAN_MSG_ID_2_0B 1 +#define dEXTENDED_CAN_MSG_ID_2_0B 2 + +bool CANSPI_Initialize(void); +void CANSPI_Sleep(void); +uint8_t CANSPI_Transmit(uCAN_MSG *tempCanMsg); +uint8_t CANSPI_Receive(uCAN_MSG *tempCanMsg); +uint8_t CANSPI_messagesInBuffer(void); +uint8_t CANSPI_isBussOff(void); +uint8_t CANSPI_isRxErrorPassive(void); +uint8_t CANSPI_isTxErrorPassive(void); + +#endif /* __CAN_SPI_H */ diff --git a/MCP2515.c b/MCP2515.c new file mode 100644 index 0000000..2e4407d --- /dev/null +++ b/MCP2515.c @@ -0,0 +1,276 @@ +/* + (c) 2016 Microchip Technology Inc. and its subsidiaries. You may use this + software and any derivatives exclusively with Microchip products. + + THIS SOFTWARE IS SUPPLIED BY MICROCHIP "AS IS". NO WARRANTIES, WHETHER + EXPRESS, IMPLIED OR STATUTORY, APPLY TO THIS SOFTWARE, INCLUDING ANY IMPLIED + WARRANTIES OF NON-INFRINGEMENT, MERCHANTABILITY, AND FITNESS FOR A + PARTICULAR PURPOSE, OR ITS INTERACTION WITH MICROCHIP PRODUCTS, COMBINATION + WITH ANY OTHER PRODUCTS, OR USE IN ANY APPLICATION. + + IN NO EVENT WILL MICROCHIP BE LIABLE FOR ANY INDIRECT, SPECIAL, PUNITIVE, + INCIDENTAL OR CONSEQUENTIAL LOSS, DAMAGE, COST OR EXPENSE OF ANY KIND + WHATSOEVER RELATED TO THE SOFTWARE, HOWEVER CAUSED, EVEN IF MICROCHIP HAS + BEEN ADVISED OF THE POSSIBILITY OR THE DAMAGES ARE FORESEEABLE. TO THE + FULLEST EXTENT ALLOWED BY LAW, MICROCHIP'S TOTAL LIABILITY ON ALL CLAIMS IN + ANY WAY RELATED TO THIS SOFTWARE WILL NOT EXCEED THE AMOUNT OF FEES, IF ANY, + THAT YOU HAVE PAID DIRECTLY TO MICROCHIP FOR THIS SOFTWARE. + + MICROCHIP PROVIDES THIS SOFTWARE CONDITIONALLY UPON YOUR ACCEPTANCE OF THESE + TERMS. +*/ + +#include "MCP2515.h" +#include "main.h" + +/* SPI related variables */ +extern SPI_HandleTypeDef hspi1; +#define SPI_CAN &hspi1 +#define SPI_TIMEOUT 10 +#define MCP2515_CS_HIGH() HAL_GPIO_WritePin(CAN_CS_GPIO_Port, CAN_CS_Pin, GPIO_PIN_SET) +#define MCP2515_CS_LOW() HAL_GPIO_WritePin(CAN_CS_GPIO_Port, CAN_CS_Pin, GPIO_PIN_RESET) + +/* Prototypes */ +static void SPI_Tx(uint8_t data); +static void SPI_TxBuffer(uint8_t *buffer, uint8_t length); +static uint8_t SPI_Rx(void); +static void SPI_RxBuffer(uint8_t *buffer, uint8_t length); + +/* initialize MCP2515 */ +bool MCP2515_Initialize(void) +{ + MCP2515_CS_HIGH(); + + uint8_t loop = 10; + + do { + /* check SPI Ready */ + if(HAL_SPI_GetState(SPI_CAN) == HAL_SPI_STATE_READY) + return true; + + loop--; + } while(loop > 0); + + return false; +} + +/* change mode as configuration mode */ +bool MCP2515_SetConfigMode(void) +{ + /* configure CANCTRL Register */ + MCP2515_WriteByte(MCP2515_CANCTRL, 0x80); + + uint8_t loop = 10; + + do { + /* confirm mode configuration */ + if((MCP2515_ReadByte(MCP2515_CANSTAT) & 0xE0) == 0x80) + return true; + + loop--; + } while(loop > 0); + + return false; +} + +/* change mode as normal mode */ +bool MCP2515_SetNormalMode(void) +{ + /* configure CANCTRL Register */ + MCP2515_WriteByte(MCP2515_CANCTRL, 0x00); + + uint8_t loop = 10; + + do { + /* confirm mode configuration */ + if((MCP2515_ReadByte(MCP2515_CANSTAT) & 0xE0) == 0x00) + return true; + + loop--; + } while(loop > 0); + + return false; +} + +/* Entering sleep mode */ +bool MCP2515_SetSleepMode(void) +{ + /* configure CANCTRL Register */ + MCP2515_WriteByte(MCP2515_CANCTRL, 0x20); + + uint8_t loop = 10; + + do { + /* confirm mode configuration */ + if((MCP2515_ReadByte(MCP2515_CANSTAT) & 0xE0) == 0x20) + return true; + + loop--; + } while(loop > 0); + + return false; +} + +/* MCP2515 SPI-Reset */ +void MCP2515_Reset(void) +{ + MCP2515_CS_LOW(); + + SPI_Tx(MCP2515_RESET); + + MCP2515_CS_HIGH(); +} + +/* read single byte */ +uint8_t MCP2515_ReadByte (uint8_t address) +{ + uint8_t retVal; + + MCP2515_CS_LOW(); + + SPI_Tx(MCP2515_READ); + SPI_Tx(address); + retVal = SPI_Rx(); + + MCP2515_CS_HIGH(); + + return retVal; +} + +/* read buffer */ +void MCP2515_ReadRxSequence(uint8_t instruction, uint8_t *data, uint8_t length) +{ + MCP2515_CS_LOW(); + + SPI_Tx(instruction); + SPI_RxBuffer(data, length); + + MCP2515_CS_HIGH(); +} + +/* write single byte */ +void MCP2515_WriteByte(uint8_t address, uint8_t data) +{ + MCP2515_CS_LOW(); + + SPI_Tx(MCP2515_WRITE); + SPI_Tx(address); + SPI_Tx(data); + + MCP2515_CS_HIGH(); +} + +/* write buffer */ +void MCP2515_WriteByteSequence(uint8_t startAddress, uint8_t endAddress, uint8_t *data) +{ + MCP2515_CS_LOW(); + + SPI_Tx(MCP2515_WRITE); + SPI_Tx(startAddress); + SPI_TxBuffer(data, (endAddress - startAddress + 1)); + + MCP2515_CS_HIGH(); +} + +/* write to TxBuffer */ +void MCP2515_LoadTxSequence(uint8_t instruction, uint8_t *idReg, uint8_t dlc, uint8_t *data) +{ + MCP2515_CS_LOW(); + + SPI_Tx(instruction); + SPI_TxBuffer(idReg, 4); + SPI_Tx(dlc); + SPI_TxBuffer(data, dlc); + + MCP2515_CS_HIGH(); +} + +/* write to TxBuffer(1 byte) */ +void MCP2515_LoadTxBuffer(uint8_t instruction, uint8_t data) +{ + MCP2515_CS_LOW(); + + SPI_Tx(instruction); + SPI_Tx(data); + + MCP2515_CS_HIGH(); +} + +/* request to send */ +void MCP2515_RequestToSend(uint8_t instruction) +{ + MCP2515_CS_LOW(); + + SPI_Tx(instruction); + + MCP2515_CS_HIGH(); +} + +/* read status */ +uint8_t MCP2515_ReadStatus(void) +{ + uint8_t retVal; + + MCP2515_CS_LOW(); + + SPI_Tx(MCP2515_READ_STATUS); + retVal = SPI_Rx(); + + MCP2515_CS_HIGH(); + + return retVal; +} + +/* read RX STATUS register */ +uint8_t MCP2515_GetRxStatus(void) +{ + uint8_t retVal; + + MCP2515_CS_LOW(); + + SPI_Tx(MCP2515_RX_STATUS); + retVal = SPI_Rx(); + + MCP2515_CS_HIGH(); + + return retVal; +} + +/* Use when changing register value */ +void MCP2515_BitModify(uint8_t address, uint8_t mask, uint8_t data) +{ + MCP2515_CS_LOW(); + + SPI_Tx(MCP2515_BIT_MOD); + SPI_Tx(address); + SPI_Tx(mask); + SPI_Tx(data); + + MCP2515_CS_HIGH(); +} + +/* SPI Tx wrapper function */ +static void SPI_Tx(uint8_t data) +{ + HAL_SPI_Transmit(SPI_CAN, &data, 1, SPI_TIMEOUT); +} + +/* SPI Tx wrapper function */ +static void SPI_TxBuffer(uint8_t *buffer, uint8_t length) +{ + HAL_SPI_Transmit(SPI_CAN, buffer, length, SPI_TIMEOUT); +} + +/* SPI Rx wrapper function */ +static uint8_t SPI_Rx(void) +{ + uint8_t retVal; + HAL_SPI_Receive(SPI_CAN, &retVal, 1, SPI_TIMEOUT); + return retVal; +} + +/* SPI Rx wrapper function */ +static void SPI_RxBuffer(uint8_t *buffer, uint8_t length) +{ + HAL_SPI_Receive(SPI_CAN, buffer, length, SPI_TIMEOUT); +} diff --git a/MCP2515.h b/MCP2515.h new file mode 100644 index 0000000..d18e2c7 --- /dev/null +++ b/MCP2515.h @@ -0,0 +1,251 @@ +/* + (c) 2016 Microchip Technology Inc. and its subsidiaries. You may use this + software and any derivatives exclusively with Microchip products. + + THIS SOFTWARE IS SUPPLIED BY MICROCHIP "AS IS". NO WARRANTIES, WHETHER + EXPRESS, IMPLIED OR STATUTORY, APPLY TO THIS SOFTWARE, INCLUDING ANY IMPLIED + WARRANTIES OF NON-INFRINGEMENT, MERCHANTABILITY, AND FITNESS FOR A + PARTICULAR PURPOSE, OR ITS INTERACTION WITH MICROCHIP PRODUCTS, COMBINATION + WITH ANY OTHER PRODUCTS, OR USE IN ANY APPLICATION. + + IN NO EVENT WILL MICROCHIP BE LIABLE FOR ANY INDIRECT, SPECIAL, PUNITIVE, + INCIDENTAL OR CONSEQUENTIAL LOSS, DAMAGE, COST OR EXPENSE OF ANY KIND + WHATSOEVER RELATED TO THE SOFTWARE, HOWEVER CAUSED, EVEN IF MICROCHIP HAS + BEEN ADVISED OF THE POSSIBILITY OR THE DAMAGES ARE FORESEEABLE. TO THE + FULLEST EXTENT ALLOWED BY LAW, MICROCHIP'S TOTAL LIABILITY ON ALL CLAIMS IN + ANY WAY RELATED TO THIS SOFTWARE WILL NOT EXCEED THE AMOUNT OF FEES, IF ANY, + THAT YOU HAVE PAID DIRECTLY TO MICROCHIP FOR THIS SOFTWARE. + + MICROCHIP PROVIDES THIS SOFTWARE CONDITIONALLY UPON YOUR ACCEPTANCE OF THESE + TERMS. +*/ + +#ifndef __MCP2515_H +#define __MCP2515_H + +#include +#include "main.h" + +/* MCP2515 SPI Instruction Set */ +#define MCP2515_RESET 0xC0 + +#define MCP2515_READ 0x03 +#define MCP2515_READ_RXB0SIDH 0x90 +#define MCP2515_READ_RXB0D0 0x92 +#define MCP2515_READ_RXB1SIDH 0x94 +#define MCP2515_READ_RXB1D0 0x96 + +#define MCP2515_WRITE 0x02 +#define MCP2515_LOAD_TXB0SIDH 0x40 /* TX0 ID location */ +#define MCP2515_LOAD_TXB0D0 0x41 /* TX0 Data location */ +#define MCP2515_LOAD_TXB1SIDH 0x42 /* TX1 ID location */ +#define MCP2515_LOAD_TXB1D0 0x43 /* TX1 Data location */ +#define MCP2515_LOAD_TXB2SIDH 0x44 /* TX2 ID location */ +#define MCP2515_LOAD_TXB2D0 0x45 /* TX2 Data location */ + +#define MCP2515_RTS_TX0 0x81 +#define MCP2515_RTS_TX1 0x82 +#define MCP2515_RTS_TX2 0x84 +#define MCP2515_RTS_ALL 0x87 +#define MCP2515_READ_STATUS 0xA0 +#define MCP2515_RX_STATUS 0xB0 +#define MCP2515_BIT_MOD 0x05 + +/* MCP25152515 Register Adresses */ +#define MCP2515_RXF0SIDH 0x00 +#define MCP2515_RXF0SIDL 0x01 +#define MCP2515_RXF0EID8 0x02 +#define MCP2515_RXF0EID0 0x03 +#define MCP2515_RXF1SIDH 0x04 +#define MCP2515_RXF1SIDL 0x05 +#define MCP2515_RXF1EID8 0x06 +#define MCP2515_RXF1EID0 0x07 +#define MCP2515_RXF2SIDH 0x08 +#define MCP2515_RXF2SIDL 0x09 +#define MCP2515_RXF2EID8 0x0A +#define MCP2515_RXF2EID0 0x0B +#define MCP2515_CANSTAT 0x0E +#define MCP2515_CANCTRL 0x0F + +#define MCP2515_RXF3SIDH 0x10 +#define MCP2515_RXF3SIDL 0x11 +#define MCP2515_RXF3EID8 0x12 +#define MCP2515_RXF3EID0 0x13 +#define MCP2515_RXF4SIDH 0x14 +#define MCP2515_RXF4SIDL 0x15 +#define MCP2515_RXF4EID8 0x16 +#define MCP2515_RXF4EID0 0x17 +#define MCP2515_RXF5SIDH 0x18 +#define MCP2515_RXF5SIDL 0x19 +#define MCP2515_RXF5EID8 0x1A +#define MCP2515_RXF5EID0 0x1B +#define MCP2515_TEC 0x1C +#define MCP2515_REC 0x1D + +#define MCP2515_RXM0SIDH 0x20 +#define MCP2515_RXM0SIDL 0x21 +#define MCP2515_RXM0EID8 0x22 +#define MCP2515_RXM0EID0 0x23 +#define MCP2515_RXM1SIDH 0x24 +#define MCP2515_RXM1SIDL 0x25 +#define MCP2515_RXM1EID8 0x26 +#define MCP2515_RXM1EID0 0x27 +#define MCP2515_CNF3 0x28 +#define MCP2515_CNF2 0x29 +#define MCP2515_CNF1 0x2A +#define MCP2515_CANINTE 0x2B +#define MCP2515_CANINTF 0x2C +#define MCP2515_EFLG 0x2D + +#define MCP2515_TXB0CTRL 0x30 +#define MCP2515_TXB1CTRL 0x40 +#define MCP2515_TXB2CTRL 0x50 +#define MCP2515_RXB0CTRL 0x60 +#define MCP2515_RXB0SIDH 0x61 +#define MCP2515_RXB1CTRL 0x70 +#define MCP2515_RXB1SIDH 0x71 + +/* Defines for Rx Status */ +#define MSG_IN_RXB0 0x01 +#define MSG_IN_RXB1 0x02 +#define MSG_IN_BOTH_BUFFERS 0x03 + +typedef union{ + struct{ + unsigned RX0IF : 1; + unsigned RX1IF : 1; + unsigned TXB0REQ : 1; + unsigned TX0IF : 1; + unsigned TXB1REQ : 1; + unsigned TX1IF : 1; + unsigned TXB2REQ : 1; + unsigned TX2IF : 1; + }; + uint8_t ctrl_status; +}ctrl_status_t; + +typedef union{ + struct{ + unsigned filter : 3; + unsigned msgType : 2; + unsigned unusedBit : 1; + unsigned rxBuffer : 2; + }; + uint8_t ctrl_rx_status; +}ctrl_rx_status_t; + +typedef union{ + struct{ + unsigned EWARN :1; + unsigned RXWAR :1; + unsigned TXWAR :1; + unsigned RXEP :1; + unsigned TXEP :1; + unsigned TXBO :1; + unsigned RX0OVR :1; + unsigned RX1OVR :1; + }; + uint8_t error_flag_reg; +}ctrl_error_status_t; + +typedef union{ + struct{ + uint8_t RXBnSIDH; + uint8_t RXBnSIDL; + uint8_t RXBnEID8; + uint8_t RXBnEID0; + uint8_t RXBnDLC; + uint8_t RXBnD0; + uint8_t RXBnD1; + uint8_t RXBnD2; + uint8_t RXBnD3; + uint8_t RXBnD4; + uint8_t RXBnD5; + uint8_t RXBnD6; + uint8_t RXBnD7; + }; + uint8_t rx_reg_array[13]; +}rx_reg_t; + +/* MCP2515 Registers */ +typedef struct{ + uint8_t RXF0SIDH; + uint8_t RXF0SIDL; + uint8_t RXF0EID8; + uint8_t RXF0EID0; +}RXF0; + +typedef struct{ + uint8_t RXF1SIDH; + uint8_t RXF1SIDL; + uint8_t RXF1EID8; + uint8_t RXF1EID0; +}RXF1; + +typedef struct{ + uint8_t RXF2SIDH; + uint8_t RXF2SIDL; + uint8_t RXF2EID8; + uint8_t RXF2EID0; +}RXF2; + +typedef struct{ + uint8_t RXF3SIDH; + uint8_t RXF3SIDL; + uint8_t RXF3EID8; + uint8_t RXF3EID0; +}RXF3; + +typedef struct{ + uint8_t RXF4SIDH; + uint8_t RXF4SIDL; + uint8_t RXF4EID8; + uint8_t RXF4EID0; +}RXF4; + +typedef struct{ + uint8_t RXF5SIDH; + uint8_t RXF5SIDL; + uint8_t RXF5EID8; + uint8_t RXF5EID0; +}RXF5; + +typedef struct{ + uint8_t RXM0SIDH; + uint8_t RXM0SIDL; + uint8_t RXM0EID8; + uint8_t RXM0EID0; +}RXM0; + +typedef struct{ + uint8_t RXM1SIDH; + uint8_t RXM1SIDL; + uint8_t RXM1EID8; + uint8_t RXM1EID0; +}RXM1; + +typedef struct{ + uint8_t tempSIDH; + uint8_t tempSIDL; + uint8_t tempEID8; + uint8_t tempEID0; +}id_reg_t; + +/* Functions */ +bool MCP2515_Initialize(void); +bool MCP2515_SetConfigMode(void); +bool MCP2515_SetNormalMode(void); +bool MCP2515_SetSleepMode(void); +void MCP2515_Reset(void); +uint8_t MCP2515_ReadByte (uint8_t address); +void MCP2515_ReadRxSequence(uint8_t instruction, uint8_t *data, uint8_t length); +void MCP2515_WriteByte(uint8_t address, uint8_t data); +void MCP2515_WriteByteSequence(uint8_t startAddress, uint8_t endAddress, uint8_t *data); +void MCP2515_LoadTxSequence(uint8_t instruction, uint8_t *idReg, uint8_t dlc, uint8_t *data); +void MCP2515_LoadTxBuffer(uint8_t instruction, uint8_t data); +void MCP2515_RequestToSend(uint8_t instruction); +uint8_t MCP2515_ReadStatus(void); +uint8_t MCP2515_GetRxStatus(void); +void MCP2515_BitModify(uint8_t address, uint8_t mask, uint8_t data); + +#endif diff --git a/main.c b/main.c new file mode 100644 index 0000000..5afbc65 --- /dev/null +++ b/main.c @@ -0,0 +1,685 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file : main.c + * @brief : Main program body + ****************************************************************************** + * @attention + * + * Copyright (c) 2025 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ +/* USER CODE END Header */ +/* Includes ------------------------------------------------------------------*/ +#include "main.h" + +/* Private includes ----------------------------------------------------------*/ +/* USER CODE BEGIN Includes */ +#include "CANSPI.h" +#include "CANSERVO.h" +#include "stdio.h" + +/* USER CODE END Includes */ + +/* Private typedef -----------------------------------------------------------*/ +/* USER CODE BEGIN PTD */ + +/* USER CODE END PTD */ + +/* Private define ------------------------------------------------------------*/ +/* USER CODE BEGIN PD */ + +/* USER CODE END PD */ + +/* Private macro -------------------------------------------------------------*/ +/* USER CODE BEGIN PM */ + +/* USER CODE END PM */ + +/* Private variables ---------------------------------------------------------*/ + +SPI_HandleTypeDef hspi1; + +TIM_HandleTypeDef htim2; + +UART_HandleTypeDef huart1; + +/* USER CODE BEGIN PV */ +volatile uint8_t g_servo_step = 0; +/* USER CODE END PV */ + +/* Private function prototypes -----------------------------------------------*/ +void SystemClock_Config(void); +static void SystemPower_Config(void); +static void MX_GPIO_Init(void); +static void MX_ICACHE_Init(void); +static void MX_SPI1_Init(void); +static void MX_USART1_UART_Init(void); +static void MX_TIM2_Init(void); +/* USER CODE BEGIN PFP */ +#ifdef __GNUC__ +/* With GCC/RAISONANCE, small printf (option LD Linker->Libraries->Small printf + set to 'Yes') calls __io_putchar() */ +#define PUTCHAR_PROTOTYPE int __io_putchar(int ch) +#else +#define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f) +#endif /* __GNUC__ */ + +/* USER CODE END PFP */ + +/* Private user code ---------------------------------------------------------*/ +/* USER CODE BEGIN 0 */ + +static void delay_step(const char *msg) +{ + printf("\r\n--- %s (wait 5s) ---\r\n", msg); + HAL_Delay(5000); +} + +static void set_limits_wide(void) +{ + // Wide soft limits: allow full 0..16383 range + set_servo_min_position_limit(0); + set_servo_max_position_limit(16383); + HAL_Delay(50); +} + +static void check_emergency_status(void) +{ + uint16_t estop = receive_can_msg(REG_EMERGENCY_STOP); + + printf("REG_EMERGENCY_STOP = 0x%04X\r\n", estop); + + if (estop == 0) + { + printf("Status: OK (no emergency flags)\r\n"); + } + else + { + printf("Status: EMERGENCY ACTIVE!\r\n"); + printf("→ Servo will likely ignore motion commands\r\n"); + } +} + +void clear_emergency_via_powercfg(void) +{ + uint16_t val = 0; + + // Disable forced emergency mode bits + val &= ~PWR_FORCED_ES_MASK; + + send_can_msg(val, REG_POWER_CONFIG); + + HAL_Delay(50); + + uint16_t st = receive_can_msg(REG_EMERGENCY_STOP); + printf("After powercfg clear: ESTOP=0x%04X\r\n", st); +} + +/* USER CODE END 0 */ + +/** + * @brief The application entry point. + * @retval int + */ +int main(void) +{ + + /* USER CODE BEGIN 1 */ + + /* USER CODE END 1 */ + + /* MCU Configuration--------------------------------------------------------*/ + + /* Reset of all peripherals, Initializes the Flash interface and the Systick. */ + HAL_Init(); + + /* USER CODE BEGIN Init */ + + /* USER CODE END Init */ + + /* Configure the System Power */ + SystemPower_Config(); + + /* Configure the system clock */ + SystemClock_Config(); + + /* USER CODE BEGIN SysInit */ + + /* USER CODE END SysInit */ + + /* Initialize all configured peripherals */ + MX_GPIO_Init(); + MX_ICACHE_Init(); + MX_SPI1_Init(); + MX_USART1_UART_Init(); + MX_TIM2_Init(); + + + /* Initialize leds */ + BSP_LED_Init(LED_GREEN); + BSP_LED_Init(LED_BLUE); + BSP_LED_Init(LED_RED); + + /* Initialize USER push-button, will be used to trigger an interrupt each time it's pressed.*/ + BSP_PB_Init(BUTTON_USER, BUTTON_MODE_EXTI); + + /* USER CODE BEGIN 2 */ + CANSPI_Initialize(); + + //HAL_Delay(5000); + servo_init(); + + + +// if (!SERVO_OK()) { +// //HAL_TIM_Base_Start_IT(&htim2); +// //servo_set_speed_down_emergeny_mode(); +// printf("CAN Servo operating outside of nominal conditions upon bootup. One hour timer started..."); +// } + + + /* USER CODE END 2 */ + + /* Infinite loop */ + /* USER CODE BEGIN WHILE */ + while (1) + { + + printf("\r\n===== MIDPOINT NON-ZERO TEST (+30 deg) =====\r\n"); + + servo_init(); + HAL_Delay(1000); + + // Move to a non-zero angle first + printf("\r\nMove to +30 deg (old reference)\r\n"); + set_servo_angle(30.0f); + HAL_Delay(3000); + + uint16_t raw_before = receive_can_msg(REG_POSITION_NEW); + printf("Raw before re-zero = 0x%04X\r\n", (unsigned)raw_before); + + // Re-zero here (THIS becomes new 0°) + printf("\r\nSet midpoint at current position (should become new 0 deg)\r\n"); + set_servo_midpoint(); + HAL_Delay(500); + + printf("New midpoint = 0x%04X\r\n", (unsigned)servo_midpoint); + + // Now command 0° (should stay basically where it is) + printf("\r\nCommand 0 deg (new reference) - should stay near same physical position\r\n"); + set_servo_angle(0.0f); + HAL_Delay(3000); + + uint16_t raw_after0 = receive_can_msg(REG_POSITION_NEW); + printf("Raw after cmd 0 (new ref) = 0x%04X\r\n", (unsigned)raw_after0); + + // Sanity: check reported angle should be near 0 + float a0 = check_servo_angle(); + printf("check_servo_angle() after cmd 0 = %.2f deg\r\n", a0); + + // Command +60° new ref + printf("\r\nCommand +60 deg (new reference)\r\n"); + set_servo_angle(60.0f); + HAL_Delay(3000); + + uint16_t raw_after60 = receive_can_msg(REG_POSITION_NEW); + printf("Raw after +60 = 0x%04X\r\n", (unsigned)raw_after60); + + float a60 = check_servo_angle(); + printf("check_servo_angle() after +60 = %.2f deg\r\n", a60); + + // Print limits + uint16_t min_lim = receive_can_msg(REG_POSITION_MIN_LIMIT); + uint16_t max_lim = receive_can_msg(REG_POSITION_MAX_LIMIT); + printf("\r\nLimits now: MIN=0x%04X MAX=0x%04X\r\n", + (unsigned)min_lim, (unsigned)max_lim); + + // Emergency status + uint16_t estop = receive_can_msg(REG_EMERGENCY_STOP); + printf("Emergency status = 0x%04X\r\n", (unsigned)estop); + + printf("\r\n===== TEST COMPLETE =====\r\n"); + + + + + //servo_soft_reset_only(); +// uint16_t power = receive_can_msg(REG_POWER_CONFIG); +// printf("Power_config: %x",power); +// set_servo_angle(0); +// uint16_t max_voltage = receive_can_msg(REG_VOLTAGE_MAX); +// printf("Max volt: %u",max_voltage); +// HAL_Delay(3000); + +// servo_soft_reset_only(); +// servo_clear_forced_es(); +// set_servo_max_voltage(SERVO_MAX_VOLTAGE); +// set_servo_min_voltage(SERVO_MIN_VOLTAGE); +// set_servo_angle (90); +// HAL_Delay(3000); +// uint16_t power = receive_can_msg(REG_POWER_CONFIG); +// uint16_t emergency = receive_can_msg(REG_EMERGENCY_STOP); +// printf("Power_config: 0x%04\r\n",power); +// printf("Emergency: 0x%04\r\n",emergency); +// HAL_Delay(3000); +// servo_clear_forced_es(); +// HAL_Delay(3000); + + + + + + + + // ========================= + // TEST 6: Voltage thresholds -> emergency flags + // ========================= +// printf("\r\n[TEST 6] Voltage min/max thresholds trigger emergency flags\r\n"); +// printf("Objective: Flags assert when supply is outside window.\r\n"); +// +// // Narrow window around nominal 12.00V +// set_servo_min_voltage(1190); // 11.90V +// set_servo_max_voltage(1210); // 12.10V +// //servo_soft_reset(); +// delay_step("Set VOLT_MIN=11.90V (1190), VOLT_MAX=12.10V (1210)"); +// +// // Step 1: nominal +// printf("Set PSU to 12.00V. Reading REG_EMERGENCY_STOP...\r\n"); +// uint16_t st = receive_can_msg(REG_EMERGENCY_STOP); +// printf("REG_EMERGENCY_STOP = 0x%04X\r\n", st); +// delay_step("Expected: No voltage flags"); +// +// // Step 2: undervoltage +// printf("Set PSU to 11.80V. Reading REG_EMERGENCY_STOP...\r\n"); +// st = receive_can_msg(REG_EMERGENCY_STOP); +// printf("REG_EMERGENCY_STOP = 0x%04X\r\n", st); +// delay_step("Expected: Undervoltage flag(s) set"); +// uint16_t estop = receive_can_msg(0x46); +// printf("estop = 0x%04X\r\n", estop); +// +// // Step 3: overvoltage +// printf("Set PSU to 12.20V. Reading REG_EMERGENCY_STOP...\r\n"); +// st = receive_can_msg(REG_EMERGENCY_STOP); +// printf("REG_EMERGENCY_STOP = 0x%04X\r\n", st); +// delay_step("Expected: Overvoltage flag(s) set"); +// +// // Step 4: back to nominal +// printf("Return PSU to 12.00V. Reading REG_EMERGENCY_STOP...\r\n"); +// st = receive_can_msg(REG_EMERGENCY_STOP); +// printf("REG_EMERGENCY_STOP = 0x%04X\r\n", st); +// delay_step("Expected: Flags clear (if auto-clear), otherwise document latch behavior"); + + /* USER CODE END WHILE */ + + /* USER CODE BEGIN 3 */ + } + /* USER CODE END 3 */ +} + +/** + * @brief System Clock Configuration + * @retval None + */ +void SystemClock_Config(void) +{ + RCC_OscInitTypeDef RCC_OscInitStruct = {0}; + RCC_ClkInitTypeDef RCC_ClkInitStruct = {0}; + + /** Configure the main internal regulator output voltage + */ + if (HAL_PWREx_ControlVoltageScaling(PWR_REGULATOR_VOLTAGE_SCALE4) != HAL_OK) + { + Error_Handler(); + } + + /** Initializes the CPU, AHB and APB buses clocks + */ + RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_MSI; + RCC_OscInitStruct.MSIState = RCC_MSI_ON; + RCC_OscInitStruct.MSICalibrationValue = RCC_MSICALIBRATION_DEFAULT; + RCC_OscInitStruct.MSIClockRange = RCC_MSIRANGE_4; + RCC_OscInitStruct.PLL.PLLState = RCC_PLL_NONE; + if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) + { + Error_Handler(); + } + + /** Initializes the CPU, AHB and APB buses clocks + */ + RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK + |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2 + |RCC_CLOCKTYPE_PCLK3; + RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_MSI; + RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; + RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1; + RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; + RCC_ClkInitStruct.APB3CLKDivider = RCC_HCLK_DIV1; + + if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_0) != HAL_OK) + { + Error_Handler(); + } +} + +/** + * @brief Power Configuration + * @retval None + */ +static void SystemPower_Config(void) +{ + + /* + * Disable the internal Pull-Up in Dead Battery pins of UCPD peripheral + */ + HAL_PWREx_DisableUCPDDeadBattery(); + + /* + * Switch to SMPS regulator instead of LDO + */ + if (HAL_PWREx_ConfigSupply(PWR_SMPS_SUPPLY) != HAL_OK) + { + Error_Handler(); + } +/* USER CODE BEGIN PWR */ +/* USER CODE END PWR */ +} + +/** + * @brief ICACHE Initialization Function + * @param None + * @retval None + */ +static void MX_ICACHE_Init(void) +{ + + /* USER CODE BEGIN ICACHE_Init 0 */ + + /* USER CODE END ICACHE_Init 0 */ + + /* USER CODE BEGIN ICACHE_Init 1 */ + + /* USER CODE END ICACHE_Init 1 */ + + /** Enable instruction cache in 1-way (direct mapped cache) + */ + if (HAL_ICACHE_ConfigAssociativityMode(ICACHE_1WAY) != HAL_OK) + { + Error_Handler(); + } + if (HAL_ICACHE_Enable() != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN ICACHE_Init 2 */ + + /* USER CODE END ICACHE_Init 2 */ + +} + +/** + * @brief SPI1 Initialization Function + * @param None + * @retval None + */ +static void MX_SPI1_Init(void) +{ + + /* USER CODE BEGIN SPI1_Init 0 */ + + /* USER CODE END SPI1_Init 0 */ + + SPI_AutonomousModeConfTypeDef HAL_SPI_AutonomousMode_Cfg_Struct = {0}; + + /* USER CODE BEGIN SPI1_Init 1 */ + + /* USER CODE END SPI1_Init 1 */ + /* SPI1 parameter configuration*/ + hspi1.Instance = SPI1; + hspi1.Init.Mode = SPI_MODE_MASTER; + hspi1.Init.Direction = SPI_DIRECTION_2LINES; + hspi1.Init.DataSize = SPI_DATASIZE_8BIT; + hspi1.Init.CLKPolarity = SPI_POLARITY_LOW; + hspi1.Init.CLKPhase = SPI_PHASE_1EDGE; + hspi1.Init.NSS = SPI_NSS_SOFT; + hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_2; + hspi1.Init.FirstBit = SPI_FIRSTBIT_MSB; + hspi1.Init.TIMode = SPI_TIMODE_DISABLE; + hspi1.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE; + hspi1.Init.CRCPolynomial = 0x7; + hspi1.Init.NSSPMode = SPI_NSS_PULSE_ENABLE; + hspi1.Init.NSSPolarity = SPI_NSS_POLARITY_LOW; + hspi1.Init.FifoThreshold = SPI_FIFO_THRESHOLD_01DATA; + hspi1.Init.MasterSSIdleness = SPI_MASTER_SS_IDLENESS_00CYCLE; + hspi1.Init.MasterInterDataIdleness = SPI_MASTER_INTERDATA_IDLENESS_00CYCLE; + hspi1.Init.MasterReceiverAutoSusp = SPI_MASTER_RX_AUTOSUSP_DISABLE; + hspi1.Init.MasterKeepIOState = SPI_MASTER_KEEP_IO_STATE_DISABLE; + hspi1.Init.IOSwap = SPI_IO_SWAP_DISABLE; + hspi1.Init.ReadyMasterManagement = SPI_RDY_MASTER_MANAGEMENT_INTERNALLY; + hspi1.Init.ReadyPolarity = SPI_RDY_POLARITY_HIGH; + if (HAL_SPI_Init(&hspi1) != HAL_OK) + { + Error_Handler(); + } + HAL_SPI_AutonomousMode_Cfg_Struct.TriggerState = SPI_AUTO_MODE_DISABLE; + HAL_SPI_AutonomousMode_Cfg_Struct.TriggerSelection = SPI_GRP1_GPDMA_CH0_TCF_TRG; + HAL_SPI_AutonomousMode_Cfg_Struct.TriggerPolarity = SPI_TRIG_POLARITY_RISING; + if (HAL_SPIEx_SetConfigAutonomousMode(&hspi1, &HAL_SPI_AutonomousMode_Cfg_Struct) != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN SPI1_Init 2 */ + + /* USER CODE END SPI1_Init 2 */ + +} + +/** + * @brief TIM2 Initialization Function + * @param None + * @retval None + */ +static void MX_TIM2_Init(void) +{ + + /* USER CODE BEGIN TIM2_Init 0 */ + + /* USER CODE END TIM2_Init 0 */ + + TIM_ClockConfigTypeDef sClockSourceConfig = {0}; + TIM_MasterConfigTypeDef sMasterConfig = {0}; + + /* USER CODE BEGIN TIM2_Init 1 */ + + /* USER CODE END TIM2_Init 1 */ + htim2.Instance = TIM2; + htim2.Init.Prescaler = 0; + htim2.Init.CounterMode = TIM_COUNTERMODE_UP; + htim2.Init.Period = 4294967295; + htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; + if (HAL_TIM_Base_Init(&htim2) != HAL_OK) + { + Error_Handler(); + } + sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL; + if (HAL_TIM_ConfigClockSource(&htim2, &sClockSourceConfig) != HAL_OK) + { + Error_Handler(); + } + sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; + sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; + if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN TIM2_Init 2 */ + + /* USER CODE END TIM2_Init 2 */ + +} + +/** + * @brief USART1 Initialization Function + * @param None + * @retval None + */ +static void MX_USART1_UART_Init(void) +{ + + /* USER CODE BEGIN USART1_Init 0 */ + + /* USER CODE END USART1_Init 0 */ + + /* USER CODE BEGIN USART1_Init 1 */ + + /* USER CODE END USART1_Init 1 */ + huart1.Instance = USART1; + huart1.Init.BaudRate = 115200; + huart1.Init.WordLength = UART_WORDLENGTH_8B; + huart1.Init.StopBits = UART_STOPBITS_1; + huart1.Init.Parity = UART_PARITY_NONE; + huart1.Init.Mode = UART_MODE_TX_RX; + huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE; + huart1.Init.OverSampling = UART_OVERSAMPLING_16; + huart1.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE; + huart1.Init.ClockPrescaler = UART_PRESCALER_DIV1; + huart1.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT; + if (HAL_UART_Init(&huart1) != HAL_OK) + { + Error_Handler(); + } + if (HAL_UARTEx_SetTxFifoThreshold(&huart1, UART_TXFIFO_THRESHOLD_1_8) != HAL_OK) + { + Error_Handler(); + } + if (HAL_UARTEx_SetRxFifoThreshold(&huart1, UART_RXFIFO_THRESHOLD_1_8) != HAL_OK) + { + Error_Handler(); + } + if (HAL_UARTEx_DisableFifoMode(&huart1) != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN USART1_Init 2 */ + + /* USER CODE END USART1_Init 2 */ + +} + +/** + * @brief GPIO Initialization Function + * @param None + * @retval None + */ +static void MX_GPIO_Init(void) +{ + GPIO_InitTypeDef GPIO_InitStruct = {0}; + /* USER CODE BEGIN MX_GPIO_Init_1 */ + + /* USER CODE END MX_GPIO_Init_1 */ + + /* GPIO Ports Clock Enable */ + __HAL_RCC_GPIOC_CLK_ENABLE(); + __HAL_RCC_GPIOA_CLK_ENABLE(); + __HAL_RCC_GPIOD_CLK_ENABLE(); + __HAL_RCC_GPIOB_CLK_ENABLE(); + + /*Configure GPIO pin Output Level */ + HAL_GPIO_WritePin(CAN_CS_GPIO_Port, CAN_CS_Pin, GPIO_PIN_RESET); + + /*Configure GPIO pin : CAN_CS_Pin */ + GPIO_InitStruct.Pin = CAN_CS_Pin; + GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + HAL_GPIO_Init(CAN_CS_GPIO_Port, &GPIO_InitStruct); + + /* USER CODE BEGIN MX_GPIO_Init_2 */ + + /* USER CODE END MX_GPIO_Init_2 */ +} + +/* USER CODE BEGIN 4 */ +PUTCHAR_PROTOTYPE +{ + /* Place your implementation of fputc here */ + /* e.g. write a character to the USART1 and Loop until the end of transmission */ + HAL_UART_Transmit(&huart1, (uint8_t *)&ch, 1, 0xFFFF); + return ch; +} + +/* USER CODE END 4 */ + +/** + * @brief Period elapsed callback in non blocking mode + * @note This function is called when TIM17 interrupt took place, inside + * HAL_TIM_IRQHandler(). It makes a direct call to HAL_IncTick() to increment + * a global variable "uwTick" used as application time base. + * @param htim : TIM handle + * @retval None + */ +void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) +{ + /* USER CODE BEGIN Callback 0 */ +// if (htim->Instance == TIM2) { +// +// servo_soft_reset_only(); +// +// if (!SERVO_OK()) { +// printf("Board still fails after 1hr wait time. Board resetting..."); +// return; +// +// } else { +// servo_clear_forced_es(); +// HAL_TIM_Base_Stop_IT(&htim2); +// } +// +// } + + /* USER CODE END Callback 0 */ + if (htim->Instance == TIM17) + { + HAL_IncTick(); + } + /* USER CODE BEGIN Callback 1 */ + + /* USER CODE END Callback 1 */ +} + +/** + * @brief This function is executed in case of error occurrence. + * @retval None + */ +void Error_Handler(void) +{ + /* USER CODE BEGIN Error_Handler_Debug */ + /* User can add his own implementation to report the HAL error return state */ + __disable_irq(); + while (1) + { + } + /* USER CODE END Error_Handler_Debug */ +} +#ifdef USE_FULL_ASSERT +/** + * @brief Reports the name of the source file and the source line number + * where the assert_param error has occurred. + * @param file: pointer to the source file name + * @param line: assert_param error line source number + * @retval None + */ +void assert_failed(uint8_t *file, uint32_t line) +{ + /* USER CODE BEGIN 6 */ + /* User can add his own implementation to report the file name and line number, + ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */ + /* USER CODE END 6 */ +} +#endif /* USE_FULL_ASSERT */