softServo.c 6.2 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211
  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. //#include <stdio.h>
  26. #include <string.h>
  27. #include <time.h>
  28. #include <sys/time.h>
  29. #include <pthread.h>
  30. #include "wiringPi.h"
  31. #include "softServo.h"
  32. // RC Servo motors are a bit of an oddity - designed in the days when
  33. // radio control was experimental and people were tryin to make
  34. // things as simple as possible as it was all very expensive...
  35. //
  36. // So... To drive an RC Servo motor, you need to send it a modified PWM
  37. // signal - it needs anything from 1ms to 2ms - with 1ms meaning
  38. // to move the server fully left, and 2ms meaning to move it fully
  39. // right. Then you need a long gap before sending the next pulse.
  40. // The reason for this is that you send a multiplexed stream of these
  41. // pulses up the radio signal into the reciever which de-multiplexes
  42. // them into the signals for each individual servo. Typically there
  43. // might be 8 channels, so you need at least 8 "slots" of 2mS pulses
  44. // meaning the entire frame must fit into a 16mS slot - which would
  45. // then be repeated...
  46. //
  47. // In practice we have a total slot width of about 20mS - so we're sending 50
  48. // updates per second to each servo.
  49. //
  50. // In this code, we don't need to be too fussy about the gap as we're not doing
  51. // the multipexing, but it does need to be at least 10mS, and preferably 16
  52. // from what I've been able to determine.
  53. // WARNING:
  54. // This code is really experimental. It was written in response to some people
  55. // asking for a servo driver, however while it works, there is too much
  56. // jitter to successfully drive a small servo - I have tried it with a micro
  57. // servo and it worked, but the servo ran hot due to the jitter in the signal
  58. // being sent to it.
  59. //
  60. // If you want servo control for the Pi, then use the servoblaster kernel
  61. // module.
  62. #define MAX_SERVOS 8
  63. static int pinMap [MAX_SERVOS] ; // Keep track of our pins
  64. static int pulseWidth [MAX_SERVOS] ; // microseconds
  65. /*
  66. * softServoThread:
  67. * Thread to do the actual Servo PWM output
  68. *********************************************************************************
  69. */
  70. static PI_THREAD (softServoThread)
  71. {
  72. register int i, j, k, m, tmp ;
  73. int lastDelay, pin, servo ;
  74. int myDelays [MAX_SERVOS] ;
  75. int myPins [MAX_SERVOS] ;
  76. struct timeval tNow, tStart, tPeriod, tGap, tTotal ;
  77. struct timespec tNs ;
  78. tTotal.tv_sec = 0 ;
  79. tTotal.tv_usec = 8000 ;
  80. piHiPri (50) ;
  81. for (;;)
  82. {
  83. gettimeofday (&tStart, NULL) ;
  84. memcpy (myDelays, pulseWidth, sizeof (myDelays)) ;
  85. memcpy (myPins, pinMap, sizeof (myPins)) ;
  86. // Sort the delays (& pins), shortest first
  87. for (m = MAX_SERVOS / 2 ; m > 0 ; m /= 2 )
  88. for (j = m ; j < MAX_SERVOS ; ++j)
  89. for (i = j - m ; i >= 0 ; i -= m)
  90. {
  91. k = i + m ;
  92. if (myDelays [k] >= myDelays [i])
  93. break ;
  94. else // Swap
  95. {
  96. tmp = myDelays [i] ; myDelays [i] = myDelays [k] ; myDelays [k] = tmp ;
  97. tmp = myPins [i] ; myPins [i] = myPins [k] ; myPins [k] = tmp ;
  98. }
  99. }
  100. // All on
  101. lastDelay = 0 ;
  102. for (servo = 0 ; servo < MAX_SERVOS ; ++servo)
  103. {
  104. if ((pin = myPins [servo]) == -1)
  105. continue ;
  106. digitalWrite (pin, HIGH) ;
  107. myDelays [servo] = myDelays [servo] - lastDelay ;
  108. lastDelay += myDelays [servo] ;
  109. }
  110. // Now loop, turning them all off as required
  111. for (servo = 0 ; servo < MAX_SERVOS ; ++servo)
  112. {
  113. if ((pin = myPins [servo]) == -1)
  114. continue ;
  115. delayMicroseconds (myDelays [servo]) ;
  116. digitalWrite (pin, LOW) ;
  117. }
  118. // Wait until the end of an 8mS time-slot
  119. gettimeofday (&tNow, NULL) ;
  120. timersub (&tNow, &tStart, &tPeriod) ;
  121. timersub (&tTotal, &tPeriod, &tGap) ;
  122. tNs.tv_sec = tGap.tv_sec ;
  123. tNs.tv_nsec = tGap.tv_usec * 1000 ;
  124. nanosleep (&tNs, NULL) ;
  125. }
  126. return NULL ;
  127. }
  128. /*
  129. * softServoWrite:
  130. * Write a Servo value to the given pin
  131. *********************************************************************************
  132. */
  133. void softServoWrite (int servoPin, int value)
  134. {
  135. int servo ;
  136. servoPin &= 63 ;
  137. /**/ if (value < -250)
  138. value = -250 ;
  139. else if (value > 1250)
  140. value = 1250 ;
  141. for (servo = 0 ; servo < MAX_SERVOS ; ++servo)
  142. if (pinMap [servo] == servoPin)
  143. pulseWidth [servo] = value + 1000 ; // uS
  144. }
  145. /*
  146. * softServoSetup:
  147. * Setup the software servo system
  148. *********************************************************************************
  149. */
  150. int softServoSetup (int p0, int p1, int p2, int p3, int p4, int p5, int p6, int p7)
  151. {
  152. int servo ;
  153. if (p0 != -1) { pinMode (p0, OUTPUT) ; digitalWrite (p0, LOW) ; }
  154. if (p1 != -1) { pinMode (p1, OUTPUT) ; digitalWrite (p1, LOW) ; }
  155. if (p2 != -1) { pinMode (p2, OUTPUT) ; digitalWrite (p2, LOW) ; }
  156. if (p3 != -1) { pinMode (p3, OUTPUT) ; digitalWrite (p3, LOW) ; }
  157. if (p4 != -1) { pinMode (p4, OUTPUT) ; digitalWrite (p4, LOW) ; }
  158. if (p5 != -1) { pinMode (p5, OUTPUT) ; digitalWrite (p5, LOW) ; }
  159. if (p6 != -1) { pinMode (p6, OUTPUT) ; digitalWrite (p6, LOW) ; }
  160. if (p7 != -1) { pinMode (p7, OUTPUT) ; digitalWrite (p7, LOW) ; }
  161. pinMap [0] = p0 ;
  162. pinMap [1] = p1 ;
  163. pinMap [2] = p2 ;
  164. pinMap [3] = p3 ;
  165. pinMap [4] = p4 ;
  166. pinMap [5] = p5 ;
  167. pinMap [6] = p6 ;
  168. pinMap [7] = p7 ;
  169. for (servo = 0 ; servo < MAX_SERVOS ; ++servo)
  170. pulseWidth [servo] = 1500 ; // Mid point
  171. return piThreadCreate (softServoThread) ;
  172. }