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)
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 poll(self):
if not self.driver:
device_path = serialprobe.probe('servo', [38400], 1)
if device_path:
#from arduino_servo.arduino_servo_python import ArduinoServo
from arduino_servo.arduino_servo import ArduinoServo
try:
device = serial.Serial(*device_path)
device.timeout = 0 #nonblocking
fcntl.ioctl(device.fileno(), TIOCEXCL) #exclusive
except Exception as e:
print 'failed to open servo on:', device_path, e
return
self.driver = ArduinoServo(device.fileno(), device_path[1])
uncorrected_max_current = max(
0, self.max_current.value -
self.current.offset.value) / self.current.factor.value
self.send_driver_params(uncorrected_max_current)
self.device = device
self.device.path = device_path[0]
self.lastpolltime = time.time()
if not self.driver:
return
self.servo_calibration.poll()
result = self.driver.poll()
if result == -1:
print 'servo lost'
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.controller.value == 'none':
device_path = [self.device.port, self.device.baudrate]
print 'arduino servo found on', device_path
serialprobe.success('servo', device_path)
self.controller.set('arduino')
self.driver.command(0)
t = time.time()
self.server.TimeStamp('servo', t)
if result & ServoTelemetry.VOLTAGE:
# apply correction
corrected_voltage = self.voltage.factor.value * self.driver.voltage
corrected_voltage += self.voltage.offset.value
self.voltage.set(round(corrected_voltage, 3))
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:
if self.driver.rudder and not math.isnan(self.driver.rudder):
data = {
'angle': self.driver.rudder,
'timestamp': t,
'device': self.device.path
}
self.sensors.write('rudder', data, 'servo')
if result & ServoTelemetry.CURRENT:
# apply correction
corrected_current = self.current.factor.value * self.driver.current
corrected_current = max(
0, corrected_current + self.current.offset.value)
self.current.set(round(corrected_current, 3))
# 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.max_current.set_max(40 if self.driver.flags
& ServoFlags.CURRENT_RANGE else 20)
flags = self.flags.value & ~ServoFlags.DRIVER_MASK | self.driver.flags
# if rudder angle comes from serial or tcp, may need to set these flags
angle = self.sensors.rudder.angle.value
if angle: # note, this is ok here for both False and 0
if abs(angle) > self.sensors.rudder.range.value:
if angle > 0:
flags |= ServoFlags.MIN_RUDDER
else:
flags |= ServoFlags.MAX_RUDDER
self.flags.update(flags)
self.engaged.update(not not self.driver.flags & ServoFlags.ENGAGED)
if result & ServoTelemetry.EEPROM: # occurs only once after connecting
self.max_current.set(self.driver.max_current)
self.max_controller_temp.set(self.driver.max_controller_temp)
self.max_motor_temp.set(self.driver.max_motor_temp)
self.max_slew_speed.set(self.driver.max_slew_speed)
self.max_slew_slow.set(self.driver.max_slew_slow)
#print "GOT EEPROM", self.driver.rudder_scale, self.driver.rudder_offset, self.driver.rudder_range
self.sensors.rudder.scale.set(self.driver.rudder_scale)
self.sensors.rudder.nonlinearity.set(
self.driver.rudder_nonlinearity)
self.sensors.rudder.offset.set(self.driver.rudder_offset)
self.sensors.rudder.range.set(self.driver.rudder_range)
self.current.factor.set(self.driver.current_factor)
self.current.offset.set(self.driver.current_offset)
self.voltage.factor.set(self.driver.voltage_factor)
self.voltage.offset.set(self.driver.voltage_offset)
self.min_speed.set(self.driver.min_motor_speed)
self.max_speed.set(self.driver.max_motor_speed)
self.gain.set(self.driver.gain)
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
if not self.sensors.rudder.invalid():
self.position.set(self.sensors.rudder.angle.value)
self.send_command()
def poll(self):
if not self.driver:
device_path = serialprobe.probe('servo', [38400], 1)
if device_path:
#from arduino_servo.arduino_servo_python import ArduinoServo
from arduino_servo.arduino_servo import ArduinoServo
try:
device = serial.Serial(*device_path)
device.timeout=0 #nonblocking
fcntl.ioctl(device.fileno(), TIOCEXCL) #exclusive
except Exception as e:
print 'failed to open servo:', e
return
self.driver = ArduinoServo(device.fileno())
uncorrected_max_current = (self.max_current.value - self.current.offset.value)/ self.current.factor.value
self.send_driver_params(uncorrected_max_current)
t0 = time.time()
if self.driver.initialize(device_path[1]):
self.device = device
print 'arduino servo found on', device_path
serialprobe.success('servo', device_path)
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()
t = time.time()
self.server.TimeStamp('servo', t)
if result & ServoTelemetry.VOLTAGE:
# apply correction
corrected_voltage = self.voltage.factor.value*self.driver.voltage;
corrected_voltage += self.voltage.offset.value
self.voltage.set(round(corrected_voltage, 3))
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:
if math.isnan(self.driver.rudder):
self.rudder.update(False)
else:
self.rudder.set(self.rudder_scale.value *
(self.driver.rudder +
self.rudder_offset.value - 0.5))
if result & ServoTelemetry.CURRENT:
# apply correction
corrected_current = self.current.factor.value*self.driver.current;
corrected_current += self.current.offset.value
self.current.set(round(corrected_current, 3))
# 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.max_current.set_max(60 if self.driver.flags & ServoFlags.CURRENT_RANGE else 20)
self.flags.update(self.flags.value & ~ServoFlags.DRIVER_MASK | self.driver.flags)
self.engaged.update(not not self.driver.flags & ServoFlags.ENGAGED)
if result & ServoTelemetry.EEPROM: # occurs only once after connecting
self.max_current.set(self.driver.max_current)
self.max_controller_temp.set(self.driver.max_controller_temp)
self.max_motor_temp.set(self.driver.max_motor_temp)
self.max_slew_speed.set(self.driver.max_slew_speed)
self.max_slew_slow.set(self.driver.max_slew_slow)
self.rudder_scale.set(self.driver.rudder_scale)
self.rudder_offset.set(self.driver.rudder_offset)
self.rudder_range.set(self.driver.rudder_range)
self.current.factor.set(self.driver.current_factor)
self.current.offset.set(self.driver.current_offset)
self.voltage.factor.set(self.driver.voltage_factor)
self.voltage.offset.set(self.driver.voltage_offset)
self.min_speed.set(self.driver.min_motor_speed);
self.max_speed.set(self.driver.max_motor_speed);
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
self.send_command()