ラズパイでラジコン制作(Rev#2-1)

2024-03-11 linux raspi /posts/2024/2024-03-11-rc-car.jpg

秋月電子通商のTB6612使用 Dual DCモータードライブキットを使ってDCモータを二つ制御するのに利用したコードです。RPi.GPIOよりもpigpioの方が良いと噂されていたので、そちらに乗り換えてみました。

import pigpio, time

# Make sure to enable the daemon:  sudo systemctl start pigpiod

class Motors:
    def __init__(self):
        self.pi = pigpio.pi()
        self.pi.set_mode(2,pigpio.OUTPUT) # BIN1
        self.pi.set_mode(3,pigpio.OUTPUT) # BIN2
        self.pi.set_mode(12,pigpio.OUTPUT) # PWM1
        self.pi.set_mode(13,pigpio.OUTPUT) # PWM2
        self.pi.set_mode(14,pigpio.OUTPUT) # AIN1
        self.pi.set_mode(15,pigpio.OUTPUT) # AIN2

    def speed2duty(self,speed):
        return int(abs(speed) * 1000000)

    def left_wheel(self,speed=1.0):
        self.pi.hardware_PWM(12,1000,self.speed2duty(speed))
        if speed == 0.0:
            self.pi.write(2,0)
            self.pi.write(3,0)
        elif speed > 0.0:
            self.pi.write(2,0)
            self.pi.write(3,1)
        else:
            self.pi.write(2,1)
            self.pi.write(3,0)

    def right_wheel(self,speed=1.0):
        self.pi.hardware_PWM(13,1000,self.speed2duty(speed))
        if speed == 0.0:
            self.pi.write(14,0)
            self.pi.write(15,0)
        elif speed > 0.0:
            self.pi.write(14,0)
            self.pi.write(15,1)
        else:
            self.pi.write(14,1)
            self.pi.write(15,0)

    def both_wheel(self,speed=1.0):
        self.right_wheel(speed)
        self.left_wheel(speed)

    def stop(self):
        self.pi.stop()

if __name__ == "__main__":
    # Test DC motors.
    m = Motors()

    print("fowrard 3 sec")
    m.left_wheel(1)
    m.right_wheel(1)    
    time.sleep(3)   
    print("stop 1 sec")
    m.both_wheel(0)
    time.sleep(1)
    print("backward left wheel with half speed 1 sec")
    m.left_wheel(-0.5)
    time.sleep(1)
    print("backward right wheel with half speed 1 sec")
    m.left_wheel(0)
    m.right_wheel(-0.5)
    time.sleep(1)
    print("stop all")
    m.both_wheel(0)
    m.stop()