Servo Code!

Show off your projects to others!
pburgess
Posts: 15
Joined: Sun Jun 05, 2011 9:05 am

Servo Code!

Post by pburgess » Thu Jun 16, 2011 10:47 am

There have been several threads in several places bemoaning the lack of a working Servo library. Had a need myself, so put something together today. This isn't compatible with the standard Servo lib (isn't even a library for that matter, just a sketch you can adapt to your own needs) and the units are somewhat ambiguous CPU 'tick' measurements. On the plus side, handles up to 8 servos with ludicrous resolution and is not terribly processor-intensive. Hope it's of some use to folks...and don't be frightened by the size, it's mostly comments!

Code: Select all

// Example chipKIT sketch for precise control of up to
// eight servos with low instruction overhead.  Exploits
// the fact that servo PWM uses a relatively small duty
// cycle range for a full range of movement: servo pulses
// are offset (rather than synchronized) so that each has
// the full resolution of a PWM timer for 1/8 of the time:
//   _               _
// _| |_____________| |___ Servo 0
//     _               _
// ___| |_____________| |_ Servo 1
//       _               _
// _____| |_____________|  Servo 2
//         _
// _______| |_____________ Servo 3
//           _
// _________| |___________ Servo 4
//             _
// ___________| |_________ Servo 5
//               _
// _____________| |_______ Servo 6
// _               _
//  |_____________| |_____ Servo 7
//
// This is a compromise between fully hardware-based
// servo PWM (zero instruction overhead or timing
// jitter, but limited to 5 outputs on PIC32) and
// "tick counting" approaches, which permit any number
// of outputs but may have limited resolution and/or
// higher instruction overhead.
// Uses one or two timers, output compare and interrupts
// to achieve better than 14-bit resolution with 8 servos
// (or better than 16-bit if EXTRA_RES is #defined
// ...though even the lower figure is already pretty
// absurd and more than typical servos can resolve).

// This uses the "communication" row on the Max32
// simply because my servo adapter doodad fit there,
// not for any important reason.  These can be changed
// to most anything EXCEPT pin 3, and they do not need
// to be consecutive pins nor in-order.
byte servoPin[8] = { 14, 15, 16, 17, 18, 19, 20, 21 };

// This code assumes servo timings with the canonical
// 50 Hz (20 millisecond) frequency with a 1.0 to 2.0
// millisecond pulse for position.  Some servos are
// capable of operating beyond this range, but the
// timings here are set up for the conservative case.
// The longest pulse this can handle is 2.5 mS due to
// the 8-way split of the 20 mS interval.
#ifdef EXTRA_RES
  #define SERVO_MIN  (F_CPU / 1000)                // 1.0 mS
  typedef int servo_t;
#else
  #define SERVO_MIN  (F_CPU / 4 / 1000)            // 1.0 mS
  typedef unsigned short servo_t;
#endif
#define SERVO_MAX    (SERVO_MIN * 2)               // 2.0 mS
#define SERVO_CENTER ((SERVO_MIN + SERVO_MAX) / 2) // 1.5 mS
// With lots of clever shenanigans, this idea could be
// expanded to upwards of 20 servos, but this would start
// to invoke limitations and assumptions that may not
// apply to all servo setups; the broadest "sure thing"
// case is implemented here.

// Just load position data into this array; interrupts will
// take care of the rest, no need for any function calls.
servo_t servoPos[8];

