import pyb
from pyb import UART
sensor.reset() # Reset and initialize the sensor.
sensor.set_pixformat(
sensor.GRAYSCALE) # Set pixel format to RGB565 (or GRAYSCALE)
sensor.set_framesize(sensor.QQVGA) # Set frame size to QQVGA (160x120)
sensor.skip_frames(time=2000) # Wait for settings take effect.
sensor.set_auto_gain(False) # must be turned off for color tracking
sensor.set_auto_whitebal(False) # must be turned off for color tracking
clock = time.clock() # Create a clock object to track the FPS.
uart = UART(1, 115200, timeout_char=1000)
tim1 = Timer(4, freq=300) # Servo Frequency in Hz
tim2 = Timer(2, freq=300) # Motor Frequency in Hz
initialPulseWidth = (15 / 10000) * (tim1.source_freq() /
(tim1.prescaler() + 1)) #servo
initialPulseWidth2 = (0 / 10000) * (tim2.source_freq() /
(tim2.prescaler() + 1)) #motor
ch1 = tim1.channel(1,
Timer.PWM,
pin=Pin("P7"),
pulse_width=int(initialPulseWidth)) #servo
ch2 = tim2.channel(4,
Timer.PWM,
pin=Pin("P5"),
pulse_width=int(initialPulseWidth2)) #motor
inA = Pin('P1', Pin.OUT_PP, Pin.PULL_NONE) #set inA to output
inB = Pin('P2', Pin.OUT_PP, Pin.PULL_NONE) #set inB to output
enA = Pin('P3', Pin.OUT_PP, Pin.PULL_NONE) #set enA to output
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)
#Setup UART
uart = UART(3, 115200, timeout_char=1000)
#Send Ok command to Bluetooth
uart.write('AT\r\n')
print(uart.readline())
#Initialize PWM & Motor Duty Cycle
pwm = 1400
motor = 0
mil = 1000000
# Timer Setup for Servo
tim = Timer(2, freq=300) # Frequency in Hz
t1prescaler = tim.source_freq() / (tim.prescaler() + 1)
div = int(t1prescaler * (pwm / mil))
ch1 = tim.channel(1, Timer.PWM, pin=Pin("P6"), pulse_width=div)
# Timer Setup for Motor
tim2 = Timer(4, freq=1000) # Frequency in Hz
t2prescaler = tim2.source_freq() / (tim2.prescaler() + 1)
div2 = int(t2prescaler * (motor / mil))
ch2 = tim2.channel(1, Timer.PWM, pin=Pin("P7"), pulse_width=div2)
Forward = False
Reverse = False
BrakeVCC = False
BrakeGND = False
