For YRL Robot Arm
servo.h
- Committer:
- jah128
- Date:
- 2017-03-03
- Revision:
- 0:b14dfd8816da
File content as of revision 0:b14dfd8816da:
/* 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 * 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; /** Servo Class * Class with functions to control a Dynamixel single-wire TTL Serial Servo * * Example code for main.cpp: * @code * #include "servo.h" * Servo servo(p9,p10) * int main(){ * arm.init(); * while(1) { //Do something! * } * } * @endcode */ 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); /** Returns the software lower limit for the given servo * * @param ID, the servo ID to return the limit for * @returns short limit (either preset value or 0) */ short GetLowerLimit(int ID); /** Returns the software upper limit for the given servo * * @param ID, the servo ID to return the limit for * @returns short limit (either preset value or 4095) */ short GetUpperLimit(int ID); 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. */