![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Code to set up an AX-12 or MX-28 (and to scan for unidentified servos) ready for use in the robot arm.
Diff: servo.h
- Revision:
- 0:03f84c95f73b
diff -r 000000000000 -r 03f84c95f73b servo.h --- /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