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 // WARNING:
  58 //      This code is really experimental. It was written in response to some people
  59 //      asking for a servo driver, however while it works, there is too much
  60 //      jitter to successfully drive a small servo - I have tried it with a micro
  61 //      servo and it worked, but the servo ran hot due to the jitter in the signal
  62 //      being sent to it.
  63 //
  64 //      If you want servo control for the Pi, then use the servoblaster kernel
  65 //      module.
  66
  67 #define MAX_SERVOS      8
  68
  69 static int pinMap     [MAX_SERVOS] ;    // Keep track of our pins
  70 static int pulseWidth [MAX_SERVOS] ;    // microseconds
  71
  72
  73 /*
  74  * softServoThread:
  75  *      Thread to do the actual Servo PWM output
  76  *********************************************************************************
  77  */
  78
  79 static PI_THREAD (softServoThread)
  80 {
  81   register int i, j, k, m, tmp ;
  82   int lastDelay, pin, servo ;
  83
  84   int myDelays [MAX_SERVOS] ;
  85   int myPins   [MAX_SERVOS] ;
  86
  87   struct timeval  tNow, tStart, tPeriod, tGap, tTotal ;
  88   struct timespec tNs ;
  89
  90   tTotal.tv_sec  =    0 ;
  91   tTotal.tv_usec = 8000 ;
  92
  93   piHiPri (50) ;
  94
  95   for (;;)
  96   {
  97     gettimeofday (&tStart, NULL) ;
  98
  99     memcpy (myDelays, pulseWidth, sizeof (myDelays)) ;
 100     memcpy (myPins,   pinMap,     sizeof (myPins)) ;
 101
 102 // Sort the delays (& pins), shortest first
 103
 104     for (m = MAX_SERVOS / 2 ; m > 0 ; m /= 2 )
 105       for (j = m ; j < MAX_SERVOS ; ++j)
 106         for (i = j - m ; i >= 0 ; i -= m)
 107         {
 108           k = i + m ;
 109           if (myDelays [k] >= myDelays [i])
 110             break ;
 111           else // Swap
 112           {
 113             tmp = myDelays [i] ; myDelays [i] = myDelays [k] ; myDelays [k] = tmp ;
 114             tmp = myPins   [i] ; myPins   [i] = myPins   [k] ; myPins   [k] = tmp ;
 115           }
 116         }
 117
 118 // All on
 119
 120     lastDelay = 0 ;
 121     for (servo = 0 ; servo < MAX_SERVOS ; ++servo)
 122     {
 123       if ((pin = myPins [servo]) == -1)
 124         continue ;
 125
 126       digitalWrite (pin, HIGH) ;
 127       myDelays [servo] = myDelays [servo] - lastDelay ;
 128       lastDelay += myDelays [servo] ;
 129     }
 130
 131 // Now loop, turning them all off as required
 132
 133     for (servo = 0 ; servo < MAX_SERVOS ; ++servo)
 134     {
 135       if ((pin = myPins [servo]) == -1)
 136         continue ;
 137
 138       delayMicroseconds (myDelays [servo]) ;
 139       digitalWrite (pin, LOW) ;
 140     }
 141
 142 // Wait until the end of an 8mS time-slot
 143
 144     gettimeofday (&tNow, NULL) ;
 145     timersub (&tNow, &tStart, &tPeriod) ;
 146     timersub (&tTotal, &tPeriod, &tGap) ;
 147     tNs.tv_sec  = tGap.tv_sec ;
 148     tNs.tv_nsec = tGap.tv_usec * 1000 ;
 149     nanosleep (&tNs, NULL) ;
 150   }
 151
 152   return NULL ;
 153 }
 154
 155
 156 /*
 157  * softServoWrite:
 158  *      Write a Servo value to the given pin
 159  *********************************************************************************
 160  */
 161
 162 void softServoWrite (int servoPin, int value)
 163 {
 164   int servo ;
 165
 166   servoPin &= 63 ;
 167
 168   /**/ if (value < -250)
 169     value = -250 ;
 170   else if (value > 1250)
 171     value = 1250 ;
 172
 173   for (servo = 0 ; servo < MAX_SERVOS ; ++servo)
 174     if (pinMap [servo] == servoPin)
 175       pulseWidth [servo] = value + 1000 ; // uS
 176 }
 177
 178
 179 /*
 180  * softServoSetup:
 181  *      Setup the software servo system
 182  *********************************************************************************
 183  */
 184
 185 int softServoSetup (int p0, int p1, int p2, int p3, int p4, int p5, int p6, int p7)
 186 {
 187   int servo ;
 188
 189   if (p0 != -1) { pinMode (p0, OUTPUT) ; digitalWrite (p0, LOW) ; }
 190   if (p1 != -1) { pinMode (p1, OUTPUT) ; digitalWrite (p1, LOW) ; }
 191   if (p2 != -1) { pinMode (p2, OUTPUT) ; digitalWrite (p2, LOW) ; }
 192   if (p3 != -1) { pinMode (p3, OUTPUT) ; digitalWrite (p3, LOW) ; }
 193   if (p4 != -1) { pinMode (p4, OUTPUT) ; digitalWrite (p4, LOW) ; }
 194   if (p5 != -1) { pinMode (p5, OUTPUT) ; digitalWrite (p5, LOW) ; }
 195   if (p6 != -1) { pinMode (p6, OUTPUT) ; digitalWrite (p6, LOW) ; }
 196   if (p7 != -1) { pinMode (p7, OUTPUT) ; digitalWrite (p7, LOW) ; }
 197
 198   pinMap [0] = p0 ;
 199   pinMap [1] = p1 ;
 200   pinMap [2] = p2 ;
 201   pinMap [3] = p3 ;
 202   pinMap [4] = p4 ;
 203   pinMap [5] = p5 ;
 204   pinMap [6] = p6 ;
 205   pinMap [7] = p7 ;
 206
 207   for (servo = 0 ; servo < MAX_SERVOS ; ++servo)
 208     pulseWidth [servo] = 1500 ;         // Mid point
 209
 210   return piThreadCreate (softServoThread) ;
 211 }