sys.path.append('control/')
from arm import Arm
import time
import json
#Clear Log File
open('logs/log.txt', 'w').close()
with open('config/config.json') as json_file:
config = json.load(json_file)
arm = Arm(config)
angle = 70
arm.calibrateArm()
while not arm.armCalibrated():
time.sleep(0.5)
while arm.checkMovement():
time.sleep(0.5)
arm.setSpeed(0,90)
arm.setSpeed(1,90)
arm.setSpeed(2,90)
arm.setSpeed(3,200)
arm.setSpeed(4,200)
arm.setSpeed(5,200)
while arm.connected:
arm.moveJoint(0,angle)
kin = Kinematic()
#Clear Log File
open('logs/log.txt', 'w').close()
with open('config/config.json') as json_file:
config = json.load(json_file)
#Create instance of Arm class
arm = Arm(config)
#reconfigure for example
arm.resetEStop()
arm.calibrateArm()
while((not arm.armCalibrated()):
time.sleep(1)
print("waiting to calibrate...")
i = 0
#kin_angles = [0, 0, 0, 10, 10, 10]
kin_angles = [-0.02 ,-90.01, 90.01, 0.02, 89.9, -0.02] #joints into spreadsheet
step = 100
angle_step = 5
result = kin.check_limits(kin_angles) #check the angles are within the model ranges
if result == 1:
cj = kin.kin2native_angles(kin_angles) #convert to native machine joint angles
t = 0.0
class Controller:
debug = False
logFilePath = "logs/log.txt"
deadzone = 0.3
#COPY: Copyright 2019, All Rights Reserved, Ryan McCartney
def __init__(self, config):
self.logging = True
self.gamepadConnected = False
self.gamepadToUse = 0
self.gamepad = None
#Create instance of Arm class
self.arm = Arm(config)
self.arm.resetArduino()
#Start Pygame
pygame.init()
pygame.joystick.init()
#Setup Variables for Buttons
self.buttonTotal = 10
self.lastButtonState = [0] * self.buttonTotal
#Setup Variables for Axis
self.axisTotal = 5
self.leftJoint = 0
self.rightJoint = 1
self.axisPositions = [0] * self.axisTotal
self.setSpeed = [0] * self.arm.joints
#Logging Function
def log(self, entry):
currentDateTime = time.strftime("%d/%m/%Y %H:%M:%S")
logEntry = currentDateTime + ": " + entry
if self.logging == True:
#open a txt file to use for logging
logFile = open(self.logFilePath, "a+")
logFile.write(logEntry + "\n")
logFile.close()
print(logEntry)
def mapJoysticks(self):
status = True
try:
#Capture Button States
for i in range(0, self.axisTotal):
self.axisPositions[i] = self.gamepad.get_axis(i)
self.mapJoystick(self.axisPositions[0], self.axisPositions[1],
self.leftJoint)
self.mapJoystick(self.axisPositions[3], self.axisPositions[4],
self.rightJoint)
except:
self.log("ERROR: Mapping Axis Error.")
status = False
return status
def mapJoystick(self, xAxis, yAxis, joint):
joint = joint + joint
self.mapJoint(joint, xAxis)
self.mapJoint(joint + 1, yAxis)
def mapButtons(self):
buttonState = [0] * self.buttonTotal
status = True
try:
#Capture Button States
for i in range(0, self.buttonTotal):
buttonState[i] = self.gamepad.get_button(i)
#A BUTTON - STOP
if (buttonState[0] and (self.lastButtonState[0] == 0)):
self.arm.resetArduino()
#B BUTTON - STOP
if (buttonState[1] and (self.lastButtonState[1] == 0)):
self.arm.stop()
#Y BUTTON - STANDUP
if (buttonState[3] and (self.lastButtonState[3] == 0)):
self.arm.setDefaults()
self.arm.standUp()
self.arm.waitToStationary()
#X BUTTON - REST
if (buttonState[2] and (self.lastButtonState[2] == 0)):
self.arm.setDefaults()
self.arm.rest()
self.arm.waitToStationary()
#START BUTTON - CALIBRATE ARM
if (buttonState[7] and (self.lastButtonState[7] == 0)):
self.arm.setDefaults()
self.arm.calibrateArm()
self.arm.waitToStationary()
#LEFT THUMB BUTTON - CHANGE JOINT
if (buttonState[8] and (self.lastButtonState[8] == 0)):
if (self.leftJoint + 1) == self.rightJoint:
self.leftJoint += 2
elif (self.leftJoint) > 2:
self.leftJoint = 0
else:
self.leftJoint += 1
self.log("INFO: Joint set " + str(self.leftJoint) +
" selected on left joystick.")
#RIGHT THUMB BUTTON - CHANGE JOINT
if (buttonState[9] and (self.lastButtonState[9] == 0)):
if (self.rightJoint + 1) == self.leftJoint:
self.rightJoint += 2
if (self.rightJoint + 1) > 2:
self.rightJoint = 0
else:
self.rightJoint += 1
self.log("INFO: Joint set " + str(self.rightJoint) +
" selected on right joystick.")
self.lastButtonState = buttonState
except:
self.log("ERROR: Mapping Buttons Error.")
status = False
return status
def gamepads(self):
gamepads = pygame.joystick.get_count()
if not gamepads:
self.gamepad = None
self.log("INFO: There are " + str(gamepads) + " connected to the PC.")
return int(gamepads)
def getGamepadData(self):
status = True
if not self.gamepad:
self.connectGamepad()
else:
try:
#Get Current Data
pygame.event.get()
except:
self.log("ERROR: Could not access gamepad data.")
status = False
return status
def connectGamepad(self):
#Initialise first gamepad
while not self.gamepad:
try:
self.gamepad = pygame.joystick.Joystick(self.gamepadToUse)
self.gamepad.init()
self.log("INFO: Gamepad Connected Succesfully.")
except:
self.log("INFO: Waiting for Gamepad to be connected.")
time.sleep(1)
def mapJoint(self, joint, rawPosition):
#Converting to Discrete Speed Control
rawPosition = round(rawPosition, 2)
minSpeed = self.arm.jointMinSpeed[joint]
maxSpeed = self.arm.jointMaxSpeed[joint]
maxRotation = abs(self.arm.jointMaxRotation[joint])
#Select Direction
if rawPosition > self.deadzone:
#Select and Set a Speed
mappedSpeed = self.mapToRange(abs(rawPosition), self.deadzone, 1,
minSpeed, maxSpeed)
if mappedSpeed != self.setSpeed[joint]:
self.arm.setMinSpeed(joint, mappedSpeed)
self.arm.setSpeed(joint, mappedSpeed)
self.setSpeed[joint] = mappedSpeed
#Move the arm
self.arm.moveJointTo(joint, maxRotation)
elif rawPosition < -self.deadzone:
#Select and Set a Speed
mappedSpeed = self.mapToRange(abs(rawPosition), self.deadzone, 1,
minSpeed, maxSpeed)
if mappedSpeed != self.setSpeed[joint]:
self.arm.setMinSpeed(joint, mappedSpeed)
self.arm.setSpeed(joint, mappedSpeed)
#Move the arm
self.setSpeed[joint] = mappedSpeed
self.arm.moveJointTo(joint, 0)
else:
if self.arm.armCalibrated():
if self.setSpeed[joint] != 0:
self.arm.setMinSpeed(joint, 0)
self.arm.setSpeed(joint, 0)
self.setSpeed[joint] = 0
@staticmethod
def mapToRange(raw, rawMin, rawMax, mapMin, mapMax):
mapped = (raw - rawMin) * (mapMax - mapMin) / (rawMax -
rawMin) + mapMin
return mapped
