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