servo.ino

Go to the documentation of this file.

#ifndef SIMULATION
// allow simulations to define their own servo libraries
#include <Servo.h>
// assume that simulation has its own callibration
#include "settings.h"
#else
// prototyping is only required if not using arduino
#include <servo.h>
#endif

#include <stdint.h>

Servo servo [SERVOS];

uint8_t current_pos [SERVOS] = { 0 };

char buf [BUFSIZE];

enum
{
    MIN_LIM = 0,
    MAX_LIM = 1,
    CANCEL_SIGNAL = 255,
    WAIT_RESPONSE = 254,
    DUMP_SIGNAL = 253,
    START_RESPONSE = 252,
    END_RESPONSE = 251,
    DUMP_RESPONSE_LEN = SERVOS * 2 + 4,
};

void setup ()
{
    int angle, pin;

    Serial.begin(SERIAL_BAUDRATE);

    serialPrint ("starting...\n");
    for (uint8_t servo_index = 0; servo_index < SERVOS; servo_index++)
    {
// simulations don't have pins
#ifndef SIMULATION
        serialPrintIntPretty ("assinging servo: ", servo_index, "\n");
        pin = getServoPinFromIndex (servo_index);
        if (pin < 0)
        {
            serialPrint ("invalid servo index.\n");
        }
        else
        {
            serialPrintIntPretty ("pin: ", pin, "\n");
            servo [servo_index].attach (pin);
            pinMode (pin, OUTPUT);
        }
#endif

        serialPrintIntPretty ("default: ", default_pos [servo_index], "\n");
        setServoFromIndex (servo_index, default_pos [servo_index]);
    }
}

void loop ()
{
    static int servo_id, servo_angle;

    // indicate that we are waiting for input
    if (! Serial.available ())
    {
        serialPrint ("WAIT\n");
        Serial.write (WAIT_RESPONSE);
    }

    servo_id = serialGetByte ();
    // WAIT_SIGNAL recieved
    if (servo_id < 0)
    {
        return;
    }
    if (servo_id == DUMP_SIGNAL)
    {
        dump ();
        return;
    }
    serialPrintIntPretty ("recieved servo id: ", servo_id, "\n");

    servo_angle = serialGetByte ();
    if (servo_angle < 0)
    {
        return;
    }
    if (servo_angle == DUMP_SIGNAL)
    {
        dump ();
        return;
    }
    serialPrintIntPretty ("recieved servo value: ", servo_angle, "\n");

    setServoFromID (servo_id, servo_angle);
}

void serialPrint (const char* s)
{
    if (VERBOSE)
    {
        Serial.print (s);
    }
}

void serialPrintInt (uint8_t i)
{
    if (VERBOSE)
    {
        Serial.print (i, DEC);
    }
}

void serialPrintIntPretty (const char* pre, uint8_t i, const char* post)
{
    if (VERBOSE)
    {
        Serial.write (pre);
        serialPrintInt (i);
        Serial.write (post);
    }
}

int serialGetByte ()
{
    int i;

    serialWait ();
    i = Serial.read ();
    if (i == CANCEL_SIGNAL)
    {
        serialPrint ("CANCEL_SIGNAL\n");
        return -1;
    }

    return i;
}

void setServoFromIndex (uint8_t servo_index, uint8_t servo_angle)
{
    if (servo_index >= SERVOS || servo_index < 0)
    {
        serialPrint ("servo index out of range.\n");
        return;
    }

    if (servo_angle > 180)
    {
        servo_angle = 180;
    }
    if (servo_angle < 0)
    {
        servo_angle = 0;
    }

    current_pos [servo_index] = servo_angle;

    if (reverse [servo_index])
    {
        servo_angle = map (servo_angle, 180, 0,
            limit [servo_index] [MIN_LIM], limit [servo_index] [MAX_LIM]);
    }
    else
    {
        servo_angle = map (servo_angle, 0, 180,
            limit [servo_index] [MIN_LIM], limit [servo_index] [MAX_LIM]);
    }

    serialPrintIntPretty ("readjusted value: ", servo_angle, "\n");

    servo [servo_index].write (servo_angle);
}

void setServoFromID (int servo_id, uint8_t servo_angle)
{
    int servo_index;

    servo_index = getServoIndexFromID (servo_id);
    if (servo_index < 0)
    {
        serialPrint ("servo ID not valid.\n");
        return;
    }

    setServoFromIndex (servo_index, servo_angle);
}

void dump ()
{
    int i = 0;

    // if you change this function, please verify DUMP_RESPONSE_LEN
    if (BUFSIZE < DUMP_RESPONSE_LEN)
    {
        return;
    }

    buf [i++] = START_RESPONSE;
    buf [i++] = BOARD_ID;
    buf [i++] = SERVOS;
    for (int servo_index = 0; servo_index < SERVOS; servo_index++)
    {
        buf [i++] = getServoIDFromIndex (servo_index);
        buf [i++] = current_pos [servo_index];
    }
    buf [i++] = END_RESPONSE;

    Serial.write (buf, i);
}

void serialWait ()
{
    while (!Serial.available ());
}