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