servo.c

/*
 * @file servo.c
 *
 * @author Andy Lindsay
 *
 * @version 0.9
 *
 * @copyright Copyright (C) Parallax, Inc. 2012.  See end of file for
 * terms of use (MIT License).
 *
 * @brief Manages control of up to 14 servos using another CMM/LMM cog.
 *
 * Please submit bug reports, suggestions, and improvements to         
 * this code to editor@parallax.com.
 */
 
 
#include "simpletools.h"
#include "servo.h"
 
 
static void pulse_outCtr(int pin, int time);          // pulseOut function definition
static int servo_start(void);                         // Function prototype for servo_start
static void servo(void *par);                         // Function prototype for servo
 
 
static volatile unsigned int servoCog = 0;            // Cog initialozed to zero
static volatile unsigned int lockID;                  // Lock ID 
static unsigned int stack[44 + 24];                   // Stack
 
static volatile int p[14] = {-1, -1, -1, -1,          // I/O pins
                     -1, -1, -1, -1, -1, -1, 
                     -1, -1, -1, -1};
static volatile int t[14] = {-1, -1, -1, -1,          // Current iteration pulse widths
                     -1, -1, -1, -1, -1, -1, 
                     -1, -1, -1, -1};
static volatile int tp[14] = {-1, -1, -1, -1,         // Previous iteration pulse widths
                     -1, -1, -1, -1, -1, -1, 
                     -1, -1, -1, -1};
static volatile int r[14] = {2000, 2000, 2000, 2000,  // Step sizes initialized to 2000
                     2000, 2000, 2000, 2000, 2000, 
                     2000, 2000, 2000, 2000, 2000};
 
 
