#include <Servo.h>
#define numChannels 8
byte channelValues[numChannels];
Servo servo[numChannels];
enum {header_1st_byte_get, header_2nd_byte_get, body} packet_state;
int servo_pins[] = {10,3,4,5,6,9,7,8};
// *********************** Arduino setup function *****************************
void setup() {
//start serial port
Serial.begin(115200);
//init servo objects
for (uint8_t i=0;i<sizeof(servo_pins)/sizeof(int); i++)
servo[i].attach(servo_pins[i]);
packet_state = header_1st_byte_get;
}
int servo_update_index; // count of received servo data bytes
void process_serial_input_char(int ch);
// *********************** Arduino loop function ******************************
void loop() {
if (Serial.available())
process_serial_input_char(Serial.read());
// Code for other functions (LED flash, sensor read, LCD output, etc).
} // end of 'loop' function
// ********************** Process one received character **********************
//
// Scan until a packet header (">>") has been detected, then copy the
// appropriate number of characters into the servo data buffer, and then
// write servo data buffer to the servo object and return to the starting
// state waiting for the next packet header.
//
// ****************************************************************************
void process_serial_input_char(int ch ) {
// in the initial un-synchronized state, check if the received character
// is the correct value for the first byte of the header. If yes,
// progress to the state which looks for the second byte of the
// header. If not, remain in the current state.
if (header_1st_byte_get == packet_state) {
if ('>' == ch)
packet_state=header_2nd_byte_get;
return;
}
// first byte of header has been detected, now check if next character
// is the correct value for the second byte of the header. If yes,
// progress to the state which handles servo data bytes. If not,
// return to the first state.
if (header_2nd_byte_get == packet_state) {
if ('>' == ch) {
servo_update_index=0;
packet_state=body;
} else
packet_state=header_1st_byte_get;
return;
}
// state for processing servo data after the packet header has been
// detected, returning to the un-synchronized state after the
// appropriate number of servo bytes have been read.
if (body == packet_state) {
if (servo_update_index < numChannels) {
channelValues[servo_update_index++] = ch;
} else {
updateServoPositions();//this function updates the servos
packet_state = header_1st_byte_get;
}
return;
}
}
// Write the servo data buffer values to the servo object.
void updateServoPositions(){
for(byte i=0; i<numChannels; i++){
servo[i].write(map(channelValues[i], 0, 255, 0, 180));
/*uncomment this to print test values to IDE serial monitor
Serial.print("controller output ");
Serial.print(i);
Serial.print(" value = ");
Serial.print(channelValues[i]);
Serial.print("/255 or ");
Serial.print(map(channelValues[i], 0, 255, 0, 180));
Serial.println("/180 degrees");
*/
}
}