wiringPi/softServo.c

   1 /*
   2  * softServo.c:
   3  *      Provide N channels of software driven PWM suitable for RC
   4  *      servo motors.
   5  *      Copyright (c) 2012 Gordon Henderson
   6  ***********************************************************************
   7  * This file is part of wiringPi:
   8  *      https://projects.drogon.net/raspberry-pi/wiringpi/
   9  *
  10  *    wiringPi is free software: you can redistribute it and/or modify
  11  *    it under the terms of the GNU Lesser General Public License as
  12  *    published by the Free Software Foundation, either version 3 of the
  13  *    License, or (at your option) any later version.
  14  *
  15  *    wiringPi is distributed in the hope that it will be useful,
  16  *    but WITHOUT ANY WARRANTY; without even the implied warranty of
  17  *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  18  *    GNU Lesser General Public License for more details.
  19  *
  20  *    You should have received a copy of the GNU Lesser General Public
  21  *    License along with wiringPi.
  22  *    If not, see <http://www.gnu.org/licenses/>.
  23  ***********************************************************************
  24  */
  25
  26 //#include <stdio.h>
  27 #include <string.h>
  28 #include <time.h>
  29 #include <sys/time.h>
  30 #include <pthread.h>
  31
  32 #include "wiringPi.h"
  33 #include "softServo.h"
  34
  35 // RC Servo motors are a bit of an oddity - designed in the days when
  36 //      radio control was experimental and people were tryin to make
  37 //      things as simple as possible as it was all very expensive...
  38 //
  39 // So... To drive an RC Servo motor, you need to send it a modified PWM
  40 //      signal - it needs anything from 1ms to 2ms - with 1ms meaning
  41 //      to move the server fully left, and 2ms meaning to move it fully
  42 //      right. Then you need a long gap before sending the next pulse.
  43 //      The reason for this is that you send a multiplexed stream of these
  44 //      pulses up the radio signal into the reciever which de-multiplexes
  45 //      them into the signals for each individual servo. Typically there
  46 //      might be 8 channels, so you need at least 8 "slots" of 2mS pulses
  47 //      meaning the entire frame must fit into a 16mS slot - which would
  48 //      then be repeated...
  49 //
  50 // In practice we have a total slot width of about 20mS - so we're sending 50
  51 //      updates per second to each servo.
  52 //
  53 // In this code, we don't need to be too fussy about the gap as we're not doing
  54 //      the multipexing, but it does need to be at least 10mS, and preferably 16
  55 //      from what I've been able to determine.
  56
  57
  58 #define MAX_SERVOS      8
  59
  60 static int pinMap     [MAX_SERVOS] ;    // Keep track of our pins
  61 static int pulseWidth [MAX_SERVOS] ;    // microseconds
  62
  63
  64 /*
  65  * softServoThread:
  66  *      Thread to do the actual Servo PWM output
  67  *********************************************************************************
  68  */
  69
  70 static PI_THREAD (softServoThread)
  71 {
  72   register int i, j, k, m, tmp ;
  73   int lastDelay, pin, servo ;
  74
  75   int myDelays [MAX_SERVOS] ;
  76   int myPins   [MAX_SERVOS] ;
  77
  78   struct timeval  tNow, tStart, tPeriod, tGap, tTotal ;
  79   struct timespec tNs ;
  80
  81   tTotal.tv_sec  =    0 ;
  82   tTotal.tv_usec = 8000 ;
  83
  84   piHiPri (50) ;
  85
  86   for (;;)
  87   {
  88     gettimeofday (&tStart, NULL) ;
  89
  90     memcpy (myDelays, pulseWidth, sizeof (myDelays)) ;
  91     memcpy (myPins,   pinMap,     sizeof (myPins)) ;
  92
  93 // Sort the delays (& pins), shortest first
  94
  95     for (m = MAX_SERVOS / 2 ; m > 0 ; m /= 2 )
  96       for (j = m ; j < MAX_SERVOS ; ++j)
  97         for (i = j - m ; i >= 0 ; i -= m)
  98         {
  99           k = i + m ;
 100           if (myDelays [k] >= myDelays [i])
 101             break ;
 102           else // Swap
 103           {
 104             tmp = myDelays [i] ; myDelays [i] = myDelays [k] ; myDelays [k] = tmp ;
 105             tmp = myPins   [i] ; myPins   [i] = myPins   [k] ; myPins   [k] = tmp ;
 106           }
 107         }
 108
 109 // All on
 110
 111     lastDelay = 0 ;
 112     for (servo = 0 ; servo < MAX_SERVOS ; ++servo)
 113     {
 114       if ((pin = myPins [servo]) == -1)
 115         continue ;
 116
 117       digitalWrite (pin, HIGH) ;
 118       myDelays [servo] = myDelays [servo] - lastDelay ;
 119       lastDelay += myDelays [servo] ;
 120     }
 121
 122 // Now loop, turning them all off as required
 123
 124     for (servo = 0 ; servo < MAX_SERVOS ; ++servo)
 125     {
 126       if ((pin = myPins [servo]) == -1)
 127         continue ;
 128
 129       delayMicroseconds (myDelays [servo]) ;
 130       digitalWrite (pin, LOW) ;
 131     }
 132
 133 // Wait until the end of an 8mS time-slot
 134
 135     gettimeofday (&tNow, NULL) ;
 136     timersub (&tNow, &tStart, &tPeriod) ;
 137     timersub (&tTotal, &tPeriod, &tGap) ;
 138     tNs.tv_sec  = tGap.tv_sec ;
 139     tNs.tv_nsec = tGap.tv_usec * 1000 ;
 140     nanosleep (&tNs, NULL) ;
 141   }
 142
 143   return NULL ;
 144 }
 145
 146
 147 /*
 148  * softServoWrite:
 149  *      Write a Servo value to the given pin
 150  *********************************************************************************
 151  */
 152
 153 void softServoWrite (int servoPin, int value)
 154 {
 155   int servo ;
 156
 157   servoPin &= 63 ;
 158
 159   /**/ if (value < -250)
 160     value = -250 ;
 161   else if (value > 1250)
 162     value = 1250 ;
 163
 164   for (servo = 0 ; servo < MAX_SERVOS ; ++servo)
 165     if (pinMap [servo] == servoPin)
 166       pulseWidth [servo] = value + 1000 ; // uS
 167 }
 168
 169
 170 /*
 171  * softServoSetup:
 172  *      Setup the software servo system
 173  *********************************************************************************
 174  */
 175
 176 int softServoSetup (int p0, int p1, int p2, int p3, int p4, int p5, int p6, int p7)
 177 {
 178   int servo ;
 179
 180   if (p0 != -1) { pinMode (p0, OUTPUT) ; digitalWrite (p0, LOW) ; }
 181   if (p1 != -1) { pinMode (p1, OUTPUT) ; digitalWrite (p1, LOW) ; }
 182   if (p2 != -1) { pinMode (p2, OUTPUT) ; digitalWrite (p2, LOW) ; }
 183   if (p3 != -1) { pinMode (p3, OUTPUT) ; digitalWrite (p3, LOW) ; }
 184   if (p4 != -1) { pinMode (p4, OUTPUT) ; digitalWrite (p4, LOW) ; }
 185   if (p5 != -1) { pinMode (p5, OUTPUT) ; digitalWrite (p5, LOW) ; }
 186   if (p6 != -1) { pinMode (p6, OUTPUT) ; digitalWrite (p6, LOW) ; }
 187   if (p7 != -1) { pinMode (p7, OUTPUT) ; digitalWrite (p7, LOW) ; }
 188
 189   pinMap [0] = p0 ;
 190   pinMap [1] = p1 ;
 191   pinMap [2] = p2 ;
 192   pinMap [3] = p3 ;
 193   pinMap [4] = p4 ;
 194   pinMap [5] = p5 ;
 195   pinMap [6] = p6 ;
 196   pinMap [7] = p7 ;
 197
 198   for (servo = 0 ; servo < MAX_SERVOS ; ++servo)
 199     pulseWidth [servo] = 1500 ;         // Mid point
 200
 201   return piThreadCreate (softServoThread) ;
 202 }