Implemented software endstops

BetzDrive · Jan 11, 2018 · f8d7804 · f8d7804
1 parent 6a3ee4f
commit f8d7804
Show file tree

Hide file tree

Showing 4 changed files with 48 additions and 3 deletions.
diff --git a/firmware/include/state.h b/firmware/include/state.h
@@ -51,6 +51,9 @@ struct Calibration {
   float foc_ki_d;               // Integral gain for FOC/d PI loop
   float foc_kp_q;               // Proportional gain for FOC/q PI loop
   float foc_ki_q;               // Integral gain for FOC/q PI loop
+  float sw_endstop_min;         // Software endstop minimum angle
+  float sw_endstop_max;         // Software endstop maximum angle
+  float sw_endstop_slope;       // Software endstop torque slope
 
   Calibration()
     : encoder_zero(0),
@@ -60,7 +63,10 @@ struct Calibration {
       foc_kp_d(0.01f),
       foc_ki_d(100.0f),
       foc_kp_q(0.01f),
-      foc_ki_q(100.0f)
+      foc_ki_q(100.0f),
+      sw_endstop_min(0.0f),
+      sw_endstop_max(0.0f),
+      sw_endstop_slope(20.0f)
   {}
 };
 

diff --git a/firmware/src/control.cpp b/firmware/src/control.cpp
@@ -162,8 +162,26 @@ void runCurrentControl() {
     // float vd = pid_id.compute();
     // float vq = pid_iq.compute();
 
+    float torque_command = parameters.cmd_duty_cycle;
+
+    if (calibration.sw_endstop_min < calibration.sw_endstop_max) {
+      // Software endstops are only active if they have different values
+
+      if (torque_command >= 0) {
+        // Positive torque command, only check the maximum endstop
+
+        float torque_limit = std::max(0.0f, (calibration.sw_endstop_max - results.encoder_radian_angle) * calibration.sw_endstop_slope);
+        torque_command = std::min(torque_command, torque_limit);
+      } else {
+        // Negative torque command, only check the minimum endstop
+
+        float torque_limit = std::min(0.0f, (calibration.sw_endstop_min - results.encoder_radian_angle) * calibration.sw_endstop_slope);
+        torque_command = std::max(torque_command, torque_limit);
+      }
+    }
+
     float vd = -2.0 * id;
-    float vq = -2.0 * (iq - parameters.cmd_duty_cycle) + parameters.cmd_duty_cycle * calibration.winding_resistance;
+    float vq = -2.0 * (iq - torque_command) + torque_command * calibration.winding_resistance;
 
     float vd_norm = vd / results.average_vin;
     float vq_norm = vq / results.average_vin;

diff --git a/firmware/src/fw_comms.cpp b/firmware/src/fw_comms.cpp
@@ -69,6 +69,15 @@ void commsRegAccessHandler(comm_addr_t start_addr, size_t reg_count, uint8_t *bu
       case 0x010f: //271
         handleVarAccess(results.xl_z, buf, index, buf_size, access_type, errors);
         break;
+      case 0x0110: //272
+        handleVarAccess(calibration.sw_endstop_min, buf, index, buf_len, access_type, errors);
+        break;
+      case 0x0111: //273
+        handleVarAccess(calibration.sw_endstop_max, buf, index, buf_len, access_type, errors);
+        break;
+      case 0x0112: //274
+        handleVarAccess(calibration.sw_endstop_slope, buf, index, buf_len, access_type, errors);
+        break;
       case 0x0200:
         handleVarAccess(results.average_ia, buf, index, buf_size, access_type, errors);
         break;

diff --git a/tools/duty_cycle.py b/tools/duty_cycle.py
@@ -28,14 +28,26 @@
 has_21_erevs_per_mrev = [18, 19, 20, 21]
 
 client.writeRegisters(address, 0x0101, 1, struct.pack('<H', angle_mapping[address]) )
-client.writeRegisters(address, 0x0106, 1, struct.pack('<f', duty_cycle) )
 client.writeRegisters(address, 0x0102, 1, struct.pack('<B', 0) )
 client.writeRegisters(address, 0x0109, 1, struct.pack('<B', int(address in needs_flip_phase)) )
 try:
     client.writeRegisters(address, 0x010a, 1, struct.pack('<B', 21 if (address in has_21_erevs_per_mrev) else 14))
 except:
     print "WARNING: Motor driver board does not support erevs_per_mrev, try updating the firmware."
 
+start_angle = struct.unpack('<f', client.readRegisters(address, 0x010b, 1))[0]
+client.writeRegisters(address, 0x0110, 1, struct.pack('<f', start_angle - 0.5))
+client.writeRegisters(address, 0x0111, 1, struct.pack('<f', start_angle))
+client.writeRegisters(address, 0x0112, 1, struct.pack('<f', 20.0))
+
+client.writeRegisters(address, 0x0106, 1, struct.pack('<f', duty_cycle))
+
+# while True:
+#     client.writeRegisters(address, 0x0106, 1, struct.pack('<f', duty_cycle))
+#     time.sleep(5)
+#     client.writeRegisters(address, 0x0106, 1, struct.pack('<f', -duty_cycle))
+#     time.sleep(5)
+
 # while True:
 #     try:
 #         # adc_averages = struct.unpack('<7f', client.readRegisters(address, 0x0200, 7))