Esempio n. 1
0
    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]})
Esempio n. 2
0
 def get_Encoder(self):
     sensordict = self.read_wheels()
     msg = EncoderMsg()
     msg.lWheelDist = sensordict['left_position']
     msg.rWheelDist = sensordict['right_position']
     self.encoderPub.publish(msg)