Alex Tyler / Mbed 2 deprecated FzeroXcontroller

Control program for FzeroX controller via USBHID interface.

Dependencies:   Radio USBDevice mbed

Fork of FzeroXcontroller by Interactive Device Design

main.cpp@1:ec00f549a691, 2014-09-28 (annotated)

Committer:
alexandertyler
Date:
Sun Sep 28 20:11:23 2014 +0000
Revision:
1:ec00f549a691
Parent:
0:9f6d029d0d52
Child:
2:6c9d5fec13e3
Finished minus radio

Who changed what in which revision?

UserRevisionLine numberNew contents of line
alexandertyler 0:9f6d029d0d52 1 #include "mbed.h"
alexandertyler 1:ec00f549a691 2 #include "MPU6050.h"
alexandertyler 1:ec00f549a691 3 #include "USBJoystick.h"
alexandertyler 1:ec00f549a691 4
alexandertyler 1:ec00f549a691 5 float sum = 0;
alexandertyler 1:ec00f549a691 6 uint32_t sumCount = 0;
alexandertyler 1:ec00f549a691 7
alexandertyler 1:ec00f549a691 8 MPU6050 mpu6050;
alexandertyler 1:ec00f549a691 9 Timer t;
alexandertyler 1:ec00f549a691 10 Serial pc(USBTX, USBRX);
alexandertyler 1:ec00f549a691 11
alexandertyler 1:ec00f549a691 12 PwmOut thruster1(D2);
alexandertyler 1:ec00f549a691 13 PwmOut thruster2(D3);
alexandertyler 1:ec00f549a691 14 PwmOut vibMotor(D4);
alexandertyler 1:ec00f549a691 15 PwmOut onboardRed(LED_RED);
alexandertyler 1:ec00f549a691 16 PwmOut onboardGreen(LED_GREEN);
alexandertyler 1:ec00f549a691 17 PwmOut onboardBlue(LED_BLUE);
alexandertyler 1:ec00f549a691 18 DigitalIn boost(D5);
alexandertyler 1:ec00f549a691 19 DigitalIn drift(D6);
alexandertyler 1:ec00f549a691 20
alexandertyler 1:ec00f549a691 21 int boostCount = 0;
alexandertyler 1:ec00f549a691 22
alexandertyler 1:ec00f549a691 23 //commented out so that we can read from serial for now
alexandertyler 1:ec00f549a691 24 USBJoystick joystick;
alexandertyler 1:ec00f549a691 25
alexandertyler 1:ec00f549a691 26 //input initializers for joystick
alexandertyler 1:ec00f549a691 27 int16_t i = 0;
alexandertyler 1:ec00f549a691 28 int16_t throttle = 0;
alexandertyler 1:ec00f549a691 29 int16_t rudder = 0;
alexandertyler 1:ec00f549a691 30 float joyX = 0;
alexandertyler 1:ec00f549a691 31 float joyY = 0;
alexandertyler 1:ec00f549a691 32 int32_t radius = 120;
alexandertyler 1:ec00f549a691 33 int32_t angle = 0;
alexandertyler 1:ec00f549a691 34 int8_t button = 0;
alexandertyler 1:ec00f549a691 35 int8_t hat = 8;
alexandertyler 1:ec00f549a691 36 float x, y;
alexandertyler 0:9f6d029d0d52 37
alexandertyler 1:ec00f549a691 38 float maxRoll = 45;
alexandertyler 1:ec00f549a691 39 float maxPitch = 135;
alexandertyler 1:ec00f549a691 40
alexandertyler 1:ec00f549a691 41
alexandertyler 1:ec00f549a691 42 float mapRoll(float IMUpitch, float maxRoll) {
alexandertyler 1:ec00f549a691 43
alexandertyler 1:ec00f549a691 44 if (IMUpitch < maxRoll && IMUpitch >= 0) {
alexandertyler 1:ec00f549a691 45 x = (IMUpitch*(127/maxRoll));
alexandertyler 1:ec00f549a691 46 } else if (IMUpitch > maxRoll) {
alexandertyler 1:ec00f549a691 47 x = 127;
alexandertyler 1:ec00f549a691 48 } else if (IMUpitch < -maxRoll) {
alexandertyler 1:ec00f549a691 49 x = -127;
alexandertyler 1:ec00f549a691 50 } else {
alexandertyler 1:ec00f549a691 51 x = (IMUpitch*(127/maxRoll));
alexandertyler 1:ec00f549a691 52 }
alexandertyler 1:ec00f549a691 53 return x;
alexandertyler 1:ec00f549a691 54 }
alexandertyler 0:9f6d029d0d52 55
alexandertyler 1:ec00f549a691 56 float mapPitch(float IMUroll, float maxPitch) {
alexandertyler 1:ec00f549a691 57 if (IMUroll > maxPitch && IMUroll <= 180) {
alexandertyler 1:ec00f549a691 58 y = ((180 - IMUroll) *(127/(180-maxPitch)));
alexandertyler 1:ec00f549a691 59 } else if (IMUroll < maxPitch && IMUroll >=0) {
alexandertyler 1:ec00f549a691 60 y = 127;
alexandertyler 1:ec00f549a691 61 } else if (IMUroll > -maxPitch && IMUroll < 0) {
alexandertyler 1:ec00f549a691 62 y = -127;
alexandertyler 1:ec00f549a691 63 } else {
alexandertyler 1:ec00f549a691 64 y = (-(180 - abs(IMUroll)) *(127/(180-maxPitch)));
alexandertyler 1:ec00f549a691 65 }
alexandertyler 1:ec00f549a691 66 return y;
alexandertyler 1:ec00f549a691 67 }
alexandertyler 1:ec00f549a691 68
alexandertyler 1:ec00f549a691 69 int main()
alexandertyler 1:ec00f549a691 70 {
alexandertyler 1:ec00f549a691 71 onboardRed = 0.0f;
alexandertyler 1:ec00f549a691 72 onboardGreen = 1.0f;
alexandertyler 1:ec00f549a691 73 onboardBlue = 1.0f;
alexandertyler 1:ec00f549a691 74 //Set up I2C
alexandertyler 1:ec00f549a691 75 i2c.frequency(400000); // use fast (400 kHz) I2C
alexandertyler 1:ec00f549a691 76 t.start();
alexandertyler 1:ec00f549a691 77 boost.mode(PullUp);
alexandertyler 1:ec00f549a691 78 drift.mode(PullUp);
alexandertyler 1:ec00f549a691 79 vibMotor = 1.0f;
alexandertyler 1:ec00f549a691 80
alexandertyler 1:ec00f549a691 81 joystick.hat(hat);
alexandertyler 1:ec00f549a691 82
alexandertyler 1:ec00f549a691 83 // Read the WHO_AM_I register, this is a good test of communication
alexandertyler 1:ec00f549a691 84 uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050
alexandertyler 1:ec00f549a691 85 pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x68\n\r");
alexandertyler 1:ec00f549a691 86
alexandertyler 1:ec00f549a691 87 if (whoami == 0x68) // WHO_AM_I should always be 0x68
alexandertyler 1:ec00f549a691 88 {
alexandertyler 1:ec00f549a691 89 pc.printf("MPU6050 is online...");
alexandertyler 1:ec00f549a691 90 wait(1);
alexandertyler 1:ec00f549a691 91
alexandertyler 1:ec00f549a691 92 mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
alexandertyler 1:ec00f549a691 93
alexandertyler 1:ec00f549a691 94 pc.printf("x-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[0]); pc.printf("% of factory value \n\r");
alexandertyler 1:ec00f549a691 95 pc.printf("y-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[1]); pc.printf("% of factory value \n\r");
alexandertyler 1:ec00f549a691 96 pc.printf("z-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[2]); pc.printf("% of factory value \n\r");
alexandertyler 1:ec00f549a691 97 pc.printf("x-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[3]); pc.printf("% of factory value \n\r");
alexandertyler 1:ec00f549a691 98 pc.printf("y-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[4]); pc.printf("% of factory value \n\r");
alexandertyler 1:ec00f549a691 99 pc.printf("z-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[5]); pc.printf("% of factory value \n\r");
alexandertyler 1:ec00f549a691 100
alexandertyler 1:ec00f549a691 101 wait(2);
alexandertyler 0:9f6d029d0d52 102
alexandertyler 1:ec00f549a691 103 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)
alexandertyler 1:ec00f549a691 104 {
alexandertyler 1:ec00f549a691 105 mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
alexandertyler 1:ec00f549a691 106 mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
alexandertyler 1:ec00f549a691 107 mpu6050.initMPU6050(); pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
alexandertyler 1:ec00f549a691 108
alexandertyler 1:ec00f549a691 109 wait(2);
alexandertyler 1:ec00f549a691 110 } else {
alexandertyler 1:ec00f549a691 111 pc.printf("Device did not the pass self-test!\n\r");
alexandertyler 1:ec00f549a691 112 }
alexandertyler 1:ec00f549a691 113 } else {
alexandertyler 1:ec00f549a691 114 pc.printf("Could not connect to MPU6050: \n\r");
alexandertyler 1:ec00f549a691 115 pc.printf("%#x \n", whoami);
alexandertyler 1:ec00f549a691 116 while(1) ; // Loop forever if communication doesn't happen
alexandertyler 1:ec00f549a691 117 }
alexandertyler 0:9f6d029d0d52 118 while(1) {
alexandertyler 1:ec00f549a691 119
alexandertyler 1:ec00f549a691 120 // If data ready bit set, all data registers have new data
alexandertyler 1:ec00f549a691 121 if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt
alexandertyler 1:ec00f549a691 122 mpu6050.readAccelData(accelCount); // Read the x/y/z adc values
alexandertyler 1:ec00f549a691 123 mpu6050.getAres();
alexandertyler 1:ec00f549a691 124
alexandertyler 1:ec00f549a691 125 // Now we'll calculate the accleration value into actual g's
alexandertyler 1:ec00f549a691 126 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
alexandertyler 1:ec00f549a691 127 ay = (float)accelCount[1]*aRes - accelBias[1];
alexandertyler 1:ec00f549a691 128 az = (float)accelCount[2]*aRes - accelBias[2];
alexandertyler 1:ec00f549a691 129
alexandertyler 1:ec00f549a691 130 mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values
alexandertyler 1:ec00f549a691 131 mpu6050.getGres();
alexandertyler 1:ec00f549a691 132
alexandertyler 1:ec00f549a691 133 // Calculate the gyro value into actual degrees per second
alexandertyler 1:ec00f549a691 134 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
alexandertyler 1:ec00f549a691 135 gy = (float)gyroCount[1]*gRes - gyroBias[1];
alexandertyler 1:ec00f549a691 136 gz = (float)gyroCount[2]*gRes - gyroBias[2];
alexandertyler 1:ec00f549a691 137
alexandertyler 1:ec00f549a691 138 tempCount = mpu6050.readTempData(); // Read the x/y/z adc values
alexandertyler 1:ec00f549a691 139 temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
alexandertyler 1:ec00f549a691 140 }
alexandertyler 1:ec00f549a691 141
alexandertyler 1:ec00f549a691 142 Now = t.read_us();
alexandertyler 1:ec00f549a691 143 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
alexandertyler 1:ec00f549a691 144 lastUpdate = Now;
alexandertyler 1:ec00f549a691 145
alexandertyler 1:ec00f549a691 146 sum += deltat;
alexandertyler 1:ec00f549a691 147 sumCount++;
alexandertyler 1:ec00f549a691 148
alexandertyler 1:ec00f549a691 149 if(lastUpdate - firstUpdate > 10000000.0f) {
alexandertyler 1:ec00f549a691 150 beta = 0.04; // decrease filter gain after stabilized
alexandertyler 1:ec00f549a691 151 zeta = 0.015; // increasey bias drift gain after stabilized
alexandertyler 1:ec00f549a691 152 }
alexandertyler 1:ec00f549a691 153
alexandertyler 1:ec00f549a691 154 // Pass gyro rate as rad/s
alexandertyler 1:ec00f549a691 155 mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
alexandertyler 1:ec00f549a691 156
alexandertyler 1:ec00f549a691 157 // Serial print and/or display at 0.5 s rate independent of data rates
alexandertyler 1:ec00f549a691 158 delt_t = t.read_ms() - count;
alexandertyler 1:ec00f549a691 159 if (delt_t > 500) { // update ////////////////////////lcd once per half-second independent of read rate
alexandertyler 1:ec00f549a691 160
alexandertyler 1:ec00f549a691 161 /*
alexandertyler 1:ec00f549a691 162 pc.printf("ax = %f", 1000*ax);
alexandertyler 1:ec00f549a691 163 pc.printf(" ay = %f", 1000*ay);
alexandertyler 1:ec00f549a691 164 pc.printf(" az = %f mg\n\r", 1000*az);
alexandertyler 1:ec00f549a691 165
alexandertyler 1:ec00f549a691 166 pc.printf("gx = %f", gx);
alexandertyler 1:ec00f549a691 167 pc.printf(" gy = %f", gy);
alexandertyler 1:ec00f549a691 168 pc.printf(" gz = %f deg/s\n\r", gz);
alexandertyler 1:ec00f549a691 169
alexandertyler 1:ec00f549a691 170 pc.printf(" temperature = %f C\n\r", temperature);
alexandertyler 0:9f6d029d0d52 171
alexandertyler 1:ec00f549a691 172 pc.printf("q0 = %f\n\r", q[0]);
alexandertyler 1:ec00f549a691 173 pc.printf("q1 = %f\n\r", q[1]);
alexandertyler 1:ec00f549a691 174 pc.printf("q2 = %f\n\r", q[2]);
alexandertyler 1:ec00f549a691 175 pc.printf("q3 = %f\n\r", q[3]); */
alexandertyler 1:ec00f549a691 176
alexandertyler 1:ec00f549a691 177
alexandertyler 1:ec00f549a691 178 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
alexandertyler 1:ec00f549a691 179 // In this coordinate system, the positive z-axis is down toward Earth.
alexandertyler 1:ec00f549a691 180 // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
alexandertyler 1:ec00f549a691 181 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
alexandertyler 1:ec00f549a691 182 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
alexandertyler 1:ec00f549a691 183 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
alexandertyler 1:ec00f549a691 184 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
alexandertyler 1:ec00f549a691 185 // applied in the correct order which for this configuration is yaw, pitch, and then roll.
alexandertyler 1:ec00f549a691 186 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
alexandertyler 1:ec00f549a691 187 yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
alexandertyler 1:ec00f549a691 188 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
alexandertyler 1:ec00f549a691 189 roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
alexandertyler 1:ec00f549a691 190 pitch *= 180.0f / PI;
alexandertyler 1:ec00f549a691 191 yaw *= 180.0f / PI;
alexandertyler 1:ec00f549a691 192 roll *= 180.0f / PI;
alexandertyler 1:ec00f549a691 193
alexandertyler 1:ec00f549a691 194 //pc.printf("Yaw, Pitch, Roll: \n\r");
alexandertyler 1:ec00f549a691 195 //pc.printf("Yaw: %f\n\r", yaw);
alexandertyler 1:ec00f549a691 196 //pc.printf(", ");
alexandertyler 1:ec00f549a691 197 //pc.printf("Pitch: %f\n\r", pitch);
alexandertyler 1:ec00f549a691 198 //pc.printf(", ");
alexandertyler 1:ec00f549a691 199 //pc.printf("%f\n\r", roll);
alexandertyler 1:ec00f549a691 200 //pc.printf("average rate = "); pc.printf("%f", (sumCount/sum)); pc.printf(" Hz\n\r");
alexandertyler 1:ec00f549a691 201
alexandertyler 1:ec00f549a691 202 //pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
alexandertyler 1:ec00f549a691 203 //pc.printf("average rate = %f\n\r", (float) sumCount/sum);
alexandertyler 1:ec00f549a691 204
alexandertyler 1:ec00f549a691 205
alexandertyler 1:ec00f549a691 206 //Pitch: base = 0, right = +, left = -
alexandertyler 1:ec00f549a691 207 //Roll: base = +-180, forward = + count down, back = - count up
alexandertyler 1:ec00f549a691 208
alexandertyler 1:ec00f549a691 209 joyX = mapRoll(pitch, maxRoll);
alexandertyler 1:ec00f549a691 210 joyY = mapPitch(roll, maxPitch);
alexandertyler 1:ec00f549a691 211 pc.printf("joyX: %i, joyY: %i\n\r", (int16_t) joyX, (int16_t) joyY);
alexandertyler 1:ec00f549a691 212
alexandertyler 1:ec00f549a691 213 if (!boost && !drift) {
alexandertyler 1:ec00f549a691 214 button = 0x03;
alexandertyler 1:ec00f549a691 215 boostCount = 75;
alexandertyler 1:ec00f549a691 216 } else if (!boost && drift) {
alexandertyler 1:ec00f549a691 217 button = 0x01;
alexandertyler 1:ec00f549a691 218 boostCount = 75;
alexandertyler 1:ec00f549a691 219 } else if (boost && !drift) {
alexandertyler 1:ec00f549a691 220 button = 0x02;
alexandertyler 1:ec00f549a691 221 } else {
alexandertyler 1:ec00f549a691 222 button = 0x00;
alexandertyler 1:ec00f549a691 223 }
alexandertyler 1:ec00f549a691 224
alexandertyler 1:ec00f549a691 225 joystick.update(throttle, rudder, (int16_t)joyX, (int16_t)joyY, button, hat);
alexandertyler 1:ec00f549a691 226 if ((int16_t) joyY < 0) {
alexandertyler 1:ec00f549a691 227 onboardRed = 1.0f;
alexandertyler 1:ec00f549a691 228 onboardGreen = 1.0f;
alexandertyler 1:ec00f549a691 229 onboardBlue = 0.75f;
alexandertyler 1:ec00f549a691 230 thruster1 = 0.25f;
alexandertyler 1:ec00f549a691 231 thruster2 = 0.25f;
alexandertyler 1:ec00f549a691 232 } else if(lastUpdate - firstUpdate > 10000000.0f){
alexandertyler 1:ec00f549a691 233 onboardRed = 1.0f;
alexandertyler 1:ec00f549a691 234 onboardGreen = 0.0f;
alexandertyler 1:ec00f549a691 235 onboardBlue = 1.0f;
alexandertyler 1:ec00f549a691 236 thruster1 = 0;
alexandertyler 1:ec00f549a691 237 thruster2 = 0;
alexandertyler 1:ec00f549a691 238 }
alexandertyler 1:ec00f549a691 239 vibMotor = 0;
alexandertyler 1:ec00f549a691 240 if (boostCount != 0) {
alexandertyler 1:ec00f549a691 241 thruster1 = 1.0f;
alexandertyler 1:ec00f549a691 242 thruster2 = 1.0f;
alexandertyler 1:ec00f549a691 243 onboardRed = 1.0f;
alexandertyler 1:ec00f549a691 244 onboardGreen = 1.0f;
alexandertyler 1:ec00f549a691 245 onboardBlue = 0.0f;
alexandertyler 1:ec00f549a691 246 vibMotor = 1;
alexandertyler 1:ec00f549a691 247 boostCount--;
alexandertyler 1:ec00f549a691 248 }
alexandertyler 1:ec00f549a691 249
alexandertyler 1:ec00f549a691 250
alexandertyler 1:ec00f549a691 251 }
alexandertyler 0:9f6d029d0d52 252 }
alexandertyler 1:ec00f549a691 253
alexandertyler 1:ec00f549a691 254 }