Gordons Projects

--> Projects Top-Level GIT

OK. So the Pi v2 I have had older firmware and it wasn't quite
[wiringPi] / 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 }