def visualServo(self):
input = self.blockHeading
P = 1.0
D = 0.1
DMax = 0.4 #Maximum of differential correction. Counter Dirac Changes
RotationMax = 0.4 #Maximum rotation command
MotorMax = 0.5
output = P * input
if self.pastInput != None and abs(input-self.pastInput) < DMax:
#Able to add D term to controller
output += D * (input - self.pastInput)
self.pastInput = input
#Transform output to equivalent L&R motor value
forward = 0.3
rotate = max(min(RotationMax, output),-RotationMax)
motormsg = MotionVoltMsg()
motormsg.leftVoltage = max(min((forward - rotate),MotorMax),-MotorMax)
motormsg.rightVoltage = max(min((forward + rotate),MotorMax),-MotorMax)
#print motormsg.leftVoltage, motormsg.rightVoltage
self.wheelPub.publish(motormsg)
time.sleep(0.03) #TODO adjust for the encoder loop
if self.capture:
self.captureTimer = time.time()+10
self.changeStates()
def dispenseForward(self):
msg = MotionVoltMsg()
msg.rightVoltage = .5
msg.leftVoltage = .5
self.wheelPub.publish(msg)
return
def spinTurn(self):
msg = MotionVoltMsg()
msg.rightVoltage = -.5
msg.leftVoltage = .5
self.wheelPub.publish(msg)
return
def backUpBoth(self):
msg = MotionVoltMsg()
msg.rightVoltage = -.4
msg.leftVoltage = -.4
self.wheelPub.publish(msg)
return
def backUpRight(self):
msg = MotionVoltMsg()
msg.rightVoltage = -.3
msg.leftVoltage = -.5
self.wheelPub.publish(msg)
return
def wanderForward(self):
msg = MotionVoltMsg()
msg.rightVoltage = .31
msg.leftVoltage = .3
self.wheelPub.publish(msg)
return
def stopWheels(self):
msg = MotionVoltMsg()
msg.rightVoltage = 0
msg.leftVoltage = 0
self.wheelPub.publish(msg)
return
