Hi all.
I've been playing around with some digital servos. The problem I'm having arises because they dont use a standard serial configuration. Best I can find, they use 19200 baud, with 8 bit packets, 2 stop bits and the line voltages inverted. Also, for return data it pulls down on a 'blank' byte (=0x00), to return data.
I have written a program which currently at least sends correctly formatted commands to the servo over serial and an inverting circuit. The servo also responds correctly to these commands. The returns however are garbled. Also, if I connect pin 0 to pin 1 the serial read data is also garbled. The problem therefore appears to be in the initilisation of the Serial connection.
First off, can anyone see what I've done wrong and how to fix it? Secondly, does anyone know how to invert the output from the serial port on a software side?
The current code :-
/* DEFINITIONS */
#define FOSC 16777216 // Clock Speed
#define BAUD 19200 // Serial Communication Speed
#define MYUBRR FOSC/16/BAUD-1 // Time per Bit
/* PROTOTYPES */
void CustomSerial(long ubrr); // Initialises the Serial Protocals on TX and RX with 2 stop, 8 bits
void dataSet(byte data1,byte data2,byte data3); // Sets Serial data to be written
void dataWrite(void); // Writes current data to the Servo via serial
void setup()
{
CustomSerial(MYUBRR);
}
byte data[7];
void loop()
{
// Servo ID set to 0x01
dataSet(0xE9,0x01,0x01); //Sets Speed to min
dataWrite();
dataSet(0x01,0x03,0x03); // Moves Servo to position 1
dataWrite();
delay(20000); // Waits for servo to reposition, V.slow on min speed
dataSet(0xE9,0x01,0xFF); // Sets Servo to max speed
dataWrite();
dataSet(0x01,0x06,0x06); // Moves servo to position 2
dataWrite();
delay(1000);
}
void CustomSerial(long ubrr)
{
UBRR0H = (unsigned char) (ubrr>>8);
UBRR0L = (unsigned char)(ubrr);
//UCSR0B = (1<<RXEN0) | (1<<TXEN0);
UCSR0B = 0x98; // Value to allow both RX and TX with servo
UCSR0C = (1<<USBS0) | (3<<UCSZ00);
}
void dataSet(byte data1,byte data2,byte data3)
{
data[0]=0x80; // Command
data[1]=data1; // Command type
data[2]=data2; // Data 1
data[3]=data3; // Data 2
data[4]=256-((data[0]+data[1]+data[2]+data[3])%256); // Checksum
data[5]=0x00; // Return 1
data[6]=0x00; // Return 2
}
void dataWrite(void)
{
Serial.write(data[0]);
Serial.write(data[1]);
Serial.write(data[2]);
Serial.write(data[3]);
Serial.write(data[4]);
Serial.flush();
Serial.write(data[5]);
data[5]=Serial.read();
Serial.flush();
Serial.write(data[6]);
data[6]=Serial.read();
}