class ZumoMotor(object): def __init__(self, use_20khz_pwm=False): self.dir_l = Pin(DIR_L, Pin.OUT) self.dir_r = Pin(DIR_R, Pin.OUT) self.pwm_l = Pin(PWM_L, Pin.OUT) self.pwm_r = Pin(PWM_R, Pin.OUT) self.tim_r = Timer(4, freq=1000 if not (use_20khz_pwm) else 20000) self.ch_r = self.tim_r.channel(2, Timer.PWM, pin=self.pwm_r) self.tim_l = Timer(14, freq=500 if not (use_20khz_pwm) else 20000) self.ch_l = self.tim_l.channel(1, Timer.PWM, pin=self.pwm_l) self.flipLeft = False self.flipRight = False initialized = True # This class is always initialised and doens't need to initialised before # every change of speed def flipLeftMotor(self, flip): self.flipleft = flip def flipRightMotor(self, flip): self.flipRight = flip def setLeftSpeed(self, speed): reverse = 0 if (speed < 0): #if speed is negatif we make tha value positif again speed = -speed #but put the reverse value to 1 so we know we need to go backwars reverse = 1 if (speed > 400): #speed can be maximum 400 speed = 400 self.ch_l.pulse_width_percent( int(speed / 4)) # value goes from 0-400 but in python we need % for PWM. #We divide by 4 to have a value that goes from 0-100 if (reverse ^ self.flipLeft): self.dir_l.value(1) else: self.dir_l.value(0) def setRightSpeed(self, speed): reverse = 0 if (speed < 0): speed = -speed reverse = 1 if (speed > 400): speed = 400 self.ch_r.pulse_width_percent(int(speed / 4)) if (reverse ^ self.flipLeft): self.dir_r.value(1) else: self.dir_r.value(0) def setSpeeds(self, leftSpeed, rightSpeed): self.setLeftSpeed(leftSpeed) self.setRightSpeed(rightSpeed)
class MOTORS(): def __init__(self): #设定Pin self._rForward = Pin('B8') self._rBackward = Pin('B9') self._lForward = Pin('B14') self._lBackward = Pin('B15') #set right motor pwm self._rTim = Timer(4, freq=3000) self._rf_ch = self._rTim.channel(3, Timer.PWM, pin=self._rForward) self._rb_ch = self._rTim.channel(4, Timer.PWM, pin=self._rBackward) #set left motor pwm self._lTim = Timer(12, freq=3000) self._lf_ch = self._lTim.channel(1, Timer.PWM, pin=self._lForward) self._lb_ch = self._lTim.channel(2, Timer.PWM, pin=self._lBackward) #设定右边电机的转速 #-1 < ratio < 1 def set_ratio_r(self, ratio): #check ratio if(ratio > 1.0): ratio = 1.0 elif(ratio < -1.0): ratio = -1.0 if(ratio > 0): self._rb_ch.pulse_width_percent(0) self._rf_ch.pulse_width_percent(ratio*100) elif(ratio < 0): self._rf_ch.pulse_width_percent(0) self._rb_ch.pulse_width_percent(-ratio*100) else: self._rf_ch.pulse_width_percent(0) self._rb_ch.pulse_width_percent(0) #设定左边电机的转速 #-1 < ratio < 1 def set_ratio_l(self, ratio): #check ratio if(ratio > 1.0): ratio = 1.0 elif(ratio < -1.0): ratio = -1.0 if(ratio > 0): self._lb_ch.pulse_width_percent(0) self._lf_ch.pulse_width_percent(ratio*100) elif(ratio < 0): self._lf_ch.pulse_width_percent(0) self._lb_ch.pulse_width_percent(-ratio*100) else: self._lf_ch.pulse_width_percent(0) self._lb_ch.pulse_width_percent(0) def all_stop(self): self.set_ratio_l(0) self.set_ratio_r(0)
class MotorWheel: # lrv_pct_vel (Low Range Value) # urv_pct_vel (Upper Range Value) # srv_pct_vel (Scale Range Value) def __init__(self, pin_cw, timer_cw, channel_cw, pin_ccw, timer_ccw, channel_ccw): out_cw = Pin(pin_cw, Pin.OUT_PP) out_ccw = Pin(pin_ccw, Pin.OUT_PP) self.t_conf_cw = Timer(timer_cw, freq=1000) self.t_conf_ccw = Timer(timer_ccw, freq=1000) self.pwm_cw = self.t_conf_cw.channel(channel_cw, Timer.PWM, pin=out_cw) self.pwm_ccw = self.t_conf_ccw.channel(channel_ccw, Timer.PWM, pin=out_ccw) self.lrv_pct_vel_cw = 0.0 self.urv_pct_vel_cw = 100.0 self.srv_pct_vel_cw = 0.0 self.lrv_pct_vel_ccw = 0.0 self.urv_pct_vel_ccw = 100.0 self.srv_pct_vel_ccw = 0.0 def update_config(self, conf_srv_vel): self.lrv_pct_vel_cw = conf_srv_vel[0] self.urv_pct_vel_cw = conf_srv_vel[1] self.lrv_pct_vel_ccw = conf_srv_vel[2] self.urv_pct_vel_ccw = conf_srv_vel[3] def run_scale(self, pct_vel): vel_pct_cw = (abs(pct_vel * -1) - (pct_vel * -1)) / 2 vel_pct_ccw = (abs(pct_vel) - pct_vel) / 2 if vel_pct_cw > 0: self.srv_pct_vel_cw = (vel_pct_cw * ( (self.urv_pct_vel_cw - self.lrv_pct_vel_cw) / 100) ) + self.lrv_pct_vel_cw else: self.srv_pct_vel_cw = 0 if vel_pct_ccw > 0: self.srv_pct_vel_ccw = (vel_pct_ccw * ( (self.urv_pct_vel_ccw - self.lrv_pct_vel_ccw) / 100) ) + self.lrv_pct_vel_ccw else: self.srv_pct_vel_ccw = 0 self.pwm_cw.pulse_width_percent(self.srv_pct_vel_cw) self.pwm_ccw.pulse_width_percent(self.srv_pct_vel_ccw) #print(pct_vel) #print("Avance = %.2f Retorno = %.2f" %(self.srv_pct_vel_cw, self.srv_pct_vel_ccw)) return self.srv_pct_vel_cw, self.srv_pct_vel_ccw
class LedController(): def __init__(self): self.red_led = pyb.LED(1) self.green_led = pyb.LED(2) self.blue_led = pyb.LED(3) self.led_timer = Timer(LED_TIMER_ID, freq=50, callback=self.leds_on) self.red_channel = self.led_timer.channel(1, Timer.PWM, callback=self.red_led_off, pulse_width_percent=0) self.green_channel = self.led_timer.channel( 2, Timer.PWM, callback=self.green_led_off, pulse_width_percent=0) self.blue_channel = self.led_timer.channel(3, Timer.PWM, callback=self.blue_led_off, pulse_width_percent=0) def leds_on(self, timer): self.red_led.on() self.green_led.on() self.blue_led.on() def red_led_off(self, timer): self.red_led.off() def green_led_off(self, timer): self.green_led.off() def blue_led_off(self, timer): self.blue_led.off() def set_pw_colors(self, i, red_channel, green_channel, blue_channel): red_on = max(0, math.sin(i) * RBG_MAX) green_on = max(0, math.sin(i - DEG_120) * RBG_MAX) blue_on = max(0, math.sin(i - DEG_240) * RBG_MAX) debug("%f %f %f" % (red_on, blue_on, green_on)) self.red_channel.pulse_width_percent(red_on) self.green_channel.pulse_width_percent(green_on) self.blue_channel.pulse_width_percent(blue_on) def pulse(self, duration_ms): startTime = pyb.millis() while (duration_ms < 0 or pyb.elapsed_millis(startTime) < duration_ms): self.set_pw_colors(pyb.millis() / 1000, self.red_channel, self.green_channel, self.blue_channel) pyb.udelay(self.led_timer.period()) self.led_timer.deinit() self.red_led_off(None) self.green_led_off(None) self.blue_led_off(None)
class MOTORS(): def __init__(self): #设定Pin self._rForward = Pin('B8') self._rBackward = Pin('B9') self._lForward = Pin('B14') self._lBackward = Pin('B15') #set right motor pwm self._rTim = Timer(4, freq=3000) self._rf_ch = self._rTim.channel(3, Timer.PWM, pin=self._rForward) self._rb_ch = self._rTim.channel(4, Timer.PWM, pin=self._rBackward) #set left motor pwm self._lTim = Timer(12, freq=3000) self._lf_ch = self._lTim.channel(1, Timer.PWM, pin=self._lForward) self._lb_ch = self._lTim.channel(2, Timer.PWM, pin=self._lBackward) #设定右边电机的转速 #-1 < ratio < 1 def set_ratio_r(self, ratio): #check ratio if (ratio > 1.0): ratio = 1.0 elif (ratio < -1.0): ratio = -1.0 if (ratio > 0): self._rb_ch.pulse_width_percent(0) self._rf_ch.pulse_width_percent(ratio * 100) elif (ratio < 0): self._rf_ch.pulse_width_percent(0) self._rb_ch.pulse_width_percent(-ratio * 100) else: self._rf_ch.pulse_width_percent(0) self._rb_ch.pulse_width_percent(0) #设定左边电机的转速 #-1 < ratio < 1 def set_ratio_l(self, ratio): #check ratio if (ratio > 1.0): ratio = 1.0 elif (ratio < -1.0): ratio = -1.0 if (ratio > 0): self._lb_ch.pulse_width_percent(0) self._lf_ch.pulse_width_percent(ratio * 100) elif (ratio < 0): self._lf_ch.pulse_width_percent(0) self._lb_ch.pulse_width_percent(-ratio * 100) else: self._lf_ch.pulse_width_percent(0) self._lb_ch.pulse_width_percent(0) def all_stop(self): self.set_ratio_l(0) self.set_ratio_r(0)
def move_motor1(right, left): #W16 has Timer2, Channel1 Apwm = Pin('W16') timerA = Timer(2, freq=1000) chA = timerA.channel(1, Timer.PWM, pin=Apwm) Ain1 = Pin('W22', Pin.OUT_PP) Ain2 = Pin('W24', Pin.OUT_PP) standBy = Pin('W29', Pin.OUT_PP) #W29 has Timer1, Channel3 Bpwm = Pin('Y12') timerB = Timer(1, freq=1000) chB = timerB.channel(3, Timer.PWM, pin=Bpwm) Bin1 = Pin('W30', Pin.OUT_PP) Bin2 = Pin('W32', Pin.OUT_PP) def clockwise(motor, speed): if motor == 'a': Ain1.high() Ain2.low() chA.pulse_width_percent(speed) elif motor == 'b': Bin1.high() Bin2.low() chB.pulse_width_percent(100 - speed) def stop(motor): if motor == 'a': Ain1.low() Ain2.low() chA.pulse_width_percent(0) elif motor == 'b': Bin1.low() Bin2.low() chB.pulse_width_percent(0) def main(): standBy.high() while (True): stop('a') stop('b') clockwise('a', 50) clockwise('b', 50) utime.sleep_ms(2500) break utime.sleep(1) main()
def main(): # Configure the timer as a microsecond counter. tim = Timer(config["timer"], prescaler=(machine.freq()[0] // 1000000) - 1, period=config["timer-period"]) print(tim) # Configure channel for timer IC. ch = tim.channel(1, Timer.IC, pin=config["pin-capture"], polarity=Timer.FALLING) # Slave mode disabled in order to configure mem32[config["timer-addr"] + TIM_SMCR] = 0 # Reset on rising edge (or falling in case of inverted detection). Ref: 25.4.3 of STM32F76xxx_reference_manual.pdf mem32[config["timer-addr"] + TIM_SMCR] = (mem32[config["timer-addr"] + TIM_SMCR] & 0xfffe0000) | 0x54 # Capture sensitive to rising edge. Ref: 25.4.9 of STM32F76xxx_reference_manual.pdf mem32[config["timer-addr"] + TIM_CCER] = 0b1001 try: capture(ch, 50) finally: tim.deinit()
def start(self, speed, direction): """ method to start a motor Arguments: speed : speed of the motor 0-100 (as percentage of max) direction : CW or CCW, for clockwise or counterclockwise """ PWMpin = Pin(self.PWMpin) DIRpin = Pin(self.DIRpin, Pin.OUT_PP) # DIR1 and DIR2 have to be opposite to move, toggle to change direction if direction in ('cw','CW','clockwise'): DIRpin.high() elif direction in ('ccw','CCW','counterclockwise'): DIRpin.low() else: raise ValueError('Please enter CW or CCW for direction.') # Start the motor if 0 <= speed <= 100: # self.PWMpin = Pin('X4') tim = Timer(self.timer_id, freq=1000) ch = tim.channel(self.channel_id, Timer.PWM, pin=PWMpin) ch.pulse_width_percent(speed) # PWM.start(self.PWMpin, speed) else: raise ValueError("Please enter speed between 0 and 100") # set the status attributes self.isRunning = True self.currentDirection = direction self.currentSpeed = speed
def __init__(self): global __bz_pin, __bz_tim, __bz_ch if not (__bz_pin): # Initialize timer and channels if not yet done __bz_pin = Pin("S5") # Broche S5 avec timer 4 et Channel 3 __bz_tim = Timer(4, freq=3000) __bz_ch = __bz_tim.channel(3, Timer.PWM, pin=__bz_pin)
def __init__(self): global __bz_pin, __bz_tim, __bz_ch if not (__bz_pin): # Initialize timer and channels if not yet done __bz_pin = Pin("Y11") # Broche Y2 avec timer 8 et Channel 2 __bz_tim = Timer(8, freq=3000) __bz_ch = __bz_tim.channel(2, Timer.PWM_INVERTED, pin=__bz_pin)
def start(self, speed, direction): PWM_py_pin = Pin(self.PWMpin) DIR_py_pin = Pin(self.DIRpin, Pin.OUT_PP) tim = Timer(self.timer_id, freq=1000) ch = tim.channel(self.channel_id, Timer.PWM, pin=PWM_py_pin) if direction in ('cw', 'CW', 'clockwise'): DIR_py_pin.high() elif direction in ('ccw', 'CCW', 'counterclockwise'): DIR_py_pin.low() else: raise ValueError('Please enter CW or CCW') if 0 <= speed <= 100: ch.pulse_width_percent(speed) else: raise ValueError("Please enter a speed between 0 and 100") self.isRunning = True self.currentDirection = direction self.currentSpeed = speed
def __init__(self, pin, cfreq, asize, duty, verbose): if ESP32: self._rmt = RMT(0, pin=pin, clock_div=80, carrier_freq=cfreq, carrier_duty_percent=duty) # 1μs resolution elif RP2: # PIO-based RMT-like device self._rmt = RP2_RMT(pin_pulse=None, carrier=(pin, cfreq, duty)) # 1μs resolution else: # Pyboard if not IR._active_high: duty = 100 - duty tim = Timer(2, freq=cfreq) # Timer 2/pin produces 36/38/40KHz carrier self._ch = tim.channel(1, Timer.PWM, pin=pin) self._ch.pulse_width_percent(self._space) # Turn off IR LED # Pyboard: 0 <= pulse_width_percent <= 100 self._duty = duty self._tim = Timer(5) # Timer 5 controls carrier on/off times self._tcb = self._cb # Pre-allocate self._arr = array('H', 0 for _ in range(asize)) # on/off times (μs) self._mva = memoryview(self._arr) # Subclass interface self.verbose = verbose self.carrier = False # Notional carrier state while encoding biphase self.aptr = 0 # Index into array
def motor_control(): # define various I/O pins for ADC adc_1 = ADC(Pin('X19')) adc_2 = ADC(Pin('X20')) # set up motor with PWM and timer control A1 = Pin('Y9',Pin.OUT_PP) A2 = Pin('Y10',Pin.OUT_PP) pwm_out = Pin('X1') tim = Timer(2, freq = 1000) motor = tim.channel(1, Timer.PWM, pin = pwm_out) A1.high() # motor in brake position A2.high() # Calibrate the neutral position for joysticks MID = adc_1.read() # read the ADC 1 value now to calibrate DEADZONE = 10 # middle position when not moving # Use joystick to control forward/backward and speed while True: # loop forever until CTRL-C speed = int(100*(adc_1.read()-MID)/MID) if (speed >= DEADZONE): # forward A1.high() A2.low() motor.pulse_width_percent(speed) elif (speed <= -DEADZONE): A1.low() # backward A2.high() motor.pulse_width_percent(-speed) else: A1.low() # stop A2.low()
class Servo: """ Class representing a standard servo """ def __init__(self, pin_name, timer_no, channel): """ Create a pwm channel on the specified pin using the specified timer """ self.pin = Pin(pin_name) self.timer = Timer(timer_no, freq=50) self.channel = self.timer.channel(channel, Timer.PWM, pin=self.pin) def forward(self): """ Intended for use with continuous modified servos. Move the motor forward """ self.degrees(180) def backward(self): """ Intended for use with continuous modified servos. Move the motor backward """ self.degrees(0) def stop(self): """ Stop the PWM signal, causes the servo to stop where it is """ self.channel.pulse_width_percent(0) def degrees(self, degrees): """ Move the servo to the specified position in degrees (between 0 and 180) This is not 100% accurate. """ # Map the angle into the range 2.5-12.5 fractional_angle = degrees / 180.0 duty_percent = 2.5 + fractional_angle * 10 # Set the pulse width self.channel.pulse_width_percent(duty_percent)
def main(): # Configure timer2 as a microsecond counter. tim = Timer(config["timer"], prescaler=(machine.freq()[0] // 1000000) - 1, period=config["timer-period"]) tim2 = Timer(config["led-timer"], freq=config["led-freq"]) tim2.callback(lambda t: pyb.LED(config["led-id"]).toggle()) # Configure channel for timer IC. ch = tim.channel(1, Timer.IC, pin=config["pin-capture"], polarity=Timer.FALLING) # Slave mode disabled in order to configure mem32[config["timer-addr"] + TIM_SMCR] = 0 # Reset on rising edge (or falling in case of inverted detection). Ref: 25.4.3 of STM32F76xxx_reference_manual.pdf mem32[config["timer-addr"] + TIM_SMCR] = (mem32[config["timer-addr"] + TIM_SMCR] & 0xfffe0000) | 0x54 # Capture sensitive to rising edge. Ref: 25.4.9 of STM32F76xxx_reference_manual.pdf mem32[config["timer-addr"] + TIM_CCER] = 0b1011 print("capturing") capture(ch)
class SPWM(): def __init__(self, freq=100): self.pwm_pin = Pin('PA5', Pin.OUT) self.pwm_tim = Timer(2) #not start self.flag = 0 self.pp = 50 def start(self): self.pwm() self.modulate() while 1: if self.flag: self.flag = 0 #计算self.pp 占空比 #配置pwm占空比 self.ch.pulse_width_percent(self.pp) def pwm(self, enable=True): self.pwm_tim = Timer(2, freq=10000) self.ch = self.pwm_tim.channel(1, Timer.PWM, pin=self.pwm_pin) self.ch.pulse_width_percent(50) def modulate(self, freq_m=10): self.m_tim = Timer(5, freq=freq_m) self.m_tim.callback(lambda t: self.change()) def change(self): self.flag = 1
def main(): # X1,X2路PWM波分别作为激励和采样保持开关信号,非晶丝串联150ohm电阻 # 激励电流25-30mA tim = Timer(2, freq=500000) #500Khz ch1 = tim.channel(1, Timer.PWM, pin=pyb.Pin('X1', pyb.Pin.OUT_PP)) ch2 = tim.channel(2, Timer.PWM, pin=pyb.Pin('X2', pyb.Pin.OUT_PP)) ch1.pulse_width_percent(20) ch2.pulse_width_percent(30)
def _make_channel(cls, timer: Timer, channel_number, pin): channel = timer.channel(channel_number, Timer.PWM, pin=pin, pulse_width=16000) cls.set_percent(channel, 0) return channel
def __init__( self, adc=CTRL_ADC, p_pin=PWM_PIN, t_num=PWM_TIMER_NO, chnum=PWM_TIMER_CHANNEL ): self.adc = adc dim_tim = Timer(t_num, freq=1000) self.ch = dim_tim.channel(chnum, Timer.PWM, pin=p_pin) self.current = self.increment = self.target = self.ticks_left = 0 self.tr = self.tick_response
def change_speed(self,newspeed): PWM_py_pin = Pin(self.PWMpin) DIR_py_pin = Pin(self.DIRpin, Pin.OUT_PP) tim = Timer(self.timer_id, freq=1000) ch = tim.channel(self.channel_id, Timer.PWM, pin=PWM_py_pin) ch.pulse_width_percent(newspeed) self.currentSpeed = newspeed self.isRunning = True
def change_speed(self, newspeed): PWM_py_pin = Pin(self.PWMpin) DIR_py_pin = Pin(self.DIRpin, Pin.OUT_PP) tim = Timer(self.timer_id, freq=1000) ch = tim.channel(self.channel_id, Timer.PWM, pin=PWM_py_pin) ch.pulse_width_percent(newspeed) self.currentSpeed = newspeed self.isRunning = True
def drive_motor(): A1 = Pin('Y9',Pin.OUT_PP) A2 = Pin('Y10',Pin.OUT_PP) A1.high() A2.low() motor = Pin('X1') tim = Timer(2, freq = 1000) ch = tim.channel(1, Timer.PWM, pin = motor) while True: ch.pulse_width_percent(50)
def attach(self): """ description: Attach driver motor and start all PWM timers. """ for i in range(len(self.__pins)): tim = Timer(self.__timers[i], freq=self.__frequencies[i]) channel = tim.channel(self.__channels[i], Timer.PWM, pin=self.__pins[i]) self.__pwm[i] = channel self.update()
class RgbLed(object): def __init__(self, r_pin, g_pin, b_pin): self.timer = Timer(2, freq=300) self.r_ch = self.timer.channel(1, Timer.PWM, pin=r_pin, pulse_width=0) self.g_ch = self.timer.channel(3, Timer.PWM, pin=g_pin, pulse_width=0) self.b_ch = self.timer.channel(2, Timer.PWM, pin=b_pin, pulse_width=0) def red(self, i): self.r_ch.pulse_width_percent(i) def blue(self, i): self.b_ch.pulse_width_percent(i) def green(self, i): self.g_ch.pulse_width_percent(i) def off(self): self.red(0) self.green(0) self.blue(0)
class Motor: _index = None _pin = None _timer = None _channel = None _rate = None _rate_min = None _rate_max = None # Each range represents 50% of the complete range _step = None _debug = False # ######################################################################## # ### Properties # ######################################################################## @property def rate(self): return self._rate @rate.setter def rate(self, value): self._rate = max(min(value, 100), 0) pulse_value = int(self._rate_min + self._rate * self._step) if not self._debug: self._channel.pulse_width(pulse_value) else: print("<<ESC>> %s: %.3d" % (self._pin, self._rate)) # ######################################################################## # ### Constructors and destructors # ######################################################################## def __init__(self, esc_index, rate_min=1000, rate_max=2000, debug=False): if esc_index >= 0 & esc_index <= 3: self._debug = debug self._rate_min = rate_min self._rate_max = rate_max self._step = (rate_max - rate_min) / 100 self._index = esc_index self._pin = ESC_PIN[esc_index] self._timer = Timer(ESC_TIMER[esc_index], prescaler=83, period=19999) self._channel = self._timer.channel(ESC_CHANNEL[esc_index], Timer.PWM, pin=Pin(self._pin)) self.rate = 0 else: raise Exception("Invalid ESC index") def __del__(self): self.rate(self._rate_min) self._timer.deinit()
def set_speed(self, newSpeed): """ Method to change the speed of the motor, direciton is unchanged Arugments newSpeed : the desired new speed 0-100 (as percentage of max) """ self.PWMpin = Pin('X4') tim = Timer(2, freq=1000) ch = tim.channel(4, Timer.PWM, pin=self.PWMpin) ch.pulse_width_percent(newSpeed) # PWM.set_duty_cycle(self.PWMpin, newSpeed) self.currentSpeed = newSpeed
def set_speed(self, newSpeed): """ Method to change the speed of the motor, direciton is unchanged Arugments newSpeed : the desired new speed 0-100 (as percentage of max) """ PWMpin = Pin(self.PWMpin) tim = Timer(self.timer_id, freq=1000) ch = tim.channel(self.channel_id, Timer.PWM, pin=PWMpin) ch.pulse_width_percent(newSpeed) # PWM.set_duty_cycle(self.PWMpin, newSpeed) self.currentSpeed = newSpeed
class DCMotor: def __init__(self, tim_num, channel, frequency, pin): self.tim = Timer(tim_num, freq=frequency) self.pin = Pin(pin) self.channel = channel def set_control(self, in_a, in_b, en_a, en_b): self.INA = Pin(in_a, Pin.OUT_PP) self.INB = Pin(in_b, Pin.OUT_PP) self.ENA = Pin(en_a, Pin.OUT_PP) self.ENB = Pin(en_b, Pin.OUT_PP) self.ENA.high() self.ENB.high() def set_current_sense(self, cs, cs_dis): self.CS_DIS = Pin(cs_dis, Pin.OUT_PP) self.CS = Pin(cs, Pin.IN) self.current = ADC(self.CS) def get_current(self): return self.current.read() def set_duty_cycle(self, duty_cycle): self.tim.channel(self.channel, Timer.PWM, pin=self.pin, pulse_width_percent=duty_cycle) def forward(self): self.INA.low() self.INB.high() def reverse(self): self.INA.high() self.INB.low() def brake_gnd(self): self.INA.low() self.INB.low() def brake_vcc(self): self.INA.high() self.INB.high()
def hard_stop(self): """ Method to hard stop an individual motor""" self.PWMpin = Pin('X4') tim = Timer(2, freq=1000) ch = tim.channel(4, Timer.PWM, pin=self.PWMpin) ch.pulse_width_percent(0) # PWM.set_duty_cycle(self.PWMpin, 0.0) # set the status attributes self.isRunning = True self.currentDirection = None self.currentSpeed = 0
def stop(self): PWM_py_pin = Pin(self.PWMpin) DIR_py_pin = Pin(self.DIRpin, Pin.OUT_PP) tim = Timer(self.timer_id, freq=1000) ch = tim.channel(self.channel_id, Timer.PWM, pin=PWM_py_pin) for i in range(self.currentSpeed): ch.pulse_width_percent(self.currentSpeed - i) time.sleep(0.01) ch.pulse_width_percent(0) self.isRunning = False self.currentDirection = None self.currentSpeed = 0.0
def stop(self): PWM_py_pin = Pin(self.PWMpin) DIR_py_pin = Pin(self.DIRpin, Pin.OUT_PP) tim = Timer(self.timer_id, freq=1000) ch = tim.channel(self.channel_id, Timer.PWM, pin=PWM_py_pin) for i in range(self.currentSpeed): ch.pulse_width_percent(self.currentSpeed-i) time.sleep(0.01) ch.pulse_width_percent(0) self.isRunning = False self.currentDirection = None self.currentSpeed = 0.0
def init_motors(): A1 = Pin('X3', Pin.OUT_PP) A2 = Pin('X4', Pin.OUT_PP) PWMA = Pin('X1') B1 = Pin('X7', Pin.OUT_PP) B2 = Pin('X8', Pin.OUT_PP) PWMB = Pin('X2') tim = Timer(2, freq=1000) motorA = tim.channel(1, Timer.PWM, pin=PWMA) motorB = tim.channel(2, Timer.PWM, pin=PWMB) return A1, A2, B1, B2, motorA, motorB
def __init__(self, uart, pwm_freq): self.uart = uart self.i_state = array('i', [0] * len(INAMES)) self.f_state = array('f', [0] * len(FNAMES)) # controller timer self.controller_timer = Timer(1) # motor power control self.nstby = Pin('NSTBY', mode=Pin.OUT_PP) self.nstby.value(0) # motors motor_pwm_timer = Timer(8, freq=pwm_freq) self.motor1 = TB6612( motor_pwm_timer.channel(3, Timer.PWM_INVERTED, pin=Pin('PWM_A')), Pin('AIN1', mode=Pin.OUT_PP), Pin('AIN2', mode=Pin.OUT_PP)) self.motor2 = TB6612( motor_pwm_timer.channel(1, Timer.PWM_INVERTED, pin=Pin('PWM_B')), Pin('BIN1', mode=Pin.OUT_PP), Pin('BIN2', mode=Pin.OUT_PP)) # encoders self.enc1 = init_encoder(4, 'ENC_A1', 'ENC_A2', Pin.AF2_TIM4) self.enc2 = init_encoder(3, 'ENC_B1', 'ENC_B2', Pin.AF2_TIM3) # gyro i2c = I2C(1, freq=400_000) imu = BNO055(i2c, crystal=True, transpose=(2, 0, 1), sign=(0, 0, 1)) imu.mode(NDOF_FMC_OFF_MODE) sleep_ms(800) imu.iget(QUAT_DATA) self.imu = imu # pid controllers self.pid = [ PID(1, 7, 50, 0, -100, 100), PID(1, 7, 50, 0, -100, 100), ]
class ServoMotor: def __init__(self, tim_num, channel, frequency, pin): self.tim = Timer(tim_num, freq=frequency) self.pin = Pin(pin) self.channel = channel self.scale = Timer.source_freq(self.tim)/(Timer.prescaler(self.tim)+1) def set_range(self, max_pw, min_pw): self.max_pw = max_pw self.min_pw = min_pw self.mid_pw = (max_pw + min_pw) / 2 def saturate_pw(self, sec): if(sec > self.max_pw): sec = self.max_pw elif(sec < self.min_pw): sec = self.min_pw return sec def set_pulse_width(self, sec): sec = self.saturate_pw(sec) self.tim.channel(self.channel, Timer.PWM, pin=self.pin, pulse_width=round(sec * self.scale))
def music(): global i x1 = Pin('X4', Pin.OUT_PP) tm3 = Timer(2, freq=MyScore[i][0]) led3 = tm3.channel(4, Timer.PWM, pin=x1, pulse_width_percent=50) a = Pin.value(x1) for i in range(16): print(MyScore[i][0]) tm3.freq(MyScore[i][0]) pyb.delay(int(MyScore[i][1])) #i=i+1 #music()
def soft_stop(self): """ Method to soft stop (coast to stop) an individual motor""" PWMpin = Pin(self.PWMpin) tim = Timer(self.timer_id, freq=1000) ch = tim.channel(self.channel_id, Timer.PWM, pin=PWMpin) for i in range(self.currentSpeed): ch.pulse_width_percent(self.currentSpeed-i) time.sleep(0.01) ch.pulse_width_percent(0) # set the status attributes self.isRunning = False self.currentDirection = None self.currentSpeed = 0.0
def gradient(ri,gi,bi,rf,gf,bf,wait,cycles): """ ri = initial, percent rf = final, percent m gradient(0,10,0,0,100,0,10,4) for inverting: gradient(20,255,255,95,255,255,10,2) """ tim = Timer(2, freq=1000) cr = tim.channel(2, Timer.PWM_INVERTED, pin=r.pin) cg = tim.channel(3, Timer.PWM_INVERTED, pin=g.pin) cb = tim.channel(4, Timer.PWM_INVERTED, pin=b.pin) for i in range(cycles): for a in range(100): cr.pulse_width_percent((rf-ri)*0.01*a+ri) cg.pulse_width_percent((gf-gi)*0.01*a+gi) cb.pulse_width_percent((bf-bi)*0.01*a+gi) pyb.delay(wait) for a in range(100): cr.pulse_width_percent((rf-ri)*0.01*(100-a)+ri) cg.pulse_width_percent((gf-gi)*0.01*(100-a)+gi) cb.pulse_width_percent((bf-bi)*0.01*(100-a)+gi) pyb.delay(wait)
class ESC: freq_min = 950 freq_max = 1950 def __init__(self, index): self.timer = Timer(esc_pins_timers[index], prescaler=83, period=19999) self.channel = self.timer.channel(esc_pins_channels[index], Timer.PWM, pin=Pin(esc_pins[index])) self.trim = esc_trim[index] def move(self, freq): freq = min(self.freq_max, max(self.freq_min, freq + self.trim)) self.channel.pulse_width(int(freq)) def __del__(self): self.timer.deinit()
def __init__(self, PWMpin, DIRpin, timer_id, channel_id): self.PWMpin = PWMpin self.DIRpin = DIRpin self.timer_id = timer_id self.channel_id = channel_id self.isRunning = False self.currentDirection = None self.currentSpeed = 0 # Set up the GPIO pins as output PWMpin = Pin(self.PWMpin) DIRpin = Pin(self.DIRpin, Pin.OUT_PP) tim = Timer(self.timer_id, freq=1000) ch = tim.channel(self.channel_id, Timer.PWM, pin=PWMpin)
def remote(): #initialise UART communication uart = UART(6) uart.init(9600, bits=8, parity = None, stop = 2) # define various I/O pins for ADC adc_1 = ADC(Pin('X19')) adc_2 = ADC(Pin('X20')) # set up motor with PWM and timer control A1 = Pin('Y9',Pin.OUT_PP) A2 = Pin('Y10',Pin.OUT_PP) pwm_out = Pin('X1') tim = Timer(2, freq = 1000) motor = tim.channel(1, Timer.PWM, pin = pwm_out) # Motor in idle state A1.high() A2.high() speed = 0 DEADZONE = 5 # Use keypad U and D keys to control speed while True: # loop forever until CTRL-C while (uart.any()!=10): #wait we get 10 chars n = uart.any() command = uart.read(10) if command[2]==ord('5'): if speed < 96: speed = speed + 5 print(speed) elif command[2]==ord('6'): if speed > - 96: speed = speed - 5 print(speed) if (speed >= DEADZONE): # forward A1.high() A2.low() motor.pulse_width_percent(speed) elif (speed <= -DEADZONE): A1.low() # backward A2.high() motor.pulse_width_percent(-speed) else: A1.low() # idle A2.low()
def soft_stop(self): """ Method to soft stop (coast to stop) an individual motor""" # Make both control pins low self.ControlPin1.low() # GPIO.output(self.ControlPin1, GPIO.LOW) self.ControlPin2.low() # GPIO.output(self.ControlPin2, GPIO.LOW) self.PWMpin = Pin('X4') tim = Timer(2, freq=1000) ch = tim.channel(4, Timer.PWM, pin=self.PWMpin) ch.pulse_width_percent(0) self.STBYpin.low() # GPIO.output(self.STBYpin, GPIO.LOW) # set the status attributes self.isRunning = True self.currentDirection = None self.currentSpeed = 0.0
def __init__(self, ControlPin1, ControlPin2, PWMpin, STBYpin): self.ControlPin1 = ControlPin1 self.ControlPin2 = ControlPin2 self.PWMpin = PWMpin self.STBYpin = STBYpin self.isRunning = False self.currentDirection = None self.currentSpeed = 0 # Set up the GPIO pins as output self.STBYpin = Pin(self.STBYpin, Pin.OUT_PP) # GPIO.setup(self.STBYpin, GPIO.OUT) self.ControlPin1 = Pin(self.ControlPin1, Pin.OUT_PP) # GPIO.setup(self.ControlPin1, GPIO.OUT) self.ControlPin2 = Pin(self.ControlPin2, Pin.OUT_PP) # GPIO.setup(self.ControlPin2, GPIO.OUT) self.PWMpin = Pin(self.PWMpin) tim = Timer(2, freq=1000) ch = tim.channel(4, Timer.PWM, pin=self.PWMpin)
def start(self, speed, direction): """ method to start a motor Arguments: speed : speed of the motor 0-100 (as percentage of max) direction : CW or CCW, for clockwise or counterclockwise """ # Standby pin should go high to enable motion self.STBYpin.high() # STBYpin.high() # GPIO.output(self.STBYpin, GPIO.HIGH) # x01 and x02 have to be opposite to move, toggle to change direction if direction in ('cw','CW','clockwise'): self.ControlPin1.low() # GPIO.output(self.ControlPin1, GPIO.LOW) self.ControlPin2.high() # GPIO.output(self.ControlPin2, GPIO.HIGH) elif direction in ('ccw','CCW','counterclockwise'): self.ControlPin1.high() # GPIO.output(self.ControlPin1, GPIO.HIGH) self.ControlPin2.low() # GPIO.output(self.ControlPin2, GPIO.LOW) else: raise ValueError('Please enter CW or CCW for direction.') # Start the motor # PWM.start(channel, duty, freq=2000, polarity=0) if 0 <= speed <= 100: self.PWMpin = Pin('X4') tim = Timer(2, freq=1000) ch = tim.channel(4, Timer.PWM, pin=self.PWMpin) ch.pulse_width_percent(speed) # PWM.start(self.PWMpin, speed) else: raise ValueError("Please enter speed between 0 and 100, \ representing a percentage of the maximum \ motor speed.") # set the status attributes self.isRunning = True self.currentDirection = direction self.currentSpeed = speed
def start(self, speed, direction): """ method to start a motor Arguments: speed : speed of the motor 0-100 (as percentage of max) direction : CW or CCW, for clockwise or counterclockwise """ PWMpin = Pin(self.PWMpin) DIRpin = Pin(self.DIRpin, Pin.OUT_PP) # DIR1 and DIR2 have to be opposite to move, toggle to change direction if direction in ('cw','CW','clockwise'): DIRpin.high() elif direction in ('ccw','CCW','counterclockwise'): DIRpin.low() else: raise ValueError('Please enter CW or CCW for direction.') # Start the motor if 0 <= speed <= 100: # self.PWMpin = Pin('X4') tim = Timer(self.timer_id, freq=1000) ch = tim.channel(self.channel_id, Timer.PWM, pin=PWMpin) ch.pulse_width_percent(speed) # PWM.start(self.PWMpin, speed) else: raise ValueError("Please enter speed between 0 and 100") # set the status attributes self.isRunning = True self.currentDirection = direction self.currentSpeed = speed while self.isRunning: print("\nMotorA =", enc_timer.counter()) print("\nMotorB =", enc_timer_1.counter())
class Encoder(): """ Abstracts a quadrature encoder to give a tick count. The count is a signed integer starting at zero. It wraps the count from the attached timer to give a seamless and continuous tick count. Overflows of the internal timer counter register should be adequatly handled. If overflows or weird behaviour occurs around the overflow points, try increasing Encoder.HYSTERESIS. Note: Only works on pin pairs 'X1' & 'X2', 'X9' & 'X10', or 'Y1', 'Y2'. The timer will be automatically selected. Both Timer 2 and 5 work for 'X1' & 'X2', but Timer 5 is preferred because Timer 2 is used for LED PWM but both can be used by changing the values of Encoder.AF_MAP. """ # Constant for decoding in single line mode SINGLE_MODE = Timer.ENC_A # Constant for decoding in quad mode DUAL_MODE = Timer.ENC_AB # Maps alternate pin function descriptions to the required timer number TIMER_MAP = {'AF1_TIM2': 2, 'AF2_TIM4': 4, 'AF2_TIM5': 5, 'AF3_TIM8': 8} # Maps pin names to the alternate function to use AF_MAP = {'X1' : 'AF2_TIM5', 'X2': 'AF2_TIM5', 'X9': 'AF2_TIM4', 'X10': 'AF2_TIM4', 'Y1': 'AF3_TIM8', 'Y2': 'AF3_TIM8'} # Defines the pin pairs that must be used PIN_PAIRS = [['X1','X9','Y1'],['X2','X10','Y2']] # Hysteresis value to overflow detection HYSTERESIS = 12 # One full rotation of encoder def __init__(self, pinA, pinB, mode = DUAL_MODE): """ Instantiate an Encoder object. Initalises the Pins, Timer and TimerChannel for use as a quadrature decoder. Registers an overflow callback to elegantly handle counter register overflows. pinA: Any valid value taken by pyb.Pin constructor pinB: Any valid value taken by pyb.Pin constructor mode: Mode to use for decoding (Encoder.SINGLE_MODE or Encoder.DUAL_MODE) raises: Any exception thrown by pyb.Pin, pyb.Timer, pyb.Timer.channel or Exception if pins are not compatible pairs """ self._chA = Pin(pinA) self._chB = Pin(pinB) self._ticks = 0 # Check pins are compatible self._checkPins(pinA, pinB) # init pins for alternate encoder function af = self.AF_MAP[self._chA.names()[1]] channel = self.TIMER_MAP[af] af = getattr(Pin, af) self._chA.init(Pin.AF_PP, pull = Pin.PULL_NONE, af = af) self._chB.init(Pin.AF_PP, pull = Pin.PULL_NONE, af = af) # init timer self._timer = Timer(channel, prescaler = 0, period = 100000) # init encoder mode # self._channel = self._timer.channel(1, mode) self._timer.channel(1, mode) # setup overflow callback self._timer.callback(self._overflow) # init count register to middle of count self._timer.counter(self._timer.period()//2) self._lastRead = self._timer.counter() def _checkPins(self, pinA, pinB): """ Check that two pins can be used for a decoding and are on the same timer. """ try: if pinA in self.PIN_PAIRS[0]: if self.PIN_PAIRS[0].index(pinA) != self.PIN_PAIRS[1].index(pinB): raise Exception() elif pinA in self.PIN_PAIRS[1]: if self.PIN_PAIRS[0].index(pinB) != self.PIN_PAIRS[1].index(pinA): raise Exception() else: raise Exception() except: raise Exception(pinA + ' & ' + pinB + ' are not on the same Timer') def ticks(self, ticks = None): """ Get or set the current tick count. Ticks is a signed integer. """ if ticks is not None: # set ticks to desired value self._ticks = ticks else: # retrieve latest count and update internals count = self._timer.counter() self._ticks = self._ticks + (count - self._lastRead) self._lastRead = count return self._ticks def _overflow(self, timer): """ Timer overflow callback to gracefully handle overflow events. If weird things are occurring, try increasing the HYSTERESIS value. """ count = timer.counter() if 0 <= count <= self.HYSTERESIS: # overflow self._ticks = self._ticks + (timer.period() - self._lastRead + count) self._lastRead = count elif (timer.period() - self.HYSTERESIS) <= count <= timer.period(): # underflow self._ticks = self._ticks - (self._lastRead + timer.period() - count) self._lastRead = count else: # hysteresis not big enough #LED(1).on() # turn on inbuilt red led to show error pass
class Robot: def __init__(self): # Timer for motor PWM self.tim2 = Timer(2, freq=10000) # Timer to dim lights self.tim4 = Timer(4, freq=1000) # Timer for motion self.tim7 = Timer(7, freq=800) # Variables self.step_R = 0 #counter for (micro) step self.step_L = 0 #counter for (micro) step self.step = 0 #counter for motion interrupt self.n_steps = 64 #number of steps self.dir_R = 0 #direction 1=positive, -1=negative, 0=none self.dir_L = 0 #direction 1=positive, -1=negative, 0=none self.speed_R = 0 #speed counter. If >255 step will be executed self.speed_L = 0 #speed counter. If >255 step will be executed self.max_speed_R = 256 #maximum speed self.max_speed_L = 256 #maximum speed self.sspeed_R = 0 #set speed self.sspeed_L = 0 #set speed self.dist_R = 0 #distance counter self.dist_L = 0 #distance counter self.power_R = 0 #PWM power setting 0 - 100% self.power_L = 0 #PWM power setting 0 - 100% self.low_light = 20 #PWM setting for tail light self.target_R = 0 #motion control position target self.target_L = 0 #motion control position target self.acc = 1 #motion control acceleration self.dts_R = 0 #motion distance to stop self.dts_L = 0 #motion distance to stop self.move = 0 #move motors or not # Lights: self.LH = Pin('PD12', pyb.Pin.OUT_PP) self.RH = Pin('PD13', pyb.Pin.OUT_PP) self.TL = self.tim4.channel(3, Timer.PWM, pin=Pin.cpu.D14) self.HL = Pin('PD15', pyb.Pin.OUT_PP) # RH Motor self.RH_STBY = Pin('PE6', pyb.Pin.OUT_PP) self.RH_PWMA = self.tim2.channel(1, Timer.PWM, pin=Pin.cpu.A15) self.RH_AIN1 = Pin('PE8', pyb.Pin.OUT_PP) self.RH_AIN2 = Pin('PE9', pyb.Pin.OUT_PP) self.RH_PWMB = self.tim2.channel(2, Timer.PWM, pin=Pin.cpu.A1) self.RH_BIN1 = Pin('PE10', pyb.Pin.OUT_PP) self.RH_BIN2 = Pin('PE11', pyb.Pin.OUT_PP) # LH Motor self.LH_STBY = Pin('PE7', pyb.Pin.OUT_PP) self.LH_PWMA = self.tim2.channel(3, Timer.PWM, pin=Pin.cpu.A2) self.LH_AIN1 = Pin('PE12', pyb.Pin.OUT_PP) self.LH_AIN2 = Pin('PE13', pyb.Pin.OUT_PP) self.LH_PWMB = self.tim2.channel(4, Timer.PWM, pin=Pin.cpu.A3) self.LH_BIN1 = Pin('PE14', pyb.Pin.OUT_PP) self.LH_BIN2 = Pin('PE15', pyb.Pin.OUT_PP) # Switch lights off self.LH.low() self.RH.low() self.TL.pulse_width_percent(0) self.HL.low() # Switch motor drivers off and PWM to low self.RH_PWMA.pulse_width_percent(self.power_R) self.RH_PWMB.pulse_width_percent(self.power_R) self.LH_PWMA.pulse_width_percent(self.power_L) self.LH_PWMB.pulse_width_percent(self.power_L) self.RH_STBY.low() self.LH_STBY.low() # Set all other motor pins low self.RH_AIN1.low() self.RH_AIN2.low() self.RH_BIN1.low() self.RH_BIN2.low() self.LH_AIN1.low() self.LH_AIN2.low() self.LH_BIN1.low() self.LH_BIN2.low() def Lights(self, status): if status == 1: self.HL.high() self.TL.pulse_width_percent(self.low_light) else: self.HL.low() self.TL.pulse_width_percent(0) def Brakes(self, status): if status == 1: self.TL.pulse_width_percent(100) elif status == 0: if self.HL.value() == 0: self.TL.pulse_width_percent(0) else: self.TL.pulse_width_percent(self.low_light) def Blink(self, side): if side == -1: self.RH.low() if self.LH.value() == 0: self.LH.high() else: self.LH.low() if side == 1: self.LH.low() if self.RH.value() == 0: self.RH.high() else: self.RH.low() if side == 2: if self.RH.value() == 0: self.RH.high() self.LH.high() else: self.RH.low() self.LH.low() if side == 0: self.RH.low() self.LH.low() def mcallback(self, t): self.step += 1 def MotorStatus(self, status): if status == 1: self.LH_STBY.high() self.RH_STBY.high() self.tim7.callback(self.mcallback) elif status != 1: self.LH_STBY.low() self.RH_STBY.low() self.tim7.callback(None) def DoStep(self, dir_L, dir_R): percent_A = self.power_L * math.sin(self.step_L * 2 * math.pi / self.n_steps) percent_B = self.power_L * math.cos(self.step_L * 2 * math.pi / self.n_steps) if percent_A > 0: self.LH_AIN1.high() self.LH_AIN2.low() else: self.LH_AIN1.low() self.LH_AIN2.high() percent_A *= -1 if percent_B > 0: self.LH_BIN1.high() self.LH_BIN2.low() else: self.LH_BIN1.low() self.LH_BIN2.high() percent_B *= -1 self.LH_PWMA.pulse_width_percent(percent_A) self.LH_PWMB.pulse_width_percent(percent_B) self.step_L += dir_L if self.step_L < 0: self.step_L = self.n_steps + dir_L if self.step_L >= self.n_steps: self.step_L = -1 + dir_L percent_A = self.power_R * math.sin(self.step_R * 2 * math.pi / self.n_steps) percent_B = self.power_R * math.cos(self.step_R * 2 * math.pi / self.n_steps) if percent_A > 0: self.RH_AIN1.high() self.RH_AIN2.low() else: self.RH_AIN1.low() self.RH_AIN2.high() percent_A *= -1 if percent_B > 0: self.RH_BIN1.high() self.RH_BIN2.low() else: self.RH_BIN1.low() self.RH_BIN2.high() percent_B *= -1 self.RH_PWMA.pulse_width_percent(percent_A) self.RH_PWMB.pulse_width_percent(percent_B) self.step_R += dir_R if self.step_R < 0: self.step_R = self.n_steps + dir_R if self.step_R >= self.n_steps: self.step_R = -1 + dir_R def MoveStep(self, n): if self.sspeed_L < 0: ldir = -n else: ldir = n self.speed_L += (self.sspeed_L * ldir) if self.speed_L > 255: self.dir_L = ldir self.dist_L += ldir self.speed_L -= 256 * n else: self.dir_L = 0 if self.sspeed_R < 0: rdir = -n else: rdir = n self.speed_R += (self.sspeed_R * rdir) if self.speed_R > 255: self.dir_R = rdir self.dist_R += rdir self.speed_R -= 256 * n else: self.dir_R = 0 self.DoStep(self.dir_L, self.dir_R) def SetPower(self, left, right): self.power_R = right self.power_L = left self.DoStep(0,0) def GetDist(self): return (self.dist_L, self.dist_R) def SetDist(self, dl, dr): self.dist_L = dl self.dist_R = dr def SetTarget(self, t_L, t_R): self.target_L = t_L self.target_R = t_R def GetTarget(self): return(self.target_L, self.target_R) def SetMaxSpeed(self, s_L, s_R): self.max_speed_L = s_L self.max_speed_R = s_R def Stop(self): self.dts_L = (self.acc * (self.sspeed_L // self.acc) ** 2) // 512 self.dts_R = (self.acc * (self.sspeed_R // self.acc) ** 2) // 512 if self.sspeed_L < 0: self.dts_L *= -1 if self.sspeed_R < 0: self.dts_R *= -1 self.target_L = self.dist_L + self.dts_L self.target_R = self.dist_R + self.dts_R def Motion(self, n): self.dts_L = (self.acc * (self.sspeed_L // self.acc) ** 2) // 512 self.dts_R = (self.acc * (self.sspeed_R // self.acc) ** 2) // 512 if self.sspeed_L < 0: self.dts_L *= -1 if self.sspeed_R < 0: self.dts_R *= -1 if self.target_L > (self.dist_L + self.dts_L) and self.sspeed_L < self.max_speed_L: self.sspeed_L += self.acc elif self.target_L < (self.dist_L + self.dts_L) and self.sspeed_L > -self.max_speed_L: self.sspeed_L -= self.acc elif self.target_L == self.dist_L and abs(self.sspeed_L) < 50: self.sspeed_L = 0 if self.target_R > (self.dist_R + self.dts_R) and self.sspeed_R < self.max_speed_R: self.sspeed_R += self.acc elif self.target_R < (self.dist_R + self.dts_R) and self.sspeed_R > -self.max_speed_R: self.sspeed_R -= self.acc elif self.target_R == self.dist_R and abs(self.sspeed_R) < 50: self.sspeed_R = 0 self.MoveStep(n) def FWD(self, dist): dl = int(self.dist_L + dist * 3.5866) dr = int(self.dist_R + dist * 3.5866) self.SetTarget(dl, dr) self.move = 1 self.step = 0 while (self.move == 1): if self.step > 0: self.Motion(self.step) self.step = 0 if self.dist_L == dl and self.dist_R == dr: self.move = 0 def TRN(self, r, alpha): dl = self.dist_L + int((r + 40.75) * 2 * math.pi * alpha / 360 * 3.5866) dr = self.dist_R + int((r - 40.75) * 2 * math.pi * alpha / 360 * 3.5866) self.SetTarget(dl, dr) self.move = 1 self.step = 0 while (self.move == 1): if self.step > 0: self.Motion(self.step) self.step = 0 if self.dist_L == dl and self.dist_R == dr: self.move = 0
def __init__(self, confData): p = Pin(confData['p'] , mode=Pin.OUT_PP) tim = Timer(confData['t'],freq=confData['f']) self.control = tim.channel(confData['c'],Timer.PWM, pin=p) self.control.pulse_width_percent(0)
# PWM Control Example # # This example shows how to do PWM with your OpenMV Cam. import time from pyb import Pin, Timer tim = Timer(4, freq=1000) # Frequency in Hz # Generate a 1KHz square wave on TIM4 with 50% and 75% duty cycles on channels 1 and 2, respectively. ch1 = tim.channel(1, Timer.PWM, pin=Pin("P7"), pulse_width_percent=50) ch2 = tim.channel(2, Timer.PWM, pin=Pin("P8"), pulse_width_percent=75) while (True): time.sleep(1000)
class PWM(object): """docstring for PWMM""" #Dict pin name: timer #, channel #. PinChannels = { 'X1': [(2,1), (5,1)], 'X2': [(2,2), (5,2)], 'X3': [(2,3), (5,3), (9,1)], 'X4': [(2,4), (5,4), (9,2)], 'X6': [(2,1), (8,1)], 'X7': [(13, 1)], 'X8': [(1,1), (8,1), (14,1)], 'X9': [(4,1)], 'X10': [(4,2)], 'Y1': [(8,1)], 'Y2': [(8,2)], 'Y3': [(4,3), (10,1)], 'Y4': [(4,4), (11,1)], 'Y6': [(1,1)], 'Y7': [(1,2), (8,2), (12,1)], 'Y8': [(1,3), (8,3), (12,2)], 'Y9': [(2,3)], 'Y10': [(2,4)], 'Y11': [(1,2), (8,2)], 'Y12': [(1,3), (8,3)] } class PWMException(Exception): def __init__( self, msg ) : self.msg = msg @staticmethod def timerandchannel( pinname, timernum ) : try: a = PWM.PinChannels[pinname] if timernum <= 0: return a[0] else: for v in a: if v[0] == timernum: return v except Exception as e : raise PWM.PWMException("Pin {} cannot be used for PWM".format(pinname)) raise PWM.PWMException("Pin {} cannot use timer {}".format(pinname, timernum)) def __init__( self, p, timernum, afreq = 100 ) : isname = type(p) == str pinname = p if isname else p.name() timernum, channel = PWM.timerandchannel(pinname, timernum) if isname: p = Pin(pinname) self._timer = Timer(timernum, freq = afreq) self._channel = self._timer.channel(channel, Timer.PWM, pin = p) @property def pulse_width( self ) : return self._channel.pulse_width() @pulse_width.setter def pulse_width( self, value ) : self._channel.pulse_width(value) @property def pulse_width_percent( self ) : return self._channel.pulse_width_percent() @pulse_width_percent.setter def pulse_width_percent( self, value ) : self._channel.pulse_width_percent(value) @property def freq( self ) : return self._timer.freq() @freq.setter def freq( self, value ) : self._timer.freq(value) def callback( self, value ) : self._channel.callback(value)
# Task 4: Joystick Controlling the Motor # Author: BSG # Version 1.0 # 26 May 2016 print ('This is Test 4: Joystick Controlling the Motor') from pyb import Pin, Timer, ADC while True: pyb.delay(1) A1 = Pin('Y9',Pin.OUT_PP) A2 = Pin('Y10',Pin.OUT_PP) A1.high() A2.low() motor = Pin('X1') tim = Timer(2, freq = 1000) ch = tim.channel(1, Timer.PWM, pin = motor) adc_1 = ADC(Pin('X19')) #vertical adc_2 = ADC(Pin('X20')) #horizontal J_sw = Pin('Y11', Pin.IN) #switch vertical = (int(adc_2.read()) / 1700)*100 ch.pulse_width_percent(vertical)
class DRIVE(object): MEASUREMENT_PERIOD = 0.1 SPEED_SCALE = 7/MEASUREMENT_PERIOD # in mm/sec def __init__(self): # set up motor with PWM and timer control self.A1 = Pin('X3',Pin.OUT_PP) # A is right motor self.A2 = Pin('X4',Pin.OUT_PP) self.B1 = Pin('X7',Pin.OUT_PP) # B is left motor self.B2 = Pin('X8',Pin.OUT_PP) self.PWMA = Pin('X1') self.PWMB = Pin('X2') self.speed = 0 # +100 full speed forward, -100 full speed back self.turn = 0 # turn is +/-100; 0 = left/right same speed, # ... +50 = left at speed, right stop, +100 = right back full # Configure counter 2 to produce 1kHz clock signal self.tim = Timer(2, freq = 1000) # Configure timer to provide PWM signal self.motorA = self.tim.channel(1, Timer.PWM, pin = self.PWMA) self.motorB = self.tim.channel(2, Timer.PWM, pin = self.PWMB) self.lsf = 0 # left motor speed factor +/- 1 self.rsf = 0 # right motor speed factor +/- 1 self.countA = 0 # speed pulse count for motorA self.countB = 0 # speed pulse count for motorB self.speedA = 0 # actual speed of motorA self.speedB = 0 # actual speed of motorB # Create external interrupts for motorA and motorB Hall Effect Senors self.motorA_int = pyb.ExtInt ('Y4', pyb.ExtInt.IRQ_RISING, pyb.Pin.PULL_NONE,self.isr_motorA) self.motorB_int = pyb.ExtInt ('Y6', pyb.ExtInt.IRQ_RISING, pyb.Pin.PULL_NONE,self.isr_motorB) self.speed_timer = pyb.Timer(8, freq=10) self.speed_timer.callback(self.isr_speed_timer) def isr_motorA(self, line): self.countA += 1 def isr_motorB(self, line): self.countB += 1 def isr_speed_timer(self,t): self.speedA = self.countA self.speedB = self.countB self.countA = 0 self.countB = 0 pyb.LED(3).toggle() def set_speed(self, value): self.speed = value def get_speedA(self): return self.speedA def get_speedB(self): return self.speedB def set_turn(self, value): self.turn = value def read_turn(self): return self.turn def right_forward(self,value): self.A2.high() self.A1.low() self.motorA.pulse_width_percent(abs(value)) def right_back(self,value): self.A2.low() self.A1.high() self.motorA.pulse_width_percent(abs(value)) def left_forward(self,value): self.B1.high() self.B2.low() self.motorB.pulse_width_percent(abs(value)) def left_back(self,value): self.B1.low() self.B2.high() self.motorB.pulse_width_percent(abs(value)) def stop(self): # Motor in idle state self.speed = 0 self.A1.low() self.A2.low() self.B1.low() self.B2.low() self.motorA.pulse_width_percent(0) self.motorB.pulse_width_percent(0) def drive(self): DEADZONE = 5 if (self.speed >= DEADZONE) and (self.turn == 0): # straight forward self.right_forward(self.speed) self.left_forward(self.speed) elif (self.speed <= -DEADZONE) and (self.turn == 0): # straight backward self.right_back(self.speed) self.left_back(self.speed) elif (self.speed >= DEADZONE) and (self.turn > 0): # forward right self.lsf = 1 # left wheel speed factor is 1 self.rsf = 1 - self.turn/50 self.left_forward(self.speed) if (self.rsf > 0): self.right_forward(self.rsf*self.speed) else: self.right_back(self.rsf*self.speed) elif (self.speed >= DEADZONE) and (self.turn < 0): # forward left self.rsf = 1 # right wheel speed factor is 1 self.lsf = 1 + self.turn/50 self.right_forward(self.rsf*self.speed) if (self.lsf > 0): self.left_forward(self.lsf*self.speed) else: self.left_back(self.lsf*self.speed) elif (self.speed <= DEADZONE) and (self.turn > 0): # backward right self.rsf = 1 # right wheel speed factor is 1 self.lsf = 1 - self.turn/50 self.right_back(self.speed) if (self.lsf > 0): self.left_back(self.lsf*self.speed) else: self.left_forward(self.lsf*self.speed) elif (self.speed <= DEADZONE) and (self.turn < 0): # backward left self.lsf = 1 # left wheel speed factor is 1 self.rsf = 1 + self.turn/50 self.left_back(self.speed) if (self.lsf < 0): self.right_back(self.rsf*self.speed) else: self.right_forward(self.rsf*self.speed) else: self.stop()
class Robot: def __init__(self): # Timer for motor PWM self.tim2 = Timer(2, freq=10000) # Timer to dim lights self.tim4 = Timer(4, freq=1000) # Variables self.step_R = 0 #counter for (micro) step self.step_L = 0 #counter for (micro) step self.n_steps = 16 #number of steps self.dir_R = 0 #direction 1=positive, -1=negative, 0=none self.dir_L = 0 #direction 1=positive, -1=negative, 0=none self.speed_R = 0 #speed counter. If >255 step will be executed self.speed_L = 0 #speed counter. If >255 step will be executed self.max_speed_R = 256 #maximum speed self.max_speed_L = 256 #maximum speed self.sspeed_R = 0 #set speed self.sspeed_L = 0 #set speed self.dist_R = 0 #distance counter self.dist_L = 0 #distance counter self.power_R = 0 #PWM power setting 0 - 100% self.power_L = 0 #PWM power setting 0 - 100% self.low_light = 20 #PWM setting for tail light self.target_R = 0 #motion control position target self.target_L = 0 #motion control position target self.acc = 0.5 #motion control acceleration self.dts_R = 0 #motion distance to stop self.dts_L = 0 #motion distance to stop # Lights: self.LH = Pin('PD12', pyb.Pin.OUT_PP) self.RH = Pin('PD13', pyb.Pin.OUT_PP) self.TL = self.tim4.channel(3, Timer.PWM, pin=Pin.cpu.D14) self.HL = Pin('PD15', pyb.Pin.OUT_PP) # RH Motor self.RH_STBY = Pin('PE6', pyb.Pin.OUT_PP) self.RH_PWMA = self.tim2.channel(1, Timer.PWM, pin=Pin.cpu.A15) self.RH_AIN1 = Pin('PE8', pyb.Pin.OUT_PP) self.RH_AIN2 = Pin('PE9', pyb.Pin.OUT_PP) self.RH_PWMB = self.tim2.channel(2, Timer.PWM, pin=Pin.cpu.A1) self.RH_BIN1 = Pin('PE10', pyb.Pin.OUT_PP) self.RH_BIN2 = Pin('PE11', pyb.Pin.OUT_PP) # LH Motor self.LH_STBY = Pin('PE7', pyb.Pin.OUT_PP) self.LH_PWMA = self.tim2.channel(3, Timer.PWM, pin=Pin.cpu.A2) self.LH_AIN1 = Pin('PE12', pyb.Pin.OUT_PP) self.LH_AIN2 = Pin('PE13', pyb.Pin.OUT_PP) self.LH_PWMB = self.tim2.channel(4, Timer.PWM, pin=Pin.cpu.A3) self.LH_BIN1 = Pin('PE14', pyb.Pin.OUT_PP) self.LH_BIN2 = Pin('PE15', pyb.Pin.OUT_PP) # Switch lights off self.LH.low() self.RH.low() self.TL.pulse_width_percent(0) self.HL.low() # Switch motor drivers off and PWM to low self.RH_PWMA.pulse_width_percent(self.power_R) self.RH_PWMB.pulse_width_percent(self.power_R) self.LH_PWMA.pulse_width_percent(self.power_L) self.LH_PWMB.pulse_width_percent(self.power_L) self.RH_STBY.low() self.LH_STBY.low() # Set all other motor pins low self.RH_AIN1.low() self.RH_AIN2.low() self.RH_BIN1.low() self.RH_BIN2.low() self.LH_AIN1.low() self.LH_AIN2.low() self.LH_BIN1.low() self.LH_BIN2.low() def Lights(self, status): if status == 1: self.HL.high() self.TL.pulse_width_percent(self.low_light) else: self.HL.low() self.TL.pulse_width_percent(0) def Brakes(self, status): if status == 1: self.TL.pulse_width_percent(100) elif status == 0: if self.HL.value() == 0: self.TL.pulse_width_percent(0) else: self.TL.pulse_width_percent(self.low_light) def Blink(self, side): if side == -1: self.RH.low() if self.LH.value() == 0: self.LH.high() else: self.LH.low() if side == 1: self.LH.low() if self.RH.value() == 0: self.RH.high() else: self.RH.low() if side == 2: if self.RH.value() == 0: self.RH.high() self.LH.high() else: self.RH.low() self.LH.low() if side == 0: self.RH.low() self.LH.low() def MotorStatus(self, left, right): if left == 1: self.LH_STBY.high() elif left != 1: self.LH_STBY.low() if right == 1: self.RH_STBY.high() elif right != 1: self.RH_STBY.low() def DoStep(self, dir_L, dir_R): if self.step_L == 0: self.LH_AIN1.high() self.LH_AIN2.low() self.LH_PWMA.pulse_width_percent(self.power_L) self.LH_PWMB.pulse_width_percent(0) elif self.step_L == 1: self.LH_AIN1.high() self.LH_AIN2.low() self.LH_PWMA.pulse_width_percent(self.power_L) self.LH_BIN1.high() self.LH_BIN2.low() self.LH_PWMB.pulse_width_percent(self.power_L) elif self.step_L == 2: self.LH_PWMA.pulse_width_percent(0) self.LH_BIN1.high() self.LH_BIN2.low() self.LH_PWMB.pulse_width_percent(self.power_L) elif self.step_L == 3: self.LH_AIN1.low() self.LH_AIN2.high() self.LH_PWMA.pulse_width_percent(self.power_L) self.LH_BIN1.high() self.LH_BIN2.low() self.LH_PWMB.pulse_width_percent(self.power_L) elif self.step_L == 4: self.LH_AIN1.low() self.LH_AIN2.high() self.LH_PWMA.pulse_width_percent(self.power_L) self.LH_PWMB.pulse_width_percent(0) elif self.step_L == 5: self.LH_AIN1.low() self.LH_AIN2.high() self.LH_PWMA.pulse_width_percent(self.power_L) self.LH_BIN1.low() self.LH_BIN2.high() self.LH_PWMB.pulse_width_percent(self.power_L) elif self.step_L == 6: self.LH_PWMA.pulse_width_percent(0) self.LH_BIN1.low() self.LH_BIN2.high() self.LH_PWMB.pulse_width_percent(self.power_L) elif self.step_L == 7: self.LH_AIN1.high() self.LH_AIN2.low() self.LH_PWMA.pulse_width_percent(self.power_L) self.LH_BIN1.low() self.LH_BIN2.high() self.LH_PWMB.pulse_width_percent(self.power_L) self.step_L += dir_L if self.step_L < 0: self.step_L = self.n_steps + dir_L if self.step_L >= self.n_steps: self.step_L = -1 + dir_L if self.step_R == 0: self.RH_AIN1.high() self.RH_AIN2.low() self.RH_PWMA.pulse_width_percent(self.power_R) self.RH_PWMB.pulse_width_percent(0) elif self.step_R == 1: self.RH_AIN1.high() self.RH_AIN2.low() self.RH_PWMA.pulse_width_percent(self.power_R) self.RH_BIN1.high() self.RH_BIN2.low() self.RH_PWMB.pulse_width_percent(self.power_R) elif self.step_R == 2: self.RH_PWMA.pulse_width_percent(0) self.RH_BIN1.high() self.RH_BIN2.low() self.RH_PWMB.pulse_width_percent(self.power_R) elif self.step_R == 3: self.RH_AIN1.low() self.RH_AIN2.high() self.RH_PWMA.pulse_width_percent(self.power_R) self.RH_BIN1.high() self.RH_BIN2.low() self.RH_PWMB.pulse_width_percent(self.power_R) elif self.step_R == 4: self.RH_AIN1.low() self.RH_AIN2.high() self.RH_PWMA.pulse_width_percent(self.power_R) self.RH_PWMB.pulse_width_percent(0) elif self.step_R == 5: self.RH_AIN1.low() self.RH_AIN2.high() self.RH_PWMA.pulse_width_percent(self.power_R) self.RH_BIN1.low() self.RH_BIN2.high() self.RH_PWMB.pulse_width_percent(self.power_R) elif self.step_R == 6: self.RH_PWMA.pulse_width_percent(0) self.RH_BIN1.low() self.RH_BIN2.high() self.RH_PWMB.pulse_width_percent(self.power_R) elif self.step_R == 7: self.RH_AIN1.high() self.RH_AIN2.low() self.RH_PWMA.pulse_width_percent(self.power_R) self.RH_BIN1.low() self.RH_BIN2.high() self.RH_PWMB.pulse_width_percent(self.power_R) self.step_R += dir_R if self.step_R < 0: self.step_R = self.n_steps + dir_R if self.step_R >= self.n_steps: self.step_R = -1 + dir_R def MoveStep(self, n): if self.sspeed_L < 0: ldir = -n else: ldir = n self.speed_L += (self.sspeed_L * ldir) if self.speed_L > 255: self.dir_L = ldir self.dist_L += ldir self.speed_L -= 256 * n else: self.dir_L = 0 if self.sspeed_R < 0: rdir = -n else: rdir = n self.speed_R += (self.sspeed_R * rdir) if self.speed_R > 255: self.dir_R = rdir self.dist_R += rdir self.speed_R -= 256 * n else: self.dir_R = 0 self.DoStep(self.dir_L, self.dir_R) def SetPower(self, left, right): self.power_R = right self.power_L = left self.DoStep(0,0) def GetDist(self): return (self.dist_L, self.dist_R) def SetDist(self, dl, dr): self.dist_L = dl self.dist_R = dr def SetTarget(self, t_L, t_R): self.target_L = t_L self.target_R = t_R def GetTarget(self): return(self.target_L, self.target_R) def SetMaxSpeed(self, s_L, s_R): self.max_speed_L = s_L self.max_speed_R = s_R def Stop(self): self.dts_L = (self.acc * (self.sspeed_L // self.acc) ** 2) // 512 self.dts_R = (self.acc * (self.sspeed_R // self.acc) ** 2) // 512 if self.sspeed_L < 0: self.dts_L *= -1 if self.sspeed_R < 0: self.dts_R *= -1 self.target_L = self.dist_L + self.dts_L self.target_R = self.dist_R + self.dts_R def Motion(self, n): self.dts_L = (self.acc * (self.sspeed_L // self.acc) ** 2) // 512 self.dts_R = (self.acc * (self.sspeed_R // self.acc) ** 2) // 512 if self.sspeed_L < 0: self.dts_L *= -1 if self.sspeed_R < 0: self.dts_R *= -1 if self.target_L > (self.dist_L + self.dts_L) and self.sspeed_L < self.max_speed_L: self.sspeed_L += self.acc elif self.target_L < (self.dist_L + self.dts_L) and self.sspeed_L > -self.max_speed_L: self.sspeed_L -= self.acc elif self.target_L == self.dist_L and abs(self.sspeed_L) < 50: self.sspeed_L = 0 if self.target_R > (self.dist_R + self.dts_R) and self.sspeed_R < self.max_speed_R: self.sspeed_R += self.acc elif self.target_R < (self.dist_R + self.dts_R) and self.sspeed_R > -self.max_speed_R: self.sspeed_R -= self.acc elif self.target_R == self.dist_R and abs(self.sspeed_R) < 50: self.sspeed_R = 0 self.MoveStep(n)
# These will be the standard pins required for motor control LMotorB = Pin('GPIO7', af=0, mode=Pin.OUT) RMotorB = Pin('GPIO8', af=0, mode=Pin.OUT) LMotorB.low() RMotorB.low() # assign GPIO9 and 10 to alternate function 3 (PWM) # These will be the pins to control speed LMotorA = Pin('GPIO9', af=3, type=Pin.STD) RMotorA = Pin('GPIO10', af=3, type=Pin.STD) # Enable timer channels 3B and 4A for PWM pins LMTimer = Timer(3, mode=Timer.PWM, width=16) RMTimer = Timer(4, mode=Timer.PWM, width=16) # enable channel A @1KHz with a 50% duty cycle LMT_a = LMTimer.channel(Timer.B, freq=1000, duty_cycle=50) RMT_a = RMTimer.channel(Timer.A, freq=1000, duty_cycle=50) def Setup_WIFI() wifi = WLAN(WLAN.STA) # go for fixed IP settings wifi.ifconfig('192.168.0.107', '255.255.255.0', '192.168.0.1', '8.8.8.8') wifi.scan() # scan for available netrworks wifi.connect(ssid='mynetwork', security=2, key='mynetworkkey') while not wifi.isconnected(): pass print(wifi.ifconfig()) # enable wake on WLAN wifi.callback(wakes=Sleep.SUSPENDED) # go to sleep Sleep.suspend()
gradient(0,0,50,100,0,200,10,3) gradient(40,0,250,0,40,80,10,3) r=Led(Pin.cpu.A1) g=Led(Pin.cpu.A2) b=Led(Pin.cpu.A3) adc = pyb.ADC(Pin.cpu.A4) val = adc.read() ADCMAX=4096 F=100 wait=10 tim = Timer(2, freq=1000) cr = tim.channel(2, Timer.PWM_INVERTED, pin=r.pin) cg = tim.channel(3, Timer.PWM_INVERTED, pin=g.pin) cb = tim.channel(4, Timer.PWM_INVERTED, pin=b.pin) ################ Red color intensity by PWM while True: val = adc.read() width=int(F*val/ADCMAX) tim = Timer(2, freq=1000) cb = tim.channel(4, Timer.PWM_INVERTED, pin=b.pin) cb.pulse_width_percent(width) pyb.delay(wait) ############### R B color variation
print('Task 9: Keypad controlling motor') #initialise UART communication uart = UART(6) uart.init(9600, bits=8, parity = None, stop = 2) # define various I/O pins for ADC adc_1 = ADC(Pin('X19')) adc_2 = ADC(Pin('X20')) # set up motor with PWM and timer control A1 = Pin('Y9',Pin.OUT_PP) A2 = Pin('Y10',Pin.OUT_PP) pwm_out = Pin('X1') tim = Timer(2, freq = 1000) motor = tim.channel(1, Timer.PWM, pin = pwm_out) # Motor in idle state A1.high() A2.high() speed = 0 DEADZONE = 5 # Use keypad U and D keys to control speed while True: # loop forever until CTRL-C while (uart.any()!=10): #wait we get 10 chars n = uart.any() command = uart.read(10) if command[2]==ord('5'): if speed < 96: speed = speed + 5