void setup()
{
  // Enable output pins for servos, set inital states to 'off'
  for(int i = 0; i < 8; i++) {
    pinMode(servoPin[i], OUTPUT);
    digitalWrite(servoPin[i], LOW);
    servoPos[i] = 0;
  }

  // Initialize timer to 400 Hz (50 Hz * 8 servos)
#ifdef EXTRA_RES
  // Extra positional accuracy requires joining Timers 2 and 3.
  // Interrupt function is attached to Timer 3 -- this issues
  // the 'up' tick for each servo.
  ConfigIntTimer23(T23_INT_ON | T23_INT_PRIOR_3);
  OpenTimer23(T23_ON | T23_PS_1_1 | T23_32BIT_MODE_ON, F_CPU / 50 / 8);
  // Output Compare 1 w/interrupt handles the 'down' tick.
  // OC1 is the reason pin 3 can't be used; its output state
  // will be the combined PWM for all servos together.
  ConfigIntOC1(OC_INT_ON | OC_INT_PRIOR_3);
  OpenOC1(OC_ON | OC_IDLE_CON | OC_TIMER_MODE32 | OC_CONTINUE_PULSE, 0, 0);
#else
  // Standard resolution uses Timer 3 with 1:4 prescaler.
  // Same interrupts and output compare situation.
  ConfigIntTimer3(T3_INT_ON | T3_INT_PRIOR_3);
  OpenTimer3(T3_ON | T3_PS_1_4, F_CPU / 4 / 50 / 8);
  ConfigIntOC1(OC_INT_ON | OC_INT_PRIOR_3);
  OpenOC1(OC_ON | OC_IDLE_CON | OC_TIMER3_SRC | OC_CONTINUE_PULSE, 0, 0);
#endif
}

// Motion example generates a nice millipede-like sine wave
// across a row of eight servos.

float x = 0.0;  // Used only for motion demo below; not servo driver

void loop()
{
  float y = x;
  for(int i = 0; i < 8;i ++) {
    servoPos[i] = SERVO_CENTER +
      (int)((float)(SERVO_MAX - SERVO_CENTER) * sin(y));
    y += M_PI / 5.0;
  }
  x += M_PI / 100.0;

  delay(20);  // No point updating faster than servo pulses
}

extern "C"
{

static volatile byte servoNum = 0;  // Cycles through servos

// This is the timer interrupt, invoked at a uniform 400 Hz.
// Generates the 'up' tick for each of 8 servos (unless
// position is set to 0) and sets the OC1 PWM duration
// for the subsequent 'down' tick.
// These two functions would obviously benefit from some
// direct PORT love (instead of digitalWrite())...just done
// this way for quick and easy implementation.
void __ISR(_TIMER_3_VECTOR,ipl3) pwmOn(void)
{
  mT3ClearIntFlag();  // Clear interrupt flag
  if(servoPos[servoNum] > 0) {
    digitalWrite(servoPin[servoNum], HIGH);
    SetDCOC1PWM(servoPos[servoNum]);
  }
}

// This is the output compare interrupt, also invoked 400
// times per second, but the spacing is not uniform; this
// establishes the position of each servo.  'Down' tick
// occurs here, then servo number is advanced by 1.
void __ISR(_OUTPUT_COMPARE_1_VECTOR,ipl3) pwmOff(void)
{
  mOC1ClearIntFlag();
  digitalWrite(servoPin[servoNum], LOW);
  if(++servoNum > 7) servoNum = 0;  // Back to start
}

} // end extern "C"

KM6VV
Posts: 121
Joined: Fri Jun 03, 2011 7:45 pm
Location: Central Coast, CA

Re: Servo Code!

Post by KM6VV » Thu Jun 16, 2011 7:16 pm

Wow!

You beat me to it! And probably better then I could have done it.

Lots of nice clues how to do interrupts, and how to access the hardware registers directly. A little different then the 18F PICs, so that really helps!

Thanks!

Alan KM6VV
Last edited by KM6VV on Fri Jun 17, 2011 3:34 am, edited 1 time in total.

jbeale
Posts: 16
Joined: Fri May 27, 2011 12:11 am

thanks for a good example!

Post by jbeale » Fri Jun 17, 2011 12:48 am

That is some nice example code, very readable. I look forward to any other contributions you might make!

Any chance you'll be looking into input capture? (measuring pulsewidth of external signal using a timer). Well, I may have to bite the bullet and figure it out myself...

