moveit.py

# Online updated version at: https://pastebin.com/Wm30BmJE
# Running:
#  python <FILENAME>.py
#  NOTE: IF THIS SCRIPT FAILS WITH AN ERROR SUCH AS:
#   Failed to import "curses"
#  Run "pip install curses" in Terminal (or in SSH)
# Usage:
#  Tap a key to perform an action, YOU DO NOT HAVE TO HOLD THEM.
#  The default 'Control+C' CANNOT be used to exit the script, you must tap
#  the 'X' key
#  'w'    Begin moving foraward
#  's'    Begin moving backward
#  'd'    Begin turning right
#  'a'    Begin turning left
#  SPACE  Stop all movement
#  'x'    Exit the python execution (exit the script)

# Import the required libraries
# Curses is used to detect keypresses in the console
import curses
import RPi.GPIO as GPIO
import time, sys, tty, termios

# Pulses per second (Don't change)
Frequency = 120

# Speed bounds (probably don't change)
MinSpeed = 20
MaxSpeed = 100

# Motor imperfection correction (Change this until your robot moves straight enough)
FineA = -0.5

# No movement (Don't change)
Stop = 0

motorBBackwards = None;
motorBForwards = None;
motorABackwards = None;
motorAForwards = None;

def StopAllMotors():
    motorAForwards.ChangeDutyCycle(Stop)
    motorABackwards.ChangeDutyCycle(Stop)
    motorBForwards.ChangeDutyCycle(Stop)
    motorBBackwards.ChangeDutyCycle(Stop)

# Initializes the pins and such on the robot
def Init():
    print("Initilizing movement");
    GPIO.setmode(GPIO.BCM)
    GPIO.setwarnings(False)
    global motorBBackwards;
    global motorBForwards;
    global motorABackwards;
    global motorAForwards;
    GPIO.setup(7, GPIO.OUT)
    GPIO.setup(8, GPIO.OUT)
    GPIO.setup(9, GPIO.OUT)
    GPIO.setup(10, GPIO.OUT)
    motorBBackwards = GPIO.PWM(7, Frequency)
    motorBForwards = GPIO.PWM(8, Frequency)
    motorABackwards = GPIO.PWM(9, Frequency)
    motorAForwards = GPIO.PWM(10, Frequency)
    motorAForwards.start(Stop)
    motorABackwards.start(Stop)
    motorBForwards.start(Stop)
    motorBBackwards.start(Stop)
    print("Initilized movement");

# Cleans up
def Exit():
    StopAllMotors()
    GPIO.cleanup()

# Time (t) is in seconds
# Speed is in percent of maximum speed (20-100 by default)
def MoveBackward(t, speed, stop):
    motorABackwards.ChangeDutyCycle(ClampSpeed(speed + FineA))
    motorBBackwards.ChangeDutyCycle(ClampSpeed(speed))
    if stop:
        time.sleep(t)
        StopAllMotors()

# For the following move methods,
#  if stop is 'True', then the motors will stop
#  automatically after 't' seconds, otherwise,
#  the robot will continue to move until 'StopAllMotors()'
#  is called or another movement method is called.
def MoveForward(t, speed, stop):
    motorAForwards.ChangeDutyCycle(ClampSpeed(speed + FineA))
    motorBForwards.ChangeDutyCycle(ClampSpeed(speed))
    if stop:
        time.sleep(t)
        StopAllMotors()

def TurnLeft(t, speed, stop):
    motorABackwards.ChangeDutyCycle(ClampSpeed(speed + FineA))
    motorBForwards.ChangeDutyCycle(ClampSpeed(speed))
    if stop:
        time.sleep(t)
        StopAllMotors()

def TurnRight(t, speed, stop):
    motorAForwards.ChangeDutyCycle(ClampSpeed(speed + FineA))
    motorBBackwards.ChangeDutyCycle(ClampSpeed(speed))
    if stop:
        time.sleep(t)
        StopAllMotors()

# Returns a value that is no less than "mi" nor greater than "ma"
def Clamp(val, mi, ma):
    if (val > ma):
        return ma
    if (val < mi):
        return mi
    return val

# Makes sure the supplied speed cannot
#  be less than the minimum speed or greater than
#  the maximum speed
def ClampSpeed(speed):
    return Clamp(speed, MinSpeed, MaxSpeed)

Init()

# Movement code
# This will detect keypesses on the keyboard
#  and allow them to control the car. You can
#  change the keys to what ever you like, just
#  be sure the keys have an actual character associated
#  with them, keys like 'l', 'a', 'w', 'u', etc, will work,
#  but 'shift', 'control', 'up', 'right', etc will not.
# Note:
#  If the robot moves in the wrong direction (such as 'w' moves backwards, or 'd' turns right),
#    Change the method calls to the necessary actions, such as 'MoveForward' to 'MoveBackward' and 'MoveBackward' to 'MoveForward'
#  If only one of the wheels turns in the right direction, while the other turns in the incorrect direction,
#    The only solution is to edit the specific function which turns the incorrect direction.
#    For example, if pressing 'w' moves one wheel forward and the other wheel backwards,
#      Change the code inside of the 'MoveForward()' method from 'motorBForwards.ChangeDutyCycle(ClampSpeed(speed))' to
#        'motorBBackwards.ChangeDutyCycle(ClampSpeed(speed))'
scr = curses.initscr()
while True: # This will keep executing until the program is exited, can be dangerous. KEEP IN MIND THAT 'Control+C' DOES NOT WORK.
    scr.nodelay(True) # Don't freeze the program if the user presses nothing
    key = ''
    try:
    key = scr.getkey()  # Retrieve the most recent keypress
    except:
        key = ''        # No key pressed this frame
    if key == ' ':      # Space will stop all movement
        StopAllMotors()
    elif key == 'x':        # 'x' will exit the program
        break
    elif key == 'w':        # 'w' will move forward until interrupted
        StopAllMotors()
        MoveForward(0, 75, False)
    elif key == 's':        # 's' will move backward until interrupted
        StopAllMotors()
        MoveBackward(0, 75, False)
    elif key == 'a':        # 'a' will turn left until interrupted
        StopAllMotors()
        TurnLeft(0, 100, False)
    elif key == 'd':        # 'd' will turn right until interrupted
        StopAllMotors()
        TurnRight(0, 100, False)

StopAllMotors() # Cleans up and exits the program
Exit()