Maestro.cpp

00001 /**
00002  * @author Przemyslaw Kochanski <przemyslaw@kochanski.biz>
00003  *
00004  * @Section LICENSE
00005  *
00006  * Copyright (C) 2014 Przemyslaw Kochanski, MIT License
00007  *
00008  * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
00009  * and associated documentation files (the "Software"), to deal in the Software without restriction,
00010  * including without limitation the rights to use, copy, modify, merge, publish, distribute,
00011  * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
00012  * furnished to do so, subject to the following conditions:
00013  *
00014  * The above copyright notice and this permission notice shall be included in all copies or
00015  * substantial portions of the Software.
00016  *
00017  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
00018  * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
00019  * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
00020  * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00021  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
00022  *
00023  * @section DESCRIPTION
00024  *
00025  * Library for Pololu Maestro Servo Controller
00026  * Serial Servo Commands: http://www.pololu.com/docs/0J40/5.e
00027  */
00028 
00029 #include "Maestro.h"
00030 
00031 Maestro::Maestro(PinName tx, PinName rx) : serial(tx, rx)
00032 {
00033     serial.format(8, SerialBase::None, 1); //8N1
00034 }
00035 
00036 void Maestro::setBaudRate(uint16_t baud)
00037 {
00038     serial.baud(baud);
00039     serial.putc(BAUD_RATE_IDICATION);
00040 }
00041 
00042 void Maestro::setTarget(uint8_t channel, uint16_t target)
00043 {
00044     serial.putc(SET_TARGET);
00045     serial.putc(channel);
00046     serial.putc(target & 0x7F);
00047     serial.putc((target >> 7) & 0x7F);
00048 }
00049 
00050 void Maestro::setServoAngle(uint8_t channel, int8_t angle)
00051 {
00052     setTarget(channel, angle * 40 + 6000);
00053 }
00054 
00055 void Maestro::setMultipleTargets(uint8_t firstChannel, uint8_t count, uint16_t* targets)
00056 {
00057     serial.putc(SET_MULTIPLE_TARGETS);
00058     serial.putc(count);
00059     serial.putc(firstChannel);
00060 
00061     for (uint8_t i = 0; i < count; i++) {
00062         serial.putc(*targets & 0x7F);
00063         serial.putc((*targets >> 7) & 0x7F);
00064         targets++;
00065     }
00066 }
00067 
00068 void Maestro::setServosAngles(uint8_t firstChannel, uint8_t count, int8_t* angles)
00069 {
00070     uint16_t targets[count];
00071 
00072     for (uint8_t i = 0; i < count; i++) {
00073         targets[i] = 6000 + angles[i] * 40;
00074     }
00075 
00076     setMultipleTargets(firstChannel, count, targets);
00077 }
00078 
00079 void Maestro::setSpeed(uint8_t channel, uint16_t speed)
00080 {
00081     serial.putc(SET_SPEED);
00082     serial.putc(channel);
00083     serial.putc(speed & 0x7F);
00084     serial.putc((speed >> 7) & 0x7F);
00085 }
00086 
00087 void Maestro::setSpeed(uint16_t speed)
00088 {
00089     for (uint8_t i = 0; i < 18; i++) {
00090         setSpeed(i, speed);
00091     }
00092 }
00093 
00094 void Maestro::setAcceleration(uint8_t channel, uint16_t acceleration)
00095 {
00096     serial.putc(SET_ACCELERATION);
00097     serial.putc(channel);
00098     serial.putc(acceleration & 0x7F);
00099     serial.putc((acceleration >> 7) & 0x7F);
00100 }
00101 
00102 void Maestro::setAcceleration(uint16_t acceleration)
00103 {
00104     for (uint8_t i = 0; i < 18; i++) {
00105         setAcceleration(i, acceleration);
00106     }
00107 }
00108 
00109 void Maestro::setPWM(uint8_t channel, uint16_t time, uint16_t period)
00110 {
00111     serial.putc(SET_PWM);
00112     serial.putc(channel);
00113     serial.putc(time & 0x7F);
00114     serial.putc((time >> 7) & 0x7F);
00115     serial.putc(period & 0x7F);
00116     serial.putc((period >> 7) & 0x7F);
00117 }
00118 
00119 uint16_t Maestro::getPosition(uint8_t channel)
00120 {
00121     serial.putc(GET_POSITION);
00122     serial.putc(channel);
00123     return serial.getc() | (serial.getc() << 8);
00124 }
00125 
00126 bool Maestro::getMovingState()
00127 {
00128     serial.putc(GET_MOVING_STATE);
00129     return serial.getc();
00130 }
00131 
00132 uint16_t Maestro::getErrors()
00133 {
00134     serial.putc(GET_ERRORS);
00135     return serial.getc() | (serial.getc() << 8);
00136 }
00137 
00138 void Maestro::goHome()
00139 {
00140     serial.putc(GO_HOME);
00141 }