|
|
|
|
|
My other sites |
|
|
|
|||
| [Home] [Projects] [Hardware] [Software] [Books] [Links] [Downloads] | |||
|
|
|||
|
|
|||
The solution is elegant and effective, with a wide range of speed, size and torque servos to choose from. However, as always, there are a couple of drawbacks.
In most cases the resulting servo needs to be calibrated. This means tweaking the pot, or voltage divider to be able to direct the servo to stop rotating. Depending on how the servo was modified this may need to be re-done from time to time. Another glitch is that although it might appear that it's also possible to vary the drive speed of the servo, there is really only a very narrow range of control, and this range is somewhat unpredictable. It's usually only possible to a few speed variations between Stopped and Full Speed.
If a robot needs a wide variety of speeds, it's typical to either remove all of the drive electronics from the servo and only use the gearing, or to abandon the servo all together. If the servo is just used for the motor and gearing, then a separate H-Bridge driver is used, and the robot controller must generate the correct PWM (Pulse Width Modulation) signals to run the motor bridge.
My first project was to see if I could come up with an alternative to these approaches. I wanted to see if I could hack the Servo internals such that the robot would still think that it's controlling a servo, but that the servo itself would respond with a more linear range of speeds.
I did some basic research and found a [great page] by Lee Buse on the [Seattle Robotics Society] website that illustrated the internals of a typical servo. This page was showing another way to achieve a similar "variable speed" result, but it still used the basic feedback-pot elimination method. The real benefit to me was that it identified the pin functions on the motor driver IC that the Futaba Servo uses. I decided that I would replace all the circuitry except that driver IC with my own tiny embedded processor. That processor would read the pulsed control signal coming from the robot and signal the motor driver IC to vary the speed of the motor. I had already played around with the cool [PIC12F683] [Microchip] processor, and since it was pretty small (just an 8-pin DIP), and I already had some spares floating around, I decided to use it for my project.
I decided that I has some basic goals that I needed to meet with my solution:
After checking on the PIC12F683 one more time, I felt confidant that I could write the code, so I started on the Servo conversion. Naturally, this began with stripping off all the parts that I didn't need. I decided to leave the main supply capacitor, the motor drive IC and a few surface-mount decoupling caps. Here's a picture of the servo board after I was done wielding my soldering iron. Click any image for an enlargement.
 |
 |
