Merge pull request #88 from arduino-libraries/limit-angular-velocity

aentinger · web-flow · commit 24c10bf923b2 · 2022-08-24T10:34:54.000+02:00
Add configuration of joint angular velocity and replace the outdated speed API.
diff --git a/examples/Braccio_Record_and_Replay/Braccio_Record_and_Replay.ino b/examples/Braccio_Record_and_Replay/Braccio_Record_and_Replay.ino
@@ -18,8 +18,11 @@ RecordAndReplayApp app;
 
 void setup()
 {
-  if (Braccio.begin(custom_main_menu)) {
+  if (Braccio.begin(custom_main_menu))
+  {
     app.enableButtons();
+    /* Allow greater angular velocity than the default one. */
+    Braccio.setAngularVelocity(45.0f);
   }
 }
 
diff --git a/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino b/examples/Controlling_Manually_Braccio/Controlling_Manually_Braccio.ino
@@ -54,8 +54,6 @@ void setup()
 
   if (Braccio.begin(directionScreen))
   {
-    /* Configure Braccio. */
-    Braccio.speed(speed_grade_t(120)/*MEDIUM*/);
     /* Move to home position. */
     Braccio.moveTo(HOME_POS[0], HOME_POS[1], HOME_POS[2], HOME_POS[3], HOME_POS[4], HOME_POS[5]);
     delay(500);
diff --git a/examples/Platform_Tutorials/projects/p01-moving-braccio/01_aligning_braccio/01_aligning_braccio.ino b/examples/Platform_Tutorials/projects/p01-moving-braccio/01_aligning_braccio/01_aligning_braccio.ino
@@ -18,9 +18,6 @@ void setup() {
   Braccio.begin();
   delay(500); // Waits for the Braccio initialization
   
-  // You can choose the speed beforehand with
-  Braccio.speed(SLOW); // could be FAST or MEDIUM or SLOW
-
   // Send motors initial angle
   gripper.move().to(initialGripper);
   delay(100);
diff --git a/examples/Platform_Tutorials/projects/p01-moving-braccio/02_waving_with_Braccio/02_waving_with_Braccio.ino b/examples/Platform_Tutorials/projects/p01-moving-braccio/02_waving_with_Braccio/02_waving_with_Braccio.ino
@@ -21,9 +21,6 @@ void setup() {
   Braccio.begin();
   delay(500); // Waits for the Braccio initialization
 
-  // You can choose the speed beforehand with
-  Braccio.speed(SLOW); // could be FAST or MEDIUM or SLOW
-
   // Set motors initial angle
   // Should move all the motors at once
   Braccio.moveTo(homePos[0], homePos[1], homePos[2], homePos[3], homePos[4], homePos[5]);
diff --git a/examples/Platform_Tutorials/projects/p01-moving-braccio/03_moving_challenge/03_moving_challenge.ino b/examples/Platform_Tutorials/projects/p01-moving-braccio/03_moving_challenge/03_moving_challenge.ino
@@ -20,9 +20,6 @@ void setup() {
   Braccio.begin();
   delay(500); // Waits for the Braccio initialization
 
-  // You can choose the speed beforehand with
-  Braccio.speed(SLOW); // could be FAST or MEDIUM or SLOW
-
   // Set motors initial angle
   // Should move all the motors at once
   Braccio.moveTo(homePos[0], homePos[1], homePos[2], homePos[3], homePos[4], homePos[5]);
diff --git a/examples/Platform_Tutorials/projects/p02-controlling-braccio-manually/01_Controlling_Manually_Braccio/01_Controlling_Manually_Braccio.ino b/examples/Platform_Tutorials/projects/p02-controlling-braccio-manually/01_Controlling_Manually_Braccio/01_Controlling_Manually_Braccio.ino
@@ -54,8 +54,6 @@ void setup()
 
   if (Braccio.begin(directionScreen))
   {
-    /* Configure Braccio. */
-    Braccio.speed(speed_grade_t(120)/*MEDIUM*/);
     /* Move to home position. */
     Braccio.moveTo(HOME_POS[0], HOME_POS[1], HOME_POS[2], HOME_POS[3], HOME_POS[4], HOME_POS[5]);
     delay(500);
diff --git a/examples/Platform_Tutorials/projects/p02-controlling-braccio-manually/02_Manual_Control_Challenge/02_Manual_Control_Challenge.ino b/examples/Platform_Tutorials/projects/p02-controlling-braccio-manually/02_Manual_Control_Challenge/02_Manual_Control_Challenge.ino
@@ -54,8 +54,6 @@ void setup()
 
   if (Braccio.begin(directionScreen))
   {
-    /* Configure Braccio. */
-    Braccio.speed(speed_grade_t(120)/*MEDIUM*/);
     /* Move to home position. */
     Braccio.moveTo(HOME_POS[0], HOME_POS[1], HOME_POS[2], HOME_POS[3], HOME_POS[4], HOME_POS[5]);
     delay(500);
diff --git a/examples/Platform_Tutorials/projects/p03-record-replay-mode/01_Braccio_Record_and_Replay/01_Braccio_Record_and_Replay.ino b/examples/Platform_Tutorials/projects/p03-record-replay-mode/01_Braccio_Record_and_Replay/01_Braccio_Record_and_Replay.ino
@@ -20,6 +20,8 @@ void setup()
 {
   if (Braccio.begin(custom_main_menu)) {
     app.enableButtons();
+    /* Allow greater angular velocity than the default one. */
+    Braccio.setAngularVelocity(45.0f);
   }
 }
 
diff --git a/examples/Tools/Braccio_Basic/Braccio_Basic.ino b/examples/Tools/Braccio_Basic/Braccio_Basic.ino
@@ -79,15 +79,16 @@ void setup()
 
   Braccio.moveTo(home_position[0], home_position[1], home_position[2], home_position[3], home_position[4], home_position[5]);
   delay(1000);
+  Braccio.setAngularVelocity(45.0f); /* 45 deg/sec */
 }
 
 void loop()
 {
   if (move_joint)
   {
-    Braccio.move(4).to((SmartServoClass::MAX_ANGLE / 2.0f) - 45.0f).in(1s);
-    delay(1000);
-    Braccio.move(4).to((SmartServoClass::MAX_ANGLE / 2.0f) + 45.0f).in(1s);
-    delay(1000);
+    Braccio.move(4).to((SmartServoClass::MAX_ANGLE / 2.0f) - 45.0f);
+    delay(2000);
+    Braccio.move(4).to((SmartServoClass::MAX_ANGLE / 2.0f) + 45.0f);
+    delay(2000);
   }
 }
diff --git a/examples/Tools/Factory_Set_Initial_Servo_Position/Factory_Set_Initial_Servo_Position.ino b/examples/Tools/Factory_Set_Initial_Servo_Position/Factory_Set_Initial_Servo_Position.ino
@@ -85,7 +85,7 @@ bool set_initial_servo_position(int const id, float const target_angle)
   for ( float current_angle = Braccio.get(id).position();
        !isTargetAngleReached(EPSILON) && !isTimeout(start);)
   {
-    Braccio.get(id).move().to(target_angle).in(200ms);
+    Braccio.get(id).move().to(target_angle);
     delay(250);
   
     char msg[64] = {0};
diff --git a/examples/Tools/Test_Motor_Angular_Control/Test_Motor_Angular_Control.ino b/examples/Tools/Test_Motor_Angular_Control/Test_Motor_Angular_Control.ino
@@ -31,13 +31,13 @@ void test_motor(int const id)
   delay(500);
 
   Serial.print("Drive to start . ");
-  Braccio.get(id).move().to(0.0f).in(1s);
+  Braccio.get(id).move().to(0.0f);
   Serial.println("OK.");
   delay(1500);
 
   for (float target_angle = 0.0f; target_angle < SmartServoClass::MAX_ANGLE; target_angle += 1.0f)
   {
-    Braccio.get(id).move().to(target_angle).in(200ms);
+    Braccio.get(id).move().to(target_angle);
     delay(250);
 
     char msg[64] = {0};
diff --git a/src/Braccio++.cpp b/src/Braccio++.cpp
@@ -136,7 +136,8 @@ bool BraccioClass::begin(voidFuncPtr custom_menu, bool const wait_for_all_motor_
     lvgl_defaultMenu();
 
   _servos.begin();
-  _servos.setTime(SLOW);
+  _servos.setMaxTorque(SmartServoClass::TORQUE_MAX);
+  _servos.setAngularVelocity(SmartServoClass::DEFAULT_ANGULAR_VELOCITY_deg_per_sec);
   _servos.setPositionMode(PositionMode::IMMEDIATE);
 
   _motors_connected_thd.start(mbed::callback(this, &BraccioClass::motorConnectedThreadFunc));
diff --git a/src/Braccio++.h b/src/Braccio++.h
@@ -38,16 +38,6 @@
 #include <chrono>
 using namespace std::chrono;
 
-/**************************************************************************************
- * TYPEDEF
- **************************************************************************************/
-
-enum speed_grade_t {
-  FAST = 10,
-  MEDIUM = 100,
-  SLOW = 1000,
-};
-
 /**************************************************************************************
  * FORWARD DECLARATION
  **************************************************************************************/
@@ -83,8 +73,8 @@ class BraccioClass
   inline void setMaxTorque(uint16_t const max_torque) { _servos.setMaxTorque(max_torque); }
   inline void setMaxTorque(int const id, uint16_t const max_torque) { _servos.setMaxTorque(id, max_torque); }
 
-  inline void speed(speed_grade_t const speed_grade) { _servos.setTime(speed_grade); }
-  inline void speed(int const id, speed_grade_t const speed_grade) { _servos.setTime(id, speed_grade); }
+  inline void  setAngularVelocity(float const angular_velocity_deg_per_sec) { _servos.setAngularVelocity(angular_velocity_deg_per_sec); }
+  inline float getAngularVelocity() const { return _servos.getAngularVelocity(); }
 
   inline void disengage(int const id = SmartServoClass::BROADCAST) { _servos.disengage(id); }
   inline void engage   (int const id = SmartServoClass::BROADCAST) { _servos.engage(id); }
@@ -189,15 +179,12 @@ class Servo
   inline bool engaged()   { return _servos.isEngaged(_id); }
   inline void setMaxTorque(uint16_t const max_torque) { _servos.setMaxTorque(_id, max_torque); }
 
-  inline Servo & move()                                    { return *this; }
-  inline Servo & to  (float const angle)                   { _servos.setPosition(_id, angle); return *this; }
-  inline Servo & in  (std::chrono::milliseconds const len) { _servos.setTime(_id, len.count()); return *this; }
+  inline Servo & move()                  { return *this; }
+  inline Servo & to  (float const angle) { _servos.setPosition(_id, angle); return *this; }
 
   inline float position()            { return _servos.getPosition(_id); }
   inline bool  connected()           { return Braccio.connected(_id); }
   inline void  info(Stream & stream) { _servos.getInfo(stream, _id); }
-  inline uint16_t runtime() { return _servos.getTime(_id); }
-  inline void     setRuntime(uint16_t const time) { _servos.setTime(_id, time); }
 
   operator bool() { return connected(); }
 
diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp
@@ -31,6 +31,7 @@
 SmartServoClass::SmartServoClass(RS485Class & RS485)
 : _RS485{RS485}
 , _errors{0}
+, _angular_velocity_deg_per_sec{DEFAULT_ANGULAR_VELOCITY_deg_per_sec}
 , _onError{}
 , _mtx{}
 {
@@ -198,27 +199,43 @@ void SmartServoClass::begin()
     _targetSpeed[idToArrayIndex(i)] = 1000;
 }
 
-void SmartServoClass::setPosition(uint8_t const id, float const angle)
+void SmartServoClass::setPosition(uint8_t const id, float const angle_deg)
 {
-  if (!isValidAngle(angle))
+  if (!isValidAngle(angle_deg))
     return;
 
-  mbed::ScopedLock<rtos::Mutex> lock(_mtx);
-  if (isValidId(id))
+  if (!isValidId(id))
+    return;
+
+  float const target_position_deg = angle_deg;
+  float const actual_position_deg = getPosition(id);
+  if (actual_position_deg < 0.0f)
+    return;
+
+  float    const abs_position_diff_deg = fabs(target_position_deg - actual_position_deg);
+  float    const limited_runtime_sec   = abs_position_diff_deg / _angular_velocity_deg_per_sec;
+  uint16_t const limited_runtime_ms    = static_cast<uint16_t>(limited_runtime_sec * 1000.f);
+
+  if (_positionMode == PositionMode::IMMEDIATE)
   {
-    _targetPosition[idToArrayIndex(id)] = angleToPosition(angle);
-    if (_positionMode==PositionMode::IMMEDIATE) {
-      writeWordCmd(id, REG(SmartServoRegister::TARGET_POSITION_H), angleToPosition(angle));
-    }
-  }  
+    mbed::ScopedLock<rtos::Mutex> lock(_mtx);
+    writeWordCmd(id, REG(SmartServoRegister::RUN_TIME_H), limited_runtime_ms);
+    writeWordCmd(id, REG(SmartServoRegister::TARGET_POSITION_H), angleToPosition(target_position_deg));
+  }
+  else if (_positionMode == PositionMode::SYNC)
+  {
+    _targetSpeed[idToArrayIndex(id)] = limited_runtime_ms;
+    _targetPosition[idToArrayIndex(id)] = angleToPosition(target_position_deg);
+  }
 }
 
 float SmartServoClass::getPosition(uint8_t const id)
 {
+  if (!isValidId(id))
+    return -1.0f;
+
   mbed::ScopedLock<rtos::Mutex> lock(_mtx);
-  float ret = -1;
-  if (isValidId(id))
-    return positionToAngle(readWordCmd(id, REG(SmartServoRegister::POSITION_H)));
+  return positionToAngle(readWordCmd(id, REG(SmartServoRegister::POSITION_H)));
 }
 
 void SmartServoClass::center(uint8_t const id, uint16_t const position)
@@ -261,16 +278,24 @@ void SmartServoClass::setTorque(uint8_t const id, bool const torque)
 
 void SmartServoClass::setTime(uint16_t const time)
 {
-  mbed::ScopedLock<rtos::Mutex> lock(_mtx);
   for (int i = MIN_MOTOR_ID; i <= MAX_MOTOR_ID; i++)
     _targetSpeed[idToArrayIndex(i)] = time;
+
+  mbed::ScopedLock<rtos::Mutex> lock(_mtx);
   writeWordCmd(BROADCAST, REG(SmartServoRegister::RUN_TIME_H), time);
 }
 
 void SmartServoClass::setTime(uint8_t const id, uint16_t const time)
 {
+  if (!isValidId(id))
+    return;
+
+  if (id == BROADCAST)
+    return;
+
+  _targetSpeed[idToArrayIndex(id)] = time;
+
   mbed::ScopedLock<rtos::Mutex> lock(_mtx);
-  if ((id >= MIN_MOTOR_ID) && (id <= MAX_MOTOR_ID)) _targetSpeed[idToArrayIndex(id)] = time;
   writeWordCmd(id, REG(SmartServoRegister::RUN_TIME_H), time);
 }
 
diff --git a/src/lib/motors/SmartServo.h b/src/lib/motors/SmartServo.h
@@ -46,14 +46,15 @@ class SmartServoClass
 
   inline void setPositionMode(PositionMode const mode) { _positionMode = mode; }
 
-  void setPosition(uint8_t const id, float const angle);
-
+  void  setPosition(uint8_t const id, float const angle_deg);
   float getPosition(uint8_t const id);
 
+  inline void  setAngularVelocity(float const angular_velocity_deg_per_sec) { _angular_velocity_deg_per_sec = angular_velocity_deg_per_sec; }
+  inline float getAngularVelocity() const { return _angular_velocity_deg_per_sec; }
+
   void synchronize();
 
   void setTorque(bool const torque);
-
   void setTorque(uint8_t const id, bool const torque);
 
          void setMaxTorque(uint8_t const id, uint16_t const max_torque);
@@ -97,6 +98,7 @@ class SmartServoClass
   static int   constexpr MAX_MOTOR_ID = 6;
   static int   constexpr NUM_MOTORS = 6;
   static float constexpr MAX_ANGLE = 315.0f;
+  static float constexpr DEFAULT_ANGULAR_VELOCITY_deg_per_sec = 20.0f;
 
   static uint16_t constexpr TORQUE_MIN =    0;
   static uint16_t constexpr TORQUE_MAX = 1000;
@@ -129,6 +131,7 @@ class SmartServoClass
 
   RS485Class& _RS485;
   int _errors;
+  float _angular_velocity_deg_per_sec;
   mbed::Callback<void()> _onError;
   rtos::Mutex _mtx;
 

Original file line number	Diff line number	Diff line change
`@@ -18,8 +18,11 @@ RecordAndReplayApp app;`
`18`	`18`
`19`	`19`	`void setup()`
`20`	`20`	`{`
`21`		`- if (Braccio.begin(custom_main_menu)) {`
	`21`	`+ if (Braccio.begin(custom_main_menu))`
	`22`	`+ {`
`22`	`23`	`app.enableButtons();`
	`24`	`+ /* Allow greater angular velocity than the default one. */`
	`25`	`+ Braccio.setAngularVelocity(45.0f);`
`23`	`26`	`}`
`24`	`27`	`}`
`25`	`28`
Original file line number	Diff line number	Diff line change
`@@ -54,8 +54,6 @@ void setup()`
`54`	`54`
`55`	`55`	`if (Braccio.begin(directionScreen))`
`56`	`56`	`{`
`57`		`- /* Configure Braccio. */`
`58`		`- Braccio.speed(speed_grade_t(120)/MEDIUM/);`
`59`	`57`	`/* Move to home position. */`
`60`	`58`	`Braccio.moveTo(HOME_POS[0], HOME_POS[1], HOME_POS[2], HOME_POS[3], HOME_POS[4], HOME_POS[5]);`
`61`	`59`	`delay(500);`
Original file line number	Diff line number	Diff line change
`@@ -20,6 +20,8 @@ void setup()`
`20`	`20`	`{`
`21`	`21`	`if (Braccio.begin(custom_main_menu)) {`
`22`	`22`	`app.enableButtons();`
	`23`	`+ /* Allow greater angular velocity than the default one. */`
	`24`	`+ Braccio.setAngularVelocity(45.0f);`
`23`	`25`	`}`
`24`	`26`	`}`
`25`	`27`
Original file line number	Diff line number	Diff line change
`@@ -79,15 +79,16 @@ void setup()`
`79`	`79`
`80`	`80`	`Braccio.moveTo(home_position[0], home_position[1], home_position[2], home_position[3], home_position[4], home_position[5]);`
`81`	`81`	`delay(1000);`
	`82`	`+ Braccio.setAngularVelocity(45.0f); /* 45 deg/sec */`
`82`	`83`	`}`
`83`	`84`
`84`	`85`	`void loop()`
`85`	`86`	`{`
`86`	`87`	`if (move_joint)`
`87`	`88`	`{`
`88`		`- Braccio.move(4).to((SmartServoClass::MAX_ANGLE / 2.0f) - 45.0f).in(1s);`
`89`		`- delay(1000);`
`90`		`- Braccio.move(4).to((SmartServoClass::MAX_ANGLE / 2.0f) + 45.0f).in(1s);`
`91`		`- delay(1000);`
	`89`	`+ Braccio.move(4).to((SmartServoClass::MAX_ANGLE / 2.0f) - 45.0f);`
	`90`	`+ delay(2000);`
	`91`	`+ Braccio.move(4).to((SmartServoClass::MAX_ANGLE / 2.0f) + 45.0f);`
	`92`	`+ delay(2000);`
`92`	`93`	`}`
`93`	`94`	`}`
Original file line number	Diff line number	Diff line change
`@@ -85,7 +85,7 @@ bool set_initial_servo_position(int const id, float const target_angle)`
`85`	`85`	`for ( float current_angle = Braccio.get(id).position();`
`86`	`86`	`!isTargetAngleReached(EPSILON) && !isTimeout(start);)`
`87`	`87`	`{`
`88`		`- Braccio.get(id).move().to(target_angle).in(200ms);`
	`88`	`+ Braccio.get(id).move().to(target_angle);`
`89`	`89`	`delay(250);`
`90`	`90`
`91`	`91`	`char msg[64] = {0};`