def __init__(self):
self.wheelDiameterMillimeters = 32.0 # diameter of wheels [mm]
self.axleLengthMillimeters = 80.0 # separation of wheels [mm]
self.ticksPerRev = 16.0 # encoder tickers per motor revolution
self.gearRatio = 30.0 # 30:1 gear ratio
self.enc2mm = (pi * self.wheelDiameterMillimeters) / (self.gearRatio * self.ticksPerRev) # encoder ticks to distance [mm]
self.counter=0;
self.prevEncPos = (0,0,0) # store previous readings for odometry
self.prevOdoPos = (0,0,0) # store previous x [mm], y [mm], theta [rad]
self.currEncPos = (0,0,0) # current reading for odometry
self.currOdoPos = (0,0,0) # store odometry x [mm], y [mm], theta [rad]
self.currOdoVel = (0,0,0) # store current velocity dxy [mm], dtheta [rad], dt [s]
self.prevGyro = (0,0,0,0)
self.currGyro = (0,0,0,0)
self.biasGyro = (0,0,0)#(-339, 32, -37)
self.zeroGyro = (0,0,0)
self.prevRawGyro = (0,0,0)
self.currRawGyro = (0,0,0) #Only for calibration
self.sgyro = [0,0,0,0] # for gyro bias calibration
WheeledRobot.__init__(self, self.wheelDiameterMillimeters/2.0, self.axleLengthMillimeters/2.0)
# LCM Initialization and Subscription
self.lc = lcm.LCM()
lcmSensorSub = self.lc.subscribe("MAEBOT_SENSOR_DATA", self.sensorDataHandler)
#self.gyro_calibration()
lcmMotorSub = self.lc.subscribe("MAEBOT_MOTOR_FEEDBACK", self.motorFeedbackHandler) # to read from a channel
def __init__(self):
self.wheelDiameterMillimeters = 32.0 # diameter of wheels [mm]
self.axleLengthMillimeters = 80.0 # separation of wheels [mm]
self.ticksPerRev = 16.0 # encoder tickers per motor revolution
self.gearRatio = 30.0 # 30:1 gear ratio
self.enc2mm = (pi * self.wheelDiameterMillimeters) / (self.gearRatio * self.ticksPerRev) # encoder ticks to distance [mm]
print self.enc2mm
self.prevEncPos = (0,0,0) # store previous readings for odometry
self.prevOdoPos = (0,0,0) # store previous x [mm], y [mm], theta [rad]
self.currEncPos = (0,0,0) # current reading for odometry
self.currOdoPos = (0,0,0) # store odometry x [mm], y [mm], theta [rad]
self.currOdoVel = (0,0,0) # store current velocity dxy [mm], dtheta [rad], dt [s]
self.prevGyroData = (0, 0)
self.currGyroData = (0, 0)
WheeledRobot.__init__(self, self.wheelDiameterMillimeters/2.0, self.axleLengthMillimeters/2.0)
# LCM Initialization and Subscription
self.lc = lcm.LCM()
lcmMotorSub = self.lc.subscribe("MAEBOT_MOTOR_FEEDBACK", self.motorFeedbackHandler)
lcmSensorSub = self.lc.subscribe("MAEBOT_SENSOR_DATA", self.sensorDataHandler)
self.init_odo = False
self.init_gyro = 0
self.gyro_bias = [] # initializing gyro bias
self.cali_time = 10 * 1000000 # calibration time in us
self.init_time = 0
self.c_l = 1.0058
self.c_r = 0.9942
self.e_b = 1.0403
def __init__(self):
self._env = Env()
WheeledRobot.__init__(self, self._env.wheel_radius,
(self._env.robot_size.get_values()[0] * 10) / 2)
self.ticks_per_cycle = self._env.encoder_resolution # 바퀴가 1cycle을 도는데 몇 encoder가 필요한지 (soscon에서 encoder_resolution에 해당)
self._prev_obs = None
self._env.on_observation = self.on_observation
self.stop()
def computeVelocities(self):
if self._prev_obs is None:
raise RuntimeError("[ERROR] No measured not yet")
#0.1초 단위로 lidar센서 정보와 velocites정보를 받겠다.
time.sleep(0.1)
return self.get_lidar(), WheeledRobot.computeVelocities(
self,
time.time() * 1e6, self._prev_obs.encoder.left,
self._prev_obs.encoder.right)
def computeVelocities(self, odometry):
return WheeledRobot.computeVelocities(self, odometry[0], odometry[1], odometry[2])
def __str__(self):
return '<%s ticks_per_cycle=%d>' % (WheeledRobot.__str__(self), self.ticks_per_cycle)
def __init__(self):
WheeledRobot.__init__(self, 77, 165)
self.ticks_per_cycle = 2000
def __str__(self):
return '<%s ticks_per_cycle=%d>' % (WheeledRobot.__str__(self), self.ticks_per_cycle)
def __init__(self):
WheeledRobot.__init__(self, 77, 165)
self.ticks_per_cycle = 2000
def computeVelocities(self, odometry):
return WheeledRobot.computeVelocities(self, odometry[0], odometry[1], odometry[2])