Now I had a motor controller board with the high current drivers intact. It turns out that there are two pins on the driver IC. One causes the motor to spin in the forward direction, the other causes it to spin in the opposite direction. So now I needed to add the micro-controller to drive either of these pins with a PWM signal.
OK, the 12F683 is an 8 pin dip microcontroller with 6 multi-function I/O lines. If your application can work with a clock less than 8MHz, then you don't need any external oscillator or crystal, as the 12F683 has it's own internal 8MHz clock. I decided to use one line as a digital input, and four as outputs. I only really needed 2 outputs but I wasn't sure how much current I needed to run the Motor Driver IC, so I decided to double them up to be "better safe than sorry". I ended up only using the PIC processor and a diode in series with the VCC line to drop the voltage to an acceptable value. Here's a picture of the CRUDE prototype.
Since I was going to be using PIC assembly language, I wanted the timing to be as straight forward as possible, so I started figuring out the ideal timing frequency. I needed to be able to measure the classic servo control signal, which is a variable width pulse that repeats about once every 20mSec. The width of the pulse goes from 1mSec (for full reverse) to 2mSec (for full forward). A pulse width of 1.5mSec is taken to be the Stop signal.
One easy way that I set up accurate timing in the 12F683 is to use Timer 2 which can be set to repeatedly count down from a preset value to zero. Each time it hits zero it sets an interrupt bit and starts again. Even if I don't use the interrupt feature, I can check and reset this bit and use it as a moderate speed counter. So the goal in this instance is to determine the ideal Timer 2 interrupt rate, and then just use this to run a software counter (or timer).
I'm need to measure the Servo Control Pulse, and I want 10 forward and 10 reverse speeds, so I need a timing resolution of about 50 uSec (1mSec Full Span / 20 steps). If I measure the width of the control pulse with 50 uSec timer clock, my active control range would be 20 - 40 counts, with a count of 30 being the neutral position. So:
I then need to generate a PWM signal on either the forward or reverse pin on the Motor Driver IC based on this count. The original servo circuit had a motor PWM frequency that matched control pulse frequency, which was about 50 Hz (a 20 mSec repeat rate). Since I knew this rate wasn't an absolute requirement, I chose a slightly different rate that made my program easier to write. Since I knew that I was going to have a 50 uSec Timer 2 running in my PIC, I divided this by 256 (a wonderful 8 bit number) and saw that it gave me a period of 12.8 mSec (or 78 Hz). This faster rate would do just fine for the motor PWM..
To see how I used these different timers and ratios, let's look at the forward speed range for a minute.
On my front-end, I would be seeing a Servo Control Pulse width ranging from 1.5 to 2.0 mSec. With a counter running at 50 uSec, this corresponds to a count range of 30 - 40 for the forward speed range. My code would subtract 30 from this to get a range of 0 - 10. A count of zero means stop, so I'm only interested in the range of 1-10. Since I've decided that my Motor Drive PWM is going to be 256 Timer 2 clocks, I can approximate each speed range as an additional 25 clocks of pulse width (I really need 25.6 clocks... but who's counting :)
My procedure is determine the speed range (1-10) and multiply it by 25 to get the Motor Pulse width (in Timer 2 Clocks).
So my program is really two independent state machines.
The full program is called PBot_ServoHack1.asm and is available on my [download] page. Here are some highlights:
The first thing I always do is define the various constants that I want to use in the program. This way if I need to change one, it's easy to find.
;***** VARIABLE DEFINITIONS Temp EQU 0x20 ; Flags EQU 0x21 ; variable used for context saving ICntLSB EQU 0x22 ; Timer for measuring Servo High/Low Period ICntMSB EQU 0x23 ; Timer for measuring Servo High/Low Period Length EQU 0x24 ; Pulse Length (Ticks) PulseTimer EQU 0x25 ; Pulse countdown timer CycleTimer EQU 0x26 ; Full Cycle Timer
;***** GPIO Allocations IO_SERVO EQU 3 ; GPIO Bit 3, Servo Control Pulse Input. IO_FWD_ON EQU 0x03 ; GPIO Bits 0,1 Forward Drive Out IO_FWD_OFF EQU 0xFC IO_REV_ON EQU 0x30 ; GPIO Bits 4,5 Reverse Drive Out IO_REV_OFF EQU 0xCF ; ***** FLAG BIT DEFINITIONS F_Forward EQU 0 ; Set = Forward. F_Reverse EQU 1 ; Set = Forward. ;**** TIMER Constants TICK EQU 0x64 ; 50uS (8MHz Clock, 20KHz Sample rate) REV_THRESH EQU 0x1D ; < 1.5 mSec FWD_THRESH EQU 0x1E ; > 1.5 mSec MAX_RANGE EQU 0x0A ; 10 steps per axis (plus 1) MAX_RANGEP1 EQU 0x0B ; Max Range Plus 1The next part of the program just sets up the various registers and puts everything in motion. Loading the TICK value into Timer 2 preload register will cause this timer to roll over every 50 uSeconds (0.000050 Seconds).
main bcf STATUS,RP0 ; Set file register bank to 0 clrf GPIO ; Init GPIO movlw 0x07 ; Set all Comparitors off (GPIO On) movwf CMCON0 bsf STATUS,RP0 ; Set file register bank to 1 clrf ANSEL ; Disable Analog Chanels movlw 0x70 ; Set Clock Frequency to 8MHz movwf OSCCON clrf TRISIO ; Set GPIO Directions bsf TRISIO,IO_SERVO ; Setup servo as input movlw TICK ; Load Timer 2 up with period movwf PR2 ; bsf PIE1,TMR2IE ; Turn On Timer 2 Interrupt bcf STATUS,RP0 ; Set file register bank to 0 clrf Flags ; Clear out all variables clrf ICntLSB ; clrf ICntMSB ; clrf Length ; clrf PulseTimer ; clrf CycleTimer ; bsf T2CON,TMR2ON ; Turn On Timer 2
After setting up the registers, the program enters it's main processing loop. Here it's operation depends heavily on a routine called CheckForTic. CheckForTic is used to watch the Timer2 Interrupt bit. If the bit is set, it means that another 50 uSec has elapsed so the ICntLSB is incremented, and the interrupt is reset.
The program loop basically follows the low to high and high to low transitions of the Servo Control input. When the input goes high, the code resets the ICntLSB counter, and when it goes low again, the code checks the value of the counter to see how long the control pulse was.
Start ; Wait for Input to go low before continuing btfsc GPIO,IO_SERVO ; Test Servo Input goto Start ; Loop if not clear call ResetTimer2 ; Clear out all timer variables WaitingForHigh call CheckForTic ; Accumulate Ticks btfss GPIO,IO_SERVO ; Test Servo Input goto WaitingForHigh ; Loop if not Set
PositiveEdge call ResetTimer2 ; Resynch timer2 & related variables
WaitingForLow call CheckForTic ; Get in synch with Timer 2 btfsc GPIO,IO_SERVO ; Test Servo Input goto WaitingForLow ; Loop if not Clear NegativeEdgeNext the program determines if the pulse count indicates forward, reverse, or stop.
GetDirection ; First check to see if we are in reverse movlw REV_THRESH ; Set the Reverse threshold subwf ICntLSB,F ; Subtract Thresh from Pulse time btfss ICntLSB,7 ; Is the result negative? goto NotRev ; No, Try the next threshold InReverse ; Calculate 1 to X timer count. Current value = -1 to -X comf ICntLSB,F ; Negate Count (Complement and add 1) incf ICntLSB,F ; Val is now 1 to REV_THRESH . ; Signal drive Reverse call SetPulseLength ; Convert count to pulse length bcf Flags,F_Forward bsf Flags,F_Reverse goto DirDone NotRev ; Now check to see if we are in Neutral decf ICntLSB,F ; Decrement Pulse time for Neutral gap btfss ICntLSB,7 ; Is the result negative? goto InForward ; No, Must be forward InNeutral ; Signal Neutral bcf Flags,F_Reverse bcf Flags,F_Forward clrf Length goto DirDone InForward ; Signal drive Forward incf ICntLSB,F ; Start range at 1 not o call SetPulseLength ; Convert count to pulse length bcf Flags,F_Reverse bsf Flags,F_Forward goto DirDone DirDone call ClearTimer2 ; Clear out all timer variables PulseComplete goto WaitingForHigh ; Repeat loop looking for next pulse
; --------------------------------------------------------- SetPulseLength ; Assumes iCntLSB has a tick count of 1 to some number ; Clip the number to 10 and then set the Pulse length movf ICntLSB,W ; Make temp. copy of Count movwf Temp ; movlw MAX_RANGE ; Compare with MAX_RANGE subwf Temp,F ; btfss Temp,7 ; If Negative, do nothing movwf ICntLSB ; otherwise, Set count to Max Range ; Calculate pulse lengh by multiplying count by 25 (16+8+1) movf ICntLSB,W ; Make temp copy of Count movwf Temp ; W starts out with Count rlf Temp,F ; Multiply Count by 8 (shift 3 bits) rlf Temp,F rlf Temp,F addwf Temp,F ; Accumulate in W rlf Temp,F ; Multiply Count by 16 (one more shift) addwf Temp,W ; Accumulate in W movwf Length ; Save in pulse length return
The final part of the program is actually the CheckForTic function which not only increments the ICntLSB but it also runs the Motor Drive PWM output. Every time the CycleTimer wraps back to zero, we set the appropriate Motor Drive output bit high, and then wait for the pre-calculated number of clock ticks to go by before we drop it back to low.
; --------------------------------------------------------- CheckForTic btfss PIR1,TMR2IF ; Check Timer2 interrupt bit goto CFT_Done ; Exit if not set IsTick bcf PIR1,TMR2IF ; Clear Timer Interrupt incfsz ICntLSB,F ; Inc 2 byte Input Counter goto PulseCheck incf ICntMSB,F btfss ICntMSB,3 ; Have we reached 2048? (100ms) goto PulseCheck ; Nope, OK, run pulse code ; Servo Timeout bcf Flags,F_Reverse ; Set output to Neutral bcf Flags,F_Forward clrf Length PulseCheck ;Look for end of cycle decfsz CycleTimer,F ; Has Cycle timer wrapped goto PulseEndCheck ; Start of New Cycle, Start next pulse NewCycle movf Length,W ; Put pulse length in PulseTimer movwf PulseTimer CheckForward btfss Flags,F_Forward ; Going Forward? goto CheckReverse SetForward movlw IO_REV_OFF ; Set Reverse Bits off andwf GPIO,F ; movlw IO_FWD_ON ; Set Forward Bits on iorwf GPIO,F ; goto CFT_Done CheckReverse btfss Flags,F_Reverse ; Skip if going Forward goto SetNeutral SetReverse movlw IO_FWD_OFF ; Set Forward Bits off andwf GPIO,F ; movlw IO_REV_ON ; Set Reverse Bits on iorwf GPIO,F ; goto CFT_Done SetNeutral movlw IO_FWD_OFF ; Set Forward Bits off andwf GPIO,F ; movlw IO_REV_OFF ; Set Reverse Bits off andwf GPIO,F ; goto CFT_Done PulseEndCheck ; Look to see if it's time to end the pulse movf PulseTimer,F ; Get pulse time and check for zero btfsc STATUS,Z ; Is pulse still running? goto CFT_Done ; No, goto end decfsz PulseTimer,F ; Count down Pulse goto CFT_Done ; Pulse has ended so clear all drive bits movlw IO_FWD_OFF ; Set Forward Bits off andwf GPIO,F ; movlw IO_REV_OFF ; Set Reverse Bits off andwf GPIO,F ; CFT_Done returnThe final result seems to work pretty well. I haven't had a chance to test it under load yet, so I don't know what the torque is like, but you can clearly hear the 10 different speed steps in each direction. I'll post test results once I have two units modified.
Web content is copyright © PhilBot.com
2005, Deep Creek Lake, MD.
Contact: Phil Malone 301.387.2331, webmaster@PhilBot.com