def processData(self): minDist = 500 minIndex= 0 for i in xrange(0,len(self.leftDistance)): if self.leftDistance[i] is None: self.leftDistance[i] = self.rightDistance[i] if self.leftDistance[i] < minDist and self.leftDistance[i] is not None: minDist = self.leftDistance[i] minIndex=i postProcessMove = Movement() postProcessMove.theta = i postProcessMove.modType='Set' postProcessMove.x = 0 postProcessMove.y = 0 self.switcher.state = 2 self.printDebug("\nInitialAngle: %6.2f MinDist: %6.2f at angle %6.2f\n"%(self.initialAngle,minDist,minIndex)) publisher = rospy.Publisher('/mappingOut',Movement) publisher.publish(postProcessMove) rospy.sleep(10.0)
def interpretJoystick(self,preMove): move = deep(preMove) publisher = rospy.Publisher('/angleIntegratorOut',Movement) xAxisL = move.xL yAxisL = move.yL xAxisR = move.xR yAxisR = move.yR xButton = move.xButton bButton = move.bButton leftBumperMag = move.bumperL rightBumperMag= move.bumperR #X/B button toggle logic if bButton == 1 and not self.bButtonDepressed: self.bButtonDepressed = True self.buttonTargetAngle += 90 elif xButton == 1 and not self.xButtonDepressed: self.xButtonDepressed = True self.buttonTargetAngle += -90 if bButton == 0 and self.bButtonDepressed: self.bButtonDepressed = False if xButton == 0 and self.xButtonDepressed: self.xButtonDepressed = False #Bumper logic (rotational spin using shoulders) #Right overrides left bumperMag = 0 if rightBumperMag != 1: bumperMag = (1 - rightBumperMag) elif leftBumperMag != 1: bumperMag = (1 - leftBumperMag) #Get the arctangent of xAxis/yAxis to get the angle in radians. #Convert it to degrees and make it so that it goes from 0-360 starting #at the positive y axis (to match with the front of the hovercraft). #Uses extreme deadzone to makesure accidental rotations don't happen. magnitudeThreshold = 1 magnitude = math.sqrt(xAxisL**2 + yAxisL**2) rotationalAngle = 0 if magnitude >= magnitudeThreshold: rotationalAngle = round(math.atan2(xAxisL,yAxisL)*(180.0/3.141593),4) if (rotationalAngle > 0): rotationalAngle = rotationalAngle - 360 rotationalAngle = math.fabs(rotationalAngle) #Ships off the message to the arbitrator #Joystick overrides button target commands moveOut = Movement() moveOut.modType = 'Bound' #Joystick Logic if magnitude >= magnitudeThreshold: #Reset button upon hitting the joystick self.buttonTargetAngle = 0 moveOut.theta = rotationalAngle moveOut.modType = 'Bound' #Trigger absolute rotation if trigger/bumper is held down if rightBumperMag != 1: self.rightBumperAngle = self.rightBumperAngle + (rightBumperMag * 100) self.buttonTargetAngle = 0 moveOut.theta = self.rightBumperAngle moveOut.modType = 'Add' magnitude = 1 else: self.rightBumperAngle = 0 if leftBumperMag != 1: self.buttonTargetAngle = 0 self.leftBumperAngle = self.leftBumperAngle + (leftBumperMag * 100) moveOut.theta = -1 * self.leftBumperAngle moveOut.modType = 'Add' magnitude = 1 else: self.leftBumperAngle = 0 #For 90 degree button rotations if math.fabs(self.buttonTargetAngle) > 0: magnitude = 1 moveOut.theta = self.buttonTargetAngle moveOut.modType = 'Add' moveOut.x = xAxisR moveOut.y = yAxisR moveOut.mag = magnitude publisher.publish(moveOut) #Prints all information related to the integrator if need be if (self.debug == 1): print("xL: %6.2f yL: %6.2f Angle: %6.2f Magnitude:%6.2f " "xR: %6.2f yR: %6.2f Theta: %6.2f Button Target: %6.2f" "rB: %6.2f lB: %6.2f rM: %6.2f lM: %6.2f " % (xAxisL,yAxisL,rotationalAngle,magnitude,xAxisR,yAxisR,moveOut.theta, self.buttonTargetAngle, self.rightBumperAngle, self.leftBumperAngle, rightBumperMag, leftBumperMag))