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 index ba9dab4..52e5c9c 100644 --- a/examples/Tools/Test_Motor_Angular_Control/Test_Motor_Angular_Control.ino +++ b/examples/Tools/Test_Motor_Angular_Control/Test_Motor_Angular_Control.ino @@ -3,18 +3,59 @@ * angle for each smart servo motor. */ +/************************************************************************************** + * INCLUDE + **************************************************************************************/ + #include +/************************************************************************************** + * FUNCTIONS + **************************************************************************************/ + +void test_motor(int const id) +{ + Serial.print("Connecting ..... "); + for (; !Braccio.get(id).connected(); delay(10)) { } + Serial.println("OK."); + delay(500); + + Serial.print("Disengaging .... "); + Braccio.get(id).disengage(); + Serial.println("OK."); + delay(500); + + Serial.print("Engaging ....... "); + Braccio.get(id).engage(); + Serial.println("OK."); + delay(500); + + Serial.print("Drive to start . "); + Braccio.get(id).move().to(0.0f).in(1s); + 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); + delay(250); + + char msg[64] = {0}; + snprintf(msg, sizeof(msg), "Angle (Target | Current) = %.2f | %.2f", target_angle, Braccio.get(id).position()); + Serial.println(msg); + } +} + +/************************************************************************************** + * SETUP/LOOP + **************************************************************************************/ + void setup() { Serial.begin(115200); while(!Serial){} - /* Call Braccio.begin() for default menu - * or pass a function for custom menu. - */ Braccio.begin(); - Serial.println("Testing motor angular movement!"); } @@ -24,25 +65,13 @@ void loop() Serial.println(">> "); while((Serial.available() <= 0)) { } - int const selected_motor = Serial.parseInt(); + int const motor_id = Serial.parseInt(); while(Serial.read() != '\n') { } - if (selected_motor < 1 || selected_motor > 6) { + if (motor_id < 1 || motor_id > 6) { Serial.println("Error, wrong motor id, choose motor id between 1 and 6"); return; } - float const ANGLE_START = 0.0; - float const ANGLE_STOP = 180.0; - float const ANGLE_INCREMENT = 10.0; - - for (float angle = ANGLE_START; angle <= ANGLE_STOP; angle += ANGLE_INCREMENT) - { - Braccio.move(selected_motor).to(angle); - Serial.print("Target angle: " + String(angle)); - Serial.print(" | "); - Serial.print("Current angle: " + String(Braccio.get(selected_motor).position())); - Serial.println(); - delay(100); - } + test_motor(motor_id); } diff --git a/src/Braccio++.h b/src/Braccio++.h index 79dd5c9..117d90e 100644 --- a/src/Braccio++.h +++ b/src/Braccio++.h @@ -255,4 +255,4 @@ inline void attachInterrupt(pin_size_t interruptNum, mbed::Callback func attachInterruptParam(interruptNum, callback, mode, (void*)a); } -#endif //__BRACCIO_PLUSPLUS_H__ \ No newline at end of file +#endif //__BRACCIO_PLUSPLUS_H__ diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp index 7536e0e..76dd225 100644 --- a/src/lib/motors/SmartServo.cpp +++ b/src/lib/motors/SmartServo.cpp @@ -180,7 +180,11 @@ int SmartServoClass::begin() { } } -void SmartServoClass::setPosition(uint8_t const id, float const angle, uint16_t const speed) { +void SmartServoClass::setPosition(uint8_t const id, float const angle, uint16_t const speed) +{ + if (!isValidAngle(angle)) + return; + mutex.lock(); if (isValidId(id)) { @@ -295,27 +299,31 @@ void SmartServoClass::setStallProtectionTime(uint8_t const id, uint8_t const ti mutex.unlock(); } -void SmartServoClass::setMinAngle(float const angle) { +void SmartServoClass::setMinAngle(uint16_t const min_angle) +{ mutex.lock(); - writeWordCmd(BROADCAST, REG(SmartServoRegister::MIN_ANGLE_LIMIT_H), angleToPosition(angle)); + writeWordCmd(BROADCAST, REG(SmartServoRegister::MIN_ANGLE_LIMIT_H), min_angle); mutex.unlock(); } -void SmartServoClass::setMinAngle(uint8_t const id, float const angle) { +void SmartServoClass::setMinAngle(uint8_t const id, uint16_t const min_angle) +{ mutex.lock(); - writeWordCmd(id, REG(SmartServoRegister::MIN_ANGLE_LIMIT_H), angleToPosition(angle)); + writeWordCmd(id, REG(SmartServoRegister::MIN_ANGLE_LIMIT_H), min_angle); mutex.unlock(); } -void SmartServoClass::setMaxAngle(float const angle) { +void SmartServoClass::setMaxAngle(uint16_t const max_angle) +{ mutex.lock(); - writeWordCmd(BROADCAST, REG(SmartServoRegister::MAX_ANGLE_LIMIT_H), angleToPosition(angle)); + writeWordCmd(BROADCAST, REG(SmartServoRegister::MAX_ANGLE_LIMIT_H), max_angle); mutex.unlock(); } -void SmartServoClass::setMaxAngle(uint8_t const id, float const angle) { +void SmartServoClass::setMaxAngle(uint8_t const id, uint16_t const max_angle) +{ mutex.lock(); - writeWordCmd(id, REG(SmartServoRegister::MAX_ANGLE_LIMIT_H), angleToPosition(angle)); + writeWordCmd(id, REG(SmartServoRegister::MAX_ANGLE_LIMIT_H), max_angle); mutex.unlock(); } @@ -336,4 +344,4 @@ void SmartServoClass::getInfo(Stream & stream, uint8_t const id) { for (i = 0; i < sizeof(regs); i++) { stream.println(String(i, HEX) + " : " + String(regs[i], HEX)); } -} \ No newline at end of file +} diff --git a/src/lib/motors/SmartServo.h b/src/lib/motors/SmartServo.h index 3e7c5ae..c0a8125 100644 --- a/src/lib/motors/SmartServo.h +++ b/src/lib/motors/SmartServo.h @@ -50,13 +50,10 @@ class SmartServoClass void setStallProtectionTime(uint8_t const id, uint8_t const time); - void setMinAngle(float const angle); - - void setMinAngle(uint8_t const id, float const angle); - - void setMaxAngle(float const angle); - - void setMaxAngle(uint8_t const id, float const angle); + void setMinAngle(uint16_t const min_angle); + void setMinAngle(uint8_t const id, uint16_t const min_angle); + void setMaxAngle(uint16_t const max_angle); + void setMaxAngle(uint8_t const id, uint16_t const max_angle); void setTime(uint8_t const id, uint16_t const time); @@ -68,8 +65,6 @@ class SmartServoClass bool isEngaged(uint8_t const id); - void printTimestamps(); - void getInfo(Stream & stream, uint8_t const id); inline void onErrorCb(mbed::Callback onError) { _onError = onError; } @@ -77,6 +72,7 @@ class SmartServoClass inline int getErrors() const { return _errors; } static const int BROADCAST = 0xFE; + static float constexpr MAX_ANGLE = 315.0f; private: @@ -88,6 +84,7 @@ class SmartServoClass static int constexpr MIN_MOTOR_ID = 1; static int constexpr MAX_MOTOR_ID = 6; + inline bool isValidAngle(float const angle) { return ((angle >= 0.0f) && (angle <= MAX_ANGLE)); } inline bool isValidId(int const id) const { return ((id >= MIN_MOTOR_ID) && (id <= MAX_MOTOR_ID)); } inline int idToArrayIndex(int const id) const { return (id - 1); } @@ -101,10 +98,9 @@ class SmartServoClass int readWordCmd (uint8_t const id, uint8_t const address); int readByteCmd (uint8_t const id, uint8_t const address); void action (uint8_t const id); - void writeSyncCmd (uint8_t *id, uint8_t const num, uint8_t const address, uint8_t const len, uint8_t const * data); - inline uint16_t angleToPosition(float const angle) { return (angle*MAX_POSITION)/360.0; } - inline float positionToAngle(uint16_t const position) { return (360.0*position)/MAX_POSITION; } + inline uint16_t angleToPosition(float const angle) { return (angle*MAX_POSITION)/MAX_ANGLE; } + inline float positionToAngle(uint16_t const position) { return (MAX_ANGLE*position)/MAX_POSITION; } RS485Class& _RS485; @@ -129,4 +125,4 @@ class SmartServoClass rtos::Mutex mutex; }; -#endif // _SMARTMOTOR_H_ \ No newline at end of file +#endif // _SMARTMOTOR_H_