Sunday, February 6, 2011

Arduino Servo Control

A sketch to drive an arbitrary number of stepper motors and control each one of them independently with potentiometers. It should be able to drive one motor for each available analog input. The sketch uses no libraries, the motors are driven by PWM signals.


// Many Servos no libraries by using arrays, sketch by Fp-2010.
// Change next three to match number of servos, servo pins and pot pins.
const int totalServos=2; // used in the 'for' loops below
const int theServos[]={7,8}; // pins connected to servos
const int thePots[]={4,3}; // pins connected to pots

const int refreshServo=20; // 20mS servo refresh cycle
long servoSpeed[20]; // stores potentiometer values
long pulseWidths[20]; // stores pulseWidths for each servo
long prevPulse[20]; // keeps track of last pulse

void setup(){
     for (int i=0; i < totalServos; i++) { // for every servo
     pinMode(theServos[i],OUTPUT); // declare servo pins as outputs
     pinMode(thePots[i], INPUT); } // declare pot pins as inputs
}

void loop(){
    for (int i=0; i < totalServos; i++) { // for every servo
      servoSpeed[i] = analogRead(thePots[i]); // set speed to pot value
        doServo(theServos[i],servoSpeed[i]);   // move servo
     }
}

void doServo(int thisPin, long pulseWidth){
    pulseWidths[thisPin] = map(pulseWidth,0,1023,560,2310); // map pot input to pulse width range
    if (millis() - prevPulse[thisPin] >= refreshServo){
    prevPulse[thisPin] = millis();
    digitalWrite(thisPin,HIGH);
    delayMicroseconds(pulseWidths[thisPin]);
    digitalWrite(thisPin,LOW); }
}

No comments:

Post a Comment