def getRpm(self):
"""Gets the current rpm of the motors"""
left_rpm = i2c.read_int16(rd.MOTOR_I2C_ADDRESS, rd.MOTOR_LEFT_RPM,
False)
right_rpm = i2c.read_int16(rd.MOTOR_I2C_ADDRESS, rd.MOTOR_RIGHT_RPM,
False)
return left_rpm, right_rpm
def getPwm(self):
"""Gets the current PWM signals send to the motors"""
left_pwm = i2c.read_int16(rd.MOTOR_I2C_ADDRESS, rd.MOTOR_LEFT_PWM,
False)
right_pwm = i2c.read_int16(rd.MOTOR_I2C_ADDRESS, rd.MOTOR_RIGHT_PWM,
False)
return left_pwm, right_pwm
def getMagneticFieldVector(self): #better name?
mag_x_raw = i2c.read_int16(rd.IMU_MAG_I2C_ADDRESS, rd.IMU_MAG_X, False)
mag_y_raw = i2c.read_int16(rd.IMU_MAG_I2C_ADDRESS, rd.IMU_MAG_Y, False)
mag_z_raw = i2c.read_int16(rd.IMU_MAG_I2C_ADDRESS, rd.IMU_MAG_Z, False)
scale_factor = 0.1499 # (4912 / 32760)
mag_x = mag_x_raw * scale_factor
mag_y = mag_y_raw * scale_factor
mag_z = mag_z_raw * scale_factor
return mag_x, mag_y, mag_z
def getLinearAcceleration(self):
"""Returns the processed linear acceleration from the sensor"""
# TODO: check if the return values are actually the angular velocity
# get raw data
acceleration_x_raw = i2c.read_int16(rd.IMU_I2C_ADDRESS,
rd.IMU_ACCELERATION_X, True)
acceleration_y_raw = i2c.read_int16(rd.IMU_I2C_ADDRESS,
rd.IMU_ACCELERATION_Y, True)
acceleration_z_raw = i2c.read_int16(rd.IMU_I2C_ADDRESS,
rd.IMU_ACCELERATION_Z, True)
# scale to get actual value
acceleration_x = acceleration_x_raw / 16384.0
acceleration_y = acceleration_y_raw / 16384.0
acceleration_z = acceleration_z_raw / 16384.0
return acceleration_x, acceleration_y, acceleration_z
def getAngularVelocity(self):
"""Returns the processed angular acceleration from the sensor"""
# TODO: check if the return values are actually the angular velocity
# get raw data
gyroscope_x_raw = i2c.read_int16(rd.IMU_I2C_ADDRESS, rd.IMU_GYRO_X,
True)
gyroscope_y_raw = i2c.read_int16(rd.IMU_I2C_ADDRESS, rd.IMU_GYRO_Y,
True)
gyroscope_z_raw = i2c.read_int16(rd.IMU_I2C_ADDRESS, rd.IMU_GYRO_Z,
True)
# scale to get actual value
gyroscope_x = gyroscope_x_raw / 131.0
gyroscope_y = gyroscope_y_raw / 131.0
gyroscope_z = gyroscope_z_raw / 131.0
return gyroscope_x, gyroscope_y, gyroscope_z
def getPid(self, side):
"""Returns the current PID values for the controller of the specified wheel side"""
p = None
i = None
d = None
if (side == 'left'):
p = i2c.read_int16(rd.MOTOR_I2C_ADDRESS, rd.MOTOR_LEFT_P_VALUE,
False)
i = i2c.read_int16(rd.MOTOR_I2C_ADDRESS, rd.MOTOR_LEFT_I_VALUE,
False)
d = i2c.read_int16(rd.MOTOR_I2C_ADDRESS, rd.MOTOR_LEFT_D_VALUE,
False)
elif (side == 'right'):
p = i2c.read_int16(rd.MOTOR_I2C_ADDRESS, rd.MOTOR_RIGHT_P_VALUE,
False)
i = i2c.read_int16(rd.MOTOR_I2C_ADDRESS, rd.MOTOR_RIGHT_I_VALUE,
False)
d = i2c.read_int16(rd.MOTOR_I2C_ADDRESS, rd.MOTOR_RIGHT_D_VALUE,
False)
else:
p = -1
i = -1
d = -1
return p, i, d