def step(self):
sensordict = self.read_wheels()
#print sensordict
##Encoder##
msg = EncoderMsg()
msg.lWheelDist = sensordict['left_position']
msg.rWheelDist = sensordict['right_position']
self.encoderPub.publish(msg)
##Ping Sensor#
# TODO: get bump sensors, sonars working
# send out wheel error
[leftWheelErr, rightWheelErr]=self.wController.get_error(sensordict['left_position'], sensordict['right_position'])
msg = WheelErrorMsg()
msg.leftWheelError = leftWheelErr
msg.rightWheelError = rightWheelErr
self.wheelErrPub.publish(msg)
##Control##
t= time.time()
motors = self.wController.step(sensordict['left_position'],sensordict['right_position'],t)
self.command_actuators({'left_wheel':motors[0], 'right_wheel':motors[1]})
def get_Encoder(self):
sensordict = self.read_wheels()
msg = EncoderMsg()
msg.lWheelDist = sensordict['left_position']
msg.rWheelDist = sensordict['right_position']
self.encoderPub.publish(msg)