Przemysław Kochański / Maestro

Library for Pololu Maestro Servo Controller http://www.pololu.com/docs/0J40

Library for Pololu Maestro Servo Controller

Library docs: https://developer.mbed.org/users/kochansky/code/Maestro/docs/f374c7b60318/classMaestro.html

Example usage:

#include "mbed.h"
#include "Maestro.h"

Maestro maestro(PTC4,PTC3);

int main()
{
    maestro.setBaudRate(9600);

    // limit speed for each servo
    for (uint8_t i = 0; i < 17; i++) {
        maestro.setSpeed(i, 10);
    }

    while (true) {
       // set servo on channel 0 to 90 degrees
       maestro.setServoAngle(0, 90);
       wait(2);

        // set servo on channel 0 to 45 degrees
       maestro.setServoAngle(0, 45);
       wait(2);
    }
}

Serial commands based on manual: http://www.pololu.com/docs/0J40/5.e

Device pinout:

/media/uploads/kochansky/maestro.jpg

Maestro.cpp@9:a15ea85b40ca, 2014-08-25 (annotated)

Committer:
kochansky
Date:
Mon Aug 25 19:22:10 2014 +0000
Revision:
9:a15ea85b40ca
Parent:
8:c952ac46bd39
Child:
10:f374c7b60318
setSpeed and setAcceleration for all channels

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kochansky 0:5c8cc5bd6403 1 /**
kochansky 8:c952ac46bd39 2 * @author Przemyslaw Kochanski <przemyslaw@kochanski.biz>
kochansky 8:c952ac46bd39 3 *
kochansky 8:c952ac46bd39 4 * @Section LICENSE
kochansky 8:c952ac46bd39 5 *
kochansky 8:c952ac46bd39 6 * Copyright (C) 2014 Przemyslaw Kochanski, MIT License
kochansky 8:c952ac46bd39 7 *
kochansky 8:c952ac46bd39 8 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
kochansky 8:c952ac46bd39 9 * and associated documentation files (the "Software"), to deal in the Software without restriction,
kochansky 8:c952ac46bd39 10 * including without limitation the rights to use, copy, modify, merge, publish, distribute,
kochansky 8:c952ac46bd39 11 * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
kochansky 8:c952ac46bd39 12 * furnished to do so, subject to the following conditions:
kochansky 8:c952ac46bd39 13 *
kochansky 8:c952ac46bd39 14 * The above copyright notice and this permission notice shall be included in all copies or
kochansky 8:c952ac46bd39 15 * substantial portions of the Software.
kochansky 8:c952ac46bd39 16 *
kochansky 8:c952ac46bd39 17 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
kochansky 8:c952ac46bd39 18 * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
kochansky 8:c952ac46bd39 19 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
kochansky 8:c952ac46bd39 20 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
kochansky 8:c952ac46bd39 21 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
kochansky 0:5c8cc5bd6403 22 *
kochansky 0:5c8cc5bd6403 23 * @section DESCRIPTION
kochansky 0:5c8cc5bd6403 24 *
kochansky 0:5c8cc5bd6403 25 * Library for Pololu Maestro Servo Controller
kochansky 0:5c8cc5bd6403 26 * Serial Servo Commands: http://www.pololu.com/docs/0J40/5.e
kochansky 0:5c8cc5bd6403 27 */
kochansky 0:5c8cc5bd6403 28
kochansky 0:5c8cc5bd6403 29 #include "Maestro.h"
kochansky 0:5c8cc5bd6403 30
kochansky 7:18a57f597ec4 31 Maestro::Maestro(PinName tx, PinName rx) : serial(tx, rx)
kochansky 0:5c8cc5bd6403 32 {
kochansky 1:c14b79e3f39b 33 serial.format(8, SerialBase::None, 1); //8N1
kochansky 3:1c654893354d 34 }
kochansky 3:1c654893354d 35
kochansky 4:ef4d23c023ea 36 void Maestro::setBaudRate(uint16_t baud)
kochansky 3:1c654893354d 37 {
kochansky 4:ef4d23c023ea 38 serial.baud(baud);
kochansky 3:1c654893354d 39 serial.putc(BAUD_RATE_IDICATION);
kochansky 1:c14b79e3f39b 40 }
kochansky 0:5c8cc5bd6403 41
kochansky 3:1c654893354d 42 void Maestro::setTarget(uint8_t channel, uint16_t target)
kochansky 1:c14b79e3f39b 43 {
kochansky 3:1c654893354d 44 serial.putc(SET_TARGET);
kochansky 3:1c654893354d 45 serial.putc(channel);
kochansky 3:1c654893354d 46 serial.putc(target & 0x7F);
kochansky 3:1c654893354d 47 serial.putc((target >> 7) & 0x7F);
kochansky 4:ef4d23c023ea 48 }
kochansky 4:ef4d23c023ea 49
kochansky 4:ef4d23c023ea 50 void Maestro::setServoAngle(uint8_t channel, int8_t angle)
kochansky 4:ef4d23c023ea 51 {
kochansky 4:ef4d23c023ea 52 setTarget(channel, angle * 40 + 6000);
kochansky 5:0fd491357750 53 }
kochansky 5:0fd491357750 54
kochansky 6:1d8357775b6d 55 void Maestro::setMultipleTargets(uint8_t firstChannel, uint8_t count, uint16_t* targets)
kochansky 5:0fd491357750 56 {
kochansky 5:0fd491357750 57 serial.putc(SET_MULTIPLE_TARGETS);
kochansky 5:0fd491357750 58 serial.putc(count);
kochansky 5:0fd491357750 59 serial.putc(firstChannel);
kochansky 5:0fd491357750 60
kochansky 5:0fd491357750 61 for (uint8_t i = 0; i < count; i++) {
kochansky 5:0fd491357750 62 serial.putc(*targets & 0x7F);
kochansky 5:0fd491357750 63 serial.putc((*targets >> 7) & 0x7F);
kochansky 5:0fd491357750 64 targets++;
kochansky 5:0fd491357750 65 }
kochansky 5:0fd491357750 66 }
kochansky 5:0fd491357750 67
kochansky 6:1d8357775b6d 68 void Maestro::setServosAngles(uint8_t firstChannel, uint8_t count, int8_t* angles)
kochansky 5:0fd491357750 69 {
kochansky 5:0fd491357750 70 uint16_t targets[count];
kochansky 5:0fd491357750 71
kochansky 5:0fd491357750 72 for (uint8_t i = 0; i < count; i++) {
kochansky 6:1d8357775b6d 73 targets[i] = 6000 + angles[i] * 40;
kochansky 5:0fd491357750 74 }
kochansky 5:0fd491357750 75
kochansky 6:1d8357775b6d 76 setMultipleTargets(firstChannel, count, targets);
kochansky 5:0fd491357750 77 }
kochansky 5:0fd491357750 78
kochansky 5:0fd491357750 79 void Maestro::setSpeed(uint8_t channel, uint16_t speed)
kochansky 5:0fd491357750 80 {
kochansky 5:0fd491357750 81 serial.putc(SET_SPEED);
kochansky 5:0fd491357750 82 serial.putc(channel);
kochansky 5:0fd491357750 83 serial.putc(speed & 0x7F);
kochansky 5:0fd491357750 84 serial.putc((speed >> 7) & 0x7F);
kochansky 5:0fd491357750 85 }
kochansky 5:0fd491357750 86
kochansky 9:a15ea85b40ca 87 void Maestro::setSpeed(uint16_t speed)
kochansky 9:a15ea85b40ca 88 {
kochansky 9:a15ea85b40ca 89 for (uint8_t i = 0; i < 18; i++) {
kochansky 9:a15ea85b40ca 90 setSpeed(speed);
kochansky 9:a15ea85b40ca 91 }
kochansky 9:a15ea85b40ca 92 }
kochansky 9:a15ea85b40ca 93
kochansky 5:0fd491357750 94 void Maestro::setAcceleration(uint8_t channel, uint16_t acceleration)
kochansky 5:0fd491357750 95 {
kochansky 5:0fd491357750 96 serial.putc(SET_ACCELERATION);
kochansky 5:0fd491357750 97 serial.putc(channel);
kochansky 5:0fd491357750 98 serial.putc(acceleration & 0x7F);
kochansky 5:0fd491357750 99 serial.putc((acceleration >> 7) & 0x7F);
kochansky 5:0fd491357750 100 }
kochansky 5:0fd491357750 101
kochansky 9:a15ea85b40ca 102 void Maestro::setAcceleration(uint16_t acceleration)
kochansky 9:a15ea85b40ca 103 {
kochansky 9:a15ea85b40ca 104 for (uint8_t i = 0; i < 18; i++) {
kochansky 9:a15ea85b40ca 105 setAcceleration(acceleration);
kochansky 9:a15ea85b40ca 106 }
kochansky 9:a15ea85b40ca 107 }
kochansky 9:a15ea85b40ca 108
kochansky 5:0fd491357750 109 void Maestro::setPWM(uint8_t channel, uint16_t time, uint16_t period)
kochansky 5:0fd491357750 110 {
kochansky 5:0fd491357750 111 serial.putc(SET_PWM);
kochansky 5:0fd491357750 112 serial.putc(channel);
kochansky 5:0fd491357750 113 serial.putc(time & 0x7F);
kochansky 5:0fd491357750 114 serial.putc((time >> 7) & 0x7F);
kochansky 5:0fd491357750 115 serial.putc(period & 0x7F);
kochansky 5:0fd491357750 116 serial.putc((period >> 7) & 0x7F);
kochansky 5:0fd491357750 117 }
kochansky 5:0fd491357750 118
kochansky 5:0fd491357750 119 uint16_t Maestro::getPosition(uint8_t channel)
kochansky 5:0fd491357750 120 {
kochansky 5:0fd491357750 121 serial.putc(GET_POSITION);
kochansky 5:0fd491357750 122 serial.putc(channel);
kochansky 5:0fd491357750 123 return serial.getc() | (serial.getc() << 8);
kochansky 5:0fd491357750 124 }
kochansky 5:0fd491357750 125
kochansky 5:0fd491357750 126 bool Maestro::getMovingState()
kochansky 5:0fd491357750 127 {
kochansky 5:0fd491357750 128 serial.putc(GET_MOVING_STATE);
kochansky 5:0fd491357750 129 return serial.getc();
kochansky 5:0fd491357750 130 }
kochansky 5:0fd491357750 131
kochansky 5:0fd491357750 132 uint16_t Maestro::getErrors()
kochansky 5:0fd491357750 133 {
kochansky 5:0fd491357750 134 serial.putc(GET_ERRORS);
kochansky 5:0fd491357750 135 return serial.getc() | (serial.getc() << 8);
kochansky 5:0fd491357750 136 }
kochansky 5:0fd491357750 137
kochansky 5:0fd491357750 138 void Maestro::goHome()
kochansky 5:0fd491357750 139 {
kochansky 5:0fd491357750 140 serial.putc(GO_HOME);
kochansky 7:18a57f597ec4 141 }