Strawman removed - start of actual implementation

Neon22 2014-05-19 16:14:50 -07:00
parent f37948d871
commit a046ee030f
1 changed files with 7 additions and 152 deletions

159
Servos.md

@ -7,17 +7,17 @@ Each servo is listed with its update rate (pulse cycle) and pulse width.
* Cheap. * Cheap.
* Generally have +/-90 degree rotations about a centered rest position * Generally have +/-90 degree rotations about a centered rest position
* but can also be continuous rotation or other ranges (e.g. +/- 180 degree). * but can also be continuous rotation or other ranges (e.g. +/- 180 degree).
* Use a PWM mechanism to control their desired position with a central frequency approximately 1500 usecs (666 Hz). * Use a PWM mechanism (but not standard PWM) to control their desired position with a central frequency approximately 1500 usecs (666 Hz).
* Need to get updates at from 20-65 times per second (Hz). * Need to get updates at from 20-65 times per second (Hz).
####Digital servos: ####Digital servos:
* have a higher refresh frequency - upto 400Hz and therefore: * have a higher refresh frequency - upto 400Hz and therefore:
* hold position better, * hold position better,
* have better torque, speed, and accuracy * have better torque, speed, and accuracy
* and therefore also use more power. * therefore use more power.
* This higher rate is handled internally and so the digital servo keeps updating its position even when it receives external updates infrequently. * This higher rate is handled internally and so the digital servo keeps updating its position even when it receives external updates infrequently.
* but if signals are received at a higher rate the servo can update more responsively. * but if signals are received at a higher rate the servo can update more responsively.
* are more expensive. * more expensive than analog.
* some also have other communication options. E.g. serial ports. * some also have other communication options. E.g. serial ports.
* This Servo class is for traditional PWM control only and does not address these. * This Servo class is for traditional PWM control only and does not address these.
@ -87,153 +87,8 @@ The pyboard has 23 pins that can output hardware PWM. It has 12x 16bit timers an
The Teensy3.1 has 12 pins that can output PWM but (I think) only 3 Timers. So a software interrupt routine would be an excellent library for the Teensy. The Teensy 3.1 has PWM 12bit resolution. Some data here: https://www.pjrc.com/teensy/td_pulse.html The Teensy3.1 has 12 pins that can output PWM but (I think) only 3 Timers. So a software interrupt routine would be an excellent library for the Teensy. The Teensy 3.1 has PWM 12bit resolution. Some data here: https://www.pjrc.com/teensy/td_pulse.html
####Straw man code ####Implementation
So Servo setup might look like the following. The actual implementation on the pyboard is set to only adjust the 4 lines dedicated to Servo control.
ISR defined within - need to be weritten in c so as to use interrupts. Initially just software based. The code wouuld need to be updated or replaced to drive more pins as servos.
``` The Servo is driven by hardware using TIM5 which neatly is designed to control exactly the four pins allocated to Servos on the Pyboard.
#!/usr/bin/python
### Potential Servo implementation ideas
### top level servo pool
# servos = Servo_pool(update_rate, Servo_pool.SOFTWARE) # optional rate in Hz
# servos.set_desired_update_rate(freq_in_Hz)
# servos.update_rate() # actual update rate
##
# s1 = servos.add(pin, serrvoargs)
# s1.setup(min_width, detent_width, max_width)
# s1.move(percent) # set pulse width for this percent
# s1.remove() # deletes servo from pool
## Optionally if angle is important.
# s1.set_angles(min, max) # set angle values for rotate function
# s1.rotate(angle)
import Servo_ISR # C lib which exposes interrupt request routine for servos.
# has:
# - isr = Servo_ISR(type) # creates a non-runing interrupt service routine for Servos (h/w or s/w)
# - Servo_ISR.start() #
# - Servo_ISR.stop() #
# - Servo_ISR.set_rate(Hz) # ISR will interrupt at this rate
# - Servo_ISR.set_active(pin_width_pairs) # list of python class variables of pin, pulse_width pairs
# - E.g. [[s1.pin, s1.value], [s2.pin, s2.value]]
# - When started this ISR routine will step through the list one pair at each interrupt.
# Setting the pin to active high pulse of that width in usecs
# H/w interrupt wil exit very quickly as pulse happens without its attention
# S/w interrupt wil need to wait or trigger itself to come back and switch pulse off at right time.
class Servo_pool(object):
SOFTWRE = 0
HARDWARE = 1
def __init__(self, update_rate=50, ISR_type=Servo_pool.HARDWARE):
" holds all servos, allocates to ISRs "
self.pool = []
self.desired_rate = update_rate # user wants this rate
self.rate = update_rate # the rate we can actually do (may be lower than desired)
self.isrs = []
class ISR(self):
""" holds a number of servos - externally managed
- rate is in Hz
"""
IDLE = 0
STARTED = 1
def __init__(self, rate, ISR_type):
self.isr_type = ISR_type
self.my_servos = []
self.pin_width_pairs = []
self.state = ISR.IDLE
self.isr = Servo_ISR(ISR_type)
self.rate = rate
self.set_rate(rate)
def add_servo(self, servo):
" "
self.my_servos.append(servo)
self.pin_width_pairs.append([servo.pin, servo.value])
def remove_servo(self, servo):
" "
self.my_servos.pop(servo)
pos = self.pin_width_pairs.index([servo.pin, servo.value])
self.pin_width_pairs.pop(pos)
# !! deal with empty my_servos
def start(self):
self.isr.start()
self.state = ISR.STARTED
def stop(self):
self.isr.stop()
self.state = ISR.IDLE
def set_rate(self, rate):
self.isr.set_rate(rate)
def add(self, pin, *args, **kwargs):
s = Servo(self, pin, *args, **kwargs)
self.pool.append(s) # !! check for existing etc...
# do we have enough ISRs
# add servo to ISR's my_servos list
# !! should we degrade update_rate ?
if not self.isrs:
isr = ISR(self.rate, self.isr_type)
self.isrs.append(isr)
else:
if self.need_new_isr_p():
isr = ISR(self.rate, self.isr_type)
self.isrs.append(isr)
else: # use existing (last)
isr = self.isrs[-1]
#
isr.add_servo(s)
if isr.state == ISR.IDLE:
isr.start() # we added one - lets go!
return s
def remove_servo(self, servo):
" servo wants itself removed form ISR routines and list of servos "
pass # !!
def need_new_isr_p(self):
" do we need a new isr "
# check to see if adding one more to latest isr group will bust interrupts
if len(self.isrs[-1]) * 3/100.0 > 10.0/self.desired_rate:
#rate will drop if we add - need new ISR
return True
else: return False
class Servo(object):
" hold servo settings "
def __init__(self, pool, pin, min_angle=-90, detent=0, max_angle=90, min_width=1000, detent_width=1500, max_width=2000):
self.pool = pool
self.pin = pin
self.min_width = min_width
self.detent_width = detent_width
self.max_width = max_width
#
self.detent = 0
self.min_angle = min_angle
self.max_angle = max_angle
#
self.value = detent_width # pulse width used by ISR routine
def move(self, percent):
" move servo to this percent of range defined by angles"
# !! set self.value to pulse width needed.
# !! two linear interps - min-detent, detent-max
pass
def rotate(self, angle):
" move servo to angle as cal using min. max angles "
# !! set self.value to pulse width needed
# !! two linear interps - min-detent, detent-max
pass
def remove(self):
" take self out of servo pool "
self.pool.remove_servo(self)
```