Statistics
| Branch: | Revision:

ddr4s / fw / wiringPi / wiringPi / softServo.c @ 32:cadb9025f1e0

History | View | Annotate | Download (6.2 KB)

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
}