By the way, for the beginner at this, where do I find descriptions of SetDCOC1PWM() and similar things?

jumpin_jack
Posts: 22
Joined: Wed Jun 15, 2011 7:04 am

Re: Servo Code!

Post by jumpin_jack » Fri Jun 17, 2011 1:40 am

Yes, thank you for the well-documented example code.

pburgess
Posts: 15
Joined: Sun Jun 05, 2011 9:05 am

Re: thanks for a good example!

Post by pburgess » Fri Jun 17, 2011 5:47 am

Regarding SetDOC1PWM(), etc: there's a Microchip document (PDF) called "PIC32 Peripheral Libraries for MPLAB C32 Compiler" that covers most of these functions. It's now deprecated, they prefer folks use the .chm version that's included when you install MPLAB (info is more current, but not covered as thoroughly). If you Google around you can still find the older PDF.

Afraid I haven't messed with input capture, and not anticipating a need among any upcoming projects yet, sorry about that. Thanks for the feedback though, and I'm glad the code comes across as legible.

darthpic
Posts: 5
Joined: Sat Jun 18, 2011 3:57 pm

Re: Servo Code!

Post by darthpic » Sat Jun 18, 2011 4:47 pm

Hello ,
I remember a very old project i have write for servos.
Your code have a limitation to max 10 servos due to the method you use (one servo at time)
The method i have made was set the servos ports from the UC to 1 and sequentially
put 0 for the servo who reach it's pulse length in µS.
For doing this i have set one interrupt who run that servos routine every 20mS.
That the code work you have to made a servos array variable and sort all servo from
less µS to most µS (positioning).
I don't know now how to do it with my brand new chipkit max32 board but for those
who want try here are how i made it.

1 : variables declaration ( array of servos and how much servos)

Code: Select all

int16 Servo[16][2];          // the servos array
int16 NbServo = 15 ;        // How much servos you are going to use
                                       // We start with servo number 0
                                       // They are 16 servos in this example
2 : Assign servos to UC Pins

Code: Select all

void SetupServosPin(void)
{
   servo[0][0] =PIN_D0;   // servo[nn][0] = pin to use
   servo[1][0] =PIN_D1;
   servo[2][0] =PIN_D2;
   servo[3][0] =PIN_D3;
   servo[4][0] =PIN_D4;
   servo[5][0] =PIN_D5;
   servo[6][0] =PIN_D6;
   servo[7][0] =PIN_D7;

   servo[8][0] =PIN_B0;
   servo[9][0] =PIN_B1;
   servo[10][0]=PIN_B2;
   servo[11][0]=PIN_B3;
   servo[12][0]=PIN_B4;
   servo[13][0]=PIN_B5;
   servo[14][0]=PIN_B6;
   servo[15][0]=PIN_B7;
3 : We give initial values for each servo (set to 0° = 1000µS = 1mS)

Code: Select all

void GiveInitialValue(void)
{
   int16 a;
   // Give initial values to all servos before we start
   for(a=0;a<=NbServo;a++)
      {
         Servo[a][1]=1000;   // Servo[nn][1] = pulse lenght in µS
      }
}
4 : The most important routine is to sort the servos pulse length that
we can stop them sequentially one after the other.

Code: Select all

void SortServos(void)
{
   int8 a,b;
   int16 temp[1][2];
   // Sorting Algorythm
   for(a=0;a<=NbServo-1;a++)
   {
      for(b=a+1;b<=NbServo;b++)
      {
         if(Servo[a][1] > Servo[b][1])
         {
            temp[0][0]=servo[a][0];temp[0][1]=servo[a][1];
            servo[a][0]=servo[b][0];servo[a][1]=servo[b][1];
            servo[b][0]=temp[0][0];servo[b][1]=temp[0][1];
         }
      }
   }
}
5 : Now with this routine we pilot all servos.

Code: Select all

void CommandServos(void)
{
   int8 a;
   // All servos pins are set to 1
   output_d(255);
   output_b(255);
   set_timer1(0);
   for(a=0;a<=NbServo;a++)
   {
      while(get_timer1()<servo[a][1])continue;
      output_bit(servo[a][0],0);
   }
}
6 : Now for test all servos we need to change the values for all servos
I made this little test routine for that.

Code: Select all

void ModifyServos(void)
{
   int8 a;
   for(a=0;a<=NbServo;a++)
   {
      servo[a][1]+=10;
      if(servo[a][1]>2000) servo[a][1]=1000; // if the pulse is more than 2mS set it to 1mS (0°)
   }
}
7 : Now the main program can be like that

Code: Select all

void main()
{
   set_tris_b(0x00);  // set port B and D as output
   set_tris_d(0x00);
   output_b (0);       // set port B and D to 0
   output_d (0);

   SetupServosPin();
   GiveInitialValue();

   // main loop
   While(TRUE)
   {
      SortServos();
      CommandServos();
      
      // Do something for 18mS
      // .....
      // in this time you can get servos position from 
      // Serial , SPI , etc .. or just continue
      // with the ModifyServos() test routine

     ModifyServos();
   }
}
I have do this servo tricks long time ago on a PIC18F4680 and the
soft was made with CCS C Compiler.
Just don't forget that the servo routine need 2mS for set all servos in
requested position , then you have to do something for 18mS or use
one timer interrupt every 20mS.
You are not limited in number of servos you can use , just in number of
digital output you can use on your CPU :?
That's it , i hope it will help you to made the next super servos board :lol:

And sorry for my French English !

Darth.
Theory is when we know everything but nothing work ...
Practice is when everything work but no one know how :)

