def sensorDataHandler(self,channel,data):
msg = maebot_sensor_data_t.decode(data)
# IMPLEMENT ME
# TASK: PROCESS GYRO DATA
self.prevGyroData = self.currGyroData
self.currGyroData = (msg.gyro[2], msg.utime_sama5)
if self.init_gyro <= 2:
self.init_gyro += 1
def sensorDataHandler(self,channel,data):
msg = maebot_sensor_data_t.decode(data)
# IMPLEMENT ME
# TASK: PROCESS GYRO DATA
self.gyro = (msg.gyro[0]-self.gyro_bias[0],\
msg.gyro[1] - self.gyro_bias[1], msg.gyro[2] - self.gyro_bias[2])
self.gyro_int = msg.gyro_int
self.cur_time = msg.utime
t_step = self.cur_time - self.prev_time
self.prev_time = self.cur_time
self.dTheta_gyro = ((self.gyro_int[2] - self.prev_gyro_int[2] - self.gyro_bias[2]*t_step)/(131*1e6))*3.1415926/180
self.prev_gyro_int = self.gyro_int
def sensorDataHandler(self,channel,data):
msg = maebot_sensor_data_t.decode(data)
self.prevRawGyro = self.currRawGyro
self.currRawGyro = (msg.gyro[0], msg.gyro[1], msg.gyro[2])
self.prevGyro = self.currGyro
if self.counter == 0 :
currGyro = [(msg.gyro_int[0])/(131*1000000)-self.biasGyro[0]-self.zeroGyro[0],
(msg.gyro_int[1])/(131*1000000)-self.biasGyro[1]-self.zeroGyro[1],
(msg.gyro_int[2])/(131*1000000)-self.biasGyro[2]-self.zeroGyro[2],
msg.utime]
#if currGyro[0] > 180 : currGyro[0] = currGyro[0] - 360
#if currGyro[0] <-180 : currGyro[0] = currGyro[0] + 360
#if currGyro[1] > 180 : currGyro[1] = currGyro[1] - 360
#if currGyro[1] <-180 : currGyro[1] = currGyro[1] + 360
#if currGyro[2] > 180 : currGyro[2] = currGyro[2] - 360
#if currGyro[2] <-180 : currGyro[2] = currGyro[2] + 360
self.currGyro = (currGyro[0], currGyro[1], currGyro[2], currGyro[3])
self.counter = 1
self.zeroGyro = self.currGyro
print "Zerogyro: " , self.zeroGyro
else :
currGyro = [(msg.gyro_int[0])/(131*1000000)-self.biasGyro[0]-self.zeroGyro[0],
(msg.gyro_int[1])/(131*1000000)-self.biasGyro[1]-self.zeroGyro[1],
(msg.gyro_int[2])/(131*1000000)-self.biasGyro[2]-self.zeroGyro[2],
msg.utime]
currGyro[2] = currGyro[2]*0.8
if currGyro[2] > 180 :
currGyro[2] = currGyro[2] - 360
if currGyro[2] <-180 :
currGyro[2] = currGyro[2] + 360
self.currGyro = (currGyro[0], currGyro[1], currGyro[2], currGyro[3])
self.biasGyro = (self.biasGyro[0]+(self.currGyro[3]-self.prevGyro[3])*(-362.0)/(131*1000000),
self.biasGyro[1]+(self.currGyro[3]-self.prevGyro[3])*(+100.0)/(131*1000000),
self.biasGyro[2]+(self.currGyro[3]-self.prevGyro[3])*(32.0)/(131*1000000))
def sensorDataHandler(self,channel,data): msg = maebot_sensor_data_t.decode(data) # IMPLEMENT ME # TASK: PROCESS GYRO DATA self.lcm_msg_counter[1] = self.lcm_msg_counter[1] + 1
