def gyroCallback(self, gyro): targetRate = self.movePass.theta #Proportional and Derivative computations r = self.P*(targetRate - gyro.rate) r = r + self.D*((targetRate - gyro.rate)-self.previousError) self.previousError = targetRate - gyro.rate #Only reset r if the hovercraft wants to go faster in the offending direction move = Movement() move.theta = r if gyro.rate < -280 and r <= 0: self.debugPrint("Gyro Limiter Engaged -\n") move.theta = 0 elif gyro.rate > 600 and r >= 0: self.debugPrint("Gyro Limiter Engaged +\n") move.theta = 0 #Ship message off to thrusterMapping pub = rospy.Publisher('/angularVelocityOut',Movement) move.x = self.movePass.x move.y = self.movePass.y move.lift = self.movePass.lift pub.publish(move) self.debugPrint('VelPID: TargetRate:{:5.3f} Gyro Rate:{:5.3f} ' 'RateDiff:{:5.3f} r:{:5.3f}'.format(targetRate, gyro.rate,self.previousError,r))
def gyroCallback(self, gyro): currentAngle = deep(gyro.angle) #Initialize the target angle to the angle of the #hovercraft when it first turns on (prevents #spinning when the craft is launched) if self.first: self.movement.theta = currentAngle self.initialHeading = currentAngle self.previousAngle = currentAngle self.first = False #Check to see if the magnitude is low. If so, #set the target angle to the current angle (so if #the joy isn't depressed anymore, it stops moving). #Should be hard coded to 1 for any input that didn't #originate from the left joystick. if self.movement.mag < .5: self.movement.theta = self.previousAngle else: self.previousAngle = self.movement.theta targetAngle = self.movement.theta #Determine how the target angle should be affected given theta #(see Movement.msg for details) if self.movement.modType is 'Add': targetAngle = currentAngle + self.movement.theta elif self.movement.modType is 'Set': targetAngle = self.movement.theta elif self.movement.modType is 'Bound': #NOTE: CHECK THIS LOGIC targetAngle = self.movement.theta #Proportional and Derivative computations r = self.P*(targetAngle - currentAngle) r = r + self.D*((targetAngle - currentAngle)-self.previousError) self.previousError = targetAngle - currentAngle #Deadband if math.fabs(targetAngle - currentAngle) < 3: r = 0 self.debugPrint("PosPID: Theta:{:6.2f} TargetAngle:{:6.2f} GyroAngle:{:6.2f} " "Diff: {:6.2f} ModType: {:10s} GyroRate: {:6.2f}".format(self.movement.theta, targetAngle, currentAngle, self.previousError,self.movement.modType,gyro.rate )) #Ship message off to VelocityPID move = Movement() pub = rospy.Publisher('/angularPositionOut',Movement) move.theta = r move.lift = self.movement.lift move.x = self.movement.x move.y = self.movement.y pub.publish(move)