class Raspi_MotorHAT: FORWARD = 1 BACKWARD = 2 BRAKE = 3 RELEASE = 4 SINGLE = 1 DOUBLE = 2 INTERLEAVE = 3 MICROSTEP = 4 def __init__(self, addr=0x60, freq=1600): self._i2caddr = addr # default addr on HAT self._frequency = freq # default @1600Hz PWM freq self.motors = [Raspi_DCMotor(self, m) for m in range(4)] self.steppers = [ Raspi_StepperMotor(self, 1), Raspi_StepperMotor(self, 2) ] self._pwm = PWM(addr, debug=False) self._pwm.setPWMFreq(self._frequency) def setPin(self, pin, value): if (pin < 0) or (pin > 15): raise NameError('PWM pin must be between 0 and 15 inclusive') if (value != 0) and (value != 1): raise NameError('Pin value must be 0 or 1!') if (value == 0): self._pwm.setPWM(pin, 0, 4096) if (value == 1): self._pwm.setPWM(pin, 4096, 0) def getStepper(self, steps, num): if (num < 1) or (num > 2): raise NameError( 'MotorHAT Stepper must be between 1 and 2 inclusive') return self.steppers[num - 1] def getMotor(self, num): if (num < 1) or (num > 4): raise NameError('MotorHAT Motor must be between 1 and 4 inclusive') return self.motors[num - 1]
class Servos(object): def __init__(self, addr=0x6f, deflect_90_in_ms = 1.15): """addr: The i2c address of the PWM chip. deflect_90_in_ms: set this to calibrate the servo motors. It is what a deflection of 90 degrees is in turns of a pulse length in milliseconds. """ # deflect_90_in_ms - set this to calibrate the servo motors. self._pwm = PWM(addr) pwm_frequency = 60 self._pwm.setPWMFreq(pwm_frequency) period_in_ms = 1000 / pwm_frequency pulse_steps = 4096.0 servo_mid_point_ms = 1.5 # Steps for every millisecond steps_per_ms = pulse_steps / period_in_ms self.steps_per_degree = (deflect_90_in_ms * steps_per_ms) / 90.0 # Mid point of the servo in steps self.servo_mid_point_steps = servo_mid_point_ms * steps_per_ms def stop_all(self): #0 in start is nothing, 4096 sets the off bit. self._pwm(0, 0, 4096) self._pwm(1, 0, 4096) self._pwm(14, 0, 4096) self._pwm(15, 0, 4096) def convert_degrees_to_pwm(self, position): return int(self.servo_mid_point_steps + (position * self.steps_per_degree)) def set_servo_angle(self, channel, angle): """position: The position in degrees from the center. -90 to 90""" if angle > 90 or angle < -90: raise ValueError("Angle outside of range") off_step = self.convert_degrees_to_pwm(angle) self._pwm.setPWM(channel, 0, off_step)
class Servos: """PCA 9865 Servo motors""" def __init__(self, addr=0x6f, deflect_90_in_ms=0.5): """addr: The i2c address of the PWM chip. deflect_90_in_ms: set this to calibrate the servo motors. it is what a deflection of 90 degrees is in terms of a pulse length in milliseconds.""" self._pwm = PWM(addr) # This sets the timebase for it all pwm_frequency = 100 self._pwm.setPWMFreq(pwm_frequency) # Mid-point of the servo pulse length in milliseconds. servo_mid_point_ms = 1.5 # Frequency is 1/period, but working ms, we can use 1000 period_in_ms = 1000 / pwm_frequency # The chip has 4096 steps in each period. pulse_steps = 4096 # Steps for every millisecond. steps_per_ms = pulse_steps / period_in_ms # Steps for a degree self.steps_per_degree = (deflect_90_in_ms * steps_per_ms) / 90 # Mid-point of the servo in steps self.servo_mid_point_steps = servo_mid_point_ms * steps_per_ms # Map for channels self.channels = [0, 1, 14, 15] def stop_all(self): # 0 in start is nothing off_bit = 4096 # bit 12 is the OFF bit. self._pwm.setPWM(self.channels[0], 0, off_bit) self._pwm.setPWM(self.channels[1], 0, off_bit) self._pwm.setPWM(self.channels[2], 0, off_bit) self._pwm.setPWM(self.channels[3], 0, off_bit) def _convert_degrees_to_steps(self, position): return int(self.servo_mid_point_steps + (position * self.steps_per_degree)) def set_servo_angle(self, channel, angle): """position: The position in degrees from the center. -90 to 90""" # Validate if angle > 90 or angle < -90: raise ValueError("Angle outside of range") # Then set the position off_step = self._convert_degrees_to_steps(angle) self._pwm.setPWM(self.channels[channel], 0, off_step)
class Servos: ## Constructor # @param[in] addr: the I2C address of the PWM chip. # @param[in] deflect_90_in_ms: set this to calibrate the seros # it is the deflection of 90 degrees # in terms of pulse length in ms def __init__(self, addr=0x6f, deflect_90_in_ms=0.9): self._pwm = PWM(addr) pwm_freqeuncy = 60 # this sets the timebase for the motorhat self._pwm.setPWMFreq(pwm_freqeuncy) #Frequency is 1/period, but working in ms, we can use 1000 period_in_ms = 1000.0 / pwm_freqeuncy #the chip has 4096 steps in each period. pulse_steps = 4096.0 # mid point of the servo pulse length in milliseconds servo_mid_point_ms = 1.6 # steps for every milisecond steps_per_ms = pulse_steps / period_in_ms # steps for a degree self.steps_per_degree = (deflect_90_in_ms * steps_per_ms) / 90.0 # Mid point of the Servo in steps self.servo_mid_point_steps = servo_mid_point_ms * steps_per_ms def stop_all(self): # 0 in start is nothing, 4096 sets the OFF bit. self._pwm.setPWM(0, 0, 4096) self._pwm.setPWM(1, 0, 4096) self._pwm.setPWM(14, 0, 4096) self._pwm.setPWM(15, 0, 4096) def _convert_degrees_to_pwm(self, position): return int(self.servo_mid_point_steps + (position * self.steps_per_degree)) ## Function to set the angle of a servo # @param[in] angle: the angle in degrees from the center. -90 to 90 # @param[in] channel: channel of servo you wish to move def set_servo_angle(self, channel, angle): # validate angle if angle > 90 or angle < -90: raise ValueError("Angle outside of range") # Then set the position off_step = self._convert_degrees_to_pwm(angle) self._pwm.setPWM(channel, 0, off_step)
class Servos(object): def __init__(self, addr=0x6f, deflect_90_in_ms=0.9): """addr: The i2c address of the PWM chip. deflect_90_in_ms: set this to calibrate the servo motors. it is what a deflection of 90 degrees is in terms of a pulse length in milliseconds.""" self._pwm = PWM(addr) # This sets the timebase for it all pwm_frequency = 60 self._pwm.setPWMFreq(pwm_frequency) # Frequency is 1/period, but working ms, we can use 1000 period_in_ms = 1000.0 / pwm_frequency # The chip has 4096 steps in each period. pulse_steps = 4096.0 # Mid point of the servo pulse length in milliseconds. servo_mid_point_ms = 1.5 # Steps for every millisecond. steps_per_ms = pulse_steps / period_in_ms # Steps for a degree self.steps_per_degree = (deflect_90_in_ms * steps_per_ms) / 90.0 # Mid point of the servo in steps self.servo_mid_point_steps = servo_mid_point_ms * steps_per_ms # Prepare servo's turned off self.stop_all() def stop_all(self): # 0 in start is nothing, 4096 sets the OFF bit. self._pwm.setPWM(0, 0, 4096) self._pwm.setPWM(1, 0, 4096) self._pwm.setPWM(14, 0, 4096) self._pwm.setPWM(15, 0, 4096) def _convert_degrees_to_pwm(self, position): return int(self.servo_mid_point_steps + (position * self.steps_per_degree)) def set_servo_angle(self, channel, angle): """position: The position in degrees from the center. -90 to 90""" # Validate if angle > 90 or angle < -90: raise ValueError("Angle outside of range") # Then set the position off_step = self._convert_degrees_to_pwm(angle) self._pwm.setPWM(channel, 0, off_step)
# What a deflection of 90 degrees is in pulse length in milliseconds deflect_90_in_ms = 1.15 # Steps for every millisecond steps_per_ms = pulse_steps / period_in_ms # Steps for a degree steps_per_degree = (deflect_90_in_ms * steps_per_ms) / 90.0 # Mid point of the servo in steps servo_mid_point_steps = servo_mid_point_ms * steps_per_ms def convert_degrees_to_pwm(position): return int(servo_mid_point_steps + (position * steps_per_degree)) def stop(): pwm.setPWM(0, 0, 4096) while (True): print("First line inside Loop") position = int( input("Type your position in degrees (90 to -90, 0 is middle): ")) print("Second line inside Loop") end_step = convert_degrees_to_pwm(position) print("The end step is {0}".format(end_step)) print("Third line inside Loop") pwm.setPWM(0, 0, end_step)
import atexit import time r = Robot() pwm = PWM(0x6f) servo_min = 150 # Servo minimum position. servo_max = 600 # Maximum position servo_mid = 225 + 150 # Middle position def stop(): pwm.setPWM(0, 0, 0) atexit.register(stop) pwm.setPWMFreq(60) # Set frequency to 60 Hz while (True): r.set_left(255) r.set_right(255) print("min") pwm.setPWM(0, 0, servo_min) time.sleep(1) print("max") pwm.setPWM(0, 0, servo_max) time.sleep(1) r.stop_motors() print("mid") pwm.setPWM(0, 0, servo_mid) time.sleep(1)
# =========================================================================== # Example Code # =========================================================================== # Initialise the PWM device using the default address # bmp = PWM(0x40, debug=True) pwm = PWM(0x6F) servoMin = 150 # Min pulse length out of 4096 servoMax = 600 # Max pulse length out of 4096 def setServoPulse(channel, pulse): pulseLength = 1000000 # 1,000,000 us per second pulseLength //= 60 # 60 Hz print("%d us per period" % pulseLength) pulseLength //= 4096 # 12 bits of resolution print("%d us per bit" % pulseLength) pulse *= 1000 pulse //= pulseLength pwm.setPWM(channel, 0, pulse) pwm.setPWMFreq(60) # Set frequency to 60 Hz while (True): # Change speed of continuous servo on channel O pwm.setPWM(0, 0, servoMin) time.sleep(1) pwm.setPWM(0, 0, servoMax) time.sleep(1)