int servo_angle(int pin, int degreeTenths)            // Set continuous rotation speed
{
  return servo_set(pin, degreeTenths + 500);          // Add center pulse width to speed
}
 
 
int servo_speed(int pin, int speed)                   // Set continuous rotation speed
{
  return servo_set(pin, speed + 1500);                // Add center pulse width to speed
}
 
 
int servo_set(int pin, int time)                      // Set pulse width to servo on pin 
{
  if(servoCog == 0)                                   // If cog not started
  {
    int result = servo_start();                       // Start the cog
    if(result == 0) return -1;                        // No cogs open
    if(result == -1) return -2;                       // No locks open
  }
  int s = sizeof(p)/sizeof(int);                      // Array size to s
  int i;                                              // Index variable
  while(lockset(lockID));                             // Set the lock
  for(i = 0; i < s; i++)                              // Check if existing servo
  {
    if(p[i] == pin)                                   // Yes?
    {
      t[i] = time;                                    // Set pulse duration
      lockclr(lockID);                                // Clear lock
      return 1;                                       // Return success 
    }
  }
  for(i= 0; i < s; i++)                               // Look for empty slot
  {
    if(p[i]==-1)                                      // Found one?
    {
      break;                                          // Exit for loop, keep index
    }
  }
  if(i < s)                                          // Found empty slot?
  {
    p[i] = pin;                                      // Set up pin and pulse durations
    t[i] = time;
    tp[i] = time;
    lockclr(lockID);                                 // Clear the lock 
    return pin;                                      // Return success 
  }
  else                                               // servo not found, no empty slots?
  {
    lockclr(lockID);                                 // Clear lock
    return -3;                                       // Return, no pins
  }
}
 
 
int servo_setramp(int pin, int stepSize)             // Set ramp step for a servo
{
  int s = sizeof(p)/sizeof(int);                     // Get array size
  int i;                                             // Local index variable
  while(lockset(lockID));                            // Set lock
  for(i = 0; i < s; i++)                             // Find index for servo pin
  {
    if(p[i] == pin)                                  // Found pin in array?
    {
      r[i] = stepSize;                               // Set ramp step
      lockclr(lockID);                               // Clear lock
      return pin;                                    // Return success
    }
  }
  lockclr(lockID);                                   // Clear lock
  return -4;                                         // Return -1, pin not found
}
 
 
int servo_get(int pin)                               // Get servo position
{
  int s = sizeof(p)/sizeof(int);                     // Get size of servo arrays
  int i;                                             // Declare local index
  for(i = 0; i < s; i++)                             // Look for matching pin in array
  {
    if(p[i] == pin)                                  // Found matching pin?
    {
      return tp[i];                                  // Return associated pulse time
    }
  }
  return -4;                                         // No pin match?  Return -4
}
 
 
int servo_disable(int pin)
{
  // 0 to time param causes servo funciton running 
  // in the other cog to disable the servo
  int result = servo_set(pin, 0);                    
  return result;
}
  
 
static void servo(void *par)                         // servo process in other cog
{
  int dtpw = (CLKFREQ/1000000)*2500;
  int pw = CNT;
 
  int dtTw = (CLKFREQ/1000000)*20000;
  int Tw = pw;
  Tw += dtTw;
 
  int s = sizeof(p)/sizeof(int);                     // Get size of servo array
  int i;                                             // Local index variable
  while(1)                                           // servo control loop
  {
    while(lockset(lockID));                          // Set the lock 
    for(i = 0; i < s; i++)                           // Go through all possible servos
    {
      if(t[i] == 0)                                  // Detach servo? 
      {
        input(p[i]);                                 // Set I/O pin to input
        p[i] = -1;                                   // Remove from list
        t[i] = -1;
        tp[i] = -1;
        r[i] = 2000;
      }
      if(p[i] != -1)                                 // If servo entry in pin array
      {
        int tPulse =  t[i];                          // Copy requested position to var
        int diff = tPulse - tp[i];                   // Check size of change
        int d = abs(diff);                           // Take absolute value
        if(r[i] < d)                                 // If change larger than ramp step
        {
          int step = r[i];                           // Copy step entry to variable
          if(diff < 0)                               // If negative
          {
            step = -step;                            // Make step negative
          }
          tPulse = tp[i] + step;                     // Increment pulse by step 
        }
        pulse_outCtr(p[i], tPulse);                  // Send pulse to servo
        tp[i] = tPulse;                              // Remember pulse for next time
      }
      if(i%2)
      {
         while((CNT - pw) <= dtpw);                  // Wait for servo pulse window to close
         pw += dtpw;
      }
    }
    lockclr(lockID);                                 // Clear the lock
    while((CNT - pw) <= dtpw);                       // Wait for 20 ms since first pulse
    pw += dtpw;
  }
}
 
 
void servo_stop(void)                                // Stop servo process, free a cog
{
  if(servoCog)                                       // If the cog is running
  {
    cogstop(servoCog-1);                             // Stop it
    servoCog = 0;                                    // Remember that it's stopped
    lockclr(lockID);
    lockret(lockID);                                 // Return the lock
  }
}
 
 
/*
 * @brief Starts the servo process and takes over a cog.
 *
 * @details You do not need to call this function from your code because
 * the servo_set function calls it if it detects that the servo cog has not
 * been started.
 *
 * @returns 1..8 if successful.  0 if no available cogs, -1 if no available
 * locks.
 */
static int servo_start(void)                         // Take cog & start servo process
{
  lockID = locknew();                              // Check out a lock
  if(lockID == -1) return -1;                        // Return -1 if no locks
  else lockclr(lockID);
  servo_stop();                                      // Stop in case cog is running
  servoCog = cogstart(servo, NULL, stack,            // Launch servo into new cog
             sizeof(stack)) + 1;
  return servoCog;
}
 
 
static int ta = 0, tb = 0, dta = 0, dtb = 0;
 
static void pulse_outCtr(int pin, int time)           // pulseOut function definition
{
  /*
  if(st_iodt == 0)
  {
    set_io_dt(CLKFREQ/1000000);
    set_io_timeout(CLKFREQ/4);
  }
  */
  signed long phsVal = -time * st_iodt;
  int ctr = 0;
  int frq = 1;
  int phs = 0;
  int state = get_output(pin);
  if(state == 1)
  {
    phsVal = -phsVal;
    phs = -1;
    frq = -1;
  }
  if(ta == 0 || (dta && (CNT - ta > dta)))
  {
    PHSA = phs;
    FRQA = frq;
    CTRA = pin;
    CTRA += (4 << 26);
    low(pin);
    PHSA = phsVal;
    dta = abs(phsVal);
    ta = CNT;
  }
  else if(tb == 0 || (dtb && (CNT - tb > dtb)))
  {
    PHSB = phs;
    FRQB = frq;
    CTRB = pin;
    CTRB += (4 << 26);
    low(pin);
    PHSB = phsVal;
    dtb = abs(phsVal);
    tb = CNT;
  }
}