def __init__( self, pwm_i2c=0x40, pwm_freq=50, left_channel=0, right_channel=1, aux_channel1=4, aux_channel2=5, aux_channel3=6, aux_channel4=7, debug=False ): # Main set of motor controller ranges self.servo_min = 900 self.servo_mid = 1650 self.servo_max = 2300 # Full speed range self.servo_full_min = 900 self.servo_full_max = 2300 # Low speed range is 1/4 of full speed speed_divisor = 2 self.servo_low_min = int(self.servo_mid - (self.servo_mid-self.servo_full_min) / speed_divisor) self.servo_low_max = int(self.servo_mid + (self.servo_full_max-self.servo_mid) / speed_divisor) self.channels = { 'left': left_channel, 'right': right_channel, } self.pwm = PWM(pwm_i2c, debug=debug) self.pwm.setPWMFreq(pwm_freq) # Flag set to True when motors are allowed to move self.drive_enabled = False self.disable_drive()
class DriveTrain(): """Instantiate a 2WD drivetrain, utilising 2x ESCs, controlled using a 2 axis (throttle, steering) system""" def __init__( self, pwm_i2c=0x40, pwm_freq=50, left_channel=0, right_channel=1, aux_channel1=4, aux_channel2=5, aux_channel3=6, aux_channel4=7, debug=False ): # Main set of motor controller ranges self.servo_min = 900 self.servo_mid = 1650 self.servo_max = 2300 # Full speed range self.servo_full_min = 900 self.servo_full_max = 2300 # Low speed range is 1/4 of full speed speed_divisor = 2 self.servo_low_min = int(self.servo_mid - (self.servo_mid-self.servo_full_min) / speed_divisor) self.servo_low_max = int(self.servo_mid + (self.servo_full_max-self.servo_mid) / speed_divisor) self.channels = { 'left': left_channel, 'right': right_channel, } self.pwm = PWM(pwm_i2c, debug=debug) self.pwm.setPWMFreq(pwm_freq) # Flag set to True when motors are allowed to move self.drive_enabled = False self.disable_drive() def set_servo_pulse(self, channel, pulse): """Send a raw servo pulse length to a specific speed controller channel""" # Only send servo pulses if drive is enabled if self.drive_enabled: # 1,000,000 us per second pulseLength = 1000000 # 60 Hz pulseLength /= 50 # logging.debug("%d us per period" % pulseLength) # 12 bits of resolution pulseLength /= 4096 # logging.debug("%d us per bit" % pulseLength) # pulse *= 1000 pulse /= pulseLength logging.debug( "pulse {0} - channel {1}".format( int(pulse), channel ) ) self.pwm.setPWM(channel, 0, int(pulse)) def enable_drive(self): """Allow motors to be used""" self.drive_enabled = True def disable_drive(self): """Disable motors so they cant be used""" self.set_neutral() self.drive_enabled = False def set_neutral(self): """Send the neutral servo position to both motor controllers""" self.set_servo_pulse(self.channels['left'], self.servo_mid) self.set_servo_pulse(self.channels['right'], self.servo_mid) def set_full_speed(self): """Set servo range to FULL extents""" self.servo_min = self.servo_full_min self.servo_max = self.servo_full_max def set_low_speed(self): """Limit servo range extents""" self.servo_min = self.servo_low_min self.servo_max = self.servo_low_max # TODO - flesh out setters for raw pulse values (both channels) def mix_channels_and_assign(self, throttle, steering): """Take values for the throttle and steering channels in the range -1 to 1, convert into servo pulses, and then mix the channels and assign to the left/right motor controllers""" if not self.drive_enabled: return pulse_throttle = self._map_channel_value(throttle) pulse_steering = self._map_channel_value(steering) output_pulse_left = clip( (pulse_throttle + pulse_steering) / 2, self.servo_min, self.servo_max ) output_pulse_right = clip( (pulse_throttle - pulse_steering) / 2 + self.servo_mid, self.servo_min, self.servo_max ) # Set the servo pulses for left and right channels self.set_servo_pulse(self.channels['left'], output_pulse_left) self.set_servo_pulse(self.channels['right'], output_pulse_right) def _map_channel_value(self, value): """Map the supplied value from the range -1 to 1 to a corresponding value within the range servo_min to servo_max""" return int( interp( value, [-1, 1], [self.servo_min, self.servo_max] ) )
#!/usr/bin/env python import cwiid import time from libs.Adafruit_PWM_Servo_Driver import PWM from numpy import interp, clip #connecting to the wiimote. This allows several attempts # as first few often fail. pwm = PWM(0x40, debug=False) pwm.setPWMFreq(50) #servos.setPWM(15,4095) def set_servo_pulse(channel, pulse): pulseLength = 1000000 # 1,000,000 us per second pulseLength /= 50 # 60 Hz print "%d us per period" % pulseLength pulseLength /= 4096 # 12 bits of resolution print "%d us per bit" % pulseLength # pulse *= 1000 pulse /= pulseLength print "pulse {0}".format(pulse) pwm.setPWM(channel, 0, pulse) print("Press 1+2 on your Wiimote now...") wm = None i = 2 while not wm: try: wm = cwiid.Wiimote()
def __init__( self, pwm_i2c=0x40, pwm_freq=50, left_channel=0, right_channel=1, aux_channel1=4, aux_channel2=5, aux_channel3=6, aux_channel4=7, debug=False ): # Main set of motor controller ranges self.servo_min = 900 #self.servo_mid = 1555 self.servo_mid = 1650 self.servo_max = 2300 # Full speed range self.servo_full_min = 900 self.servo_full_max = 2300 # Low speed range is 1/4 of full speed speed_divisor = 2 self.servo_low_min = int(self.servo_mid - (self.servo_mid-self.servo_full_min) / speed_divisor) self.servo_low_max = int(self.servo_mid + (self.servo_full_max-self.servo_mid) / speed_divisor) # Skittle launcher servos self.skittle_left_servo_closed = 1650 self.skittle_left_servo_open = 1950 self.skittle_right_servo_closed = 1600 self.skittle_right_servo_open = 1200 self.skittle_left_servo_init = 1300 self.skittle_right_servo_init = 1300 # Skittle launcher motors self.skittle_left_motor_stopped = 2000 self.skittle_left_motor_full_speed = 1000 self.skittle_right_motor_stopped = 2000 self.skittle_right_motor_full_speed = 1000 # Proximity probe servo limites self.proximity_servo_min = 1000 self.proximity_servo_max = 2000 # Proximity probe fan limits self.proximity_blower_esc_stopped = 1000 self.proximity_blower_esc_max = 2000 self.channels = { 'left': left_channel, 'right': right_channel, 'blower': aux_channel1, 'probe_servo': aux_channel2, 'skittle_left_servo': aux_channel1, 'skittle_right_servo': aux_channel2, 'skittle_left_motor': aux_channel3, 'skittle_right_motor': aux_channel4 } self.pwm = PWM(pwm_i2c, debug=debug) self.pwm.setPWMFreq(pwm_freq) # Flag set to True when motors are allowed to move self.drive_enabled = False self.disable_drive()
def __init__( self, pwm_i2c=0x40, pwm_freq=50, left_channel=0, right_channel=1, aux_channel1=4, aux_channel2=5, aux_channel3=6, aux_channel4=7, debug=False ): # Main set of motor controller ranges self.servo_min = 900 #self.servo_mid = 1555 self.servo_mid = 1650 self.servo_max = 2300 # Full speed range self.servo_full_min = 900 self.servo_full_max = 2300 # Low speed range is 1/4 of full speed speed_divisor = 2 self.servo_low_min = int(self.servo_mid - (self.servo_mid-self.servo_full_min)/speed_divisor) self.servo_low_max = int(self.servo_mid + (self.servo_full_max-self.servo_mid)/speed_divisor) # Skittle launcher servos self.skittle_left_servo_closed = 1600 self.skittle_left_servo_open = 1400 self.skittle_right_servo_closed = 1600 self.skittle_right_servo_open = 1400 # Skittle launcher motors self.skittle_left_motor_stopped = 1000 self.skittle_left_motor_full_speed = 2000 self.skittle_left_motor_stopped = 1000 self.skittle_left_motor_full_speed = 2000 # Proximity probe servo limites self.proximity_servo_min = 1000 self.proximity_servo_max = 2000 # Proximity probe fan limits self.proximity_blower_esc_stopped = 1000 self.proximity_blower_esc_max = 2000 self.channels = { 'left': left_channel, 'right': right_channel 'blower': aux_channel1 'probe_servo': aux_channel2 'skittle_left_servo':aux_channel1 'skittle_right_servo':aux_channel2 'skittle_left_motor':aux_channel3 'skittle_right_motor':aux_channel4 } self.pwm = PWM(pwm_i2c, debug=debug) self.pwm.setPWMFreq(pwm_freq) # Flag set to True when motors are allowed to move self.drive_enabled = False self.disable_drive()
class DriveTrain(): """Instantiate a 2WD drivetrain, utilising 2x ESCs, controlled using a 2 axis (throttle, steering) system + skittle accessories""" def __init__( self, pwm_i2c=0x40, pwm_freq=50, left_channel=0, right_channel=1, aux_channel1=4, aux_channel2=5, aux_channel3=6, aux_channel4=7, debug=False ): # Main set of motor controller ranges self.servo_min = 900 #self.servo_mid = 1555 self.servo_mid = 1650 self.servo_max = 2300 # Full speed range self.servo_full_min = 900 self.servo_full_max = 2300 # Low speed range is 1/4 of full speed speed_divisor = 2 self.servo_low_min = int(self.servo_mid - (self.servo_mid-self.servo_full_min)/speed_divisor) self.servo_low_max = int(self.servo_mid + (self.servo_full_max-self.servo_mid)/speed_divisor) # Skittle launcher servos self.skittle_left_servo_closed = 1600 self.skittle_left_servo_open = 1400 self.skittle_right_servo_closed = 1600 self.skittle_right_servo_open = 1400 # Skittle launcher motors self.skittle_left_motor_stopped = 1000 self.skittle_left_motor_full_speed = 2000 self.skittle_left_motor_stopped = 1000 self.skittle_left_motor_full_speed = 2000 # Proximity probe servo limites self.proximity_servo_min = 1000 self.proximity_servo_max = 2000 # Proximity probe fan limits self.proximity_blower_esc_stopped = 1000 self.proximity_blower_esc_max = 2000 self.channels = { 'left': left_channel, 'right': right_channel 'blower': aux_channel1 'probe_servo': aux_channel2 'skittle_left_servo':aux_channel1 'skittle_right_servo':aux_channel2 'skittle_left_motor':aux_channel3 'skittle_right_motor':aux_channel4 } self.pwm = PWM(pwm_i2c, debug=debug) self.pwm.setPWMFreq(pwm_freq) # Flag set to True when motors are allowed to move self.drive_enabled = False self.disable_drive() def set_servo_pulse(self, channel, pulse): """Send a raw servo pulse length to a specific speed controller channel""" # Only send servo pulses if drive is enabled if self.drive_enabled: # 1,000,000 us per second pulseLength = 1000000 # 60 Hz pulseLength /= 50 # logging.debug("%d us per period" % pulseLength) # 12 bits of resolution pulseLength /= 4096 # logging.debug("%d us per bit" % pulseLength) # pulse *= 1000 pulse /= pulseLength logging.debug("pulse {0} - channel {1}".format(int(pulse), channel)) self.pwm.setPWM(channel, 0, int(pulse)) def enable_drive(self): """Allow motors to be used""" self.drive_enabled = True def disable_drive(self): """Disable motors so they cant be used""" self.set_neutral() self.drive_enabled = False def set_neutral(self): """Send the neutral servo position to both motor controllers""" self.set_servo_pulse(self.channels['left'], self.servo_mid) self.set_servo_pulse(self.channels['right'], self.servo_mid) def set_full_speed(self): """Set servo range to FULL extents""" self.servo_min = self.servo_full_min self.servo_max = self.servo_full_max def set_low_speed(self): """Limit servo range extents""" self.servo_min = self.servo_low_min self.servo_max = self.servo_low_max # TODO - flesh out setters for raw pulse values (both channels) def mix_channels_and_assign(self, throttle, steering): """Take values for the throttle and steering channels in the range -1 to 1, convert into servo pulses, and then mix the channels and assign to the left/right motor controllers""" if not self.drive_enabled: return pulse_throttle = self._map_channel_value(throttle) pulse_steering = self._map_channel_value(steering) output_pulse_left = clip( #(-pulse_throttle + pulse_steering) / 2 + self.servo_mid, (pulse_throttle + pulse_steering) / 2, self.servo_min, self.servo_max ) output_pulse_right = clip( #(pulse_throttle + pulse_steering) / 2, (pulse_throttle - pulse_steering) / 2 + self.servo_mid, self.servo_min, self.servo_max ) logging.info( "output pulse left: {0} output pulse right : {1}".format( output_pulse_left, output_pulse_right ) ) # Set the servo pulses for left and right channels self.set_servo_pulse(self.channels['left'], output_pulse_left) self.set_servo_pulse(self.channels['right'], output_pulse_right) def _map_channel_value(self, value): """Map the supplied value from the range -1 to 1 to a corresponding value within the range servo_min to servo_max""" return int( interp( value, [-1, 1], [self.servo_min, self.servo_max] ) ) def set_skittle_arms_closed(self): """Send the back servo position to both skittle servos""" self.set_servo_pulse(self.channels['skittle_left_servo'], self.skittle_left_servo_closed) self.set_servo_pulse(self.channels['skittle_right_servo'], self.skittle_right_servo_closed) def set_skittle_arms_open(self): """Send the forward servo position to both skittle servos""" self.set_servo_pulse(self.channels['skittle_left_servo'], self.skittle_left_servo_open) self.set_servo_pulse(self.channels['skittle_right_servo'], self.skittle_right_servo_open) def set_skittle_motors_on(self): """Send the on servo position to both skittle escs""" self.set_servo_pulse(self.channels['skittle_left_motor'], self.skittle_left_motor_full_speed) self.set_servo_pulse(self.channels['skittle_right_motor'], self.skittle_right_motor_full_speed) def set_skittle_motors_off(self): """Send the stopped servo position to both skittle escs""" self.set_servo_pulse(self.channels['skittle_left_motor'], self.skittle_left_motor_stopped) self.set_servo_pulse(self.channels['skittle_right_motor'], self.skittle_right_motor_stopped)