My MotoRun Project

影片:https://instagram.com/p/4AfK-uRKzy/?taken-by=yaowen1978

需要搭配Grove Pi的RBG背光LED字幕.

#!/usr/bin/python
import RPi.GPIO as GPIO
import time
import subprocess
import pygame
from grovepi import *
from grove_rgb_lcd import *

# Use physical pin numbers
GPIO.setmode(GPIO.BOARD)

# Set up header 6 GPIO pin as an input
PinIN1 = 31 # GPIO6 as IN1
PinIN2 = 33 # GPIO13 as IN2
PinIN3 = 35 # GPIO19 as IN3
PinIN4 = 37 # GPIO26 as IN4
GPIO.setup(PinIN1, GPIO.OUT)
GPIO.setup(PinIN2, GPIO.OUT)
GPIO.setup(PinIN3, GPIO.OUT)
GPIO.setup(PinIN4, GPIO.OUT)

# Connect the Grove Button to digital port D3
#button = 4
#grovepi.pinMode(button,"INPUT")

def CallWamoPRO():
    subprocess.call('bluez-test-input connect 00:17:02:01:1E:74  yes')
    subprocess.call('bluez-test-device trusted 00:17:02:01:1E:74  yes')

def InitialMoto():
    GPIO.output(PinIN1, False)
    GPIO.output(PinIN2, False)
    GPIO.output(PinIN3, False)
    GPIO.output(PinIN4, False)

def TurnRight():
    GPIO.output(PinIN1, False)
    GPIO.output(PinIN2, True)
    GPIO.output(PinIN3, False)
    GPIO.output(PinIN4, True)

def TurnLeft():
    GPIO.output(PinIN1, True)
    GPIO.output(PinIN2, False)
    GPIO.output(PinIN3, True)
    GPIO.output(PinIN4, False)

def Backward():
    GPIO.output(PinIN1, False)
    GPIO.output(PinIN2, True)
    GPIO.output(PinIN3, True)
    GPIO.output(PinIN4, False)

def Fordward():
    GPIO.output(PinIN1, True)
    GPIO.output(PinIN2, False)
    GPIO.output(PinIN3, False)
    GPIO.output(PinIN4, True)

def StopMoving():
    GPIO.output(PinIN1, False)
    GPIO.output(PinIN2, False)
    GPIO.output(PinIN3, False)
    GPIO.output(PinIN4, False)

pygame.init()
Wamo = pygame.joystick.Joystick(0)
Wamo.init()
print('Start Moto')
setText("Start Moto\nHello World")
setRGB(0,128,0)
InitialMoto()

while True:
    pygame.event.pump()
#    if grovepi.digitalRead(button) == 0:
#       CallWamoPRO()
    if Wamo.get_axis(1) > 0: # Backward
       Backward()
       setText("Backward\nR!R!R!")
       setRGB(255,0,0)
    if Wamo.get_axis(1) < 0: # Fordward
       Fordward()
       setText("Fordward\nGo!Go!Go!")
       setRGB(0,255,0)
    if Wamo.get_axis(0) > 0: # Turn Right
       TurnRight()
       setText("Turn Right\n>>>>>>>")
       setRGB(0,0,255)
    if Wamo.get_axis(0) < 0: # Turn Left
       TurnLeft()
       setText("Turn Left\n><<<<<<<")
       setRGB(255,255,255)
    if Wamo.get_axis(0) == 0 and Wamo.get_axis(1) == 0:# StopMoving
       StopMoving()
       setText("Park\n>P!P!P!P!")
       setRGB(0,0,0)

subprocess.call("clear")
GPIO.cleanup()