davec
Posts: 30
Joined: Tue Jun 28, 2011 1:04 pm

Re: Servo Code!

Post by davec » Tue Jun 28, 2011 1:14 pm

pburgess wrote:There have been several threads in several places bemoaning the lack of a working Servo library. Had a need myself, so put something together today.
Thanks very much for this, I tried it on my new Uno32 and it works great.

I made a quick-and-dirty version of the Arduino Servo library that uses your code. It works like the original with the provisos that you can only use 8 servos and can't use pin 3. Is it OK with you if I post this? Cheers.

pburgess
Posts: 15
Joined: Sun Jun 05, 2011 9:05 am

Re: Servo Code!

Post by pburgess » Tue Jun 28, 2011 7:00 pm

Sounds good to me! I'd just suggest making it abundantly clear that it's a stopgap measure and not an "official" port of the Servo lib, even if the syntax is the same. My choice of timers and stuff may be different than what's eventually delivered, and I wouldn't want folks moving in and getting too comfortable, y'know?

KM6VV
Posts: 121
Joined: Fri Jun 03, 2011 7:45 pm
Location: Central Coast, CA

Re: Servo Code!

Post by KM6VV » Tue Jun 28, 2011 7:16 pm

You've got it together in a compatible servo library?

Yes, I'd sure be glad to see that!

That, and a MsTimer LIB would allow me to compile my "Table Top Challenge" 'bot code.

Thanks!

Alan KM6VV
davec wrote:
pburgess wrote:There have been several threads in several places bemoaning the lack of a working Servo library. Had a need myself, so put something together today.
Thanks very much for this, I tried it on my new Uno32 and it works great.

I made a quick-and-dirty version of the Arduino Servo library that uses your code. It works like the original with the provisos that you can only use 8 servos and can't use pin 3. Is it OK with you if I post this? Cheers.

davec
Posts: 30
Joined: Tue Jun 28, 2011 1:04 pm

Re: Servo Code!

Post by davec » Tue Jun 28, 2011 11:47 pm

OK, try this. I've named it "MakeshiftServo" so it won't be mistaken for an "official" version.
Attachments
MakeshiftServo.zip
(6.4 KiB) Downloaded 1210 times

Post Reply