next topic »

[on:]

hi all -

i've written some r/c servo controller code. i'd like to release it as a library. i downloaded the Test library example, but i just can't understand it at all. i have absolutely no idea where to put all the different parts of my code. i don't know c++, only c. could anyone give me some clues? here's the code as written into a sketch, which works fine:

Code:

// eight-channel servo system - by Jeff Mann copyright 2007 all rights reserved

// if we have an ATmega168, we can use timer 0, which is running in
// fast pwm mode, and keep timer 2 available for phase correct pwm.
// NOTE: this requires that the timer0 overflow interrupt routine in
// lib/targets/arduino/wiring.c be commented out! find the following:
//
// SIGNAL(SIG_OVERFLOW0)
// {
//      timer0_overflow_count++;
// }
// ...and comment it out. remember to re-enable it for other programs!

#if defined(__AVR_ATmega168__)
#define USE_TIMER_0
#endif

// limits on servo pulse width - standard is 1000-2000 microseconds
// wide is roughly 500-2250 microseconds, but that may damage your servo!
#define MAX_SERVO_PULSE 2000
#define MIN_SERVO_PULSE 1000

// utilities to clear/set bits in special function registers
#ifndef cbi
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#endif
#ifndef sbi
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#endif

#define TRUE 1
#define FALSE 0

#define SERVO 3 // as opposed to INPUT or OUTPUT

/* variables for Servos */
extern volatile unsigned long timer0_overflow_count; // timer0 from wiring.c
int servoStatus = 0; // define which servo pins output pulses or not
byte processServos = FALSE;
byte servoCount = 0;
byte cycleCount = 0;
byte cycles;
byte turnOff = 0;

// define the mapping of logical servo numbers to arduino pins.
// it's not recommended to change this. if you want to,
// you can re-arrange these, but you must always have exactly 8 servos,
// listed here (numbered 0-7), whether you use them or not.
#define TOTAL_DIGITAL_PINS 14
#define NOT_A_SERVO 255
byte digital_pin_to_servo_array[14] = {
  NOT_A_SERVO, // pin 0 - RX - cannot use
  NOT_A_SERVO, // pin 1 - TX - cannot use
  0,           // pin 2
  1,           // pin 3 - PWM 2B on ATmega168
  2,           // pin 4
  3,           // pin 5 - PWM 0B on ATmega168
  4,           // pin 6 - PWM 0A on ATmega168, but not working if using servos
  5,           // pin 7
  6,           // pin 8
  NOT_A_SERVO, // pin 9  - PWM 1A
  NOT_A_SERVO, // pin 10 - PWM 1B
  NOT_A_SERVO, // pin 11 - PWM 2(A) but not working on atmega8 when using servos
  7,           // pin 12
  NOT_A_SERVO, // pin 13 - LED
};


typedef struct {
  byte pin;
  byte cycles;
  byte ticks;
}
servo_t;

servo_t servo_array[8];

#ifdef USE_TIMER_0
ISR(TIMER0_OVF_vect) {
  timer0_overflow_count++; // for millis() clock
  if(processServos) {
    if((cycleCount == 0) && (servoStatus & (1 << servo_array[servoCount].pin))) {
      digitalWrite(servo_array[servoCount].pin, HIGH);
    }
    if(cycleCount == cycles) turnOff = TRUE;
    ++cycleCount;
  }
}

ISR(TIMER0_COMPA_vect) {
  if(processServos) {
    // turn off the pin for the current servo if flagged
    if(turnOff) {
      if(servoStatus & (1 << servo_array[servoCount].pin)) {
        digitalWrite(servo_array[servoCount].pin, LOW);
      }
      turnOff = FALSE;
    }
    if(cycleCount == 3) {
      if(++servoCount > 7) servoCount = 0;
      // load values for next cycle
      cycles = servo_array[servoCount].cycles;
      OCR0A = servo_array[servoCount].ticks;
      cycleCount = 0;
    }
  }
}
#else
ISR(TIMER2_OVF_vect) {
  if(processServos) {
    if((cycleCount == 0) && (servoStatus & (1 << servo_array[servoCount].pin))) {
      digitalWrite(servo_array[servoCount].pin, HIGH);
    }
    if(cycleCount == cycles) turnOff = TRUE;
    ++cycleCount;
  }
}

ISR(TIMER2_COMP_vect) {
  if(processServos) {
    // turn off the pin for the current servo if flagged
    if(turnOff) {
      if(servoStatus & (1 << servo_array[servoCount].pin)) {
        digitalWrite(servo_array[servoCount].pin, LOW);
      }
      turnOff = FALSE;
    }
    if(cycleCount == 3) {
      if(++servoCount > 7) servoCount = 0;
      // load values for next cycle
      cycles = servo_array[servoCount].cycles;
      OCR2 = servo_array[servoCount].ticks;
      cycleCount = 0;
    }
  }
}
#endif

// continued in next post!

