From e1d866f7a21f6c11aed552d76b0d77c7136821e5 Mon Sep 17 00:00:00 2001
From: rickxu2 <rickxu2@illinois.edu>
Date: Sun, 28 Jul 2024 18:30:54 +0800
Subject: [PATCH] 4310 sudden movement bug fix

---
 shared/libraries/motor.cc | 11 +++++++++--
 shared/libraries/motor.h  |  6 ++++--
 shared/libraries/utils.cc |  7 ++++++-
 3 files changed, 19 insertions(+), 5 deletions(-)

diff --git a/shared/libraries/motor.cc b/shared/libraries/motor.cc
index 540bb37b..8b26a409 100644
--- a/shared/libraries/motor.cc
+++ b/shared/libraries/motor.cc
@@ -679,6 +679,13 @@ Motor4310::Motor4310(bsp::CAN* can, uint16_t rx_id, uint16_t tx_id, mode_t mode)
   }
 }
 
+Motor4310::Motor4310(bsp::CAN* can, uint16_t rx_id, uint16_t tx_id, mode_t mode, float p_max)
+    : Motor4310(can, rx_id, tx_id, mode) {
+  can->RegisterRxCallback(rx_id, can_motor_4310_callback, this);
+  P_MAX = p_max;
+  P_MIN = -p_max;
+}
+
 void Motor4310::MotorEnable() {
   uint8_t data[8] = {0};
   data[0] = 0xff;
@@ -748,7 +755,7 @@ void Motor4310::TransmitOutput(Motor4310* motors[], uint8_t num_motors) {
       // converting float to unsigned int before transmitting
       kp_tmp = float_to_uint(motors[i]->kp_set_, KP_MIN, KP_MAX, 12);
       kd_tmp = float_to_uint(motors[i]->kd_set_, KD_MIN, KD_MAX, 12);
-      pos_tmp = float_to_uint(motors[i]->pos_set_, P_MIN, P_MAX, 16);
+      pos_tmp = float_to_uint(motors[i]->pos_set_, motors[i]->P_MIN, motors[i]->P_MAX, 16);
       vel_tmp = float_to_uint(motors[i]->vel_set_, V_MIN, V_MAX, 12);
       torque_tmp = float_to_uint(motors[i]->torque_set_, T_MIN, T_MAX, 12);
       data[0] = pos_tmp >> 8;
@@ -793,7 +800,7 @@ void Motor4310::UpdateData(const uint8_t data[]) {
   raw_mosTemp_ = data[6];
   raw_motorTemp_ = data[7];
 
-  theta_ = uint_to_float(raw_pos_, P_MIN, P_MAX, 16);
+  theta_ = wrap<float>(uint_to_float(raw_pos_, P_MIN, P_MAX, 16), P_MIN, P_MAX);
   omega_ = uint_to_float(raw_vel_, V_MIN, V_MAX, 12);
   torque_ = uint_to_float(raw_torque_, T_MIN, T_MAX, 12);
 
diff --git a/shared/libraries/motor.h b/shared/libraries/motor.h
index 9e9055fd..cfac9cfb 100644
--- a/shared/libraries/motor.h
+++ b/shared/libraries/motor.h
@@ -732,6 +732,8 @@ class Motor4310 {
    */
   Motor4310(bsp::CAN* can, uint16_t rx_id, uint16_t tx_id, mode_t mode);
 
+  Motor4310(bsp::CAN* can, uint16_t rx_id, uint16_t tx_id, mode_t mode, float P_MAX);
+
   /* implements data update callback */
   void UpdateData(const uint8_t data[]);
 
@@ -865,8 +867,8 @@ class Motor4310 {
   constexpr static float KD_MIN = 0;
   constexpr static float KD_MAX = 5;
   // position
-  constexpr static float P_MIN = -PI;
-  constexpr static float P_MAX = PI;
+  float P_MIN = -PI;
+  float P_MAX = PI;
   // velocity
   constexpr static float V_MIN = -45;
   constexpr static float V_MAX = 45;
diff --git a/shared/libraries/utils.cc b/shared/libraries/utils.cc
index d875e26f..9abd03c3 100644
--- a/shared/libraries/utils.cc
+++ b/shared/libraries/utils.cc
@@ -66,8 +66,13 @@ uint16_t float_to_uint(float x, float x_min, float x_max, int bits) {
   return (uint16_t) ((x-offset) * ((float)((1<<bits)-1))/span);
 }
 
+// WARNING: this does not produce the correct result when bits = 16
+// e.g. x_min = -PI and x_max = PI, the result is from -2PI to 0 instead if bits = 16
+// probably has to do with int is 16bit and when the bits param is 16
+
 float uint_to_float(int x_int, float x_min, float x_max, int bits) {
   float span = x_max - x_min;
   float offset = x_min;
-  return ((float)x_int) * span / ((float)((1 << bits) - 1)) + offset;
+  int32_t x_int_longer = 0x0000FFFF & x_int; // zero extend 16 bit to 32 bit
+  return ((float)x_int_longer) * span / ((float)((1 << bits) - 1)) + offset;
 }