예제 #1
0
#=================================================

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
예제 #2
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