Set current in amps

diff --git a/avr/src/config.h b/avr/src/config.h
index ac2df069f279398ff1535530948b0e9e8883ca57..e71bf5a14031106daf0ce8d084ac82985eb3fd41 100644 (file)
--- a/avr/src/config.h
+++ b/avr/src/config.h
@@ -238,7 +238,9 @@ enum {
 #define JOG_JERK_MULT            1             // Jogging jerk multipler
 #define JOG_MIN_VELOCITY         10            // mm/min
 #define CAL_ACCELERATION         500000        // mm/min^2
-
+#define CURRENT_SENSE_RESISTOR   0.05          // ohms
+#define CURRENT_SENSE_REF        2.75          // volts
+#define MAX_CURRENT              10            // amps
 
 // Arc
 #define ARC_RADIUS_ERROR_MAX   1.0   // max mm diff between start and end radius

diff --git a/avr/src/drv8711.c b/avr/src/drv8711.c
index 6f6e7b5042371a8c2c59b09494dfc3c60f4a2c81..18cd95d9486ce55611bbcf198de2e91f06fc3293 100644 (file)
--- a/avr/src/drv8711.c
+++ b/avr/src/drv8711.c
@@ -48,16 +48,21 @@
 bool motor_fault = false;
 
 
+typedef struct {
+  float current;
+  uint16_t isgain;
+  uint8_t torque;
+} current_t;
+
+
 typedef struct {
   uint8_t status;
   uint16_t flags;
 
   drv8711_state_t state;
-  float idle_current;
-  float max_current;
-  float min_current;
+  current_t drive;
+  current_t idle;
   float stall_threshold;
-  float power;
 
   uint8_t mode; // microstepping mode
   stall_callback_t stall_cb;
@@ -92,16 +97,67 @@ typedef struct {
 static spi_t spi = {0};
 
 
+static void _current_set(current_t *c, float current) {
+  c->current = current;
+
+  float torque_over_gain = current * CURRENT_SENSE_RESISTOR / CURRENT_SENSE_REF;
+
+  float gain = 0;
+  if (torque_over_gain < 1.0 / 40) {
+    c->isgain = DRV8711_CTRL_ISGAIN_40;
+    gain = 40;
+
+  } else if (torque_over_gain < 1.0 / 20) {
+    c->isgain = DRV8711_CTRL_ISGAIN_20;
+    gain = 20;
+
+  } else if (torque_over_gain < 1.0 / 10) {
+    c->isgain = DRV8711_CTRL_ISGAIN_10;
+    gain = 10;
+
+  } else if (torque_over_gain < 1.0 / 5) {
+    c->isgain = DRV8711_CTRL_ISGAIN_5;
+    gain = 5;
+  }
+
+  c->torque = round(torque_over_gain * gain * 256);
+}
+
+
+static bool _driver_get_enabled(int driver) {
+  drv8711_state_t state = drivers[driver].state;
+  return state == DRV8711_IDLE || state == DRV8711_ACTIVE;
+}
+
+
 static float _driver_get_current(int driver) {
   drv8711_driver_t *drv = &drivers[driver];
 
   switch (drv->state) {
-  case DRV8711_IDLE: return drv->idle_current;
+  case DRV8711_IDLE: return drv->idle.current;
+  case DRV8711_ACTIVE: return drv->drive.current;
+  default: return 0; // Off
+  }
+}
+
+
+static uint16_t _driver_get_isgain(int driver) {
+  drv8711_driver_t *drv = &drivers[driver];
+
+  switch (drv->state) {
+  case DRV8711_IDLE: return drv->idle.isgain;
+  case DRV8711_ACTIVE: return drv->drive.isgain;
+  default: return 0; // Off
+  }
+}
 
-  case DRV8711_ACTIVE:
-    return drv->min_current +
-      (drv->max_current - drv->min_current) * drv->power;
 
+static uint8_t _driver_get_torque(int driver) {
+  drv8711_driver_t *drv = &drivers[driver];
+
+  switch (drv->state) {
+  case DRV8711_IDLE: return drv->idle.torque;
+  case DRV8711_ACTIVE: return drv->drive.torque;
   default: return 0; // Off
   }
 }
@@ -151,13 +207,13 @@ static uint8_t _spi_next_command(uint8_t cmd) {
       break;
 
     case DRV8711_TORQUE_REG: // Update motor current setting
-      *command = (*command & 0xff00) |
-        (uint8_t)round(0xff * _driver_get_current(driver));
+      *command = (*command & 0xff00) | _driver_get_torque(driver);
       break;
 
     case DRV8711_CTRL_REG: // Set microsteps
-      *command = (*command & 0xff86) | (drv->mode << 3) |
-        (_driver_get_current(driver) ? DRV8711_CTRL_ENBL_bm : 0);
+      *command = (*command & 0xfc86) | _driver_get_isgain(driver) |
+        (drv->mode << 3) |
+        (_driver_get_enabled(driver) ? DRV8711_CTRL_ENBL_bm : 0);
       break;
 
     default: break;
@@ -310,12 +366,6 @@ void drv8711_set_state(int driver, drv8711_state_t state) {
 }
 
 
-void drv8711_set_power(int driver, float power) {
-  if (driver < 0 || DRIVERS <= driver) return;
-  drivers[driver].power = power < 0 ? 0 : (1 < power ? 1 : power);
-}
-
-
 void drv8711_set_microsteps(int driver, uint16_t msteps) {
   if (driver < 0 || DRIVERS <= driver) return;
   switch (msteps) {
@@ -333,39 +383,30 @@ void drv8711_set_stall_callback(int driver, stall_callback_t cb) {
 }
 
 
-float get_max_current(int driver) {
-  if (driver < 0 || DRIVERS <= driver) return 0;
-  return drivers[driver].max_current;
-}
-
-
-void set_max_current(int driver, float value) {
-  if (driver < 0 || DRIVERS <= driver || value < 0 || 1 < value) return;
-  drivers[driver].max_current = value;
-}
-
-
-float get_min_current(int driver) {
+float get_drive_current(int driver) {
   if (driver < 0 || DRIVERS <= driver) return 0;
-  return drivers[driver].min_current;
+  return drivers[driver].drive.current;
 }
 
 
-void set_min_current(int driver, float value) {
-  if (driver < 0 || DRIVERS <= driver || value < 0 || 1 < value) return;
-  drivers[driver].min_current = value;
+void set_drive_current(int driver, float value) {
+  if (driver < 0 || DRIVERS <= driver || value < 0 || MAX_CURRENT < value)
+    return;
+  _current_set(&drivers[driver].drive, value);
 }
 
 
 float get_idle_current(int driver) {
   if (driver < 0 || DRIVERS <= driver) return 0;
-  return drivers[driver].idle_current;
+  return drivers[driver].idle.current;
 }
 
 
 void set_idle_current(int driver, float value) {
-  if (driver < 0 || DRIVERS <= driver || value < 0 || 1 < value) return;
-  drivers[driver].idle_current = value;
+  if (driver < 0 || DRIVERS <= driver || value < 0 || MAX_CURRENT < value)
+    return;
+
+  _current_set(&drivers[driver].idle, value);
 }
 
 

diff --git a/avr/src/drv8711.h b/avr/src/drv8711.h
index e559ac9dab7957faf4f56825bdd13bc5d3795659..b077114ef2fb8d137954477aa7307cb0e22b2970 100644 (file)
--- a/avr/src/drv8711.h
+++ b/avr/src/drv8711.h
@@ -172,6 +172,5 @@ typedef enum {
 void drv8711_init();
 drv8711_state_t drv8711_get_state(int driver);
 void drv8711_set_state(int driver, drv8711_state_t state);
-void drv8711_set_power(int driver, float power);
 void drv8711_set_microsteps(int driver, uint16_t msteps);
 void drv8711_set_stall_callback(int driver, stall_callback_t cb);

diff --git a/avr/src/motor.c b/avr/src/motor.c
index 3a08b7af2d7d18a8c4b702f585cabaa9b9261031..901139a126e66f10db21092857c0a45a21e1f3a3 100644 (file)
--- a/avr/src/motor.c
+++ b/avr/src/motor.c
@@ -376,11 +376,6 @@ void motor_prep_move(int motor, float time, int32_t target) {
 
   if (!m->timer_period || !half_steps) m->timer_clock = 0;
 
-  // Compute power from axis max velocity
-  const float max_step_vel = m->steps_per_unit * axis_get_velocity_max(m->axis);
-  const float power = half_steps / (max_step_vel * time * 2);
-  drv8711_set_power(motor, power);
-
   // Power motor
   switch (m->power_mode) {
   case MOTOR_POWERED_ONLY_WHEN_MOVING:

diff --git a/avr/src/vars.def b/avr/src/vars.def
index bbdf3e7fbb8ce49d1e559948f3e081b22440d4c2..09a059eff84c5d2b54d459f8dc3acec1fdbcf8be 100644 (file)
--- a/avr/src/vars.def
+++ b/avr/src/vars.def
@@ -38,8 +38,7 @@ VAR(microstep,      mi, uint16_t, MOTORS, 1, "Microsteps per full step")
 VAR(reverse,        rv, uint8_t,  MOTORS, 1, "Reverse motor polarity")
 
 VAR(power_mode,     pm, uint8_t,  MOTORS, 1, "Motor power mode")
-VAR(max_current,    xc, float,    MOTORS, 1, "Max motor drive current")
-VAR(min_current,    lc, float,    MOTORS, 1, "Min motor drive current")
+VAR(drive_current,  dc, float,    MOTORS, 1, "Max motor drive current")
 VAR(idle_current,   ic, float,    MOTORS, 1, "Motor idle current")
 VAR(active_current, ac, float,    MOTORS, 0, "Motor current now")
 VAR(driver_flags,   df, uint16_t, MOTORS, 0, "Motor driver status flags")