Freescale Quadcopter with Freedom K64F Board

Dependencies:   FXAS21000 FXLS8471Q FXOS8700Q MAG3110 MMA8652 MPL3115A2 mbed kalman mbed-dsp

Fork of Freescale_Multi-Sensor_Shield by Shields

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 /* Copyright (c) 2010-2011 mbed.org, MIT License
00002 *
00003 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
00004 * and associated documentation files (the "Software"), to deal in the Software without
00005 * restriction, including without limitation the rights to use, copy, modify, merge, publish,
00006 * distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
00007 * Software is furnished to do so, subject to the following conditions:
00008 *
00009 * The above copyright notice and this permission notice shall be included in all copies or
00010 * substantial portions of the Software.
00011 *
00012 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
00013 * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
00014 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
00015 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00016 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
00017 */
00018 
00019 #include "mbed.h"
00020 #include "FXOS8700Q.h"
00021 #include "FXAS21000.h"
00022 #include "kalman.c"
00023 #include "arm_math.h"
00024 
00025 #define PI             3.1415926535897932384626433832795f
00026 #define Rad2Dree       57.295779513082320876798154814105f
00027 
00028 #define PID_ROLL_KP        0.0245f              /* Proporcional */ //0.015f
00029 #define PID_ROLL_KI        0.000175f              /* Integral */
00030 #define PID_ROLL_KD        0.0f              /* Derivative */
00031 
00032 #define PID_PITCH_KP       0.0245f              /* Proporcional */ //0.015f
00033 #define PID_PITCH_KI       0.000175f              /* Integral */
00034 #define PID_PITCH_KD       0.0f              /* Derivative */
00035 
00036 #define ROLL_SP            PI/2
00037 #define PITCH_SP           PI/2
00038 
00039 DigitalOut red(LED_RED);
00040 DigitalOut green(LED_GREEN);
00041 
00042 FXOS8700Q_acc combo_acc(A5, A4, FXOS8700CQ_SLAVE_ADDR0);
00043 FXOS8700Q_mag combo_mag(A5, A4, FXOS8700CQ_SLAVE_ADDR0);
00044 FXAS21000 gyro(A5, A4);
00045 
00046 Timer GlobalTime;
00047 Timer ProgramTimer;
00048 
00049 //PwmOut M1(PTB18);
00050 PwmOut M1(D13);
00051 PwmOut M2(D12);
00052 PwmOut M3(D11);
00053 PwmOut M4(D10);
00054 
00055 Serial pc(USBTX, USBRX);
00056 Serial bt(D1,D0);
00057 
00058 AnalogIn   ultra(A0);
00059 
00060 kalman filter_pitch; 
00061 kalman filter_roll;
00062 
00063 float R;
00064 double angle[3];
00065 unsigned long timer;
00066 long loopStartTime;
00067 char i;
00068 char command = ' ';
00069 char sflag = 0;
00070 float high;
00071 
00072 float ESC1 = 0.0006f;   //pitch up
00073 float ESC2 = 0.0006f;   //roll  up
00074 float ESC3 = 0.0006f;   //roll  down
00075 float ESC4 = 0.0006f;   //pitch down
00076 
00077 float roll_error;
00078 float pitch_error;
00079 
00080 float roll;
00081 float pitch;
00082 
00083 void bt_callback(void);
00084 void esc_start(void);
00085 void esc_stop(void);
00086 float esc_control(float current, float pid, float rate);
00087 
00088 int main(void)
00089 {
00090     float gyro_data[3];
00091     MotionSensorDataUnits adata;
00092     MotionSensorDataUnits mdata;
00093     
00094     bt.attach(&bt_callback);
00095     
00096     printf("\r\nStarting\r\n\r\n");
00097     
00098     arm_pid_instance_f32 RPID;
00099     arm_pid_instance_f32 PPID;
00100   
00101       //Pitch
00102     PPID.Kp = PID_PITCH_KP/1000.0f;        /* Proporcional */
00103     PPID.Ki = PID_PITCH_KI/1000.0f;        /* Integral */
00104     PPID.Kd = PID_PITCH_KD/1000.0f;        /* Derivative */
00105     
00106     //Roll
00107     RPID.Kp = PID_ROLL_KP/1000.0f;        /* Proporcional */
00108     RPID.Ki = PID_ROLL_KI/1000.0f;        /* Integral */
00109     RPID.Kd = PID_ROLL_KD/1000.0f;        /* Derivative */
00110     
00111     arm_pid_init_f32(&RPID, 1);
00112     arm_pid_init_f32(&PPID, 1);
00113     
00114     red = 0; green= 1;
00115     GlobalTime.start();
00116     
00117     M1.period(0.02f);               //Comparten el mismo timer
00118     M1.pulsewidth(ESC1);
00119     M2.pulsewidth(ESC2);
00120     M3.pulsewidth(ESC3);
00121     M4.pulsewidth(ESC4);
00122     
00123     combo_acc.enable();
00124     combo_mag.enable();
00125     printf("FXOS8700 Combo = %X\r\n", combo_acc.whoAmI());
00126     printf("FXAS21000 Gyro = %X\r\n", gyro.getWhoAmI());
00127     
00128     kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); 
00129     kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
00130     
00131     esc_start();
00132     wait(2.0f);
00133     red = 1; green= 0;
00134     
00135     ProgramTimer.start();
00136     loopStartTime = ProgramTimer.read_us();
00137     timer = loopStartTime;
00138     
00139     while(1) {
00140         
00141         high = (float)(ultra.read_u16()*2.75f/512.0f)*2.54f;
00142         
00143         combo_acc.getAxis(adata);
00144         combo_mag.getAxis(mdata);
00145         gyro.ReadXYZ(gyro_data);
00146         
00147         R = sqrt(std::pow(adata.x, 2) + std::pow(adata.y, 2) + std::pow(adata.z, 2));
00148         
00149         kalman_predict(&filter_pitch, gyro_data[0],  (ProgramTimer.read_us() - timer)); 
00150         kalman_update(&filter_pitch, acos(adata.x/R));
00151         kalman_predict(&filter_roll, gyro_data[1],  (ProgramTimer.read_us() - timer)); 
00152         kalman_update(&filter_roll, acos(adata.y/R));
00153         
00154         angle[0] = kalman_get_angle(&filter_pitch);
00155         angle[1] = kalman_get_angle(&filter_roll);
00156         
00157         if (angle[0]>PI) angle[0] = PI;
00158         else if (angle[0]<0) angle[0] = 0.0f;
00159         else angle[0] += 0.0f;
00160         
00161         if (angle[1]>PI) angle[1] = PI;
00162         else if (angle[1]<0) angle[1] = 0.0f;
00163         else angle[1] += 0.0f;
00164         
00165         pitch_error = angle[0] - PITCH_SP;
00166         roll_error = angle[1] - ROLL_SP;
00167         
00168         pitch = arm_pid_f32(&PPID, pitch_error);
00169         roll = arm_pid_f32(&RPID, roll_error);
00170         
00171         timer = ProgramTimer.read_us();
00172         
00173         if (!sflag){
00174             
00175             ESC1 = esc_control(ESC1,-pitch,0.0f);
00176             ESC2 = esc_control(ESC2,-roll,0.0f);
00177             ESC3 = esc_control(ESC3,roll,0.0f);
00178             ESC4 = esc_control(ESC4,pitch,0.0f);
00179             
00180             M1.pulsewidth(ESC1);
00181             M2.pulsewidth(ESC2); 
00182             M3.pulsewidth(ESC3); 
00183             M4.pulsewidth(ESC4);
00184         }
00185     
00186         //printf("FXOS8700 Acc:   X:%6.3f Y:%6.3f Z:%6.3f\r\n", adata.x, adata.y, adata.z);
00187         //printf("FXOS8700 Mag:   X:%6.2f Y:%6.2f Z:%6.2f\r\n", mdata.x, mdata.y, mdata.z);
00188         //printf("FXAS21000 Gyro: X:%6.2f Y:%6.2f Z:%6.2f\r\n", gyro_data[0], gyro_data[1], gyro_data[2]);
00189         bt.printf("Roll Angle X: %.6f   Pitch Angle Y: %.6f \r\n", Rad2Dree * angle[1], Rad2Dree * angle[0]);
00190         bt.printf("roll = %.6f     pitch = %.6f \r\n",roll,pitch);
00191         bt.printf("ESC1 = %.6f     ESC4 = %.6f \r\n", ESC1,ESC4);
00192         bt.printf("ESC2 = %.6f     ESC3 = %.6f \r\n", ESC2,ESC3);
00193         //printf("dist = %.2f \r\n",dist);
00194         
00195         wait(0.02f);
00196     }
00197 }
00198 
00199 void bt_callback(void) {
00200     // Note: you need to actually read from the serial to clear the RX interrupt
00201     command = bt.getc();
00202     //if (command == 'z') bt.printf("start\n\r");
00203     if (command == 'x') {
00204         sflag = 1;
00205         bt.printf("stop\n\r"); 
00206         esc_stop();
00207     }
00208     else if (command == 'w') bt.printf("up\n\r"); 
00209     else if (command == 's') bt.printf("down\n\r"); 
00210     else if (command == 'h') bt.printf("dist = %.2f \r\n",high);
00211     else bt.printf("%c\n\r", command);
00212 }
00213 
00214 float esc_control(float current, float pid, float rate){
00215     if ((current + pid + rate)>0.0014f) return 0.0014f;
00216     else if ((current + pid + rate)<0.001f) return 0.001f; 
00217     else return current + pid + rate;
00218 }
00219 
00220 void esc_start(void) {
00221     sflag = 0;
00222     ESC1 = 0.0006f;
00223     ESC2 = 0.0006f;
00224     ESC3 = 0.0006f;
00225     ESC4 = 0.0006f;
00226     for (i = 0; i < 4; i++){
00227         ESC1 += 0.0001f;
00228         ESC2 += 0.0001f;
00229         ESC3 += 0.0001f;
00230         ESC4 += 0.0001f;
00231         M1.pulsewidth(ESC1);
00232         M2.pulsewidth(ESC2);
00233         M3.pulsewidth(ESC3);
00234         M4.pulsewidth(ESC4);
00235         wait_ms(1000);
00236     }
00237 }
00238 
00239 void esc_stop(void){
00240     red = 0; green= 1; sflag = 1;
00241     while ((ESC1 > 0.0006f)||(ESC2 > 0.0006f)||(ESC3 > 0.0006f)||(ESC4 > 0.0006f)){
00242         ESC1 -= 0.0001f;
00243         ESC2 -= 0.0001f;
00244         ESC3 -= 0.0001f;
00245         ESC4 -= 0.0001f;
00246         M1.pulsewidth(ESC1);
00247         M2.pulsewidth(ESC2);
00248         M3.pulsewidth(ESC3);
00249         M4.pulsewidth(ESC4);
00250         wait_ms(250);   
00251     }
00252     if (ESC1 < 0.0006f) ESC1 = 0.0006f;
00253     if (ESC2 < 0.0006f) ESC2 = 0.0006f;
00254     if (ESC3 < 0.0006f) ESC3 = 0.0006f;
00255     if (ESC4 < 0.0006f) ESC4 = 0.0006f;
00256 }