def motorFeedbackHandler(self,channel,data): msg = maebot_motor_feedback_t.decode(data) # IMPLEMENT ME # TASK: PROCESS ENCODER DATA # get encoder positions and store them in robot, # update robots position and velocity estimate self.lcm_msg_counter[0] = self.lcm_msg_counter[0] + 1
def motorFeedbackHandler(self,channel,data):
msg = maebot_motor_feedback_t.decode(data)
#Save where we are now
lDist = msg.encoder_left_ticks * self.enc2mm
rDist = msg.encoder_right_ticks * self.enc2mm
self.curDistance = (lDist, rDist)
self.leftCtrl.input = lDist
self.rightCtrl.input = rDist
self.startTime = msg.utime_sama5 / 1000000.0
if not self.init_pos:
self.oldTime = self.startTime
self.leftCtrl.setpoint = self.leftCtrl.input
self.rightCtrl.setpoint = self.rightCtrl.input
self.leftCtrl.prevInput = lDist #added by Kevin
self.rightCtrl.prevInput = rDist #added by Kevin
self.init_pos = True
else:
self.leftCtrl.updateRate = self.startTime - self.oldTime
self.rightCtrl.updateRate = self.startTime - self.oldTime
self.leftCtrl.Compute()
self.rightCtrl.Compute()
self.oldTime = self.startTime
def motorFeedbackHandler(self,channel,data):
msg = maebot_motor_feedback_t.decode(data)
self.prevEncPos = self.currEncPos
self.currEncPos = (msg.encoder_left_ticks, msg.encoder_right_ticks, msg.utime)
if self.prevEncPos == (0,0,0):
return
self.calcOdoPosition()
self.calcVelocities()
def motorFeedbackHandler(self,channel,data): # IMPLEMENT ME msg = maebot_motor_feedback_t.decode(data) self.prevEncPos = self.currEncPos self.currEncPos = (msg.encoder_left_ticks, msg.encoder_right_ticks, msg.utime) #print self.currEncPos self.calcOdoPosition() self.calcVelocities()
def motorFeedbackHandler(self,channel,data):
# IMPLEMENT ME
msg = maebot_motor_feedback_t.decode(data)
self.currEncPos = (msg.encoder_left_ticks, msg.encoder_right_ticks, msg.utime)
self.L_pos = self.enc2mm*(self.currEncPos[0] - self.initEncPos[0])
self.R_pos = self.enc2mm*(self.currEncPos[1] - self.initEncPos[1])
if(self.enc_reset_flag):
print 'currEnc', self.currEncPos
print 'initEnc', self.initEncPos
self.enc_reset_flag = 0
def motorFeedbackHandler(self,channel,data):
msg = maebot_motor_feedback_t.decode(data)
self.prevEncPos = self.currEncPos
self.currEncPos = (msg.encoder_left_ticks, msg.encoder_right_ticks, msg.utime)
if self.prevEncPos == (0,0,0):
return
self.prevWheelPos = self.currWheelPos
self.currWheelPos = (self.currWheelPos[0] + self.enc2mm*(self.currEncPos[0]-self.prevEncPos[0]),
self.currWheelPos[1] + self.enc2mm*(self.currEncPos[1]-self.prevEncPos[1]),
self.currEncPos[2])
def motorFeedbackHandler(self,channel,data):
msg = maebot_motor_feedback_t.decode(data)
# IMPLEMENT ME
# TASK: PROCESS ENCODER DATA
# get encoder positions and store them in robot,
# update robots position and velocity estimate
self.prevEncPos = self.currEncPos
self.currEncPos = (msg.encoder_left_ticks,
msg.encoder_right_ticks,
msg.utime_sama5)
if self.init_odo and self.init_gyro > 2:
self.calcVelocities()
self.calcOdoPosition()
else:
self.init_odo = True
def motorFeedbackHandler(self,channel,data):
msg = maebot_motor_feedback_t.decode(data)
# IMPLEMENT ME
self.leftCtrl.input = msg.encoder_left_ticks * self.enc2mm
self.rightCtrl.input = msg.encoder_right_ticks * self.enc2mm
self.startTime = msg.utime_sama5 / 1000000.0
if not self.init_pos:
self.oldTime = self.startTime
self.leftCtrl.setpoint = self.leftCtrl.input
self.rightCtrl.setpoint = self.rightCtrl.input
self.init_pos = True
else:
self.leftCtrl.updateRate = self.startTime - self.oldTime
self.rightCtrl.updateRate = self.startTime - self.oldTime
self.leftCtrl.Compute()
self.rightCtrl.Compute()
self.oldTime = self.startTime
