Scribd
Upload
139 views

6 Axis Servo Control Code For Arduino

This Arduino sketch controls 6 servos on a robotic arm by receiving 2-byte commands over a serial COM port. It defines pins for each servo, initializes the servos, and maps incoming servo position values from 0-180 degrees to pulse widths to control the servos. When it receives a valid 2-byte command, it identifies the servo to move from the first byte and sets the identified servo to the position value in the second byte.

Uploaded by

Shubham Kumar Kashyap
Copyright
© © All Rights Reserved
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
139 views

6 Axis Servo Control Code For Arduino

This Arduino sketch controls 6 servos on a robotic arm by receiving 2-byte commands over a serial COM port. It defines pins for each servo, initializes the servos, and maps incoming servo position values from 0-180 degrees to pulse widths to control the servos. When it receives a valid 2-byte command, it identifies the servo to move from the first byte and sets the identified servo to the position value in the second byte.

Uploaded by

Shubham Kumar Kashyap
Copyright
© © All Rights Reserved
Available Formats
Download as TXT, PDF, TXT or read online on Scribd
You are on page 1/ 4

// 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();
}
}

You might also like