+ def set_motor_handicap(self, front, aft): # percent
+ i2c_write_reg(0x50, 0x94, struct.pack(">bb", front, aft))
+
+ def imuReceived(self, msg):
+ (roll, pitch, yaw) = tf.transformations.euler_from_quaternion(msg.orientation.__getstate__())
+ if pitch > 30*pi/180:
+ val = (100/90)*abs(pitch)*180/pi
+ print "aft handicap", val
+ self.set_motor_handicap(0, val)
+ elif pitch < -30*pi/180:
+ val = (100/90)*abs(pitch)*180/pi
+ print "front handicap", val
+ self.set_motor_handicap(val, 0)
+ else:
+ self.set_motor_handicap(0, 0)
+