main.cpp

00001 #include "mbed.h"
00002 #include "BMA180.h"
00003 #include "math.h"
00004 #include "L3G4200D.h"
00005 #include "AX12.h"
00006 
00007 Serial pc(USBTX, USBRX);
00008 L3G4200D gyro(p9,p10);
00009 BMA180 my_BMA180(p5,p6,p7,p15,p16);
00010 
00011 AX12 m1(p28,p27,1);
00012 AX12 m2(p28,p27,2);
00013 AX12 m3(p28,p27,3);
00014 AX12 m4(p28,p27,4);
00015 AX12 m5(p28,p27,5);
00016 AX12 m6(p28,p27,6);
00017 AX12 m7(p28,p27,7);
00018 AX12 m8(p28,p27,8);
00019 AX12 m9(p28,p27,9);
00020 AX12 m10(p28,p27,10);
00021 
00022 float l_theta_k;
00023 float l_theta_h;
00024 float l_theta_a_x;
00025 float l_theta_a_x_rad;
00026 float l_theta_7_x;
00027 float l_theta_8_x;
00028 float l_theta_a_y;
00029 float l_theta_a_y_rad;
00030 float l_theta_7_y;
00031 float l_theta_8_y;
00032 float l_theta_8_r_y;
00033 float l_theta_5;
00034 float l_theta_7_m;
00035 float l_theta_8_m;
00036 float l_theta_6;
00037 float h_l;
00038 float r_theta_k;
00039 float r_theta_h;
00040 float r_theta_a_x;
00041 float r_theta_a_x_rad;
00042 float r_theta_2_x;
00043 float r_theta_1_x;
00044 float r_theta_a_y;
00045 float r_theta_a_y_rad;
00046 float r_theta_2_y;
00047 float r_theta_1_y;
00048 float r_theta_1_r_y;
00049 float r_theta_4;
00050 float r_theta_2_m;
00051 float r_theta_1_m;
00052 float r_theta_3;
00053 float h_r;
00054 float angle_read;
00055 float angle_read_rad;
00056 float speed_setpoint;
00057 float speed_error;
00058 float pos_setpoint;
00059 float pos_error;
00060 float kp;
00061 float kd;
00062 float angle_x;
00063 float angle_x_a;
00064 float angle_y;
00065 float angle_y_a;
00066 float r1;
00067 float r2;
00068 float pi = 3.14159;
00069 float speed_x;
00070 float speed_y;
00071 float g[3];
00072 float output_angle_x;
00073 float output_angle_y;
00074 int output_angle_d_x;
00075 int output_angle_d_y;
00076 float force_mag; 
00077 float control_action;
00078 
00079 
00080 
00081 int main()
00082 {
00083     m5.SetGoal(150);
00084     wait(1);
00085     m6.SetGoal(150);
00086     wait(1);
00087     m7.SetGoal(150);
00088     wait(1);
00089     m8.SetGoal(150);
00090     wait(1);
00091     m1.SetGoal(150);
00092     wait(1);
00093     m2.SetGoal(150);
00094     wait(1);
00095     m3.SetGoal(150);
00096     wait(1);
00097     m4.SetGoal(150);
00098     wait(1);
00099     m9.SetGoal(150);
00100     wait(1);
00101     m10.SetGoal(150);
00102     wait(1);
00103     h_l = 0;
00104     h_r = 0;
00105 
00106     angle_x = 0;
00107     angle_y = 0;
00108     speed_setpoint = 0;
00109     pos_setpoint = -3;
00110 
00111     kd = 0;
00112     kp = 2;
00113 
00114     while(1) {
00115         gyro.read(g);                                  // Read and Interpret Gyro Data
00116         speed_x = ((g[1])*0.07) - 1.12;
00117         speed_y = ((g[0])*0.07) - 0.595;
00118 
00119         int x_msb, y_msb, z_msb;
00120         char x_lsb, y_lsb, z_lsb;
00121         short ax, ay, az;
00122         float afx, afy, afz;
00123 
00124         x_lsb = my_BMA180.readReg(ACCXLSB);                              // Read X LSB register
00125         x_msb = my_BMA180.readReg(ACCXMSB);                              // Read X MSB register
00126         ax = (x_msb << 8) | x_lsb;                             // Concatinate X MSB and LSB
00127         ax = ax >> 2;                                          // Remove unused first 2 LSB (16 bits to 14 bits)
00128         afx = (float)ax*3/16384;
00129 
00130         y_lsb = my_BMA180.readReg(ACCYLSB);                              // Read Y LSB register
00131         y_msb = my_BMA180.readReg(ACCYMSB);                              // Read Y MSB register
00132         ay = (y_msb << 8) | y_lsb;                             // Concatinate Y MSB and LSB
00133         ay = ay >> 2;                                          // Remove unused first 2 LSB
00134         afy = (float)ay*3/16384;
00135 
00136         z_lsb = my_BMA180.readReg(ACCZLSB);                              // Read Z LSB register
00137         z_msb = my_BMA180.readReg(ACCZMSB);                              // Read Z MSB register
00138         az = (z_msb << 8) | z_lsb;                             // Concatinate Z MSB and LSB
00139         az = az >> 2;                                          // Remove unused first 2 LSB
00140         afz = (float)az*3/16384;
00141 
00142         r1 =   afx / (sqrt(pow(afy,2) + pow(afz,2)));                // Determine X component of gravity force
00143         r2 =   afy / (sqrt(pow(afx,2) + pow(afz,2)));                // Determine Y component of gravity force
00144         angle_x_a = atan(r1)*180/pi;                                 // Determine X, Y angles
00145         angle_y_a = atan(r2)*180/pi;
00146 
00147         force_mag = abs(az) + abs(ay) + abs(ax);                     // Check force magnitude to reduce effects of shocks/vibration
00148 
00149 
00150         angle_x = 0.98*(angle_x + speed_x*0.005) + 0.02*angle_x_a;
00151         output_angle_x = angle_x;
00152         output_angle_d_x = output_angle_x;
00153 
00154         angle_y = 0.98*(angle_y + speed_y*0.005) + 0.02*angle_y_a;
00155         output_angle_y = angle_y;
00156         output_angle_d_y = output_angle_y;
00157         
00158         pos_error = pos_setpoint - output_angle_x;
00159         speed_error = speed_setpoint - speed_x;
00160         
00161         control_action = kp*pos_error + kd*speed_error;
00162         if(control_action > 8){
00163             control_action = 8;
00164         }else if(control_action < -8){
00165             control_action = -8;
00166         }else{
00167             control_action = control_action;
00168         } 
00169         angle_read = control_action;
00170         angle_read_rad = angle_read*(3.14159/180);
00171         
00172         if(angle_read == 0){
00173             h_l = 0;
00174             h_r = 0;
00175         }else if(angle_read > 0){
00176             h_l = 79*tan(angle_read_rad);
00177             h_r = 0;
00178         }else if(angle_read < 0){
00179             angle_read_rad = abs(angle_read_rad);
00180             h_l = 0;
00181             h_r = 79*tan(angle_read_rad);
00182         }
00183         l_theta_h = ((180/3.14159)*acos(1-(h_l/204)));
00184         l_theta_k = 2*l_theta_h;
00185         l_theta_5 = 150 + l_theta_h;
00186         l_theta_6 = 150 + l_theta_k;
00187         
00188         r_theta_h = ((180/3.14159)*acos(1-(h_r/204)));
00189         r_theta_k = 2*r_theta_h;
00190         r_theta_4 = 150 - r_theta_h;
00191         r_theta_3 = 150 - r_theta_k;
00192         
00193         l_theta_a_x = l_theta_h;
00194         l_theta_a_x_rad = l_theta_a_x*(3.14159/180);
00195         l_theta_8_x = (l_theta_a_x/0.34);
00196         l_theta_7_x = ((180/3.14159)*(asin((11.4/8)*l_theta_a_x_rad)));
00197 
00198         l_theta_a_y = angle_read;
00199         l_theta_a_y_rad = l_theta_a_y*3.14159/180;
00200         l_theta_7_y = -1.43*l_theta_a_y;
00201         l_theta_8_r_y = -asin((20.94/8)*sin(l_theta_a_y_rad));
00202         l_theta_8_y= (l_theta_8_r_y*180/3.14159);
00203         
00204         r_theta_a_x = r_theta_h;
00205         r_theta_a_x_rad = r_theta_a_x*(3.14159/180);
00206         r_theta_1_x = (r_theta_a_x/0.34);
00207         r_theta_2_x = ((180/3.14159)*(asin((11.4/8)*r_theta_a_x_rad)));
00208 
00209         r_theta_a_y = angle_read;
00210         r_theta_a_y_rad = r_theta_a_y*3.14159/180;
00211         r_theta_2_y = -1.43*r_theta_a_y;
00212         r_theta_1_r_y = -asin((20.94/8)*sin(r_theta_a_y_rad));
00213         r_theta_1_y= (r_theta_1_r_y*180/3.14159);
00214         
00215         l_theta_7_m = 150 + l_theta_7_x + l_theta_7_y;
00216         l_theta_8_m = 150 - l_theta_8_x + l_theta_8_y;
00217         
00218         r_theta_2_m = 150 - r_theta_2_x - r_theta_2_y;
00219         r_theta_1_m = 150 + r_theta_1_x - r_theta_1_y;
00220         
00221         m1.SetGoal(r_theta_1_m);
00222         m2.SetGoal(r_theta_2_m);
00223         m3.SetGoal(r_theta_3);
00224         m4.SetGoal(r_theta_4);
00225         m5.SetGoal(l_theta_5);
00226         m6.SetGoal(l_theta_6);
00227         m7.SetGoal(l_theta_7_m);
00228         m8.SetGoal(l_theta_8_m);
00229         pc.printf("\n\rAngle: %05f",output_angle_y);
00230            
00231     }
00232 }