def test(device_path, baud):
from arduino_servo.arduino_servo import ArduinoServo
print 'probing arduino servo on', device_path
device = serial.Serial(device_path, baud)
device.timeout = 0 #nonblocking
fcntl.ioctl(device.fileno(), TIOCEXCL) #exclusive
driver = ArduinoServo(device.fileno())
t0 = time.time()
if driver.initialize(baud):
print 'arduino servo found'
exit(0)
exit(1)
class Servo(object):
calibration_filename = autopilot.pypilot_dir + 'servocalibration'
def __init__(self, server, serialprobe):
self.server = server
self.serialprobe = serialprobe
self.lastdir = 0 # doesn't matter
self.servo_calibration = ServoCalibration(self)
self.calibration = self.Register(JSONValue, 'calibration', {})
self.load_calibration()
self.min_speed = self.Register(RangeProperty, 'min_speed', .5, 0, 1, persistent=True)
self.max_speed = self.Register(RangeProperty, 'max_speed', 1, 0, 1, persistent=True)
self.faults = self.Register(Property, 'faults', 0)
# power usage
self.command = self.Register(TimedProperty, 'command', 0)
self.current_timestamp = time.time()
timestamp = server.TimeStamp('servo')
self.voltage = self.Register(SensorValue, 'voltage', timestamp)
self.current = self.Register(SensorValue, 'current', timestamp)
self.controller_temp = self.Register(SensorValue, 'controller_temp', timestamp)
self.motor_temp = self.Register(SensorValue, 'motor_temp', timestamp)
self.rudder_pos = self.Register(SensorValue, 'rudder_pos', timestamp)
self.engauged = self.Register(BooleanValue, 'engauged', False)
self.max_current = self.Register(RangeProperty, 'max_current', 2, 0, 20, persistent=True)
self.max_controller_temp = self.Register(RangeProperty, 'max_controller_temp', 60, 45, 100, persistent=True)
self.max_motor_temp = self.Register(RangeProperty, 'max_motor_temp', 55, 30, 100, persistent=True)
self.min_rudder_pos = self.Register(RangeProperty, 'min_rudder_pos', -100, -100, 100, persistent=True)
self.max_rudder_pos = self.Register(RangeProperty, 'max_rudder_pos', 100, -100, 100, persistent=True)
self.period = self.Register(RangeProperty, 'period', .7, .1, 3, persistent=True)
self.compensate_current = self.Register(BooleanProperty, 'compensate_current', False, persistent=True)
self.compensate_voltage = self.Register(BooleanProperty, 'compensate_voltage', False, persistent=True)
self.amphours = self.Register(ResettableValue, 'amp_hours', 0, persistent=True)
self.watts = self.Register(SensorValue, 'watts', timestamp)
self.speed = self.Register(SensorValue, 'speed', timestamp)
self.speed.set(0)
self.position = self.Register(SensorValue, 'position', timestamp)
self.position.set(.5)
self.rawcommand = self.Register(SensorValue, 'raw_command', timestamp)
self.lastpositiontime = time.time()
self.lastpositionamphours = 0
self.windup = 0
self.windup_change = 0
self.disengauged = True
self.disengauge_on_timeout = self.Register(BooleanValue, 'disengauge_on_timeout', True, persistent=True)
self.force_engauged = False
self.last_zero_command_time = self.command_timeout = time.time()
self.driver_timeout_start = 0
self.mode = self.Register(StringValue, 'mode', 'none')
self.controller = self.Register(StringValue, 'controller', 'none')
self.flags = self.Register(ServoFlags, 'flags')
self.driver = False
self.raw_command(0)
def Register(self, _type, name, *args, **kwargs):
return self.server.Register(_type(*(['servo.' + name] + list(args)), **kwargs))
def send_command(self):
t = time.time()
if not self.disengauge_on_timeout.value:
self.disengauged = False
if self.servo_calibration.thread.is_alive():
print 'cal thread'
return
timeout = 1 # command will expire after 1 second
if self.command.value and not self.fault():
if time.time() - self.command.time > timeout:
print 'servo command timeout', time.time() - self.command.time
self.command.set(0)
self.disengauged = False
self.velocity_command(self.command.value)
else:
#print 'timeout', t - self.command_timeout
if self.disengauge_on_timeout.value and \
not self.force_engauged and \
t - self.command_timeout > self.period.value*3:
self.disengauged = True
self.speed.set(0)
self.raw_command(0)
def velocity_command(self, speed):
# complete integration from previous step
t = time.time()
dt = t - self.lastpositiontime
self.lastpositiontime = t
if speed == 0 and self.speed.value == 0: # optimization
self.raw_command(0)
return
if self.flags.value & ServoFlags.FWD_FAULT and speed > 0 or \
self.flags.value & ServoFlags.REV_FAULT and speed < 0:
self.speed.set(0)
self.raw_command(0)
return # abort
position = self.position.value + self.speed.value * dt / 10 # remove when speed is correct
self.position.set(min(max(position, 0), 1))
#print 'integrate pos', self.position, self.speed, speed, dt, self.fwd_fault, self.rev_fault
if self.position.value < .9:
self.flags.clearbit(ServoFlags.FWD_FAULT)
if self.position.value > .1:
self.flags.clearbit(ServoFlags.REV_FAULT)
if False: # don't keep moving really long in same direction.....
rng = 5;
if self.position.value > 1 + rng:
self.flags.fwd_fault()
if self.position.value < -rng:
self.flags.rev_fault()
if self.compensate_voltage.value:
speed *= 12 / self.voltage.value
if self.compensate_current.value:
# get current
ampseconds = 3600*(self.amphours.value - self.lastpositionamphours)
current = ampseconds / dt
self.lastpositionamphours = self.amphours.value
pass #todo fix this
# allow higher current with higher voltage???
#max_current = self.max_current.value
#if self.compensate_voltage.value:
# max_current *= self.voltage.value/voltage
min_speed = self.min_speed.value
# ensure max_speed is at least min_speed
if min_speed > self.max_speed.value:
self.max_speed.set(min_speed)
# integrate windup
self.windup += (speed - self.speed.value) * dt
# if windup overflows, move at minimum speed
if abs(self.windup) > self.period.value*min_speed / 1.5:
if abs(speed) < min_speed:
speed = min_speed if self.windup > 0 else -min_speed
else:
speed = 0
# don't let windup overflow
self.windup = min(max(self.windup, -1.5*self.period.value), 1.5*self.period.value)
#print 'windup', self.windup, dt, self.windup / dt, speed, self.speed
if speed * self.speed.value <= 0: # switched direction or stopped?
if t - self.windup_change < self.period.value:
# less than period, keep previous direction, but use minimum speed
if self.speed.value > 0:
speed = min_speed
elif self.speed.value < 0:
speed = -min_speed
else:
speed = 0
else:
self.windup_change = t
# clamp to max speed
speed = min(max(speed, -self.max_speed.value), self.max_speed.value)
if True:
self.speed.set(speed)
else:
# estimate true speed from voltage, current, and last command
# TODO
pass
if speed > 0:
cal = self.calibration.value['forward']
elif speed < 0:
cal = self.calibration.value['reverse']
else:
self.raw_command(0)
return
raw_speed = cal[0] + abs(speed)*cal[1]
if speed < 0:
raw_speed = -raw_speed
command = raw_speed
self.raw_command(command)
def raw_command(self, command):
self.rawcommand.set(command)
if command <= 0:
if command < 0:
self.mode.update('reverse')
self.lastdir = -1
else:
self.mode.update('idle')
else:
self.mode.update('forward')
self.lastdir = 1
t = time.time()
if command == 0:
# only send at .5 seconds when command is zero
if t > self.command_timeout and t - self.last_zero_command_time < .5:
return
self.last_zero_command_time = t
else:
self.command_timeout = t
if self.driver:
if self.disengauged: # keep sending disengauge to keep sync
self.driver.disengauge()
else:
max_current = self.max_current.value
if self.flags.value & ServoFlags.FWD_FAULT or \
self.flags.value & ServoFlags.REV_FAULT: # allow more current to "unstuck" ram
max_current *= 2
self.driver.max_values(max_current, self.max_controller_temp.value, self.max_motor_temp.value, self.min_rudder_pos.value, self.max_rudder_pos.value)
self.driver.command(command)
# detect driver timeout if commanded without measuring current
if self.current.value:
self.flags.clearbit(ServoFlags.DRIVER_TIMEOUT)
self.driver_timeout_start = 0
elif command:
if self.driver_timeout_start:
if time.time() - self.driver_timeout_start > 1:
self.flags.setbit(ServoFlags.DRIVER_TIMEOUT)
else:
self.driver_timeout_start = time.time()
def reset(self):
if self.driver:
self.driver.reset()
def close_driver(self):
print 'servo lost connection'
self.controller.set('none')
self.device.close()
self.driver = False
def poll(self):
if not self.driver:
device_path = self.serialprobe.probe('servo', [38400], 1)
if device_path:
#from arduino_servo.arduino_servo_python import ArduinoServo
from arduino_servo.arduino_servo import ArduinoServo
device = serial.Serial(*device_path)
device.timeout=0 #nonblocking
fcntl.ioctl(device.fileno(), TIOCEXCL) #exclusive
self.driver = ArduinoServo(device.fileno())
self.driver.max_values(self.max_current.value, self.max_controller_temp.value, self.max_motor_temp.value, self.min_rudder_pos.value, self.max_rudder_pos.value)
t0 = time.time()
if self.driver.initialize(device_path[1]):
self.device = device
print 'arduino servo found on', device_path
self.serialprobe.probe_success('servo')
self.controller.set('arduino')
self.driver.command(0)
self.lastpolltime = time.time()
else:
print 'failed in ', time.time()-t0
device.close()
self.driver = False
print 'failed to initialize servo on', device
if not self.driver:
return
self.servo_calibration.poll()
result = self.driver.poll()
#print 'servo poll', result
if result == -1:
print 'servo poll -1'
self.close_driver()
return
if result == 0:
d = time.time() - self.lastpolltime
if d > 5: # correct for clock skew
self.lastpolltime = time.time()
elif d > 4:
print 'servo timeout', d
self.close_driver()
else:
self.lastpolltime = time.time()
if self.fault():
if not self.flags.value & ServoFlags.FWD_FAULT and \
not self.flags.value & ServoFlags.REV_FAULT:
self.faults.set(self.faults.value + 1)
# if overcurrent then fault in the direction traveled
# this prevents moving further in this direction
if self.flags.value & ServoFlags.OVERCURRENT:
if self.lastdir > 0:
self.flags.fwd_fault()
self.position.set(1)
elif self.lastdir < 0:
self.flags.rev_fault()
self.position.set(-1)
self.reset() # clear fault condition
t = time.time()
self.server.TimeStamp('servo', t)
if result & ServoTelemetry.VOLTAGE:
self.voltage.set(self.driver.voltage)
if result & ServoTelemetry.CONTROLLER_TEMP:
self.controller_temp.set(self.driver.controller_temp)
if result & ServoTelemetry.MOTOR_TEMP:
self.motor_temp.set(self.driver.motor_temp)
if result & ServoTelemetry.RUDDER_POS:
if math.isnan(self.driver.rudder_pos):
self.rudder_pos.update(False)
else:
self.rudder_pos.set(self.driver.rudder_pos)
if result & ServoTelemetry.CURRENT:
self.current.set(self.driver.current)
# integrate power consumption
dt = (t - self.current_timestamp)
#print 'have current', round(1/dt), dt
self.current_timestamp = t
if self.current.value:
amphours = self.current.value*dt/3600
self.amphours.set(self.amphours.value + amphours)
lp = .003*dt # 5 minute time constant to average wattage
self.watts.set((1-lp)*self.watts.value + lp*self.voltage.value*self.current.value)
if result & ServoTelemetry.FLAGS:
self.flags.updatedriver(self.driver.flags)
self.engauged.update(not not self.driver.flags & ServoFlags.ENGAUGED)
self.send_command()
def fault(self):
if not self.driver:
return False
return self.driver.fault()
def load_calibration(self):
try:
filename = Servo.calibration_filename
print 'loading servo calibration', filename
file = open(filename)
self.calibration.set(json.loads(file.readline()))
except:
print 'WARNING: using default servo calibration!!'
self.calibration.set({'forward': [.2, .6], 'reverse': [.2, .6]})
def save_calibration(self):
file = open(Servo.calibration_filename, 'w')
file.write(json.dumps(self.calibration))
