Freescale Quadcopter with Freedom K64F Board
Dependencies: FXAS21000 FXLS8471Q FXOS8700Q MAG3110 MMA8652 MPL3115A2 mbed kalman mbed-dsp
Fork of Freescale_Multi-Sensor_Shield by
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 }
Generated on Wed Jul 27 2022 17:20:37 by 1.7.2