diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index cb704404..d7659d35 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -377,7 +377,7 @@ bool Axis::run_homing() { controller_.vel_setpoint_ = 0.0f; // Change directions without decelerating // Set our current position in encoder counts to make control more logical - encoder_.set_linear_count(static_cast(controller_.pos_setpoint_)); + encoder_.set_linear_count((int32_t)controller_.pos_setpoint_); controller_.config_.control_mode = Controller::CTRL_MODE_POSITION_CONTROL; controller_.config_.input_mode = Controller::INPUT_MODE_TRAP_TRAJ; diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index ed9163f7..60af0ee0 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -293,7 +293,7 @@ bool Controller::update(float* current_setpoint_output) { // We get the current position and apply a current feed-forward // ensuring that we handle negative encoder positions properly (-1 == motor->encoder.encoder_cpr - 1) if (anticogging_valid_ && config_.anticogging.enable) { - Iq += config_.anticogging.cogging_map[std::clamp(mod(static_cast(anticogging_pos), 3600), 0, 3600)]; + Iq += config_.anticogging.cogging_map[std::clamp(mod((int)anticogging_pos, 3600), 0, 3600)]; } float v_err = 0.0f; diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index e0e29a0b..d1545fc1 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -110,7 +110,7 @@ void Encoder::set_linear_count(int32_t count) { // Update states shadow_count_ = count; - pos_estimate_ = static_cast(count); + pos_estimate_ = (float)count; tim_cnt_sample_ = count; //Write hardware last @@ -132,7 +132,7 @@ void Encoder::set_circular_count(int32_t count, bool update_offset) { // Update states count_in_cpr_ = mod(count, config_.cpr); - pos_cpr_ = static_cast(count_in_cpr_); + pos_cpr_ = (float)count_in_cpr_; cpu_exit_critical(prim); } @@ -182,7 +182,7 @@ bool Encoder::run_direction_find() { // TODO: Do the scan with current, not voltage! bool Encoder::run_offset_calibration() { static const float start_lock_duration = 1.0f; - static const int num_steps = (int)(config_.calib_scan_distance / config_.calib_scan_omega * static_cast(current_meas_hz)); + static const int num_steps = (int)(config_.calib_scan_distance / config_.calib_scan_omega * (float)current_meas_hz); // Require index found if enabled if (config_.use_index && !index_found_) { @@ -219,7 +219,7 @@ bool Encoder::run_offset_calibration() { // scan forward i = 0; axis_->run_control_loop([&]() { - float phase = wrap_pm_pi(config_.calib_scan_distance * static_cast(i) / static_cast(num_steps) - config_.calib_scan_distance / 2.0f); + float phase = wrap_pm_pi(config_.calib_scan_distance * (float)i / (float)num_steps - config_.calib_scan_distance / 2.0f); float v_alpha = voltage_magnitude * our_arm_cos_f32(phase); float v_beta = voltage_magnitude * our_arm_sin_f32(phase); if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta)) @@ -248,7 +248,7 @@ bool Encoder::run_offset_calibration() { //TODO avoid recomputing elec_rad_per_enc every time // Check CPR - float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / static_cast(config_.cpr)); + float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr)); float expected_encoder_delta = config_.calib_scan_distance / elec_rad_per_enc; calib_scan_response_ = std::abs(shadow_count_ - init_enc_val); if (std::abs(calib_scan_response_ - expected_encoder_delta) / expected_encoder_delta > config_.calib_range) { @@ -259,7 +259,7 @@ bool Encoder::run_offset_calibration() { // scan backwards i = 0; axis_->run_control_loop([&]() { - float phase = wrap_pm_pi(-config_.calib_scan_distance * static_cast(i) / static_cast(num_steps) + config_.calib_scan_distance / 2.0f); + float phase = wrap_pm_pi(-config_.calib_scan_distance * (float)i / (float)num_steps + config_.calib_scan_distance / 2.0f); float v_alpha = voltage_magnitude * our_arm_cos_f32(phase); float v_beta = voltage_magnitude * our_arm_sin_f32(phase); if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta)) @@ -275,7 +275,7 @@ bool Encoder::run_offset_calibration() { config_.offset = encvaluesum / (num_steps * 2); int32_t residual = encvaluesum - ((int64_t)config_.offset * (int64_t)(num_steps * 2)); - config_.offset_float = static_cast(residual) / static_cast(num_steps * 2) + 0.5f; // add 0.5 to center-align state to phase + config_.offset_float = (float)residual / (float)(num_steps * 2) + 0.5f; // add 0.5 to center-align state to phase is_ready_ = true; return true; @@ -508,13 +508,13 @@ bool Encoder::update() { pos_estimate_ += current_meas_period * vel_estimate_; pos_cpr_ += current_meas_period * vel_estimate_; // discrete phase detector - float delta_pos = static_cast(shadow_count_) - static_cast(std::floor(pos_estimate_)); - float delta_pos_cpr = static_cast(count_in_cpr_) - static_cast(std::floor(pos_cpr_)); - delta_pos_cpr = wrap_pm(delta_pos_cpr, 0.5f * static_cast(config_.cpr)); + float delta_pos = (float)(shadow_count_ - (int32_t)std::floor(pos_estimate_)); + float delta_pos_cpr = (float)(count_in_cpr_ - (int32_t)std::floor(pos_cpr_)); + delta_pos_cpr = wrap_pm(delta_pos_cpr, 0.5f * (float)(config_.cpr)); // pll feedback pos_estimate_ += current_meas_period * pll_kp_ * delta_pos; pos_cpr_ += current_meas_period * pll_kp_ * delta_pos_cpr; - pos_cpr_ = fmodf_pos(pos_cpr_, static_cast(config_.cpr)); + pos_cpr_ = fmodf_pos(pos_cpr_, (float)(config_.cpr)); vel_estimate_ += current_meas_period * pll_ki_ * delta_pos_cpr; bool snap_to_zero_vel = false; if (std::abs(vel_estimate_) < 0.5f * current_meas_period * pll_ki_) { @@ -543,7 +543,7 @@ bool Encoder::update() { //// compute electrical phase //TODO avoid recomputing elec_rad_per_enc every time - float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / static_cast(config_.cpr)); + float elec_rad_per_enc = axis_->motor_.config_.pole_pairs * 2 * M_PI * (1.0f / (float)(config_.cpr)); float ph = elec_rad_per_enc * (interpolated_enc - config_.offset_float); // ph = fmodf(ph, 2*M_PI); phase_ = wrap_pm_pi(ph); diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index fda04cf4..0dcc8754 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -647,7 +647,7 @@ void update_brake_current() { return; } - int high_on = static_cast(TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty)); + int high_on = (int)(TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty)); int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS; if (low_off < 0) low_off = 0; safety_critical_apply_brake_resistor_timings(low_off, high_on); diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index faafb7f5..ac03e863 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -216,7 +216,7 @@ float Motor::phase_current_from_adcval(uint32_t ADCValue) { // TODO check Ibeta balance to verify good motor connection bool Motor::measure_phase_resistance(float test_current, float max_voltage) { static const float kI = 10.0f; // [(V/s)/A] - static const int num_test_cycles = static_cast(3.0f / CURRENT_MEAS_PERIOD); // Test runs for 3s + static const int num_test_cycles = (int)(3.0f / CURRENT_MEAS_PERIOD); // Test runs for 3s float test_voltage = 0.0f; size_t i = 0;