Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
beaglebuggy
#!/usr/bin/env python3
import cv2.cv as cv
import time
cv.NamedWindow("camera", 1)
capture = cv.CaptureFromCAM(0)
#cv.SetCaptureProperty(capture, 3, 360)
#cv.SetCaptureProperty(capture, 4, 240)
while True:
img = cv.QueryFrame(capture)
cv.ShowImage("camera", img)
if cv.WaitKey(10) == 27:
break
cv.DestroyAllWindows()
#!/usr/bin/env python3
# Run the motors
# Based on: https://github.com/mcdeoliveira/rcpy/raw/master/examples/rcpy_test_motors.py
# import python libraries
import rcpy
import rcpy.motor as motor
import sys
import time
class Manual:
# CONSTANT VARIABLES #
# CONTROL CONSTANTS #
BACKWARD = "\033[B" # Down
FORWARD = "\033[A" # Up
KILLSWITCH = "^C" # Select
LEFT = "\033[D" # Left
RIGHT = "\033[C" # Right
SELECT_MODE = "3" # Start / Pause
THROTTLE_DOWN = "4" # Left Trigger
THROTTLE_UP = "6" # Right Trigger
# THROTTLE CONSTANTS (Forward and Backward) #
DIRECTION_THROTTLE_BACKWARD = -1 # Negative to go backward
DIRECTION_THROTTLE_FORWARD = 1 # Positive to go forward
DIRECTION_THROTTLE_DEFAULT = DIRECTION_THROTTLE_FORWARD # The direction to start travelling in
DUTY_THROTTLE_MAX = 1.0 # Max duty allowed for throttle
DUTY_THROTTLE_STEPS = 10 # Number of 'gears' to max throttle
DUTY_THROTTLE_ITERATION_VALUE = DUTY_THROTTLE_MAX / DUTY_THROTTLE_STEPS
# TURN CONSTANTS (Left and Right) #
DIRECTION_TURN_CENTER = 0 # Directional value to keep wheels straight
DIRECTION_TURN_DEFAULT = DIRECTION_TURN_CENTER # The direction the wheels start in
DIRECTION_TURN_LEFT = 1 # Directional value to turn wheels towards the left
DIRECTION_TURN_RIGHT = -1 # Directional value to turn wheels towards the right
DUTY_TURN_MAX = 1.0 # Max duty allowed for turning
DUTY_TURN_STEPS = 30 # Number of steps before reaching max turn radius
DUTY_TURN_ITERATION_VALUE = DUTY_TURN_MAX / DUTY_TURN_STEPS
# CONSTRUCTOR #
def __init__(self):
self.direction = self.DIRECTION_THROTTLE_DEFAULT
self.direction_turn = self.DIRECTION_TURN_DEFAULT
self.duty_throttle = 0
self.duty_turn = 0
rcpy.set_state(rcpy.RUNNING)
print("Manual Initiated.")
def change_direction(self, direction):
if abs(direction) == 1: # Check if direction is either 1 or -1
self.duty_throttle = 0 # Stop first
self.direction = direction
def check_hold(self, direction):
if abs(direction) == 1 or direction == 0: # Check if direction is either 1, -1 or 0
if self.direction_turn != direction: # If the input direction doesn't match the previously chosen...
self.direction_turn = direction # ...change to new direction
self.duty_turn = 0 # and reset the turn duty
def decrease_throttle(self):
if self.duty_throttle >= self.DUTY_THROTTLE_ITERATION_VALUE: # Check if duty is at least what will be subtracted
self.duty_throttle = self.duty_throttle - self.DUTY_THROTTLE_ITERATION_VALUE # Iterate negatively
else: # Duty is less than iteration value
self.duty_throttle = 0 # Set duty to min
def increase_throttle(self):
if self.duty_throttle <= self.DUTY_THROTTLE_MAX - self.DUTY_THROTTLE_ITERATION_VALUE: # Check if duty is at most one iteration from max
self.duty_throttle = self.duty_throttle + self.DUTY_THROTTLE_ITERATION_VALUE #Iterate Positively
else: # Duty is greater than one iteration from max
self.duty_throttle = self.DUTY_THROTTLE_MAX # Set duty to max
def get_duty_throttle(self):
return self.duty_throttle * self.direction # Return duty in the current direction
def get_duty_turn(self):
return self.duty_turn * self.direction_turn # Return duty in the current direction
def kill(self):
self.direction = 1
self.direction_turn = 0
self.duty_throttle = 0
self.duty_turn = 0
motor.set(1, 0)
motor.set(2, 0)
print("Killswitch activated.")
def read_key(self):
key = sys.stdin.read(1)
if ord(key) == 27:
key = key + sys.stdin.read(2)
elif ord(key) == 3:
raise KeyboardInterrupt
return key
def turning(self, key):
# TODO: Use 'keyboard' module to read multiple keys at once
if (key == self.LEFT) ^ (key == self.RIGHT): # If keystrokes contain -EITHER- LEFT or RIGHT...
if key == self.LEFT: # If it's LEFT
self.check_hold(self.DIRECTION_TURN_LEFT)
else: # If it's RIGHT
self.check_hold(self.DIRECTION_TURN_RIGHT)
if self.duty_turn <= self.DUTY_TURN_MAX - self.DUTY_TURN_ITERATION_VALUE: # If the next iteration is at most one iteration from max duty...
self.duty_turn = self.duty_turn + self.DUTY_TURN_ITERATION_VALUE # Iterate
else: # Value is greater than one iteration away from max duty
self.duty_turn = self.DUTY_TURN_MAX # Set duty to its max value
else: # Neither or both directions selected
self.check_hold(self.DIRECTION_TURN_CENTER) # Set direction to straight and reset duty
try:
m = Manual()
while rcpy.get_state() != rcpy.EXITING:
key = read_key()
if key == BACKWARD:
m.change_direction()
elif key == FORWARD:
m.change_direction()
elif key == KILLSWITCH:
m.kill()
elif key == LEFT:
m.turning()
m.check_hold()
elif key == RIGHT:
m.turning()
m.check_hold()
elif key == SELECT_MODE:
pass
elif key == THROTTLE_DOWN:
m.decrease_throttle()
elif key == THROTTLE_UP:
m.increase_throttle()
motor.set(1, m.get_duty_turn())
motor.set(2, m.get_duty_throttle())
time.sleep(.1) # sleep some
except KeyboardInterrupt:
m.kill()
pass
finally:
print("\nBye BeagleBone!")
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment