Control program for FzeroX controller via USBHID interface.

Dependencies:   Radio USBDevice mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include "MPU6050.h"
00003 #include "USBJoystick.h"
00004 
00005 float sum = 0;
00006 uint32_t sumCount = 0;
00007 
00008 MPU6050 mpu6050;
00009 Timer t;
00010 Serial pc(USBTX, USBRX); 
00011 
00012 PwmOut thruster1(D2);
00013 PwmOut thruster2(D3);
00014 PwmOut vibMotor(D4);
00015 PwmOut onboardRed(LED_RED);
00016 PwmOut onboardGreen(LED_GREEN);
00017 PwmOut onboardBlue(LED_BLUE);
00018 DigitalIn boost(D5);
00019 DigitalIn drift(D6);
00020 
00021 int boostCount = 0;
00022 
00023 //commented out so that we can read from serial for now
00024 USBJoystick joystick;
00025 
00026 //input initializers for joystick
00027 int16_t i = 0;
00028 int16_t throttle = 0;
00029 int16_t rudder = 0;    
00030 float joyX = 0;
00031 float joyY = 0;
00032 int32_t radius = 120;
00033 int32_t angle = 0;
00034 int8_t button = 0;    
00035 int8_t hat = 8;
00036 float x, y;    
00037 
00038 float maxRoll = 45;
00039 float maxPitch = 135;
00040    
00041    
00042 float mapRoll(float IMUpitch, float maxRoll) {
00043     
00044     if (IMUpitch < maxRoll && IMUpitch >= 0) {
00045         x = (IMUpitch*(127/maxRoll));
00046     } else if (IMUpitch > maxRoll) {
00047         x = 127;
00048     } else if (IMUpitch < -maxRoll) {
00049         x = -127;
00050     } else {
00051         x =  (IMUpitch*(127/maxRoll));
00052     }
00053     return x;
00054 }
00055 
00056 float mapPitch(float IMUroll, float maxPitch) {
00057     if (IMUroll > maxPitch && IMUroll <= 180) {
00058         y = ((180 - IMUroll) *(127/(180-maxPitch)));
00059     } else if (IMUroll < maxPitch && IMUroll >=0) {
00060         y = 127;
00061     } else if (IMUroll > -maxPitch && IMUroll < 0) {
00062         y = -127;
00063     } else {
00064         y =  (-(180 - abs(IMUroll)) *(127/(180-maxPitch)));
00065     }
00066     return y;
00067 }
00068         
00069 int main()
00070 {
00071     onboardRed = 0.0f;
00072     onboardGreen = 1.0f;
00073     onboardBlue = 1.0f;
00074     //Set up I2C
00075     i2c.frequency(400000);  // use fast (400 kHz) I2C    
00076     t.start();
00077     boost.mode(PullUp);
00078     drift.mode(PullUp); 
00079     vibMotor = 1.0f;
00080            
00081     joystick.hat(hat);
00082     
00083     // Read the WHO_AM_I register, this is a good test of communication
00084     uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);  // Read WHO_AM_I register for MPU-6050
00085     pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x68\n\r");
00086     
00087     if (whoami == 0x68) // WHO_AM_I should always be 0x68
00088     {  
00089         pc.printf("MPU6050 is online...");
00090         wait(1);
00091             
00092         mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
00093          
00094         pc.printf("x-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[0]); pc.printf("% of factory value \n\r");
00095         pc.printf("y-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[1]); pc.printf("% of factory value \n\r");
00096         pc.printf("z-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[2]); pc.printf("% of factory value \n\r");
00097         pc.printf("x-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[3]); pc.printf("% of factory value \n\r");
00098         pc.printf("y-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[4]); pc.printf("% of factory value \n\r");
00099         pc.printf("z-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[5]); pc.printf("% of factory value \n\r");
00100         
00101         wait(2);
00102 
00103         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) 
00104         {
00105             mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
00106             mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers  
00107             mpu6050.initMPU6050(); pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
00108         
00109             wait(2);
00110         } else {
00111                 pc.printf("Device did not the pass self-test!\n\r");    
00112         } 
00113     } else {
00114         pc.printf("Could not connect to MPU6050: \n\r");
00115         pc.printf("%#x \n",  whoami);
00116         while(1) ; // Loop forever if communication doesn't happen
00117     }
00118     while(1) {
00119   
00120         // If data ready bit set, all data registers have new data
00121         if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {  // check if data ready interrupt
00122             mpu6050.readAccelData(accelCount);  // Read the x/y/z adc values
00123             mpu6050.getAres();
00124         
00125             // Now we'll calculate the accleration value into actual g's
00126             ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
00127             ay = (float)accelCount[1]*aRes - accelBias[1];   
00128             az = (float)accelCount[2]*aRes - accelBias[2];  
00129            
00130             mpu6050.readGyroData(gyroCount);  // Read the x/y/z adc values
00131             mpu6050.getGres();
00132          
00133             // Calculate the gyro value into actual degrees per second
00134             gx = (float)gyroCount[0]*gRes - gyroBias[0];  // get actual gyro value, this depends on scale being set
00135             gy = (float)gyroCount[1]*gRes - gyroBias[1];  
00136             gz = (float)gyroCount[2]*gRes - gyroBias[2];   
00137         
00138             tempCount = mpu6050.readTempData();  // Read the x/y/z adc values
00139             temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
00140         }  
00141    
00142         Now = t.read_us();
00143         deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
00144         lastUpdate = Now;
00145         
00146         sum += deltat;
00147         sumCount++;
00148         
00149         if(lastUpdate - firstUpdate > 10000000.0f) {
00150             beta = 0.04;  // decrease filter gain after stabilized
00151             zeta = 0.015; // increasey bias drift gain after stabilized
00152         }
00153         
00154         // Pass gyro rate as rad/s
00155         mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
00156     
00157         // Serial print and/or display at 0.5 s rate independent of data rates
00158         delt_t = t.read_ms() - count;
00159         if (delt_t > 500) { // update ////////////////////////lcd once per half-second independent of read rate
00160 
00161         /*
00162         pc.printf("ax = %f", 1000*ax); 
00163         pc.printf(" ay = %f", 1000*ay); 
00164         pc.printf(" az = %f  mg\n\r", 1000*az); 
00165     
00166         pc.printf("gx = %f", gx); 
00167         pc.printf(" gy = %f", gy); 
00168         pc.printf(" gz = %f  deg/s\n\r", gz); 
00169         
00170         pc.printf(" temperature = %f  C\n\r", temperature); 
00171         
00172         pc.printf("q0 = %f\n\r", q[0]);
00173         pc.printf("q1 = %f\n\r", q[1]);
00174         pc.printf("q2 = %f\n\r", q[2]);
00175         pc.printf("q3 = %f\n\r", q[3]);  */    
00176     
00177     
00178       // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
00179       // In this coordinate system, the positive z-axis is down toward Earth. 
00180       // 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.
00181       // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
00182       // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
00183       // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
00184       // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
00185       // applied in the correct order which for this configuration is yaw, pitch, and then roll.
00186       // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
00187         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]);   
00188         pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
00189         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]);
00190         pitch *= 180.0f / PI;
00191         yaw   *= 180.0f / PI; 
00192         roll  *= 180.0f / PI;
00193     
00194         //pc.printf("Yaw, Pitch, Roll: \n\r");
00195         //pc.printf("Yaw: %f\n\r", yaw);
00196         //pc.printf(", ");
00197         //pc.printf("Pitch: %f\n\r", pitch);
00198         //pc.printf(", ");
00199         //pc.printf("%f\n\r", roll);
00200         //pc.printf("average rate = "); pc.printf("%f", (sumCount/sum)); pc.printf(" Hz\n\r");
00201     
00202         //pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
00203         //pc.printf("average rate = %f\n\r", (float) sumCount/sum);
00204   
00205     
00206         //Pitch: base = 0, right = +, left = -
00207         //Roll: base = +-180, forward = + count down, back = - count up
00208         
00209         joyX = mapRoll(pitch, maxRoll);
00210         joyY = mapPitch(roll, maxPitch);
00211         pc.printf("joyX: %i, joyY: %i\n\r", (int16_t) joyX, (int16_t) joyY);
00212         
00213         if (!boost && !drift) {
00214             button = 0x03;
00215             boostCount = 75;
00216         } else if (!boost && drift) {
00217             button = 0x01;
00218             boostCount = 75;
00219         } else if (boost && !drift) {
00220             button = 0x02;
00221         } else {
00222             button = 0x00;
00223         }            
00224         
00225         joystick.update(throttle, rudder, (int16_t)joyX, (int16_t)joyY, button, hat);
00226         if ((int16_t) joyY < 0) {
00227             onboardRed = 1.0f;
00228             onboardGreen = 1.0f;
00229             onboardBlue = 0.75f;
00230             thruster1 = 0.25f;
00231             thruster2 = 0.25f;
00232         } else if(lastUpdate - firstUpdate > 10000000.0f){
00233             onboardRed = 1.0f;
00234             onboardGreen = 0.0f;
00235             onboardBlue = 1.0f;
00236             thruster1 = 0;
00237             thruster2 = 0;
00238         }
00239         vibMotor = 0;
00240         if (boostCount != 0) {
00241             thruster1 = 1.0f;
00242             thruster2 = 1.0f;
00243             onboardRed = 1.0f;
00244             onboardGreen = 1.0f;
00245             onboardBlue = 0.0f;
00246             vibMotor = 1;
00247             boostCount--;
00248         }
00249         
00250         
00251         }
00252     }
00253  
00254 }