[Reply #1 on:]

(the rest of the code)

Code:

// (continued from previous post!!
// eight-channel servos - copyright Jeff Mann 2007 all rights reserved

// user api
// -----------------------------------------------------------------------------
// turns on a servo pulse of the specified length, on the specified pin
// note: disables pwm on pin 11 (pin 6 on ATmega168) if used.
void servoWrite(byte pin, int microseconds) {
  if(digital_pin_to_servo_array[pin] != NOT_A_SERVO) {
    if((! servoStatus) || (processServos == FALSE)) {
      // no servos are active, need to reconfigure pwm timer for servos' use
#ifdef USE_TIMER_0
      // using timer 0 on ATmega168 "arduino plus", already in fastpwm mode
      cbi(TCCR0A, COM0A1); // disconnect timer 0A from pwm output pin 6
#else
      // using timer 2 on ATmega8
      cbi(TCCR2,COM21);  // disconnect timer 2 from pwm output pin 11
      sbi(TCCR2,WGM21);  // set timer 2 for fast pwm mode
#endif
      processServos = TRUE; // flag ISR to process servo pulses
    }

    if ( microseconds > MAX_SERVO_PULSE ) {
      microseconds = MAX_SERVO_PULSE;
    }
    else if ( microseconds < MIN_SERVO_PULSE ) {
      microseconds = MIN_SERVO_PULSE;
    }
    servoStorePulseWidth(digital_pin_to_servo_array[pin], microseconds);
    if(!(servoStatus & (1 << pin))) { // if this servo is not already active
      setPinMode(pin, SERVO); // register this pin as active with a servo
    }

  }
}

void setPinMode(byte pin, byte mode) {
  if(mode == SERVO) {
    if(digital_pin_to_servo_array[pin] != NOT_A_SERVO) {
      servoStatus = servoStatus | (1 << pin);
      pinMode(pin,OUTPUT);
    }
    else if(mode == INPUT) {
      servoStatus = servoStatus &~ (1 << pin);
      pinMode(pin,INPUT);
    }
    else if(mode == OUTPUT) {
      servoStatus = servoStatus &~ (1 << pin);
    }
  }
}


// private functions
// -----------------------------------------------------------------------------
// convert microseconds to timer cycles + ticks, and store in the servo array
void servoStorePulseWidth(byte servoNumber, int microseconds) {
  // divide microseconds by 4 to get total number of 4uS timer ticks
  microseconds = microseconds >> 2;
  // divide by 256 to get number of 256-tick cycles
  servo_array[servoNumber].cycles = microseconds >> 8;  
  // get leftover number of ticks
  servo_array[servoNumber].ticks = microseconds % 256;
}


// example sketch

int servoPosition1 = 1500;
int servoPosition2 = 1500;
int servoPosition3 = 1500;

void setup() {

  byte i;

  // servo stuff:
  // initialize pin data in servo array so we can do a reverse lookup
  for(i = 0; i < TOTAL_DIGITAL_PINS; ++i) {
    if (digital_pin_to_servo_array[i] != NOT_A_SERVO) {
      servo_array[digital_pin_to_servo_array[i]].pin = i;
    }
  }
  // initialize position data in servo array. these are never actually
  // output, but required to have sane values in the array.
  for (i=0; i<=7; ++i) {
    servoStorePulseWidth(i, 1500);
    

  }

  // turn on timer interrupts for servos. interrupt service routines will be
  // active, but not output any pulses until servoWrite() is called.
#ifdef USE_TIMER_0
  // using timer 0, compare unit A, for servo pulses. already set for /64
  // prescale and overflow interrupt enabled, for arduino millis() clock.
  // already set for fastpwm for "arduino plus" additional pwm outputs.
  sbi(TIMSK0, OCIE0A); // enable timer 0A compare match interrupt
#else
  // using timer 2 compare unit for servo pulses. already set for /64
  // prescale. initially set for phase correct pwm output; we will change
  // that when servoWrite() is called. here we enable the interrupts.
  sbi(TIMSK,TOIE2);  // enable timer 2 overflow interrupt
  sbi(TIMSK, OCIE2); // enable timer 2 compare match interrupt
#endif

processServos = TRUE; // turns on pulse outputs

  // user setup stuff goes here
  
  //Serial.begin(9600); // 9600, 14400, 38400, 57600, 115200

}

// symbolic names for pin numbers
#define servoPin1 2
#define servoPin2 4
#define servoPin3 6
#define ledPin 13


void loop() {
  
  digitalWrite(ledPin, HIGH);   // sets the LED on

  servoWrite(servoPin1, servoPosition1); // turns on servo pulse on pin 2
  servoWrite(servoPin2, servoPosition2);
  servoWrite(servoPin3, servoPosition3);
  servoPosition1 += 1;
  if (servoPosition1 > 2000) servoPosition1 = 1000;
  servoPosition2 += 1;
  if (servoPosition2 > 2000) servoPosition2 = 1000;
  servoPosition3 += 1;
  if (servoPosition3 > 2000) servoPosition3 = 1000;
  
  delay(5);

}

[Reply #2 on:]

I wrote a tutorial on writing libraries.  I hope it helps answer some of your questions.