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)
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
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 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 __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
