diff --git a/firmware/src/common.h b/firmware/src/common.h index 8668bc12..a92e17c7 100644 --- a/firmware/src/common.h +++ b/firmware/src/common.h @@ -125,7 +125,7 @@ #define TIMER_FREQ_HZ (ACLK_FREQ_HZ >> TXCTL_PS_DIV) -#define ENCODER_BITS (13) +#define ENCODER_BITS (16) #define ENCODER_TICKS (1 << ENCODER_BITS) #define ENCODER_TICKS_FLOAT ((float)(ENCODER_TICKS)) diff --git a/firmware/src/controller/homing_planner.c b/firmware/src/controller/homing_planner.c index 0feb9667..f9393963 100644 --- a/firmware/src/controller/homing_planner.c +++ b/firmware/src/controller/homing_planner.c @@ -8,12 +8,12 @@ #include static HomingPlannerConfig config = { - .homing_velocity = -8192.0f, // ticks/s + .homing_velocity = ENCODER_TICKS_FLOAT, // ticks/s .max_homing_t = 20.0f, // s - .max_stay_vel = 4000.0f, // ticks/s - .max_stay_dpos = 1000.0f, // ticks + .max_stay_vel = (ENCODER_TICKS_FLOAT/2.0f), // ticks/s + .max_stay_dpos = (ENCODER_TICKS_FLOAT/8.0f), // ticks .max_stay_t = 1.0f, // s - .retract_distance = 1000.0f // ticks + .retract_distance = (ENCODER_TICKS_FLOAT/8.0f) // ticks }; static HomingPlannerState state = {0}; diff --git a/firmware/src/motor/motor.c b/firmware/src/motor/motor.c index c4814de3..8e5e6bad 100644 --- a/firmware/src/motor/motor.c +++ b/firmware/src/motor/motor.c @@ -166,7 +166,7 @@ bool motor_calibrate_inductance(void) return true; } -TM_RAMFUNC uint8_t motor_find_pole_pairs(uint16_t ticks, float mpos_start, float mpos_end, float epos_rad) +TM_RAMFUNC uint8_t motor_find_pole_pairs(uint32_t ticks, float mpos_start, float mpos_end, float epos_rad) { const float mpos_diff = our_fabsf(mpos_end - mpos_start); float mpos_diff_rad = TWOPI * mpos_diff / ticks; diff --git a/firmware/src/motor/motor.h b/firmware/src/motor/motor.h index 51187ea5..7c00ffb7 100644 --- a/firmware/src/motor/motor.h +++ b/firmware/src/motor/motor.h @@ -79,7 +79,7 @@ bool motor_calibrate_resistance(void); bool motor_calibrate_inductance(void); uint8_t motor_get_pole_pairs(void); -uint8_t motor_find_pole_pairs(uint16_t ticks, float mpos_start, float mpos_end, float epos_rad); +uint8_t motor_find_pole_pairs(uint32_t ticks, float mpos_start, float mpos_end, float epos_rad); void motor_set_pole_pairs(uint8_t pairs); float motor_get_phase_resistance(void); diff --git a/firmware/src/sensor/as5047.c b/firmware/src/sensor/as5047.c index c72bfdf9..4f20028e 100644 --- a/firmware/src/sensor/as5047.c +++ b/firmware/src/sensor/as5047.c @@ -32,15 +32,16 @@ bool as5047p_init_with_port(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF bool as5047p_init_with_config(Sensor *s, const AS5047PSensorConfig *c) { AS5047PSensor *as = (AS5047PSensor *)s; - as->base.get_raw_angle_func = ma7xx_get_raw_angle; - as->base.update_func = as5047p_update; - as->base.prepare_func = as5047p_send_angle_cmd; - as->base.reset_func = as5047p_reset; - as->base.deinit_func = as5047p_deinit; - as->base.get_errors_func = as5047p_get_errors; - as->base.is_calibrated_func = as5047p_is_calibrated; - as->base.calibrate_func = as5047p_calibrate; - as->base.config.type = SENSOR_TYPE_AS5047; + s->get_raw_angle_func = ma7xx_get_raw_angle; + s->update_func = as5047p_update; + s->prepare_func = as5047p_send_angle_cmd; + s->reset_func = as5047p_reset; + s->deinit_func = as5047p_deinit; + s->get_errors_func = as5047p_get_errors; + s->is_calibrated_func = as5047p_is_calibrated; + s->calibrate_func = as5047p_calibrate; + s->config.type = SENSOR_TYPE_AS5047; + s->ticks = ENCODER_TICKS; as->config = *c; ssp_init(as->config.ssp_port, SSP_MS_MASTER, 0, 0); delay_us(10000); // Example delay, adjust based on AS5047P datasheet diff --git a/firmware/src/sensor/hall.c b/firmware/src/sensor/hall.c index 4c4f4a40..9458bef6 100644 --- a/firmware/src/sensor/hall.c +++ b/firmware/src/sensor/hall.c @@ -33,14 +33,15 @@ bool hall_init_with_defaults(Sensor *s) bool hall_init_with_config(Sensor *s, const HallSensorConfig *c) { HallSensor *ms = (HallSensor *)s; - ms->base.get_raw_angle_func = hall_get_angle; - ms->base.update_func = hall_update; - ms->base.reset_func = hall_reset; - ms->base.deinit_func = hall_deinit; - ms->base.get_errors_func = hall_get_errors; - ms->base.is_calibrated_func = hall_sector_map_is_calibrated; - ms->base.calibrate_func = hall_calibrate_sequence; - ms->base.config.type = SENSOR_TYPE_HALL; + s->get_raw_angle_func = hall_get_angle; + s->update_func = hall_update; + s->reset_func = hall_reset; + s->deinit_func = hall_deinit; + s->get_errors_func = hall_get_errors; + s->is_calibrated_func = hall_sector_map_is_calibrated; + s->calibrate_func = hall_calibrate_sequence; + s->config.type = SENSOR_TYPE_HALL; + s->ticks = HALL_SECTORS; ms->config = *c; ms->hw_defaults[0] = pac5xxx_tile_register_read(ADDR_CFGAIO7); ms->hw_defaults[1] = pac5xxx_tile_register_read(ADDR_CFGAIO8); diff --git a/firmware/src/sensor/ma7xx.c b/firmware/src/sensor/ma7xx.c index cfe5cc19..08e2d3c0 100644 --- a/firmware/src/sensor/ma7xx.c +++ b/firmware/src/sensor/ma7xx.c @@ -34,15 +34,16 @@ bool ma7xx_init_with_port(Sensor *s, const SSP_TYPE port, PAC55XX_SSP_TYPEDEF *s bool ma7xx_init_with_config(Sensor *s, const MA7xxSensorConfig *c) { MA7xxSensor *ms = (MA7xxSensor *)s; - ms->base.get_raw_angle_func = ma7xx_get_raw_angle; - ms->base.update_func = ma7xx_update; - ms->base.prepare_func = ma7xx_send_angle_cmd; - ms->base.reset_func = ma7xx_reset; - ms->base.deinit_func = ma7xx_deinit; - ms->base.get_errors_func = ma7xx_get_errors; - ms->base.is_calibrated_func = ma7xx_rec_is_calibrated; - ms->base.calibrate_func = ma7xx_calibrate; - ms->base.config.type = SENSOR_TYPE_MA7XX; + s->get_raw_angle_func = ma7xx_get_raw_angle; + s->update_func = ma7xx_update; + s->prepare_func = ma7xx_send_angle_cmd; + s->reset_func = ma7xx_reset; + s->deinit_func = ma7xx_deinit; + s->get_errors_func = ma7xx_get_errors; + s->is_calibrated_func = ma7xx_rec_is_calibrated; + s->calibrate_func = ma7xx_calibrate; + s->config.type = SENSOR_TYPE_MA7XX; + s->ticks = ENCODER_TICKS; ms->config = *c; ssp_init(ms->config.ssp_port, SSP_MS_MASTER, 0, 0); delay_us(16000); // ensure 16ms sensor startup time as per the datasheet diff --git a/firmware/src/sensor/ma7xx.h b/firmware/src/sensor/ma7xx.h index 2f57ab25..8ffbe72a 100644 --- a/firmware/src/sensor/ma7xx.h +++ b/firmware/src/sensor/ma7xx.h @@ -73,7 +73,7 @@ static inline int16_t ma7xx_get_raw_angle(const Sensor *s) static inline void ma7xx_update(Sensor *s, bool check_error) { MA7xxSensor *ms = (MA7xxSensor *)s; - const int16_t angle = ssp_read_one(ms->config.ssp_struct) >> 3; + const int16_t angle = ssp_read_one(ms->config.ssp_struct); if (check_error) { diff --git a/firmware/src/sensor/sensor.h b/firmware/src/sensor/sensor.h index d1648c1a..7c88dd8c 100644 --- a/firmware/src/sensor/sensor.h +++ b/firmware/src/sensor/sensor.h @@ -83,6 +83,7 @@ struct Sensor { // typedefd earlier sensor_update_func_t update_func; sensor_prepare_func_t prepare_func; sensor_get_errors_func_t get_errors_func; + uint32_t ticks; bool initialized : 1; bool current : 1; }; @@ -116,31 +117,9 @@ static inline void sensor_invalidate(Sensor *s) s->current = false; } -static inline uint16_t sensor_get_ticks(Sensor *s) { -#ifdef BOARD_REV_R5 - if (SENSOR_TYPE_MA7XX == s->config.type) { -#endif - // We need to derive this during call, because the motor pole pairs - // may change after calibration, or after user input - return 6 * motor_get_pole_pairs(); -#ifdef BOARD_REV_R5 - } - return twopi_by_hall_sectors; -#endif -} - -static inline float sensor_ticks_to_eangle(Sensor *s) +static inline uint16_t sensor_get_ticks(Sensor *s) { -#ifdef BOARD_REV_R5 - if (SENSOR_TYPE_MA7XX == s->config.type) { -#endif - // We need to derive this during call, because the motor pole pairs - // may change after calibration, or after user input - return twopi_by_enc_ticks * motor_get_pole_pairs(); -#ifdef BOARD_REV_R5 - } - return twopi_by_hall_sectors; -#endif + return s->ticks; } static inline void sensor_prepare(Sensor *s) diff --git a/firmware/src/sensor/sensors.c b/firmware/src/sensor/sensors.c index b4b86e09..253471a5 100644 --- a/firmware/src/sensor/sensors.c +++ b/firmware/src/sensor/sensors.c @@ -28,9 +28,7 @@ GenSensor sensors[SENSOR_COUNT] = { .reset_func = ma7xx_reset, .update_func = ma7xx_update, .prepare_func = ma7xx_send_angle_cmd, - .get_errors_func = ma7xx_get_errors, - .initialized = false, - .current = false + .get_errors_func = ma7xx_get_errors }}, {.sensor = { .config = { .type = SENSOR_TYPE_MA7XX }, @@ -41,9 +39,7 @@ GenSensor sensors[SENSOR_COUNT] = { .reset_func = ma7xx_reset, .update_func = ma7xx_update, .prepare_func = ma7xx_send_angle_cmd, - .get_errors_func = ma7xx_get_errors, - .initialized = false, - .current = false + .get_errors_func = ma7xx_get_errors }}, {.sensor = { .config = { .type = SENSOR_TYPE_HALL }, @@ -54,9 +50,7 @@ GenSensor sensors[SENSOR_COUNT] = { .reset_func = hall_reset, .update_func = hall_update, .prepare_func = 0x0, - .get_errors_func = hall_get_errors, - .initialized = false, - .current = false + .get_errors_func = hall_get_errors }} };