S00000000F
Where a 0 => set the point to straight and a 1 => set the point to turn.
The software runs a really nice little state machine to ensure robust communications. I may want to put an ACK on the serial line potentially (taking it to 2 lines to control the system.
You can see the setup in this video:
The code is below (needs comments!).
//
// Points controller using Pololu serial servo controller
// Chris Mendes 2013
//
//
// Software Serial from Arduino example by Tom Igoe
//
// Pololu code borrowed from Mike Crist...
// put() (now position_absolute) & set_speed() functions found on pololu forums & modified
// mike crist 2009-01-03
#include <SoftwareSerial.h>
#define ServoRxPin 2
#define ServoTxPin 3
#define ServoResetPin 4
// Pins for the command port
#define CmdRxPin 5
#define CmdTxPin 6
#define DebugModePin 12
typedef enum CmdStateType {initialising, waiting_good, waiting_error, started, reading_digits, waiting_finish, finished };
CmdStateType CmdState;
#define NUM_POINTS 8
byte PointAngles[NUM_POINTS][2] = { {88,94}, {86,94}, {94,86}, {85,94}, {97, 85}, {109, 96}, {93, 74}, {97, 79}};
byte NextCommand[NUM_POINTS];
byte Command[NUM_POINTS];
byte LastCommand[NUM_POINTS];
byte CmdBit;
boolean GotCommand;
int big_loop;
#define LOOP_RESET 100000
// set up a new serial port
SoftwareSerial softSerial = SoftwareSerial(ServoRxPin, ServoTxPin);
// Set Up the Command Port
SoftwareSerial CmdSerial = SoftwareSerial(CmdRxPin, CmdTxPin);
void setup()
{
int i;
int pos;
// set the data rate for the hardware serial port
Serial.begin(9600);
big_loop = 0;
CmdState = initialising;
pinMode(DebugModePin, INPUT);
//Reset the servo controller
pinMode(ServoResetPin, OUTPUT);
digitalWrite(ServoResetPin, LOW);
delay(20);
digitalWrite(ServoResetPin, HIGH);
pinMode(ServoRxPin, INPUT);
digitalWrite(ServoTxPin, HIGH);
pinMode(ServoTxPin, OUTPUT);
// set the data rate for the SoftwareSerial port
softSerial.begin(38400);
for (i = 0 ; i < 8 ; i++ )
{
set_speed(i, 5);
//delay(100);
set_neutral(i, 3000);
//delay(100);
set_servo_parameters(i, 0, 15);
//delay(100);
// Move all points to position 0 - straight
pos = PointAngles[i][0];
position_8bit(i,(byte)map(pos,0,179,0,255));
}
//wait for them all to move, then turn them off
delay(3000);
for (i = 0 ; i < 8 ; i++ )
{
set_servo_parameters(i, 0, 15);
}
pinMode(CmdRxPin, INPUT);
digitalWrite(CmdTxPin, HIGH);
pinMode(CmdTxPin, OUTPUT);
CmdSerial.begin(9600);
CmdState = waiting_good;
memset(Command,0x00,NUM_POINTS);
memset(LastCommand,0x00,NUM_POINTS);
memset(NextCommand,0x00,NUM_POINTS);
CmdBit = 0;
GotCommand = false;
}
void
HandleBit(char c)
{
byte TempBit = 0x00;
TempBit = c - '0';
NextCommand[CmdBit] = TempBit;
CmdBit++;
}
void
HandleCharacter(char c)
{
switch(CmdState)
{
case waiting_error:
case waiting_good:
if(c == 'S')
{
CmdState = started;
CmdBit = 0;
}
else
{
CmdState = waiting_error;
}
if (CmdState == waiting_error)
digitalWrite(13,HIGH);
else
digitalWrite(13,LOW);
break;
case started:
case reading_digits:
if((c == '0') or (c == '1'))
{
CmdState = reading_digits;
HandleBit(c);
if(CmdBit == 8)
{
CmdState = waiting_finish;
CmdBit = 0;
}
}
else
{
CmdState = waiting_error;
}
break;
case waiting_finish:
if(c == 'F')
{
CmdState = finished;
}
else
{
CmdState = waiting_error;
CmdBit = 0;
memset(NextCommand, 0x00, NUM_POINTS);
}
//break;
// Note no break here - fall through and handle finishing
case finished:
CmdBit = 0;
GotCommand = true;
memcpy(LastCommand, Command, NUM_POINTS);
memcpy(Command, NextCommand, NUM_POINTS);
memset(NextCommand, 0x00, NUM_POINTS);
CmdState = waiting_good;
break;
}
}
void
GetCommand()
{
char c;
if(digitalRead(DebugModePin))
{
if(Serial.available())
{
c = Serial.read();
HandleCharacter(c);
}
}
else
{
if(CmdSerial.available())
{
c = CmdSerial.read();
HandleCharacter(c);
}
}
}
void loop()
{
big_loop++;
GetCommand();
if(GotCommand == true)
{
int i;
for(i = 0; i < NUM_POINTS; i++)
{
if(LastCommand[i] != Command[i])
{
int pos;
pos = PointAngles[i][(Command[i])];
if(digitalRead(DebugModePin))
{
Serial.print("Switch ");
Serial.print(i);
Serial.print(" Set To ");
Serial.print(Command[i]);
Serial.print(" Angle ");
Serial.print(pos);
Serial.println();
}
//move it!
position_8bit(i,(byte)map(pos,0,179,0,255));
// TURN THE SERVO OFF AGAIN TO LOWER CURRENT DRAW
// set_servo_parameters(i, 0, 15);
}
else
{
// motor is now idle so turn it off
set_servo_parameters(i, 0, 15);
}
}
GotCommand = false;
memcpy(LastCommand,Command,NUM_POINTS);
big_loop = 0;
}
if(big_loop == LOOP_RESET)
{
int i;
big_loop = 0;
for (i = 0; i < NUM_POINTS; i++)
{
// motor is now idle so turn it off
set_servo_parameters(i, 0, 15);
}
}
}
void set_servo_parameters(byte servo, byte OnOff, byte rangeVal)
{
//this function uses pololu mode command 0 to set servo parameters
//servo is the servo number (typically 0-7)
//rangeVal of 15 gives 180 deg in 8-bit, 90 deg in 7 bit
byte temp;
byte parameters;
temp = OnOff << 6; //set first two bits of parameters (on = 1, forward = 0)
temp = temp + (rangeVal & 0x1f); //put first five bits of rangeVal into temp
parameters = temp & 0x7f; //take only bottom 7 bits
//Send a Pololu Protocol command
softSerial.write(0x80); //start byte
softSerial.write(0x01); //device id
softSerial.write((byte)0x00); //command number
softSerial.write(servo); //servo number
softSerial.write(parameters); //parameters
}
void set_speed(byte servo, byte speedVal)
{
//this function uses pololu mode command 1 to set speed
//servo is the servo number (typically 0-7)
//speedVal is servo speed (1=slowest, 127=fastest, 0=full)
//set speedVal to zero to turn off speed limiting
speedVal = speedVal & 0x7f; //take only lower 7 bits of the speed
//Send a Pololu Protocol command
softSerial.write(0x80); //start byte
softSerial.write(0x01); //device id
softSerial.write(0x01); //command number
softSerial.write(servo); //servo number
softSerial.write(speedVal); //speed
}
void position_7bit(byte servo, byte posValue)
{
//this function uses pololu mode command 2 to set position
//servo is the servo number (typically 0-7)
//posValue * range (set with command 0) adjusted by neutral (set with command 5)
//determines servo position
byte pos = posValue & 0x7f; //get lower 7 bits of position
//Send a Pololu Protocol command
softSerial.write(0x80); //start byte
softSerial.write(0x01); //device id
softSerial.write(0x02); //command number
softSerial.write(servo); //servo number
softSerial.write(pos); //position
}
void position_8bit(byte servo, byte posValue)
{
//this function uses pololu mode command 3 to set position
//servo is the servo number (typically 0-7)
//posValue * range (set with command 0) adjusted by neutral (set with command 5)
//determines servo position
byte temp;
byte pos_hi,pos_low;
temp = posValue & 0x80; //get bit 8 of position
pos_hi = temp >> 7; //shift bit 8 by 7
pos_low = posValue & 0x7f; //get lower 7 bits of position
//Send a Pololu Protocol command
softSerial.write(0x80); //start byte
softSerial.write(0x01); //device id
softSerial.write(0x03); //command number
softSerial.write(servo); //servo number
softSerial.write(pos_hi); //bits 8 thru 13
softSerial.write(pos_low); //bottom 7 bits
}
void position_absolute(byte servo, int angle)
{
//this function uses pololu mode command 4 to set absolute position
//servo is the servo number (typically 0-7)
//angle is the absolute position from 500 to 5500
unsigned int temp;
byte pos_hi,pos_low;
temp = angle & 0x1f80; //get bits 8 thru 13 of position
pos_hi = temp >> 7; //shift bits 8 thru 13 by 7
pos_low = angle & 0x7f; //get lower 7 bits of position
//Send a Pololu Protocol command
softSerial.write(0x80); //start byte
softSerial.write(0x01); //device id
softSerial.write(0x04); //command number
softSerial.write(servo); //servo number
softSerial.write(pos_hi); //bits 8 thru 13
softSerial.write(pos_low); //bottom 7 bits
}
void set_neutral(byte servo, int angle)
{
//this function uses pololu mode command 5 to set neutral position
//servo is the servo number (typically 0-7)
//angle is the absolute position from 500 to 5500
unsigned int temp;
byte pos_hi,pos_low;
temp = angle & 0x1f80; //get bits 8 thru 13 of position
pos_hi = temp >> 7; //shift bits 8 thru 13 by 7
pos_low = angle & 0x7f; //get lower 7 bits of position
//Send a Pololu Protocol command
softSerial.write(0x80); //start byte
softSerial.write(0x01); //device id
softSerial.write(0x05); //command number
softSerial.write(servo); //servo number
softSerial.write(pos_hi); //bits 8 thru 13
softSerial.write(pos_low); //bottom 7 bits
}
