Experimental Zumy code for interfacing with encoders (as well as IMU as in abuchan's code base).
Dependencies: MPU6050IMU QEI RPCInterface mbed
Fork of zumy_mbed by
main.cpp@4:b8eeb59b62d4, 2015-11-24 (annotated)
- Committer:
- DavidMcP555
- Date:
- Tue Nov 24 19:53:03 2015 +0000
- Revision:
- 4:b8eeb59b62d4
- Parent:
- 3:8b5700499eb8
Added encoder support for reading and communication to Zumy ROS through RPC
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
MichaelW | 0:78952cd3935b | 1 | #include "mbed.h" |
abuchan | 3:8b5700499eb8 | 2 | #include "SerialRPCInterface.h" |
abuchan | 2:2c0cd1aaae83 | 3 | #include "MPU6050.h" |
DavidMcP555 | 4:b8eeb59b62d4 | 4 | #include "QEI.h" |
abuchan | 2:2c0cd1aaae83 | 5 | |
abuchan | 3:8b5700499eb8 | 6 | SerialRPCInterface SerialRPC(USBTX, USBRX, 115200); |
DavidMcP555 | 4:b8eeb59b62d4 | 7 | //Serial pc(USBTX, USBRX); // tx, rx |
abuchan | 3:8b5700499eb8 | 8 | |
abuchan | 3:8b5700499eb8 | 9 | float accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z; |
DavidMcP555 | 4:b8eeb59b62d4 | 10 | int r_enc, l_enc; |
abuchan | 2:2c0cd1aaae83 | 11 | |
abuchan | 3:8b5700499eb8 | 12 | RPCVariable<float> rpc_accel_x(&accel_x, "accel_x"); |
abuchan | 3:8b5700499eb8 | 13 | RPCVariable<float> rpc_accel_y(&accel_y, "accel_y"); |
abuchan | 3:8b5700499eb8 | 14 | RPCVariable<float> rpc_accel_z(&accel_z, "accel_z"); |
abuchan | 3:8b5700499eb8 | 15 | RPCVariable<float> rpc_gryo_x(&gyro_x, "gyro_x"); |
abuchan | 3:8b5700499eb8 | 16 | RPCVariable<float> rpc_gryo_y(&gyro_y, "gyro_y"); |
abuchan | 3:8b5700499eb8 | 17 | RPCVariable<float> rpc_gryo_z(&gyro_z, "gyro_z"); |
DavidMcP555 | 4:b8eeb59b62d4 | 18 | RPCVariable<int> rpc_r_enc(&r_enc, "r_enc"); |
DavidMcP555 | 4:b8eeb59b62d4 | 19 | RPCVariable<int> rpc_l_enc(&l_enc, "l_enc"); |
DavidMcP555 | 4:b8eeb59b62d4 | 20 | QEI l_wheel (p29, p30, NC, 624); |
DavidMcP555 | 4:b8eeb59b62d4 | 21 | QEI r_wheel (p11, p12, NC, 624); |
abuchan | 2:2c0cd1aaae83 | 22 | |
abuchan | 2:2c0cd1aaae83 | 23 | MPU6050 mpu6050; |
abuchan | 2:2c0cd1aaae83 | 24 | |
abuchan | 2:2c0cd1aaae83 | 25 | DigitalOut init_done(LED1); |
abuchan | 2:2c0cd1aaae83 | 26 | DigitalOut imu_good(LED2); |
abuchan | 2:2c0cd1aaae83 | 27 | DigitalOut main_loop(LED3); |
abuchan | 2:2c0cd1aaae83 | 28 | |
MichaelW | 0:78952cd3935b | 29 | int main() { |
abuchan | 2:2c0cd1aaae83 | 30 | init_done = 0; |
abuchan | 2:2c0cd1aaae83 | 31 | imu_good = 0; |
abuchan | 2:2c0cd1aaae83 | 32 | main_loop = 0; |
abuchan | 2:2c0cd1aaae83 | 33 | |
abuchan | 2:2c0cd1aaae83 | 34 | //Set up I2C |
abuchan | 2:2c0cd1aaae83 | 35 | i2c.frequency(400000); // use fast (400 kHz) I2C |
abuchan | 2:2c0cd1aaae83 | 36 | |
abuchan | 3:8b5700499eb8 | 37 | volatile bool imu_ready = false; |
abuchan | 3:8b5700499eb8 | 38 | |
abuchan | 3:8b5700499eb8 | 39 | wait_ms(100); |
abuchan | 2:2c0cd1aaae83 | 40 | |
abuchan | 2:2c0cd1aaae83 | 41 | uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); |
abuchan | 2:2c0cd1aaae83 | 42 | |
abuchan | 2:2c0cd1aaae83 | 43 | if (whoami == 0x68) // WHO_AM_I should always be 0x68 |
abuchan | 2:2c0cd1aaae83 | 44 | { |
abuchan | 2:2c0cd1aaae83 | 45 | mpu6050.MPU6050SelfTest(SelfTest); |
abuchan | 2:2c0cd1aaae83 | 46 | if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) { |
abuchan | 2:2c0cd1aaae83 | 47 | mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration |
abuchan | 2:2c0cd1aaae83 | 48 | mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers |
abuchan | 2:2c0cd1aaae83 | 49 | mpu6050.initMPU6050(); |
abuchan | 2:2c0cd1aaae83 | 50 | mpu6050.getAres(); |
abuchan | 2:2c0cd1aaae83 | 51 | mpu6050.getGres(); |
abuchan | 2:2c0cd1aaae83 | 52 | imu_ready = true; |
abuchan | 2:2c0cd1aaae83 | 53 | imu_good = 1; |
abuchan | 2:2c0cd1aaae83 | 54 | } |
abuchan | 2:2c0cd1aaae83 | 55 | } |
abuchan | 2:2c0cd1aaae83 | 56 | |
abuchan | 2:2c0cd1aaae83 | 57 | init_done = 1; |
abuchan | 3:8b5700499eb8 | 58 | uint8_t loop_count = 10; |
MichaelW | 0:78952cd3935b | 59 | while(1) { |
abuchan | 3:8b5700499eb8 | 60 | wait_ms(10); |
abuchan | 2:2c0cd1aaae83 | 61 | |
DavidMcP555 | 4:b8eeb59b62d4 | 62 | // Handle the encoders |
DavidMcP555 | 4:b8eeb59b62d4 | 63 | r_enc=r_wheel.getPulses(); |
DavidMcP555 | 4:b8eeb59b62d4 | 64 | l_enc=l_wheel.getPulses(); |
DavidMcP555 | 4:b8eeb59b62d4 | 65 | //pc.printf("Pulses are: %i, %i\r\n", l_enc,r_enc); |
DavidMcP555 | 4:b8eeb59b62d4 | 66 | |
abuchan | 3:8b5700499eb8 | 67 | if (!(--loop_count)) { |
abuchan | 3:8b5700499eb8 | 68 | loop_count = 10; |
abuchan | 3:8b5700499eb8 | 69 | main_loop = !main_loop; |
abuchan | 3:8b5700499eb8 | 70 | } |
abuchan | 2:2c0cd1aaae83 | 71 | |
abuchan | 3:8b5700499eb8 | 72 | if (imu_ready) { |
abuchan | 2:2c0cd1aaae83 | 73 | |
abuchan | 2:2c0cd1aaae83 | 74 | if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt |
abuchan | 2:2c0cd1aaae83 | 75 | mpu6050.readAccelData(accelCount); // Read the x/y/z adc values |
abuchan | 2:2c0cd1aaae83 | 76 | mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values |
abuchan | 2:2c0cd1aaae83 | 77 | |
abuchan | 2:2c0cd1aaae83 | 78 | // Now we'll calculate the accleration value into actual g's |
abuchan | 3:8b5700499eb8 | 79 | accel_x = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set |
abuchan | 3:8b5700499eb8 | 80 | accel_y = (float)accelCount[1]*aRes - accelBias[1]; |
abuchan | 3:8b5700499eb8 | 81 | accel_z = (float)accelCount[2]*aRes - accelBias[2]; |
abuchan | 2:2c0cd1aaae83 | 82 | |
abuchan | 2:2c0cd1aaae83 | 83 | // Calculate the gyro value into actual degrees per second |
abuchan | 3:8b5700499eb8 | 84 | gyro_x = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set |
abuchan | 3:8b5700499eb8 | 85 | gyro_y = (float)gyroCount[1]*gRes - gyroBias[1]; |
abuchan | 3:8b5700499eb8 | 86 | gyro_z = (float)gyroCount[2]*gRes - gyroBias[2]; |
abuchan | 2:2c0cd1aaae83 | 87 | } |
abuchan | 2:2c0cd1aaae83 | 88 | } |
MichaelW | 0:78952cd3935b | 89 | } |
MichaelW | 0:78952cd3935b | 90 | } |