Pololu Micro Maestro darylrobotproject.wordpress.com

#include <SoftwareSerial.h>
#define TXPIN 6
#define RXPIN 7

#define HONRIZONTAL 0
#define VERTICAL 1

SoftwareSerial maestro(RXPIN, TXPIN);
void setup()
{
  // Define the appropriate input/output pins
  pinMode(RXPIN, INPUT);
  pinMode(TXPIN, OUTPUT);
  maestro.begin(115200);
  delay(1000);
}
void loop()
{
  settarget(VERTICAL, 79);
  settarget(HONRIZONTAL, 90);
  delay(1000);
  move();
  delay(1000);
}

void move() {

  settarget(HONRIZONTAL, 60);
  delay(500);

  int i=60;
    while(i<=120)
    {
        settarget(HONRIZONTAL, i);
        delay(100);
        i++;
    }

  delay(500);
  settarget(HONRIZONTAL, 90);
  goHome();
}

void settarget(unsigned char servo, unsigned int target)
{
  target = map(target, 0, 100, 608*4, 2224*4);
  maestro.write(0xAA); //start byte
  maestro.write(0x0C) ; //device id
  maestro.write(0x04); //command number
  maestro.write(servo); //servo number
  maestro.write(target & 0x7F);
  maestro.write((target >> 7) & 0x7F);
}

void goHome()
{
  maestro.write(0xAA); //start byte
  maestro.write(0x0C) ; //device id
  maestro.write(0x22); //go home command number
}