// This Arduino sketch controls 6 servos on a robotic arm by receiving commands
over a serial COM port
//
// by Andy Crook 2009
//
// www.c-digital-art.co.uk
//
//
#include <ServoTimer2.h> // the servo library
// define the pins for the servos
#define basePin 2
#define shoulderPin 3
#define elbowPin 4
#define wristPin 5
#define wristrotatePin 6
#define gripPin 7
#define MIN_PULSE 540 // pulse in microseconds for servo's
#define MAX_PULSE 2200
ServoTimer2
ServoTimer2
ServoTimer2
ServoTimer2
ServoTimer2
ServoTimer2
servoBase;
// declare variables for the 6 servos
servoShoulder;
servoElbow;
servoWrist;
servoWristrotate;
servoGrip;
int incomingByte = 0; // for incoming data
int servovalue = 0; // for servo value
int pos = 0;
long val0 = 0;
int
int
int
int
int
int
base = 0;
shoulder = 0;
elbow = 0;
wrist = 0;
wristrotate = 0;
grip = 0;
void setup() {
servoBase.attach(basePin);
// attach all pins to the servos
servoShoulder.attach(shoulderPin);
servoElbow.attach(elbowPin);
servoWrist.attach(wristPin);
servoWristrotate.attach(wristrotatePin);
servoGrip.attach(gripPin);
servoBase.write(1000);
servoShoulder.write(1000);
servoElbow.write(1000);
servoWrist.write(1000);
servoWristrotate.write(1300);
servoGrip.write(600);
// set initial values for all servos
// will calibrate these once arm is built
// for a good 'initial setting'
Serial.begin(9600);
// opens serial port and sets data rate to 9600 bps
void loop()
{
if (Serial.available() > 1) {
// if there are more than 1 bytes of data in
the buffer read them in 2 byte chunks
// this will cycle round every 2 bytes. As long as you send 2 bytes at a time
to the buffer
// they should all set fine. If one byte is sent it will wait for the next on
e but
// if no appropriate header byte is found, it will do nothing but clear the b
uffer
//
// the servovalue byte must be from 0 - 180 for the map function to work righ
t
// example setting for all 6 servos:
// ABCDEF
// As a byte sequence this is: 200 65 201 66 202 67 203 68 204 69 205 70
// read incoming byte
incomingByte = Serial.read();
servovalue = Serial.read();
Serial.print("Servo Command: ");
Serial.print(incomingByte, DEC);
//incomingByte = (incomingByte, DEC);
if (incomingByte ==
Serial.print(" }
if (incomingByte ==
Serial.print(" }
if (incomingByte ==
Serial.print(" -
200){
BASE ");
201){
SHOULDER ");
202){
ELBOW ");
}
if (incomingByte ==
Serial.print(" }
if (incomingByte ==
Serial.print(" }
if (incomingByte ==
Serial.print(" }
203){
WRIST ");
204){
WRIST ROTATE ");
205){
GRIP ");
if (incomingByte > 199 && incomingByte < 206) // if command is valid, state s
ervo setting
{
Serial.print(" - VALUE: ");
Serial.print(servovalue, DEC);
Serial.println(" Degrees");
// Serial.println(" ");
}
else
{
Serial.println(" Command not recognised "); // if command header byte is no
t 200-205, signal error
}
if (incomingByte == 200) // sets base to value following 200
{
pos = servovalue;
base = pos; // keep value of base set for future use (not implemented yet)
val0 = map(pos, 0, 180, MIN_PULSE , MAX_PULSE );
servoBase.write(val0);
// tell servo to go to position in vari
able 'pos' from 0 to 180
}
if (incomingByte == 201) // sets shoulder to value following 201
{
pos = servovalue;
shoulder = pos;
val0 = map(pos, 0, 180, MIN_PULSE , MAX_PULSE );
servoShoulder.write(val0);
// tell servo to go to position in
variable 'pos' from 0 to 180
}
if (incomingByte == 202) // sets elbow to value following 202
{
pos = servovalue;
elbow = pos;
val0 = map(pos, 0, 180, MIN_PULSE , MAX_PULSE );
servoElbow.write(val0);
// tell servo to go to position in var
iable 'pos' from 0 to 180
}
if (incomingByte == 203) // sets wrist to value following 203
{
pos = servovalue;
wrist = pos;
val0 = map(pos, 0, 180, MIN_PULSE , MAX_PULSE );
servoWrist.write(val0);
// tell servo to go to position in var
iable 'pos' from 0 to 180
}
if (incomingByte == 204) // sets wristrotate to value following 204
{
pos = servovalue;
wristrotate = pos;
val0 = map(pos, 0, 180, MIN_PULSE , MAX_PULSE );
servoWristrotate.write(val0);
// tell servo to go to position
in variable 'pos' from 0 to 180
}
if (incomingByte == 205) // sets grip to value following 205
{
pos = servovalue;
grip = pos;
val0 = map(pos, 0, 180, MIN_PULSE , MAX_PULSE );
servoGrip.write(val0);
// tell servo to go to position in vari
able 'pos' from 0 to 180
Serial.println(val0);
}
if (Serial.available() == 1) // if there is only one byte left after read, then
read it to discard it
{
Serial.println("Serial Buffer ERROR: One byte left -> Discarded");
Serial.read();
}
}
