Here is my code and please tell me if there are any problems with it. Thanks!
Code: Select all
int pinArray[4] = {6, 9, 10, 11}; // analog pins for the servos
int minPulse = 600; // minimum servo position
int maxPulse = 2400; // maximum servo position
int refreshTime = 10; // time (ms) between pulses (50Hz)
/** The Arduino will calculate these values for you **/
int computePulse = 1;
int lastPositionPacket;
int i; // iterator
int servoPin; // control pin for current servo
int userInput[3]; // raw input from serial buffer, 3 bytes
int pulseWidth; // servo pulse width
int servoPosition; // commanded servo position, 0-180 degrees
int pulseRange; // maxPulse - minPulse
int centerServo; // servo starting point
long lastPulse = 0; // recorded time (ms) of the last pulse
int servo; // which servo to pulse? 1-4
int servo1[2]; // servo #1 array{pin, pulsewidth}
int servo2[2]; // servo #2 array{pin, pulsewidth}
int servo3[2]; // servo #3 array{pin, pulsewidth}
int servo4[2]; // servo #4 array{pin, pulsewidth}
int pin; // digital pin for pulse() function
int puls; // pulsewidth for pulse() function
int startbyte; // start byte, begin reading input
void setup() {
// loop through all 4 servo pins
// and set them as OUTPUT
// LED connected to digital pin 13
for (i=0;i<4;i++) {
pinMode(pinArray[i], OUTPUT);
}
// servo starting point (center)
pulseRange = maxPulse - minPulse;
centerServo = maxPulse - ((pulseRange)/2);
pulseWidth = centerServo;
// map pins to servos
servo1[0] = pinArray[3]; // servo #1 is pin 11
servo2[0] = pinArray[2]; // servo #2 is pin 10
servo3[0] = pinArray[1]; // servo #3 is pin 9
servo4[0] = pinArray[0]; // servo #4 is pin 6
// center all servos
servo1[1] = pulseWidth;
servo2[1] = pulseWidth;
servo3[1] = pulseWidth;
servo4[1] = pulseWidth;
// open serial connection
Serial.begin(9600);
}
void loop() {
// wait for serial input (min 3 bytes in buffer)
if (Serial.available() > 2) {
//read the first byte
startbyte = Serial.read();
// if it's really the startbyte (255)
if (startbyte == 255) {
// then get the next two bytes
for (i=0;i<2;i++) {
userInput[i] = Serial.read();
}
// first byte = servo to move?
servo = userInput[0];
// second byte = which position?
servoPosition = userInput[1];
// packet check
if (servoPosition == 255) { servo = 255; }
// positonChangeCheck
// compute pulseWidth from servoPosition
pulseWidth = minPulse + (servoPosition * (pulseRange/180));
// stop servo pulse at min and max
if (pulseWidth > maxPulse) { pulseWidth = maxPulse; }
if (pulseWidth < minPulse) { pulseWidth = minPulse; }
// assign new pulsewidth to appropriate servo
switch (servo) {
case 1:
servo1[1] = pulseWidth;
break;
case 2:
servo2[1] = pulseWidth;
break;
case 3:
servo3[1] = pulseWidth;
break;
case 4:
servo4[1] = pulseWidth;
break;
}
}
}
// pulse each servo
if (millis() - lastPulse >= refreshTime) {
pulse(servo1[0], servo1[1]);
pulse(servo2[0], servo2[1]);
pulse(servo3[0], servo3[1]);
pulse(servo4[0], servo4[1]);
// save the time of the last pulse
lastPulse = millis();
}
}
void pulse(int pin, int puls) {
digitalWrite(pin, HIGH); // start the pulse
delayMicroseconds(puls); // pulse width
digitalWrite(pin, LOW); // stop the pulse
}
