class MyPS4Controller(Controller): def __init__(self, **kwargs): Controller.__init__(self, **kwargs) self.L3X = 0 self.L3Y = 0 self.R3X = 0 self.R3Y = 0 self.leftMotorPWM = 0 self.rightMotorPWM = 0 self.arrow = "None" self.motor = Motor() self.servo = Servo() self.motor.InitMotorHw() self.servo.InitServoHw() self.BASESTEPSIZE = 5 def on_L3_up(self, value): #print("L3 up értéke: {}".format(value)) self.L3Y = value self.motorControll() def on_L3_down(self, value): #print("L3 down értéke: {}".format(value)) self.L3Y = value self.motorControll() def on_L3_left(self, value): #print("L3 left értéke: {}".format(value)) self.L3X = value self.motorControll() def on_L3_right(self, value): #print("L3 right értéke: {}".format(value)) self.L3X = value self.motorControll() def on_L3_y_at_rest(self): self.L3Y = 0 self.motorControll() def on_L3_x_at_rest(self): self.L3X = 0 self.motorControll() def motorControll(self): #stop case if self.L3Y == 0 and self.L3X == 0: self.motor.stop() #forward case if self.L3Y < 0 and self.L3X == 0: self.leftMotorPWM = self.rightMotorPWM = int((abs(self.L3Y) / 32767) * 100) self.motor.forward() self.motor.setLeftMotorPWM(self.leftMotorPWM) self.motor.setRightMotorPWM(self.rightMotorPWM) #backward case if self.L3Y > 0 and self.L3X == 0: self.leftMotorPWM = self.rightMotorPWM = int((abs(self.L3Y) / 32767) * 100) self.motor.reverse() self.motor.setLeftMotorPWM(self.leftMotorPWM) self.motor.setRightMotorPWM(self.rightMotorPWM) #left case if self.L3Y == 0 and self.L3X < 0: self.rightMotorPWM = int((abs(self.L3X) / 32767) * 100) self.leftMotorPWM = 0 self.motor.setLeftMotorPWM(self.leftMotorPWM) self.motor.setRightMotorPWM(self.rightMotorPWM) #right case if self.L3Y == 0 and self.L3X > 0: self.leftMotorPWM = int((abs(self.L3X) / 32767) * 100) self.rightMotorPWM = 0 self.motor.setLeftMotorPWM(self.leftMotorPWM) self.motor.setRightMotorPWM(self.rightMotorPWM) #first quater if self.L3Y < 0 and self.L3X < 0: baseSpeedVector = math.sqrt((self.L3X * self.L3X) + (self.L3Y * self.L3Y)) rightMotor = baseSpeedVector * (abs(self.L3Y) / 32767) leftMotor = rightMotor * ((32767 - abs(self.L3Y)) / 32768) self.leftMotorPWM = int((leftMotor / 32767) * 100) self.rightMotorPWM = int((rightMotor/32767) * 100) if self.leftMotorPWM > 100 or self.rightMotorPWM > 100: print("----------------------------------------") print("DANGER IN FIRST QUATER") print("Left motor PWM: {}".format(self.leftMotorPWM)) print("Right motor PWM: {}".format(self.rightMotorPWM)) print("----------------------------------------") else : self.motor.forward() self.motor.setLeftMotorPWM(self.leftMotorPWM) self.motor.setRightMotorPWM(self.rightMotorPWM) #second quater if self.L3Y < 0 and self.L3X > 0: baseSpeedVector = math.sqrt((self.L3X * self.L3X) + (self.L3Y * self.L3Y)) leftMotor = baseSpeedVector * (abs(self.L3Y) / 32767) rightMotor = leftMotor * ((32767 - abs(self.L3Y)) / 32768) self.leftMotorPWM = int((leftMotor / 32767) * 100) self.rightMotorPWM = int((rightMotor/32767) * 100) if self.leftMotorPWM > 100 or self.rightMotorPWM > 100: print("----------------------------------------") print("DANGER IN SECOND QUATER") print("Left motor PWM: {}".format(self.leftMotorPWM)) print("Right motor PWM: {}".format(self.rightMotorPWM)) print("----------------------------------------") else: self.motor.forward() self.motor.setLeftMotorPWM(self.leftMotorPWM) self.motor.setRightMotorPWM(self.rightMotorPWM) #third quater if self.L3Y > 0 and self.L3X > 0: baseSpeedVector = math.sqrt((self.L3X * self.L3X) + (self.L3Y * self.L3Y)) rightMotor = baseSpeedVector * (abs(self.L3Y) / 32767) leftMotor = rightMotor * ((32767 - abs(self.L3Y)) / 32768) self.leftMotorPWM = int((leftMotor / 32767) * 100) self.rightMotorPWM = int((rightMotor/32767) * 100) if self.leftMotorPWM > 100 or self.rightMotorPWM > 100: print("----------------------------------------") print("DANGER IN THIRD QUATER") print("Left motor PWM: {}".format(self.leftMotorPWM)) print("Right motor PWM: {}".format(self.rightMotorPWM)) print("----------------------------------------") else : self.motor.reverse() self.motor.setLeftMotorPWM(self.leftMotorPWM) self.motor.setRightMotorPWM(self.rightMotorPWM) #fourth quater if self.L3Y > 0 and self.L3X < 0: baseSpeedVector = math.sqrt((self.L3X * self.L3X) + (self.L3Y * self.L3Y)) leftMotor = baseSpeedVector * (abs(self.L3Y) / 32767) rightMotor = leftMotor * ((32767 - abs(self.L3Y)) / 32768) self.leftMotorPWM = int((leftMotor / 32767) * 100) self.rightMotorPWM = int((rightMotor/32767) * 100) if self.leftMotorPWM > 100 or self.rightMotorPWM > 100: print("----------------------------------------") print("DANGER IN FOURTH QUATER") print("Left motor PWM: {}".format(self.leftMotorPWM)) print("Right motor PWM: {}".format(self.rightMotorPWM)) print("----------------------------------------") else: self.motor.reverse() self.motor.setLeftMotorPWM(self.leftMotorPWM) self.motor.setRightMotorPWM(self.rightMotorPWM) def on_up_arrow_press(self): #print("Up lenyomva") self.arrow = "Up" self.servoControll() def on_up_down_arrow_release(self): self.arrow = "None" self.servoControll() def on_down_arrow_press(self): #print("Down lenyomva") self.arrow = "Down" self.servoControll() def on_left_arrow_press(self): #print("Left lenyomva") self.arrow = "Left" self.servoControll() def on_left_right_arrow_release(self): self.arrow = "None" self.servoControll() def on_right_arrow_press(self): #print("Right lenyomva") self.arrow = "Right" self.servoControll() def on_R3_up(self, value): #print("R3 up értéke: {}".format(value)) self.R3Y = value self.servoControll() def on_R3_down(self, value): #print("R3 down értéke: {}".format(value)) self.R3Y = value self.servoControll() def on_R3_left(self, value): #print("R3 left értéke: {}".format(value)) self.R3X = value self.servoControll() def on_R3_right(self, value): #print("R3 right értéke: {}".format(value)) self.R3X = value self.servoControll() def on_R3_y_at_rest(self): self.R3Y = 0 self.servoControll() def on_R3_x_at_rest(self): self.R3X = 0 self.servoControll() def servoControll(self): #arm controll condition if self.arrow != "None" : self.ArmServoControl() else: self.CameraServoControl() def ArmServoControl(self): if self.arrow == "Down": oldPosition = self.servo.getArm0Position() if self.R3Y > 0: stepSize = int(abs(self.R3Y) / (32767 / 5)) # 1 - 7 the range newPosition = oldPosition + stepSize if (oldPosition != newPosition) and (abs(oldPosition - newPosition) <= 3): self.servo.setArm0Position(newPosition) """ now = datetime.now() print(stepSize) print("Time: {}".format(now)) print("old Position: {}".format(oldPosition)) print("new position: {}".format(self.servo.getArm0Position())) """ if self.R3Y < 0: stepSize = int(abs(self.R3Y) / (32767 / 5)) # 1 - 7 the range newPosition = oldPosition - stepSize if (oldPosition != newPosition) and (abs(oldPosition - newPosition) <= 3): self.servo.setArm0Position(newPosition) """ now = datetime.now() print(stepSize) print("Time: {}".format(now)) print("old Position: {}".format(oldPosition)) print("new position: {}".format(self.servo.getArm0Position())) """ if self.arrow == "Right": oldPosition = self.servo.getArm1Position() if self.R3Y > 0: stepSize = int(abs(self.R3Y) / (32767 / 5)) # 1 - 7 the range newPosition = oldPosition + stepSize if (oldPosition != newPosition) and (abs(oldPosition - newPosition) <= 3): self.servo.setArm1Position(newPosition) """ now = datetime.now() print(stepSize) print("Time: {}".format(now)) print("old Position: {}".format(oldPosition)) print("new position: {}".format(self.servo.getArm1Position())) """ if self.R3Y < 0: stepSize = int(abs(self.R3Y) / (32767 / 5)) # 1 - 7 the range newPosition = oldPosition - stepSize if (oldPosition != newPosition) and (abs(oldPosition - newPosition) <= 3): self.servo.setArm1Position(newPosition) """ now = datetime.now() print(stepSize) print("Time: {}".format(now)) print("old Position: {}".format(oldPosition)) print("new position: {}".format(self.servo.getArm1Position())) """ if self.arrow == "Up": oldPosition = self.servo.getArm2Position() if self.R3Y > 0: stepSize = int(abs(self.R3Y) / (32767 / 5)) # 1 - 7 the range newPosition = oldPosition + stepSize if (oldPosition != newPosition) and (abs(oldPosition - newPosition) <= 3): self.servo.setArm2Position(newPosition) """ now = datetime.now() print(stepSize) print("Time: {}".format(now)) print("old Position: {}".format(oldPosition)) print("new position: {}".format(self.servo.getArm2Position())) """ if self.R3Y < 0: stepSize = int(abs(self.R3Y) / (32767 / 5)) # 1 - 7 the range newPosition = oldPosition - stepSize if (oldPosition != newPosition) and (abs(oldPosition - newPosition) <= 3): self.servo.setArm2Position(newPosition) """ now = datetime.now() print(stepSize) print("Time: {}".format(now)) print("old Position: {}".format(oldPosition)) print("new position: {}".format(self.servo.getArm2Position())) """ if self.arrow == "Left": oldPosition = self.servo.getArm3Position() if self.R3Y > 0: stepSize = int(abs(self.R3Y) / (32767 / 5)) # 1 - 7 the range newPosition = oldPosition + stepSize if (oldPosition != newPosition) and (abs(oldPosition - newPosition) <= 3): self.servo.setArm3Position(newPosition) """ now = datetime.now() print(stepSize) print("Time: {}".format(now)) print("old Position: {}".format(oldPosition)) print("new position: {}".format(self.servo.getArm3Position())) """ if self.R3Y < 0: stepSize = int(abs(self.R3Y) / (32767 / 5)) # 1 - 7 the range newPosition = oldPosition - stepSize if (oldPosition != newPosition) and (abs(oldPosition - newPosition) <= 3): self.servo.setArm3Position(newPosition) """ now = datetime.now() print(stepSize) print("Time: {}".format(now)) print("old Position: {}".format(oldPosition)) print("new position: {}".format(self.servo.getArm3Position())) """ def CameraServoControl(self): #ToDo I have to implemention pass