import time
import RPi.GPIO as GPIO
from Servo import Servo
from Sonar import Sonar
if __name__ == "__main__":
GPIO.setmode(GPIO.BOARD)
sonar1 = Sonar(11, 13)
servo1 = Servo(15, 50, 17)
# move to 45 degrees
servo1.move(45)
time.sleep(1)
# take a reading
distance45 = sonar1.getDistance()
print(distance45)
# move to 45 degrees
servo1.move(90)
time.sleep(1)
# take a reading
distance90 = sonar1.getDistance()
print(distance90)
# move to 45 degrees
servo1.move(135)
time.sleep(1)
# take a reading
distance135 = sonar1.getDistance()
print(distance135)
class MovementController:
__instance = None
@staticmethod
def getInstance(
): # function to get the only instance of this class since the class is a singleton
# if there isn't an instance of this class yet, create it
if MovementController.__instance is None:
MovementController()
# return this class's only instance
return MovementController.__instance
def __init__(self):
if MovementController.__instance is not None: # if the constructor of this class is called more than once
raise Exception("This class is a singleton!")
else:
# puts the created instance in the "__instance" variable
MovementController.__instance = self
# array that holds the pins of all the motors
pinArray = [[20, 23, 24], [16, 5, 6]]
# creates instances of the Motor class to control the physical motors
self.leftMotor = Motor(pinArray[0])
self.rightMotor = Motor(pinArray[1])
self.servos = AX12.Ax12() # list of all servos
# creates instances of the Servo class to control the physical servos
self.armServo = Servo(self.servos, 3, 426, 576, 100)
self.gripServo = Servo(self.servos, 4, 280, 574, 512)
def moveGripper(self, x, y): # function to move the servos in the gripper
try: # try catches in case the gripper is not plugged in the robot or one of the servos has timed out
self.armServo.move(y)
except Exception:
pass
try:
self.gripServo.move(x)
except Exception:
pass
def moveMotors(self, x, y):
# turns the value from the joystick (0 up to 1023) into a value between -100 and 100
turnvariable = (
x - 511.5
) / 5.115 # joystick value 1023 turns into 100, joystick value 0 turns into -100
speedvariable = (
y / -5.115
) + 100 # joystick value 0 turns into 100, joystick value 1023 turns into -100
# if the joystick is aimed to the right
if turnvariable > 10:
# if the joystick is aimed to the right and up
if speedvariable > 10:
self.leftMotor.move("left", speedvariable)
self.rightMotor.move(
"right", speedvariable * ((100 - turnvariable) / 100))
# if the joystick is aimed to the right and down
elif speedvariable < -10:
self.leftMotor.move("right", (speedvariable * -1))
self.rightMotor.move("left", (speedvariable * -1) *
((100 - turnvariable) / 100))
# if the joystick is aimed solely to the right
else:
self.leftMotor.move("left", turnvariable)
self.rightMotor.move("left", turnvariable)
# if the joystick is aimed to the left
elif turnvariable < -10:
# if the joystick is aimed to the left and up
if speedvariable > 10:
self.leftMotor.move(
"left", speedvariable * ((100 + turnvariable) / 100))
self.rightMotor.move("right", speedvariable)
# if the joystick is aimed to the left and down
elif speedvariable < -10:
self.leftMotor.move("right", (speedvariable * -1) *
((100 + turnvariable) / 100))
self.rightMotor.move("left", (speedvariable * -1))
# if the joystick is aimed solely to the left
else:
self.leftMotor.move("right", (turnvariable * -1))
self.rightMotor.move("right", (turnvariable * -1))
# if the joystick is aimed neither to the left or the right
else:
# if the is aimed solely up
if speedvariable > 10:
self.leftMotor.move("left", speedvariable)
self.rightMotor.move("right", speedvariable)
# if the joystick is aimed solely down
elif speedvariable < -10:
self.leftMotor.move("right", (speedvariable * -1))
self.rightMotor.move("left", (speedvariable * -1))
# if the joystick is not being used
else:
self.leftMotor.off()
self.rightMotor.off()
x_angle = 90
y_angle = 90
x_servo = Servo(23)
y_servo = Servo(24)
cam = Capture()
def img():
while True:
cam.get_img()
cv2.imshow('test', cam.img)
cv2.waitKey(50)
img_process = Process(target=img)
img_process.start()
while True:
x_angle = x_servo.set_angle(x_angle)
y_angle = y_servo.set_angle(y_angle)
x_servo.move()
y_servo.move()
time.sleep(0.05)
