2nrw 发布的帖子

THRESHOLD =(35, 93, -37, 84, -39, 13) # Grayscale threshold for dark things...

import sensor, image, time

from pyb import LED

import car

from pid import PID

rho_pid = PID(p=0.4, i=0)

theta_pid = PID(p=0.001, i=0)



LED(1).on()

LED(2).on()

LED(3).on()



sensor.reset()

sensor.set_vflip(True)

sensor.set_hmirror(True)

sensor.set_pixformat(sensor.RGB565)

sensor.set_framesize(sensor.QQQVGA) # 80x60 (4,800 pixels) - O(N^2) max = 2,3040,000.

#sensor.set_windowing([0,20,80,40])

sensor.skip_frames(time = 2000)     # WARNING: If you use QQVGA it may take seconds

clock = time.clock()                # to process a frame sometimes.



while(True):

    clock.tick()

    img = sensor.snapshot().binary([THRESHOLD])

    line = img.get_regression([(100,100,0,0,0,0)], robust = True)

    if (line):

        rho_err = abs(line.rho())-img.width()/2

        if line.theta()>90:

            theta_err = line.theta()-180

        else:

            theta_err = line.theta()

        img.draw_line(line.line(), color = 127)

        print(rho_err,line.magnitude(),rho_err)

        if line.magnitude()>8:

            #if -40<b_err<40 and -30<t_err<30:

            rho_output = rho_pid.get_pid(rho_err,1)

            theta_output = theta_pid.get_pid(theta_err,1)

            output = rho_output+theta_output

            car.run(50+output, 50-output)

        else:

            car.run(0,0)

    else:

        car.run(50,-50)

        pass

    #print(clock.fps())

可是速度大的时候就不会发出滴滴的声音

from pyb import Pin, Timer
inverse_left=False  #change it to True to inverse left wheel
inverse_right=False #change it to True to inverse right wheel

ain1 =  Pin('P0', Pin.OUT_PP)
ain2 =  Pin('P1', Pin.OUT_PP)
bin1 =  Pin('P2', Pin.OUT_PP)
bin2 =  Pin('P3', Pin.OUT_PP)
ain1.low()
ain2.low()
bin1.low()
bin2.low()

pwma = Pin('P7')
pwmb = Pin('P8')
tim = Timer(4, freq=1000)
ch1 = tim.channel(1, Timer.PWM, pin=pwma)
ch2 = tim.channel(2, Timer.PWM, pin=pwmb)
ch1.pulse_width_percent(0)
ch2.pulse_width_percent(0)

def run(left_speed, right_speed):
    if inverse_left==True:
        left_speed=(-left_speed)
    if inverse_right==True:
        right_speed=(-right_speed)

    if left_speed < 0:
        ain1.low()
        ain2.high()
    else:
        ain1.high()
        ain2.low()
    ch1.pulse_width_percent(abs(left_speed))

    if right_speed < 0:
        bin1.low()
        bin2.high()
    else:
        bin1.high()
        bin2.low()
    ch2.pulse_width_percent(abs(right_speed))