moveitnew.py

# Online at: https://pastebin.com/cTW3d6JL
# 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 and display cool
# stuff
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 = -3

# 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(8, Frequency)
    motorBForwards = GPIO.PWM(7, Frequency)
    motorABackwards = GPIO.PWM(10, Frequency)
    motorAForwards = GPIO.PWM(9, 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):
    motorABackwards.ChangeDutyCycle(ClampSpeed(speed + FineA))
    motorBBackwards.ChangeDutyCycle(ClampSpeed(speed))

# 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):
    motorAForwards.ChangeDutyCycle(ClampSpeed(speed + FineA))
    motorBForwards.ChangeDutyCycle(ClampSpeed(speed))

def TurnLeft(t, speed):
    motorABackwards.ChangeDutyCycle(ClampSpeed(speed + FineA))
    motorBForwards.ChangeDutyCycle(ClampSpeed(speed))

def TurnRight(t, speed):
    motorAForwards.ChangeDutyCycle(ClampSpeed(speed + FineA))
    motorBBackwards.ChangeDutyCycle(ClampSpeed(speed))

# 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()
curses.noecho()
curses.cbreak()
scr.keypad(1)
scr.nodelay(True)   # Don't freeze the program if the user presses nothing

move = None
speed = 1
itSpeed = 0
lastKey = 0

def Move(m, s):
    global move
    global speed

    if not (s == None):
        speed = s

    itSpeed = (speed / 9)

    scr.clear()
    if m == None:
        move = None
    elif not (m == ''):
        move = m

    if move == None:
        scr.addstr(0, 0, "Stopped", curses.A_REVERSE)
    else:
        scr.addstr(0, 0, move, curses.A_REVERSE)

    scr.addstr(1, 0, "Speed: " + str(speed), curses.A_REVERSE)
    scr.addstr(3, 0, "Press 'x' to exit", curses.A_BOLD)
    scr.addstr(4, 0, "Press space to stop moving", curses.A_BOLD)
    scr.addstr(5, 0, "Use arrow keys to move", curses.A_BOLD)
    scr.refresh()

Move(None, None)
firstKey = True

while True:         # This will keep executing until the program is exited, can be dangerous.
                    # KEEP IN MIND THAT 'Control+C' DOES NOT WORK.
    global speed
    global lastKey
    global firstKey

    key = None

    try:
        key = scr.getch()           # Retrieve the most recent keypress
    except:
        key = None                  # No key pressed this frame

    if key == -1 or key == None:
        scr.addstr(7, 0, str(lastKey), curses.A_REVERSE)
    else:
        if time.time() - lastKey <= 0.1:
            firstKey = False
        lastKey = time.time()

    diff = time.time() - lastKey

    if (not firstKey and diff > 0.1) or (firstKey and diff > 0.5):
        StopAllMotors()
        Move(None, None)
        firstKey = True

    if key == ord(' '):             # Space will stop all movement
        StopAllMotors()
        Move(None, None)
    elif key == ord('x'):           # 'x' will exit the program
        break
    elif key == curses.KEY_UP:
        StopAllMotors()
        Move("Moving: Forward", None);
        MoveForward(0, 35)
    elif key == curses.KEY_DOWN:
        StopAllMotors()
        Move("Moving: Backward", None);
        MoveBackward(0, 35)
    elif key == curses.KEY_LEFT:
        StopAllMotors()
        Move("Turning: Left", None);
        TurnLeft(0, 35)
    elif key == curses.KEY_RIGHT:
        StopAllMotors()
        Move("Turning: Right", None);
        TurnRight(0, 35)
    elif key == ord('1'):
        Move('', 1);
    elif key == ord('2'):
        Move('', 2);
    elif key == ord('3'):
        Move('', 3);
    elif key == ord('4'):
        Move('', 4);
    elif key == ord('5'):
        Move('', 5);
    elif key == ord('6'):
        Move('', 6);
    elif key == ord('7'):
        Move('', 7);
    elif key == ord('8'):
        Move('', 8);
    elif key == ord('9'):
        Move('', 9);

# Cleanup curses
curses.nocbreak()
scr.keypad(0)
curses.echo()
curses.endwin()

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