Zumy code for sending struct data
Dependencies: MPU6050IMU QEI RPCInterface mbed
Fork of zumy_mbed by
main.cpp@8:1e47a6b49537, 2016-02-17 (annotated)
- Committer:
- jlamyi
- Date:
- Wed Feb 17 03:45:51 2016 +0000
- Revision:
- 8:1e47a6b49537
- Parent:
- 7:50b807559e1a
- Child:
- 9:6bff048c3b49
added reset RPCFunction
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 | |
jlamyi | 8:1e47a6b49537 | 6 | extern "C" void mbed_reset(); |
jlamyi | 6:18183b8b08f1 | 7 | |
abuchan | 3:8b5700499eb8 | 8 | SerialRPCInterface SerialRPC(USBTX, USBRX, 115200); |
jlamyi | 8:1e47a6b49537 | 9 | //Serial pc(USBTX, USBRX); // tx, rx |
jlamyi | 7:50b807559e1a | 10 | void GetSensorData(char* input, char* output); |
jlamyi | 7:50b807559e1a | 11 | RPCFunction gsd(&GetSensorData, "gsd"); |
jlamyi | 8:1e47a6b49537 | 12 | void Reset(char* input, char* output); |
jlamyi | 8:1e47a6b49537 | 13 | RPCFunction rst(&Reset, "rst"); |
abuchan | 3:8b5700499eb8 | 14 | |
jlamyi | 8:1e47a6b49537 | 15 | float accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z; |
jlamyi | 8:1e47a6b49537 | 16 | int r_enc, l_enc; |
jlamyi | 6:18183b8b08f1 | 17 | |
DavidMcP555 | 4:b8eeb59b62d4 | 18 | QEI l_wheel (p29, p30, NC, 624); |
DavidMcP555 | 4:b8eeb59b62d4 | 19 | QEI r_wheel (p11, p12, NC, 624); |
abuchan | 2:2c0cd1aaae83 | 20 | |
abuchan | 2:2c0cd1aaae83 | 21 | MPU6050 mpu6050; |
abuchan | 2:2c0cd1aaae83 | 22 | |
abuchan | 2:2c0cd1aaae83 | 23 | DigitalOut init_done(LED1); |
abuchan | 2:2c0cd1aaae83 | 24 | DigitalOut imu_good(LED2); |
abuchan | 2:2c0cd1aaae83 | 25 | DigitalOut main_loop(LED3); |
abuchan | 2:2c0cd1aaae83 | 26 | |
MichaelW | 0:78952cd3935b | 27 | int main() { |
abuchan | 2:2c0cd1aaae83 | 28 | init_done = 0; |
abuchan | 2:2c0cd1aaae83 | 29 | imu_good = 0; |
abuchan | 2:2c0cd1aaae83 | 30 | main_loop = 0; |
abuchan | 2:2c0cd1aaae83 | 31 | |
abuchan | 2:2c0cd1aaae83 | 32 | //Set up I2C |
abuchan | 2:2c0cd1aaae83 | 33 | i2c.frequency(400000); // use fast (400 kHz) I2C |
abuchan | 2:2c0cd1aaae83 | 34 | |
abuchan | 3:8b5700499eb8 | 35 | volatile bool imu_ready = false; |
abuchan | 3:8b5700499eb8 | 36 | |
abuchan | 3:8b5700499eb8 | 37 | wait_ms(100); |
abuchan | 2:2c0cd1aaae83 | 38 | |
abuchan | 2:2c0cd1aaae83 | 39 | uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); |
abuchan | 2:2c0cd1aaae83 | 40 | |
abuchan | 2:2c0cd1aaae83 | 41 | if (whoami == 0x68) // WHO_AM_I should always be 0x68 |
abuchan | 2:2c0cd1aaae83 | 42 | { |
abuchan | 2:2c0cd1aaae83 | 43 | mpu6050.MPU6050SelfTest(SelfTest); |
abuchan | 2:2c0cd1aaae83 | 44 | 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 | 45 | mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration |
abuchan | 2:2c0cd1aaae83 | 46 | mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers |
abuchan | 2:2c0cd1aaae83 | 47 | mpu6050.initMPU6050(); |
abuchan | 2:2c0cd1aaae83 | 48 | mpu6050.getAres(); |
abuchan | 2:2c0cd1aaae83 | 49 | mpu6050.getGres(); |
abuchan | 2:2c0cd1aaae83 | 50 | imu_ready = true; |
abuchan | 2:2c0cd1aaae83 | 51 | imu_good = 1; |
abuchan | 2:2c0cd1aaae83 | 52 | } |
abuchan | 2:2c0cd1aaae83 | 53 | } |
abuchan | 2:2c0cd1aaae83 | 54 | |
abuchan | 2:2c0cd1aaae83 | 55 | init_done = 1; |
abuchan | 3:8b5700499eb8 | 56 | uint8_t loop_count = 10; |
MichaelW | 0:78952cd3935b | 57 | while(1) { |
abuchan | 3:8b5700499eb8 | 58 | wait_ms(10); |
abuchan | 2:2c0cd1aaae83 | 59 | |
DavidMcP555 | 4:b8eeb59b62d4 | 60 | // Handle the encoders |
jlamyi | 8:1e47a6b49537 | 61 | r_enc = r_wheel.getPulses(); |
jlamyi | 8:1e47a6b49537 | 62 | l_enc = l_wheel.getPulses(); |
DavidMcP555 | 4:b8eeb59b62d4 | 63 | //pc.printf("Pulses are: %i, %i\r\n", l_enc,r_enc); |
DavidMcP555 | 4:b8eeb59b62d4 | 64 | |
abuchan | 3:8b5700499eb8 | 65 | if (!(--loop_count)) { |
abuchan | 3:8b5700499eb8 | 66 | loop_count = 10; |
abuchan | 3:8b5700499eb8 | 67 | main_loop = !main_loop; |
abuchan | 3:8b5700499eb8 | 68 | } |
abuchan | 2:2c0cd1aaae83 | 69 | |
abuchan | 3:8b5700499eb8 | 70 | if (imu_ready) { |
abuchan | 2:2c0cd1aaae83 | 71 | |
abuchan | 2:2c0cd1aaae83 | 72 | if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt |
abuchan | 2:2c0cd1aaae83 | 73 | mpu6050.readAccelData(accelCount); // Read the x/y/z adc values |
abuchan | 2:2c0cd1aaae83 | 74 | mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values |
abuchan | 2:2c0cd1aaae83 | 75 | |
abuchan | 2:2c0cd1aaae83 | 76 | // Now we'll calculate the accleration value into actual g's |
jlamyi | 8:1e47a6b49537 | 77 | accel_x = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set |
jlamyi | 8:1e47a6b49537 | 78 | accel_y = (float)accelCount[1]*aRes - accelBias[1]; |
jlamyi | 8:1e47a6b49537 | 79 | accel_z = (float)accelCount[2]*aRes - accelBias[2]; |
jlamyi | 6:18183b8b08f1 | 80 | |
abuchan | 2:2c0cd1aaae83 | 81 | // Calculate the gyro value into actual degrees per second |
jlamyi | 8:1e47a6b49537 | 82 | gyro_x = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set |
jlamyi | 8:1e47a6b49537 | 83 | gyro_y = (float)gyroCount[1]*gRes - gyroBias[1]; |
jlamyi | 8:1e47a6b49537 | 84 | gyro_z = (float)gyroCount[2]*gRes - gyroBias[2]; |
abuchan | 2:2c0cd1aaae83 | 85 | } |
abuchan | 2:2c0cd1aaae83 | 86 | } |
MichaelW | 0:78952cd3935b | 87 | } |
MichaelW | 0:78952cd3935b | 88 | } |
jlamyi | 6:18183b8b08f1 | 89 | |
jlamyi | 6:18183b8b08f1 | 90 | void GetSensorData(char* input, char* output){ |
jlamyi | 8:1e47a6b49537 | 91 | sprintf(output, "%f,%f,%f,%f,%f,%f,%i,%i", accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z, l_enc, r_enc); |
jlamyi | 6:18183b8b08f1 | 92 | } |
jlamyi | 8:1e47a6b49537 | 93 | |
jlamyi | 8:1e47a6b49537 | 94 | void Reset(char* input, char* output){ |
jlamyi | 8:1e47a6b49537 | 95 | mbed_reset(); |
jlamyi | 8:1e47a6b49537 | 96 | } |