File content as of revision 10:f374c7b60318:

/**
 * @author Przemyslaw Kochanski <przemyslaw@kochanski.biz>
 *
 * @Section LICENSE
 *
 * Copyright (C) 2014 Przemyslaw Kochanski, MIT License
 *
 * 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.
 *
 * @section DESCRIPTION
 *
 * Library for Pololu Maestro Servo Controller
 * Serial Servo Commands: http://www.pololu.com/docs/0J40/5.e
 */

#include "Maestro.h"

Maestro::Maestro(PinName tx, PinName rx) : serial(tx, rx)
{
    serial.format(8, SerialBase::None, 1); //8N1
}

void Maestro::setBaudRate(uint16_t baud)
{
    serial.baud(baud);
    serial.putc(BAUD_RATE_IDICATION);
}

void Maestro::setTarget(uint8_t channel, uint16_t target)
{
    serial.putc(SET_TARGET);
    serial.putc(channel);
    serial.putc(target & 0x7F);
    serial.putc((target >> 7) & 0x7F);
}

void Maestro::setServoAngle(uint8_t channel, int8_t angle)
{
    setTarget(channel, angle * 40 + 6000);
}

void Maestro::setMultipleTargets(uint8_t firstChannel, uint8_t count, uint16_t* targets)
{
    serial.putc(SET_MULTIPLE_TARGETS);
    serial.putc(count);
    serial.putc(firstChannel);

    for (uint8_t i = 0; i < count; i++) {
        serial.putc(*targets & 0x7F);
        serial.putc((*targets >> 7) & 0x7F);
        targets++;
    }
}

void Maestro::setServosAngles(uint8_t firstChannel, uint8_t count, int8_t* angles)
{
    uint16_t targets[count];

    for (uint8_t i = 0; i < count; i++) {
        targets[i] = 6000 + angles[i] * 40;
    }

    setMultipleTargets(firstChannel, count, targets);
}

void Maestro::setSpeed(uint8_t channel, uint16_t speed)
{
    serial.putc(SET_SPEED);
    serial.putc(channel);
    serial.putc(speed & 0x7F);
    serial.putc((speed >> 7) & 0x7F);
}

void Maestro::setSpeed(uint16_t speed)
{
    for (uint8_t i = 0; i < 18; i++) {
        setSpeed(i, speed);
    }
}

void Maestro::setAcceleration(uint8_t channel, uint16_t acceleration)
{
    serial.putc(SET_ACCELERATION);
    serial.putc(channel);
    serial.putc(acceleration & 0x7F);
    serial.putc((acceleration >> 7) & 0x7F);
}

void Maestro::setAcceleration(uint16_t acceleration)
{
    for (uint8_t i = 0; i < 18; i++) {
        setAcceleration(i, acceleration);
    }
}

void Maestro::setPWM(uint8_t channel, uint16_t time, uint16_t period)
{
    serial.putc(SET_PWM);
    serial.putc(channel);
    serial.putc(time & 0x7F);
    serial.putc((time >> 7) & 0x7F);
    serial.putc(period & 0x7F);
    serial.putc((period >> 7) & 0x7F);
}

uint16_t Maestro::getPosition(uint8_t channel)
{
    serial.putc(GET_POSITION);
    serial.putc(channel);
    return serial.getc() | (serial.getc() << 8);
}

bool Maestro::getMovingState()
{
    serial.putc(GET_MOVING_STATE);
    return serial.getc();
}

uint16_t Maestro::getErrors()
{
    serial.putc(GET_ERRORS);
    return serial.getc() | (serial.getc() << 8);
}

void Maestro::goHome()
{
    serial.putc(GO_HOME);
}