Update theta angle calib and gripper tuning

Author: PCrnjak

Spectral BLDC Firmware/src/EEPROM.cpp

@@ -169,6 +169,8 @@ void read_config()
   controller.Max_temperature =   readInt(TEMPERATURE_ERROR);
   controller.Max_Vbus = readInt(VOLTAGE_ERROR);
   PID.Voltage_limit = readInt(VOLTAGE_LIMIT);
+  controller.theta_offset = readFloat(THETA_OFFSET);
+
 }
 
 /// @brief Clean the config that is saved in the EEPROM
@@ -228,19 +230,20 @@ void Write_config()
 
   writeInt(TEMPERATURE_ERROR,controller.Max_temperature);
   writeInt(VOLTAGE_ERROR,controller.Max_Vbus);
+  writeFloat(THETA_OFFSET, controller.theta_offset);
   writeInt(VOLTAGE_LIMIT,PID.Voltage_limit);
 
 }
 
 /// @brief  Reset to default config.  First write to EEPROM then read it back.
 void Set_Default_config(){
 
-  writeInt(SERIAL_NUMBER_EEPROM,1);
+  writeInt(SERIAL_NUMBER_EEPROM,1111);
   writeInt(HARDWARE_VERSION_EEPROM, 1);
-  writeInt(BATCH_DATA_EEPROM, 2732024);
+  writeInt(BATCH_DATA_EEPROM, 24092024);
 
   writeInt(CAN_ID_EEPROM, 0);
-  writeInt(SOFTWARE_VERSION_EEPROM, 100);
+  writeInt(SOFTWARE_VERSION_EEPROM, 101);
   writeInt(LED_ON_OFF_EEPROM, 1);
   writeInt(THERMISTOR_ON_OFF_EEPROM, 0);
   writeInt(POLE_PAIR, 7);
@@ -280,6 +283,7 @@ void Set_Default_config(){
 
   writeInt(TEMPERATURE_ERROR,75);
   writeInt(VOLTAGE_ERROR,29000);
+  writeFloat(THETA_OFFSET, 0);
   writeInt(VOLTAGE_LIMIT,0);
 
   read_config();

Spectral BLDC Firmware/src/SerialPacketParser.cpp

@@ -366,6 +366,18 @@ bool SerialPacketParser::parse(char c, char *command, char *argument)
         {
           return true;
         }
+        else if (strcmp(command, "Calangle") == 0)
+        {
+          return true;
+        }
+        else if (strcmp(command, "Offset") == 0)
+        {
+          return true;
+        }
+        else if (strcmp(command, "Iqoffset") == 0)
+        {
+          return true;
+        }
 
         else
         {
@@ -701,6 +713,18 @@ bool SerialPacketParser::parse(char c, char *command, char *argument)
         {
           return true;
         }
+        else if (strcmp(command, "Calangle") == 0)
+        {
+          return true;
+        }
+        else if (strcmp(command, "Offset") == 0)
+        {
+          return true;
+        }
+        else if (strcmp(command, "Iqoffset") == 0)
+        {
+          return true;
+        }
 
         else
         {

Spectral BLDC Firmware/src/common.h

@@ -20,7 +20,6 @@
 /* USER CODE END Header */
 /* Includes ------------------------------------------------------------------*/
 
-
 #ifndef COMMON_H
 #define COMMON_H
 
@@ -57,6 +56,8 @@ typedef struct
     volatile int Old_Position_Ticks;                // position with multiple rotations from previous cycle
     volatile float Position_RAD;                    // Raw position but in radians
     volatile float Electric_Angle;                  // Electrical angle - depends on number of pole pairs
+    volatile float theta_offset = 0.0f; // will be determined at calibration
+
     volatile int ROTATIONS;                         // Number of full rotations that motor did
     volatile int Velocity_Filter;                   // Filtered velocity of the motor
     volatile int Velocity;                          //  velocity of the motor
@@ -88,7 +89,7 @@ typedef struct
     volatile float flux_linkage = 0;     // Flux linkage of your BLDC motor
     volatile float KV = 0;
 
-    volatile int Max_temperature = 75; // Max temperature in degrees
+    volatile int Max_temperature = 75;  // Max temperature in degrees
     volatile int Min_temperature = -20; // Min temperature in degrees
 
     volatile int Max_Vbus = 29500; // Max vbus voltage in mV
@@ -98,9 +99,9 @@ typedef struct
 
     volatile int watchdog_time_ms = 0;
     volatile bool watchdog_reset = 1;
-    volatile int watchdog_action = 0; // Hold position, Idle, brake
+    volatile int watchdog_action = 0;   // Hold position, Idle, brake
     volatile int Heartbeat_rate_ms = 0; // Motor controller will send data every Heartbeat_rate_ms
-    volatile bool Send_heartbeat = 0; 
+    volatile bool Send_heartbeat = 0;
 
     volatile bool Activated = 0;         // If controller is activated or not
     volatile bool Error = 0;             // General error
@@ -123,7 +124,7 @@ typedef struct
     volatile int Open_loop_voltage = 5000;           // Voltage to use when spinning the motor open loop. In mV
     volatile float Open_loop_speed = 20.0;           // Speed to be used when spinning the motor open loop. In RAD/s
     volatile int Number_of_rotations = 8;            // Number of open loop electrical rotations during calibration
-    volatile int Phase_voltage = 5000;                 // Max voltage to be used during Kv calculation
+    volatile int Phase_voltage = 5000;               // Max voltage to be used during Kv calculation
 
     // Calibration steps
     // 0 is neutral, 1 is error, 2 is success
@@ -156,6 +157,27 @@ typedef struct
 
     volatile bool handle_can = 0;
 
+    volatile bool hall_trigger = 1; // used to record if hall sensor was triggered or not, normal state of hall is high (1)
+    volatile bool trigger_value = 0;
+
+    // temp / testing ?
+    volatile int calibration_offset_current = 300;
+    volatile int temp_var_offset_calib = 0;
+    volatile int align_state = 0;
+    volatile int align_counter = 0;
+    volatile float aligned_angle = 0;
+    volatile bool align_done = false;
+    volatile bool calib_done = false;
+    volatile bool calib_reset = false;
+    volatile int Velocity_fwd = 0;
+    volatile int Velocity_bwd = 0;
+
+    // Open loop movements
+    volatile int microstep = 16;
+    volatile int gosteps = 0;
+    volatile int step_interval = 1000;    // step interval in microseconds
+    volatile int open_loop_move_done = 0; // 1 = finished, cleared by user
+
 } Measure;
 
 extern Measure controller;
@@ -211,11 +233,10 @@ extern _FOC FOC;
 typedef struct
 {
 
-
     volatile int Voltage_limit = 0; // Voltage limit for Ud and Uq voltages. If 0 value of controller.VBUS_mV will be used
     // if != 0 it will use that number (if bigger than controller.VBUS_mV it will use controller.VBUS_mV)
     // This is extremely important for proper operation of BLDC motor loops since their resistances can be less
-    // than 1 ohm and using full range of 24V makes the loop unstable. 
+    // than 1 ohm and using full range of 24V makes the loop unstable.
     // Usually it setting this to 0 is good (so using full controller.VBUS_mV range)
     // Use smaller numbers if you have really small phase resistances like 1 ohm or less
 
@@ -228,25 +249,25 @@ typedef struct
     volatile float P_errSum = 0;
 
     // Speed loop PID
-    volatile float Kp_v = 0.03;  //   0.006 basic values that work for most motors but not optimal
+    volatile float Kp_v = 0.03;   //   0.006 basic values that work for most motors but not optimal
     volatile float Ki_v = 0.0003; //   0.0001 basic values that work for most motors but not optimal
     volatile float V_errSum = 0;
     volatile float Feedforward_speed = 0;
     volatile float Velocity_setpoint = 0; // -1.5
 
-    volatile float Velocity_limit = 80000;        // Clamp integrals to this  [TICKS/s]
+    volatile float Velocity_limit = 80000;          // Clamp integrals to this  [TICKS/s]
     volatile float Velocity_limit_error = 20000000; // [TICKS/s] ; Velocity when we will report error
 
     // current loop PID
     volatile float Ki = 0; // zero location
 
-    volatile float Kp_id = 2.67;   // 2.67
+    volatile float Kp_id = 2.67; // 2.67
     volatile float Ki_id = 1.93; // 1.93
     volatile float Id_errSum;
     volatile float Id_setpoint = 0;
 
     volatile float Kp_iq = 2.67; // 2.67 // 12.8
-    volatile float Ki_iq = 1.93; // 1.93 // 9 
+    volatile float Ki_iq = 1.93; // 1.93 // 9
     volatile float Iq_errSum;
     volatile float Iq_setpoint;
 
@@ -263,16 +284,15 @@ typedef struct
     volatile int Uq_setpoint = 0;
     volatile int Ud_setpoint = 0;
 
-
 } PID_par;
 
 extern PID_par PID;
 
 typedef struct
 {
 
-    volatile uint8_t previousBytes[5] = {0,0,0,0,0};
-    volatile bool Same_command = 0; 
+    volatile uint8_t previousBytes[5] = {0, 0, 0, 0, 0};
+    volatile bool Same_command = 0;
     volatile bool At_position = 0;
 
     volatile uint8_t position_setpoint = 0;
@@ -288,11 +308,11 @@ typedef struct
     volatile bool activated = 0;
 
     // 0 in motion, 1 object detected closing, 2 object detected opening, 3 at positon
-    volatile int object_detection_status = 3; 
+    volatile int object_detection_status = 3;
     volatile bool temperature_error = 0;
     volatile bool timeout_error = 0;
     volatile bool estop_error = 0;
-    
+
     volatile bool release_direction = 0;
     volatile bool estop_status = 0;
 
@@ -302,7 +322,7 @@ typedef struct
 
     volatile int position_ticks = 0;
 
-    // Map this to 255 and 0 
+    // Map this to 255 and 0
     volatile int max_speed = 80000;
     volatile int min_speed = 40;