//*********************************************************************************
***********
// RC Model Servo
// Original code By EAOROBBIE (Robert Lindsay)
// Completely mangled by aarondc
// For free use for Sim Tool Motion Software
//*********************************************************************************
***********
#include <Servo.h>
//#define DEBUG 1 // comment out this line to
remove debuggin [Link] lines
const int kActuatorCount = 2; // how many Actuators we are
handling
// the letters ("names") sent from Sim Tools to identify each actuator
// NB: the order of the letters here determines the order of the remaining
constants kPins and kActuatorScale
const char kActuatorName[kActuatorCount] = { 'R', 'L' };
const int kPins[kActuatorCount] = {4, 5}; // pins to which
the Actuators are attached
const int kActuatorScale[kActuatorCount][2] = { { 0, 179 } , // Right Actuator
scaling
{ 179, 0 } // Left side
Actuator scaling
};
const char kEOL = '~'; // End of Line - the delimiter
for our acutator values
const int kMaxCharCount = 3; // some insurance...
Servo actuatorSet[kActuatorCount]; // our array of Actuators
int actuatorPosition[kActuatorCount] = {90, 90}; // current Actuator positions,
initialised to 90
int currentActuator; // keep track of the current
Actuator being read in from serial port
int valueCharCount = 0; // how many value characters
have we read (must be less than kMaxCharCount!!
// set up some states for our state machine
// psReadActuator = next character from serial port tells us the Actuator
// psReadValue = next 3 characters from serial port tells us the value
enum TPortState { psReadActuator, psReadValue };
TPortState currentState = psReadActuator;
void setup()
{
// attach the Actuators to the pins
for (int i = 0; i < kActuatorCount; i++)
actuatorSet[i].attach(kPins[i]);
// initialise actuator position
for (int i = 0; i < kActuatorCount; i++)
updateActuator(i);
[Link](9600); // opens serial port at a baud rate of 9600
}
void loop()
{
}
// this code only runs when we have serial data available. ie ([Link]() >
0).
void serialEvent() {
char tmpChar;
int tmpValue;
while ([Link]()) {
// if we're waiting for a Actuator name, grab it here
if (currentState == psReadActuator) {
tmpChar = [Link]();
// look for our actuator in the array of actuator names we set up
#ifdef DEBUG
[Link]("read in ");
[Link](tmpChar);
#endif
for (int i = 0; i < kActuatorCount; i++) {
if (tmpChar == kActuatorName[i]) {
#ifdef DEBUG
[Link]("which is actuator ");
[Link](i);
#endif
currentActuator = i; // remember which
actuator we found
currentState = psReadValue; // start looking
for the Actuator position
actuatorPosition[currentActuator] = 0; // initialise the
new position
valueCharCount = 0; // initialise
number of value chars read in
break;
}
}
}
// if we're ready to read in the current Actuator's position data
if (currentState == psReadValue) {
while ((valueCharCount < kMaxCharCount) && [Link]()) {
tmpValue = [Link]();
if (tmpValue != kEOL) {
tmpValue = tmpValue - 48;
if ((tmpValue < 0) || (tmpValue > 9)) tmpValue = 0;
actuatorPosition[currentActuator] =
actuatorPosition[currentActuator] * 10 + tmpValue;
valueCharCount++;
}
else break;
}
// if we've read the value delimiter, update the Actuator and start
looking for the next Actuator name
if (tmpValue == kEOL || valueCharCount == kMaxCharCount) {
#ifdef DEBUG
[Link]("read in ");
[Link](actuatorPosition[currentActuator]);
#endif
// scale the new position so the value is between 0 and 179
actuatorPosition[currentActuator] =
map(actuatorPosition[currentActuator], 0, 255, kActuatorScale[currentActuator][0],
kActuatorScale[currentActuator][1]);
#ifdef DEBUG
[Link]("scaled to ");
[Link](actuatorPosition[currentActuator]);
#endif
updateActuator(currentActuator);
currentState = psReadActuator;
}
}
}
}
// write the current Actuator position to the passed in Actuator
void updateActuator(int thisActuator) {
actuatorSet[thisActuator].write(actuatorPosition[thisActuator]);
}