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)
