James Hilder
/
PR_RobotArmSetup
Code to set up an AX-12 or MX-28 (and to scan for unidentified servos) ready for use in the robot arm.
Revision 0:03f84c95f73b, committed 2017-02-16
- Comitter:
- jah128
- Date:
- Thu Feb 16 20:45:33 2017 +0000
- Child:
- 1:21a24da53b7f
- Commit message:
- Initial commit
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SerialHalfDuplex.cpp Thu Feb 16 20:45:33 2017 +0000 @@ -0,0 +1,80 @@ + /* NB This is part of an older MBED library, included so the older code runs on the + * newer MBED library. There is probably a better implementation.... - jah */ + + /* mbed Microcontroller Library + * Copyright (c) 2006-2012 ARM Limited + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * NOTE: This is an unsupported legacy untested library. + */ +#include "SerialHalfDuplex.h" + +#if DEVICE_SERIAL + +#include "pinmap.h" +#include "serial_api.h" +#include "gpio_api.h" + +namespace mbed { + +SerialHalfDuplex::SerialHalfDuplex(PinName tx, PinName rx, const char *name) + : Serial(tx, rx, name) { + _txpin = tx; + + // set as input + gpio_set(_txpin); + pin_mode(_txpin, PullNone); // no pull + pin_function(_txpin, 0); // set as gpio +} + +// To transmit a byte in half duplex mode: +// 1. Disable interrupts, so we don't trigger on loopback byte +// 2. Set tx pin to UART out +// 3. Transmit byte as normal +// 4. Read back byte from looped back tx pin - this both confirms that the +// transmit has occurred, and also clears the byte from the buffer. +// 5. Return pin to input mode +// 6. Re-enable interrupts + +int SerialHalfDuplex::_putc(int c) { + int retc; + + // TODO: We should not disable all interrupts + __disable_irq(); + + serial_pinout_tx(_txpin); + + Serial::_putc(c); + retc = Serial::getc(); // reading also clears any interrupt + + pin_function(_txpin, 0); + + __enable_irq(); + + return retc; +} + +int SerialHalfDuplex::_getc(void) { + return Serial::_getc(); +} + +} // End namespace + +#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SerialHalfDuplex.h Thu Feb 16 20:45:33 2017 +0000 @@ -0,0 +1,210 @@ + /* NB This is part of an older MBED library, included so the older code runs on the + * newer MBED library. There is probably a better implementation.... - jah */ + +/* mbed Microcontroller Library + * Copyright (c) 2006-2012 ARM Limited + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * NOTE: This is an unsupported legacy untested library. + */ +#ifndef MBED_SERIALHALFDUPLEX_H +#define MBED_SERIALHALFDUPLEX_H + +#include "device.h" + +#if DEVICE_SERIAL + +#include "Serial.h" +#include "PinNames.h" +#include "PeripheralNames.h" + +namespace mbed { + +/* Class: SerialHalfDuplex + * A serial port (UART) for communication with other devices using + * Half-Duplex, allowing transmit and receive on a single + * shared transmit and receive line. Only one end should be transmitting + * at a time. + * + * Both the tx and rx pin should be defined, and wired together. + * This is in addition to them being wired to the other serial + * device to allow both read and write functions to operate. + * + * Example: + * > // Send a byte to a second HalfDuplex device, and read the response + * > + * > #include "mbed.h" + * > + * > // p9 and p10 should be wired together to form "a" + * > // p28 and p27 should be wired together to form "b" + * > // p9/p10 should be wired to p28/p27 as the Half Duplex connection + * > + * > SerialHalfDuplex a(p9, p10); + * > SerialHalfDuplex b(p28, p27); + * > + * > void b_rx() { // second device response + * > b.putc(b.getc() + 4); + * > } + * > + * > int main() { + * > b.attach(&b_rx); + * > for(int c = 'A'; c < 'Z'; c++) { + * > a.putc(c); + * > printf("sent [%c]\n", c); + * > wait(0.5); // b should respond + * > if(a.readable()) { + * > printf("received [%c]\n", a.getc()); + * > } + * > } + * > } + * + * For Simplex and Full-Duplex Serial communication, see <Serial> + */ +class SerialHalfDuplex : public Serial { + +public: + /* Constructor: SerialHalfDuplex + * Create a half-duplex serial port, connected to the specified transmit + * and receive pins. + * + * These pins should be wired together, as well as to the target device + * + * Variables: + * tx - Transmit pin + * rx - Receive pin + */ + SerialHalfDuplex(PinName tx, PinName rx, const char *name = NULL); + +#if 0 // Inherited from Serial class, for documentation + /* Function: baud + * Set the baud rate of the serial port + * + * Variables: + * baudrate - The baudrate of the serial port (default = 9600). + */ + void baud(int baudrate); + + enum Parity { + None = 0 + , Odd + , Even + , Forced1 + , Forced0 + }; + + /* Function: format + * Set the transmission format used by the Serial port + * + * Variables: + * bits - The number of bits in a word (5-8; default = 8) + * parity - The parity used (Serial::None, Serial::Odd, +Serial::Even, Serial::Forced1, Serial::Forced0; default = Serial::None) + * stop - The number of stop bits (1 or 2; default = 1) + */ + void format(int bits = 8, Parity parity = Serial::None, int stop_bits += 1); + + /* Function: putc + * Write a character + * + * Variables: + * c - The character to write to the serial port + */ + int putc(int c); + + /* Function: getc + * Read a character + * + * Read a character from the serial port. This call will block + * until a character is available. For testing if a character is + * available for reading, see <readable>. + * + * Variables: + * returns - The character read from the serial port + */ + int getc(); + + /* Function: printf + * Write a formated string + * + * Variables: + * format - A printf-style format string, followed by the + * variables to use in formating the string. + */ + int printf(const char* format, ...); + + /* Function: scanf + * Read a formated string + * + * Variables: + * format - A scanf-style format string, + * followed by the pointers to variables to store the results. + */ + int scanf(const char* format, ...); + + /* Function: readable + * Determine if there is a character available to read + * + * Variables: + * returns - 1 if there is a character available to read, else 0 + */ + int readable(); + + /* Function: writeable + * Determine if there is space available to write a character + * + * Variables: + * returns - 1 if there is space to write a character, else 0 + */ + int writeable(); + + /* Function: attach + * Attach a function to call whenever a serial interrupt is generated + * + * Variables: + * fptr - A pointer to a void function, or 0 to set as none + */ + void attach(void (*fptr)(void)); + + /* Function: attach + * Attach a member function to call whenever a serial interrupt is generated + * + * Variables: + * tptr - pointer to the object to call the member function on + * mptr - pointer to the member function to be called + */ + template<typename T> + void attach(T* tptr, void (T::*mptr)(void)); + +#endif + +protected: + PinName _txpin; + + virtual int _putc(int c); + virtual int _getc(void); + +}; // End class SerialHalfDuplex + +} // End namespace + +#endif + +#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Feb 16 20:45:33 2017 +0000 @@ -0,0 +1,100 @@ +// Dynamixel Servo Setup Code +// +// Modify the commented lines in main() to setup required servos + +#include "servo.h" +#include "mbed.h" +#include "SerialHalfDuplex.h" + +#define BASE 10 +#define SHOULDER 11 +#define ELBOW 12 +#define WRIST 13 + +Serial pc(USBTX,USBRX); +Servo servo(p9,p10); + +void scan_for_servos() +{ + pc.printf("Scanning for servos at 57600 baud:\n"); + servo.SetInitBaud(57600,250); + for(int i=0;i<254;i++){ + int model = servo.GetModelNumber(i); + if(model == AX12_MODEL) pc.printf("AX-12 found at ID %d",i); + if(model == MX28_MODEL) pc.printf("MX-28 found at ID %d",i); + } + wait(0.5); + pc.printf("\n\nScanning for servos at 1M baud:\n"); + servo.SetInitBaud(1000000,50); + for(int i=0;i<254;i++){ + int model = servo.GetModelNumber(i); + if(model == AX12_MODEL) pc.printf("AX-12 found at ID %d",i); + if(model == MX28_MODEL) pc.printf("MX-28 found at ID %d",i); + } + while(1); +} + +void initialise_mx28_servo(int current_address, int target_servo) +{ + pc.printf("RUNNING SERVO SETUP ROUTINE FOR SERVO %d\n\n",target_servo); + + // MX-28s default to a baud rate of 57600 with delay time of 500us + servo.SetInitBaud(57600, 250); + int model = servo.GetModelNumber(current_address); + if(model != MX28_MODEL) { + pc.printf("Error: No MX-28 servo with ID %d found...\n",current_address); + while(1); + } + pc.printf("\n\nSetting up MX-28 servo to ID %d\n",target_servo); + servo.SetID(current_address,target_servo); + wait(0.25); + pc.printf("Getting servo data:\n"); + servo.DebugData(target_servo); + pc.printf("\n\nDone (check messages above to verify status)\n"); + while(1); +} + +void initialise_ax12_servo(int current_address, int target_servo) +{ + pc.printf("RUNNING SERVO SETUP ROUTINE FOR SERVO %d\n\n",target_servo); + + // AX-12s default to a baud rate of 1M with delay time of 100us + servo.SetInitBaud(1000000, 50); + int model = servo.GetModelNumber(current_address); + if(model != AX12_MODEL) { + pc.printf("Error: No AX12 servo with ID %d found...\n",current_address); + while(1); + } + pc.printf("\n\nSetting up AX-12 servo to ID %d\n",target_servo); + servo.SetID(current_address,target_servo); + wait(0.25); + pc.printf("Setting return delay time for servo to 500us\n"); + servo.SetDelayTime(target_servo,250); + wait(0.25); + pc.printf("Setting baud rate for servo to 57600\n"); + servo.SetBaud(target_servo,0x22); + wait(0.25); + pc.printf("Resetting connecting baud rate\n"); + servo.SetInitBaud(57600, 250); + wait(0.25); + pc.printf("Getting servo data:\n"); + servo.DebugData(target_servo); + pc.printf("\n\nDone (check messages above to verify status)\n"); + while(1); +} +int main() +{ + pc.baud(115200); + pc.printf("Dynamixel Servo Setup\n"); + + // To scan for servos, uncomment the following line: + //scan_for_servos(); + + // To setup a new MX-28 servo, uncomment the following line and replace first number with current ID and second number with target ID: + //initialise_mx28_servo(1,BASE); + + // To setup a new AX-12 servo, uncomment the following line and replace first number with current ID and second number with target ID: + //initialise_ax12_servo(1,WRIST); + + pc.printf("Edit the comments in main() to check and program servos.\n"); +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Feb 16 20:45:33 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/25aea2a3f4e3 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/servo.cpp Thu Feb 16 20:45:33 2017 +0000 @@ -0,0 +1,895 @@ +/* University of York Robotics Laboratory Robot Arm Controller Board + * + * Dynamixel Servo Library for AX-12 and MX-28 + * + * Based on library by Chris Styles (see copyright notice at end of file) + * + * File: servo.cpp + * + * (C) Dept. Electronics & Computer Science, University of York + * Chris Styles, James Hilder, Alan Millard, Shuhei Miyashita, Homero Elizondo, Jon Timmis + * + * February 2017, Version 1.0 + */ + +#include "servo.h" +extern Serial pc; +int delay = RETURN_DELAY; +char read_timeout_counter = 0; +Servo::Servo(PinName tx, PinName rx) + : _servo(tx,rx) +{ + _servo.baud(57600); +} + +void Servo::ClearBuffer() +{ + if (_servo.readable()) { + pc.printf("\nBuffer error:"); + while(_servo.readable()) { + pc.printf("%c",_servo.getc()); + } + pc.printf("\n"); + } +} +void Servo::ScanForServos () +{ + pc.printf("Scanning for servos...\n"); + pc.printf("Checking at 57600 baud\n"); + _servo.baud(57600); + delay = 250; + int found = 0; + for(int k=0; k<2; k++) { + if(k==1) { + _servo.baud(1000000); + pc.printf("\nChecking at 1000000 baud\n"); + } + for(int id = 0; id<254; id++) { + if(DEBUG==1)pc.printf("ID %d: ",id); + char TxBuf[8]; + TxBuf[0] = 0xff; + TxBuf[1] = 0xff; + TxBuf[2] = id; + char sum = id + 7; + TxBuf[3] = 4; + TxBuf[4] = 2; + TxBuf[5] = REG_MODEL_NUMBER; + TxBuf[6] = 1; + TxBuf[7] = 0xFF - sum; + for (int i = 0; i<8 ; i++) { + _servo.putc(TxBuf[i]); + } + // Wait for data to transmit + int t_delay = 60; + wait_us(t_delay); + if(_servo.readable()) { + pc.printf("ID %d: ",id); + // Receive the Status packet 6+ number of bytes read + char status[8]; + for (int i=0; i<(7) ; i++) { + if(!_servo.readable()) {pc.printf("Data error\n");break;} + status[i] = _servo.getc(); + } + if(status[2] == id) { + found ++; + pc.printf(" FOUND ["); + char modelnumber = status[5]; + switch(modelnumber) { + case (AX12_MODEL): + pc.printf("AX12]\n"); + break; + case (MX28_MODEL): + pc.printf("MX28]\n"); + break; + default: + pc.printf("UNKNOWN MODEL]\n"); + break; + } + } else pc.printf(" ID ERROR\n"); + } else { + //pc.printf(" NOT FOUND\n"); + } + } + } + if(found > 0)pc.printf("\nScan complete [%d servos found].\n",found); + else pc.printf("\nScan complete - no servos found.\n"); + delay = RETURN_DELAY; +} + + +// Get detailed data for servo +void Servo::DebugData(int ID) +{ + pc.printf("\nGetting Current Data for Servo %d",ID); + + + char data[49]; + for(int i=0; i<12; i++) { + int offset = i*4; + int ErrorCode = read(ID, offset, 4, data+offset); + pc.printf("."); + } + pc.printf("\n"); + + + pc.printf("\nEEPROM VALUES\n"); + + int modelnumber = data[0] + (data[1] << 8); + pc.printf("Model Number : %x [",modelnumber); + switch(modelnumber) { + case (AX12_MODEL): + pc.printf("AX12]\n"); + break; + case (MX28_MODEL): + pc.printf("MX28]\n"); + break; + default: + pc.printf("UNKNOWN]\n"); + break; + } + pc.printf("Firmware Version : %x\n",data[2]); + pc.printf("ID : %x\n",data[3]); + int baudrate = 2000000 / (data[4] + 1); + //Special high-speed baudrates [for MX28 only] + if(data[4] == 250) baudrate = 2250000; + if(data[4] == 251) baudrate = 2500000; + if(data[4] == 252) baudrate = 3000000; + pc.printf("Baud Rate : %x [%d]\n",data[4],baudrate); + pc.printf("Return Delay Time : %x [%duS]\n",data[5],(data[5] * 2)); + short cw_angle_limit = data[6] + (data[7] << 8); + short ccw_angle_limit = data[8] + (data[9] << 8); + pc.printf("CW Angle Limit : %x [%d",cw_angle_limit,cw_angle_limit); + if(cw_angle_limit ==0 && ccw_angle_limit == 0)pc.printf(" - Wheel Mode]\n"); + else { + if(cw_angle_limit == 4095 && ccw_angle_limit == 4095)pc.printf(" - Multiturn Mode]\n"); + else pc.printf("- Joint Mode]\n"); + } + pc.printf("CCW Angle Limit : %x [%d",ccw_angle_limit,ccw_angle_limit); + if(cw_angle_limit ==0 && ccw_angle_limit == 0)pc.printf(" - Wheel Mode]\n"); + else { + if(cw_angle_limit == 4095 && ccw_angle_limit == 4095)pc.printf(" - Multiturn Mode]\n"); + else pc.printf("- Joint Mode]\n"); + } + //Fill in blanks + pc.printf("High Temp Limit : %x [%dC]\n",data[11],data[11]); + pc.printf("Low Voltage Limit : %x [%2.1fV]\n",data[12],(float) (data[12]*0.1f)); + pc.printf("High Voltage Limit: %x [%2.1fV]\n",data[13],(float) (data[13]*0.1f)); + short max_torque = data[14] + (data[15] << 8); + float pct_max_torque = (float) (max_torque / 10.23f); + pc.printf("Preset Max Torque : %x [%3.2f%%]\n",max_torque,pct_max_torque); + pc.printf("Status Return Lev.: %x [%d]\n",data[16]); + pc.printf("Alarm LED : %x [%d]\n",data[17]); + pc.printf("Alarm Shutdown : %x [%d]\n",data[18]); + short multiturn_offset = data[20] + (data[21] << 8); + pc.printf("Multiturn Offset : %x [%d]\n",multiturn_offset,multiturn_offset); + pc.printf("\nRAM VALUES\n"); + pc.printf("Torque Enable : %x\n",data[24]); + pc.printf("LED : %x\n",data[25]); + pc.printf("D Gain : %x [%d]\n",data[26],data[26]); + pc.printf("I Gain : %x [%d]\n",data[27],data[27]); + pc.printf("P Gain : %x [%d]\n",data[28],data[28]); + short goal_position = data[30] + (data[31] << 8); + float gp_degrees = (goal_position - 2048) * 0.087890625; + pc.printf("Goal Position : %x [%d: %3.2f degrees]\n",goal_position,goal_position,gp_degrees); + short moving_speed = data[32] + (data[33] << 8); + float mv_rpm = moving_speed * 0.114; + pc.printf("Moving Speed : %x [%d: %4.2 rpm]\n",moving_speed,moving_speed,mv_rpm); + short c_max_torque = data[34] + (data[35] << 8); + float cpct_max_torque = (float) (c_max_torque / 10.23f); + pc.printf("Current Max Torque: %x [%3.2f%%]\n",c_max_torque,cpct_max_torque); + short present_position = data[36] + (data[37] << 8); + float pp_degrees = present_position * 0.088f; + pc.printf("Present Position : %x [%d: %3.2f degrees]\n",present_position,present_position,pp_degrees); + short present_speed = data[38] + (data[39] << 8); + float p_rpm = present_speed * 0.114; + pc.printf("Present Speed : %x [%d: %4.2 rpm]\n",present_speed,present_speed,p_rpm); + short present_load = data[40] + (data[41] << 8); + if(present_load < 1024) { + float present_loadpct = (1024 - present_load) / 10.23f; + pc.printf("Present Load : %x [%3.2f%% CCW]\n",present_load,present_loadpct); + } else { + if(present_load > 1024) { + float present_loadpct_cw = (present_load - 1024) / 10.23f; + pc.printf("Present Load : %x [%3.2f%% CW]\n",present_load,present_loadpct_cw); + } else pc.printf("Present Load : %x [NONE]\n",present_load); + } + pc.printf("Voltage : %x [%fV]\n",data[42],(data[42] * 0.1f)); + pc.printf("Temperature : %x [%dC]\n",data[43],data[43]); +} + +// Set the mode of the servo +// 0 = Positional (0-300 degrees) +// 1 = Rotational -1 to 1 speed +int Servo::SetMode(int ID, int mode) +{ + if (mode == 1) { // set CR + SetCWLimit(ID, 0); + SetCCWLimit(ID, 0); + SetCRSpeed(ID, 0.0); + } else { + SetCWLimit(ID, 0); + SetCCWLimit(ID, 300); + SetCRSpeed(ID, 0.0); + } + return(0); +} + +// if flag[0] is set, were blocking +// if flag[1] is set, we're registering +// they are mutually exclusive operations +int Servo::SetGoal(int ID, short goal, int flags) +{ + + char reg_flag = 0; + char data[2]; + + // set the flag is only the register bit is set in the flag + if (flags == 0x2) { + reg_flag = 1; + } + if (DEBUG) { + pc.printf("SetGoal to 0x%x ",goal); + } + + data[0] = goal & 0xff; // bottom 8 bits + data[1] = goal >> 8; // top 8 bits + + // write the packet, return the error code + int rVal = write(ID, REG_GOAL_POSITION, 2, data, reg_flag); + + if (flags == 1) { + // block until it comes to a halt + if (DEBUG) pc.printf(" [WAITING]"); + while (isMoving(ID)) {} + } + if (DEBUG) pc.printf("\n"); + return(rVal); +} + +// if flag[0] is set, were blocking +// if flag[1] is set, we're registering +// they are mutually exclusive operations +int Servo::SetGoalDegrees(int ID, int degrees, int flags) +{ + short goal = (degrees * 11.377778) + 2048; + return SetGoal(ID,goal,flags); +} + + +// Set continuous rotation speed from -1 to 1 +int Servo::SetCRSpeed(int ID, float speed) +{ + + // bit 10 = direction, 0 = CCW, 1=CW + // bits 9-0 = Speed + char data[2]; + + int goal = (0x3ff * abs(speed)); + + // Set direction CW if we have a negative speed + if (speed < 0) { + goal |= (0x1 << 10); + } + + data[0] = goal & 0xff; // bottom 8 bits + data[1] = goal >> 8; // top 8 bits + + // write the packet, return the error code + int rVal = write(ID, 0x20, 2, data); + + return(rVal); +} + + +int Servo::SetCWLimit (int ID, int degrees) +{ + + char data[2]; + + // 1023 / 300 * degrees + short limit = (1023 * degrees) / 300; + + if (DEBUG) { + pc.printf("SetCWLimit to 0x%x\n",limit); + } + + data[0] = limit & 0xff; // bottom 8 bits + data[1] = limit >> 8; // top 8 bits + + // write the packet, return the error code + return (write(ID, REG_CW_LIMIT, 2, data)); + +} + +int Servo::SetCCWLimit (int ID, int degrees) +{ + + char data[2]; + + // 1023 / 300 * degrees + short limit = (1023 * degrees) / 300; + + if (DEBUG) { + pc.printf("SetCCWLimit to 0x%x\n",limit); + } + + data[0] = limit & 0xff; // bottom 8 bits + data[1] = limit >> 8; // top 8 bits + + // write the packet, return the error code + return (write(ID, REG_CCW_LIMIT, 2, data)); +} + +int Servo::SetTorqueEnable (int ID, int enable) +{ + char data[1]; + data[0]=enable; + if (DEBUG) { + pc.printf("SetTorqueEnable to %d\n",enable); + } + + + // write the packet, return the error code + return (write(ID, REG_TORQUE_ENABLE, 1, data)); +} + +int Servo::SetLowVoltageLimit (int ID, char lv_limit) +{ + + char data[1]; + data[0] = lv_limit; + if (DEBUG) { + pc.printf("Setting low voltage limit to %2.1f\n",(float) lv_limit / 10.0); + } + return (write(ID, REG_LOW_VOLTAGE_LIMIT, 1, data)); +} + +int Servo::LockEeprom (int ID) +{ + char data[1]; + data[0]=1; + if (DEBUG) { + pc.printf("Locking EEPROM\n"); + } + return (write(ID, REG_EEPROM_LOCK, 1, data)); +} + +int Servo::SetHighVoltageLimit (int ID, char hv_limit) +{ + + char data[1]; + data[0] = hv_limit; + if (DEBUG) { + pc.printf("Setting high voltage limit to %2.1f\n",(float) hv_limit / 10.0); + } + return (write(ID, REG_HIGH_VOLTAGE_LIMIT, 1, data)); +} + +int Servo::SetDelayTime (int ID, char delay) +{ + char data[1]; + data[0] = delay; + if (DEBUG) { + pc.printf("Setting delay time to %dus\n",delay+delay); + } + return (write(ID, REG_RETURN_DELAY, 1, data)); +} + + + +int Servo::SetTemperatureLimit (int ID, char temp_limit) +{ + + char data[1]; + data[0] = temp_limit; + if (DEBUG) { + pc.printf("Setting temperature limit to %dC\n",temp_limit); + } + return (write(ID, REG_HIGHTEMP_LIMIT, 1, data)); +} + +int Servo::SetID (int CurrentID, int NewID) +{ + + char data[1]; + data[0] = NewID; + if (DEBUG) { + pc.printf("Setting ID from 0x%x to 0x%x\n",CurrentID,NewID); + } + return (write(CurrentID, REG_ID, 1, data)); + +} + +int Servo::SetBaud (int ID, int baud) +{ + + char data[1]; + data[0] = baud; + if (DEBUG) { + pc.printf("Setting baud to %d\n",(2000000 / baud)); + } + return (write(ID, REG_BAUDRATE, 1, data)); + +} + + +// return 1 is the servo is still in flight +int Servo::isMoving(int ID) +{ + + char data[1]; + read(ID,REG_MOVING,1,data); + return(data[0]); +} + + +void Servo::trigger(void) +{ + + char TxBuf[16]; + char sum = 0; + + if (TRIGGER_DEBUG) { + pc.printf("\nTriggered\n"); + } + + // Build the TxPacket first in RAM, then we'll send in one go + if (TRIGGER_DEBUG) { + pc.printf("\nTrigger Packet\n Header : 0xFF, 0xFF\n"); + } + + TxBuf[0] = 0xFF; + TxBuf[1] = 0xFF; + + // ID - Broadcast + TxBuf[2] = 0xFE; + sum += TxBuf[2]; + + if (TRIGGER_DEBUG) { + pc.printf(" ID : %d\n",TxBuf[2]); + } + + // Length + TxBuf[3] = 0x02; + sum += TxBuf[3]; + if (TRIGGER_DEBUG) { + pc.printf(" Length %d\n",TxBuf[3]); + } + + // Instruction - ACTION + TxBuf[4] = 0x04; + sum += TxBuf[4]; + if (TRIGGER_DEBUG) { + pc.printf(" Instruction 0x%X\n",TxBuf[5]); + } + + // Checksum + TxBuf[5] = 0xFF - sum; + if (TRIGGER_DEBUG) { + pc.printf(" Checksum 0x%X\n",TxBuf[5]); + } + + // Transmit the packet in one burst with no pausing + for (int i = 0; i < 6 ; i++) { + _servo.putc(TxBuf[i]); + } + + // This is a broadcast packet, so there will be no reply + + return; +} + +int Servo::GetModelNumber(int ID) +{ + if (DEBUG) { + pc.printf("\nGetModelNumber(%d)",ID); + } + char data[2]; + int ErrorCode = read(ID, REG_MODEL_NUMBER, 2, data); + int modelnumber = data[0] + (data[1] << 8); + return (modelnumber); +} + +float Servo::GetPositionDegrees(int ID) +{ + short position = GetPosition(ID); + //float angle = (position * 300)/1024; FOR AX-12 + float angle = (position - 2048) * 0.087890625; + + return (angle); +} + +short Servo::GetPosition(int ID) +{ + + if (DEBUG) { + pc.printf("\nGetPosition(%d)",ID); + } + + char data[2]; + + int ErrorCode = read(ID, REG_POSITION, 2, data); + if (DEBUG) { + pc.printf("[EC=%d]",ErrorCode); + } + short position = data[0] + (data[1] << 8); + + return (position); +} + + +float Servo::GetTemp (int ID) +{ + + if (DEBUG) { + pc.printf("\nGetTemp(%d)",ID); + } + char data[1]; + int ErrorCode = read(ID, REG_TEMP, 1, data); + float temp = data[0]; + return(temp); +} + +short Servo::GetTemperature(int ID) +{ + if (DEBUG) { + pc.printf("\nGetTemperature(%d)",ID); + } + char data[1]; + int ErrorCode = read(ID, REG_TEMP, 1, data); + return (short) (data[0]); +} + +float Servo::GetVolts (int ID) +{ + if (DEBUG) { + pc.printf("\nGetVolts(%d)",ID); + } + char data[1]; + int ErrorCode = read(ID, REG_VOLTS, 1, data); + float volts = data[0]/10.0; + return(volts); +} + +short Servo::GetVoltage(int ID) +{ + if (DEBUG) { + pc.printf("\nGetVoltage(%d)",ID); + } + char data[1]; + int ErrorCode = read(ID, REG_VOLTS, 1, data); + return (short) (data[0]); +} + +short Servo::GetLoad(int ID) +{ + if (DEBUG) { + pc.printf("\nGetLoad(%d)",ID); + } + char data[2]; + int ErrorCode = read(ID, REG_LOAD, 2, data); + return (short) (data[0] + (data[1]<<8)); +} + +short Servo::GetSpeed(int ID) +{ + if (DEBUG) { + pc.printf("\nGetSpeed(%d)",ID); + } + char data[2]; + int ErrorCode = read(ID, REG_SPEED, 2, data); + return (short) (data[0] + (data[1]<<8)); +} + +int Servo::read(int ID, int start, int bytes, char* data) +{ + + char PacketLength = 0x4; + char TxBuf[16]; + char sum = 0; + char Status[16]; + + Status[4] = 0xFE; // return code + + if (READ_DEBUG) { + pc.printf("\nread(%d,0x%x,%d,data)\n",ID,start,bytes); + } + + // Build the TxPacket first in RAM, then we'll send in one go + if (READ_DEBUG) { + pc.printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n"); + } + + TxBuf[0] = 0xff; + TxBuf[1] = 0xff; + + // ID + TxBuf[2] = ID; + sum += TxBuf[2]; + if (READ_DEBUG) { + pc.printf(" ID : %d\n",TxBuf[2]); + } + + // Packet Length + TxBuf[3] = PacketLength; // Length = 4 ; 2 + 1 (start) = 1 (bytes) + sum += TxBuf[3]; // Accululate the packet sum + if (READ_DEBUG) { + pc.printf(" Length : 0x%x\n",TxBuf[3]); + } + + // Instruction - Read + TxBuf[4] = 0x2; + sum += TxBuf[4]; + if (READ_DEBUG) { + pc.printf(" Instruction : 0x%x\n",TxBuf[4]); + } + + // Start Address + TxBuf[5] = start; + sum += TxBuf[5]; + if (READ_DEBUG) { + pc.printf(" Start Address : 0x%x\n",TxBuf[5]); + } + + // Bytes to read + TxBuf[6] = bytes; + sum += TxBuf[6]; + if (READ_DEBUG) { + pc.printf(" No bytes : 0x%x\n",TxBuf[6]); + } + + // Checksum + TxBuf[7] = 0xFF - sum; + if (READ_DEBUG) { + pc.printf(" Checksum : 0x%x\n",TxBuf[7]); + } + + // Transmit the packet in one burst with no pausing + for (int i = 0; i<8 ; i++) { + _servo.putc(TxBuf[i]); + } + + // Wait for data to transmit + wait_us(60); //was 60 + + + // Skip if the read was to the broadcast address + if (ID != 0xFE) { + int timedout = 0; + int timeout_count = 0; + while(!_servo.readable()) { + timeout_count++; + if(timeout_count % 10000 == 0) { + timedout=1; + break; + } + } + if(timedout==1) { + read_timeout_counter++; + if(DEBUG)pc.printf(" Read timed out [%d of %d]\n",read_timeout_counter,READ_TIMEOUT_LIMIT); + if(read_timeout_counter >= READ_TIMEOUT_LIMIT){ + read_timeout_counter = 0; + return 255; + } + return read(ID,start,bytes,data); + } else { + read_timeout_counter = 0; + // Receive the Status packet 6+ number of bytes read + for (int i=0; i<(6+bytes) ; i++) { + Status[i] = _servo.getc(); + } + + // Copy the data from Status into data for return + for (int i=0; i < Status[3]-2 ; i++) { + data[i] = Status[5+i]; + } + + if (READ_DEBUG) { + pc.printf("\nStatus Packet\n"); + pc.printf(" Header : 0x%x\n",Status[0]); + pc.printf(" Header : 0x%x\n",Status[1]); + pc.printf(" ID : 0x%x\n",Status[2]); + pc.printf(" Length : 0x%x\n",Status[3]); + pc.printf(" Error Code : 0x%x\n",Status[4]); + + for (int i=0; i < Status[3]-2 ; i++) { + pc.printf(" Data : 0x%x\n",Status[5+i]); + } + + pc.printf(" Checksum : 0x%x\n",Status[5+(Status[3]-2)]); + } + + } // if (ID!=0xFE) + wait_us(5); + } + return(Status[4]); +} + + +int Servo:: write(int ID, int start, int bytes, char* data, int flag) +{ +// 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum + + char TxBuf[16]; + char sum = 0; + char Status[6]; + + if (WRITE_DEBUG) { + pc.printf("\nwrite(%d,0x%x,%d,data,%d)\n",ID,start,bytes,flag); + } + + // Build the TxPacket first in RAM, then we'll send in one go + if (WRITE_DEBUG) { + pc.printf("\nInstruction Packet\n Header : 0xFF, 0xFF\n"); + } + + TxBuf[0] = 0xff; + TxBuf[1] = 0xff; + + // ID + TxBuf[2] = ID; + sum += TxBuf[2]; + + if (WRITE_DEBUG) { + pc.printf(" ID : %d\n",TxBuf[2]); + } + + // packet Length + TxBuf[3] = 3+bytes; + sum += TxBuf[3]; + + if (WRITE_DEBUG) { + pc.printf(" Length : %d\n",TxBuf[3]); + } + + // Instruction + if (flag == 1) { + TxBuf[4]=0x04; + sum += TxBuf[4]; + } else { + TxBuf[4]=0x03; + sum += TxBuf[4]; + } + + if (WRITE_DEBUG) { + pc.printf(" Instruction : 0x%x\n",TxBuf[4]); + } + + // Start Address + TxBuf[5] = start; + sum += TxBuf[5]; + if (WRITE_DEBUG) { + pc.printf(" Start : 0x%x\n",TxBuf[5]); + } + + // data + for (char i=0; i<bytes ; i++) { + TxBuf[6+i] = data[i]; + sum += TxBuf[6+i]; + if (WRITE_DEBUG) { + pc.printf(" Data : 0x%x\n",TxBuf[6+i]); + } + } + + // checksum + TxBuf[6+bytes] = 0xFF - sum; + if (WRITE_DEBUG) { + pc.printf(" Checksum : 0x%x\n",TxBuf[6+bytes]); + } + + // Transmit the packet in one burst with no pausing + for (int i = 0; i < (7 + bytes) ; i++) { + _servo.putc(TxBuf[i]); + } + + // Wait for data to transmit + wait_us(60); + + // make sure we have a valid return + Status[4]=0x00; + + // we'll only get a reply if it was not broadcast + if (ID!=0xFE) { + int timedout = 0; + int timeout_count = 0; + while(!_servo.readable()) { + timeout_count++; + if(timeout_count % 10000 == 0) { + timedout=1; + break; + } + } + if(timedout==1) { + read_timeout_counter++; + if(DEBUG)pc.printf(" Write ack. timed out [%d of %d]\n",read_timeout_counter,READ_TIMEOUT_LIMIT); + if(read_timeout_counter >= READ_TIMEOUT_LIMIT){ + read_timeout_counter = 0; + return 255; + } + return write(ID,start,bytes,data,flag); + } else { + read_timeout_counter = 0; + // response is always 6 bytes + // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum + for (int i=0; i < 6 ; i++) { + Status[i] = _servo.getc(); + } + } + // Build the TxPacket first in RAM, then we'll send in one go + if (WRITE_DEBUG) { + pc.printf("\nStatus Packet\n Header : 0x%X, 0x%X\n",Status[0],Status[1]); + pc.printf(" ID : %d\n",Status[2]); + pc.printf(" Length : %d\n",Status[3]); + pc.printf(" Error : 0x%x\n",Status[4]); + pc.printf(" Checksum : 0x%x\n",Status[5]); + } + + + } + + return(Status[4]); // return error code +} + +//Set the baud rate for serial connection to something other than default(1000000) +void Servo::SetInitBaud(int baud, int delaytime) +{ + pc.printf("Setting serial baud rate to %d\n",baud); + _servo.baud(baud); + delay = delaytime; +} + +/* Additional copyright notice */ + +/* + * Copyright 2017 University of York + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. + * You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 + * Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and limitations under the License. + * + */ + +/* + * Copyright (c) 2010, Chris Styles (http://mbed.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ +/* + * Copyright (c) 2010, Chris Styles (http://mbed.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/servo.h Thu Feb 16 20:45:33 2017 +0000 @@ -0,0 +1,344 @@ +/* University of York Robotics Laboratory Robot Arm Controller Board + * + * Dynamixel Servo Library for AX-12 and MX-28 + * + * Based on library by Chris Styles (see copyright notice at end of file) + * + * File: servo.cpp + * + * (C) Dept. Electronics & Computer Science, University of York + * Chris Styles, James Hilder, Alan Millard, Shuhei Miyashita, Homero Elizondo, Jon Timmis + * + * February 2017, Version 1.0 + */ + +// NOTE: +// When communicating with 'new' servos the are defaulted to ID 1 +// Make sure only one such servo is connected at one time +// Also note that this library defaults to 57600 serial comms +// The MX-28s are defaulted to communicate at this speed but the AX-12s are not (1Mbaud) +// Use SetInitBaud(57600) to override the default and then change baud rate when setting up MX-28s + +#ifndef SERVO_H +#define SERVO_H + +#include "mbed.h" +#include "SerialHalfDuplex.h" + +#define WRITE_DEBUG 0 +#define READ_DEBUG 0 +#define TRIGGER_DEBUG 0 +#define DEBUG 0 + +// Number of times to retry a read\write operation if read times out +#define READ_TIMEOUT_LIMIT 3 + +#define LOW_VOLTAGE_LIMIT 90 +#define HIGH_VOLTAGE_LIMIT 140 +#define HIGH_TEMPERATURE_LIMIT 70 + +#define AX12_MODEL 0x0C +#define MX28_MODEL 0x1D + +#define RETURN_DELAY 250 + +#define REG_MODEL_NUMBER 0x00 +#define REG_FIRMWARE_VERSION 0x02 +#define REG_ID 0x03 +#define REG_BAUDRATE 0x04 +#define REG_RETURN_DELAY 0x05 +#define REG_CW_LIMIT 0x06 +#define REG_CCW_LIMIT 0x08 +#define REG_HIGHTEMP_LIMIT 0x0B +#define REG_LOW_VOLTAGE_LIMIT 0x0C +#define REG_HIGH_VOLTAGE_LIMIT 0x0D +#define REG_MAX_TORQUE 0x0E +#define REG_TORQUE_ENABLE 0x18 +#define REG_GOAL_POSITION 0x1E +#define REG_MOVING_SPEED 0x20 +#define REG_VOLTS 0x2A +#define REG_TEMP 0x2B +#define REG_SPEED 0x26 +#define REG_LOAD 0x28 +#define REG_MOVING 0x2E +#define REG_POSITION 0x24 +#define REG_EEPROM_LOCK 0x2F + +#define MODE_POSITION 0 +#define MODE_ROTATION 1 + +#define CW 1 +#define CCW 0 + +extern Serial data; + +class Servo { + +public: + + /** Create a Dynamixel servo controller object connected to the specified serial port, with the specified ID + * + * @param pin tx pin + * @param pin rx pin + */ + Servo(PinName tx, PinName rx); + + void ClearBuffer(void); + + void ScanForServos(void); + + + /** Prints (over PC serial) a detailed set of data for the servo + * + * @param ID, the Bus ID of the servo 1-255 + */ + void DebugData(int ID); + + /** Set the mode of the servo + * + * @param ID, the Bus ID of the servo 1-255 + * @param mode + * 0 = Positional, default + * 1 = Continuous rotation + */ + int SetMode(int ID, int mode); + + /** Set goal angle, in positional mode + * + * @param ID, the Bus ID of the servo 1-255 + * @param goal 0-4095 + * @param flags, defaults to 0 + * flags[0] = blocking, return when goal position reached + * flags[1] = register, activate with a broadcast trigger + * + */ + int SetGoal(int ID, short goal, int flags = 0); + + /** Set goal angle in integer degrees, in positional mode + * + * @param ID, the Bus ID of the servo 1-255 + * @param degrees 0-300 + * @param flags, defaults to 0 + * flags[0] = blocking, return when goal position reached + * flags[1] = register, activate with a broadcast trigger + * + */ + int SetGoalDegrees(int ID, int degrees, int flags = 0); + + + /** Set the speed of the servo in continuous rotation mode + * + * @param ID, the Bus ID of the servo 1-255 + * @param speed, -1.0 to 1.0 + * -1.0 = full speed counter clock wise + * 1.0 = full speed clock wise + */ + int SetCRSpeed(int ID, float speed); + + + /** Set the clockwise limit of the servo + * + * @param ID, the Bus ID of the servo 1-255 + * @param degrees, 0-300 + */ + int SetCWLimit(int ID, int degrees); + + /** Set the counter-clockwise limit of the servo + * + * @param ID, the Bus ID of the servo 1-255 + * @param degrees, 0-300 + */ + int SetCCWLimit(int ID, int degrees); + + /** Enable or disable the torque hold of the servo + * + * @param ID, the Bus ID of the servo 1-255 + * @param enable, 0=disable (floppy) 1=enable (hold) + */ + int SetTorqueEnable (int ID, int enable); + + + /** Change the low voltage limit of a servo + * + * @param ID, the Bus ID of the servo 1-255 + * @param lv_limit, 0-254 + * + * voltage = lv_limit / 10 + */ + int SetLowVoltageLimit (int ID, char lv_limit); + + /** Lock the EEPROM area of a servo + * + * @param ID, the Bus ID of the servo 1-255 + */ + int LockEeprom(int ID); + + /** Change the high voltage limit of a servo + * + * @param ID, the Bus ID of the servo 1-255 + * @param hv_limit, 0-254 + * + * voltage = hv_limit / 10 + */ + int SetHighVoltageLimit (int ID, char hv_limit); + + /** Change the delay time of a servo + * + * @param ID, the Bus ID of the servo 1-255 + * @param delay, 0-254 C + * + */ + int SetDelayTime (int ID, char delay); + + /** Change the high temperature limit of a servo + * + * @param ID, the Bus ID of the servo 1-255 + * @param temp_limit, 0-254 C + * + */ + int SetTemperatureLimit (int ID, char temp_limit); + + /** Change the ID of a servo + * + * @param CurentID 1-255 + * @param NewID 1-255 + * + * If a servo ID is not know, the broadcast address of 0 can be used for CurrentID. + * In this situation, only one servo should be connected to the bus + */ + int SetID(int CurrentID, int NewID); + + /** Change the baud of a servo + * + * @param ID, the Bus ID of the servo 1-255 + * @param baud, 0-252 + * + * baudrate = 2000000/baud (with special cases 250=2.25Mbps, 251=2.5Mbps and 252=3Mbps on MX-28 only) + */ + int SetBaud (int ID, int baud); + + /** Poll to see if the servo is moving + * + * @param ID, the Bus ID of the servo 1-255 + * @returns true is the servo is moving + */ + int isMoving(int ID); + + /** Send the broadcast "trigger" command, to activate any outstanding registered commands + */ + void trigger(void); + + /** Read the model number of the servo + * + * @param ID, the Bus ID of the servo 1-255 + * @returns int matching defined model number + */ + int GetModelNumber(int ID); + + /** Read the current angle of the servo + * + * @param ID, the Bus ID of the servo 1-255 + * @returns float in the range 0.0-300.0 + */ + float GetPositionDegrees(int ID); + + + /** Read the raw angle reading of the servo + * + * @param ID, the Bus ID of the servo 1-255 + * @returns short in the range 0 - 4095 for MX-28 + */ + short GetPosition(int ID); + + + /** Read the temperature of the servo + * + * @returns float temperature + */ + float GetTemp(int ID); + + /** Read the supply voltage of the servo + * + * @param ID, the Bus ID of the servo 1-255 + * @returns short voltage * 10 + */ + short GetVoltage(int ID) ; + + /** Read the supply voltage of the servo + * + * @param ID, the Bus ID of the servo 1-255 + * @returns float voltage + */ + float GetVolts(int ID); + + /** Read the temperature of the servo + * + * @returns short temperature + */ + short GetTemperature(int ID); + + /** Read the torque load of the servo + * + * @param ID, the Bus ID of the servo 1-255 + * @returns short load, range 0-2048, 0 is 100% CCW, 1024 is zero and 2048 is 100% CW + */ + short GetLoad(int ID) ; + + /** Read the speed of the servo + * + * @param ID, the Bus ID of the servo 1-255 + * @returns short speed + */ + short GetSpeed(int ID) ; + + /** Change the internal baud rate to something other than 1000000 (note MX-28s default to 57600) + * + * @param baud, the baud rate to set + * @param delaytime, the delay time to set (x2 us) + */ + void SetInitBaud(int baud, int delaytime); + + +private : + SerialHalfDuplex _servo; + int read(int ID, int start, int length, char* data); + int write(int ID, int start, int length, char* data, int flag=0); + +}; + + + +#endif + +/* + * Copyright 2017 University of York + * + * Licensed under the Apache License, Version 2.0 (the "License"); you may not use this file except in compliance with the License. + * You may obtain a copy of the License at http://www.apache.org/licenses/LICENSE-2.0 + * Unless required by applicable law or agreed to in writing, software distributed under the License is distributed on an "AS IS" BASIS + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and limitations under the License. + * + */ + +/* + * Copyright (c) 2010, Chris Styles (http://mbed.org) + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ \ No newline at end of file