Revision 1:ea4efc600a1e, committed 2014-05-14

Comitter:

ivanff15

Date:

Wed May 14 16:31:24 2014 +0000

Parent:

0:5af6fad57e1a

Commit message:

hhhh

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BMP085_lib.lib	Wed May 14 16:31:24 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/ivanff15/code/BMP085_lib/#2f1b74312474

--- a/main.cpp	Wed Mar 12 13:42:11 2014 +0000
+++ b/main.cpp	Wed May 14 16:31:24 2014 +0000
@@ -1,10 +1,19 @@
 #include "mbed.h"
 #include <stdio.h>
 #include "BufferSerial.h"
+#include "BMP085.h"
+//#include "MODSERIAL.h"
 
 Serial pc(USBTX,USBRX);
 I2C i2c(p28,p27);
+BMP085 alt_sensor(i2c);
 BufferSerial kirim(USBTX,USBRX,1);
+//BufferSerial kirim(p9, p10, 1);
+PwmOut Servo_1(p21);
+PwmOut Servo_2(p22);
+
+//const int SlaveAddressI2C = 0xEE;
+
 
 #define ACCEL 0xA6
 #define MAGNET 0x3C
@@ -69,10 +78,105 @@
     #define MAGN_X_SCALE (100.0f / (MAGN_X_MAX - MAGN_X_OFFSET))
     #define MAGN_Y_SCALE (100.0f / (MAGN_Y_MAX - MAGN_Y_OFFSET))
     #define MAGN_Z_SCALE (100.0f / (MAGN_Z_MAX - MAGN_Z_OFFSET))
+    
+    #define DEBUG__NO_DRIFT_CORRECTION true
+
+    //Change this parameter if you wanna use another gyroscope.
+    //by default it is ITG3200 gain.
+    #define GYRO_SCALED_RAD(x) (x * TO_RAD(GYRO_GAIN)) // Calculate the scaled gyro readings in radians per second
+    #define GYRO_GAIN (0.06956521739130434782608695652174)
+    // DCM parameters
+    #define Kp_ROLLPITCH 0.02f
+    #define Ki_ROLLPITCH 0.00002f
+    #define Kp_YAW 1.2f
+    #define Ki_YAW 0.00002f
+
+    #define GRAVITY 256.0f // "1G reference" used for DCM filter and accelerometer calibration
+    #define TO_RAD(x) (x * 0.01745329252)  // *pi/180
+    #define TO_DEG(x) (x * 57.2957795131)
+        #define GYRO_BIAS_X -74.49
+    #define GYRO_BIAS_Y -49.43
+    #define GYRO_BIAS_Z -17.06
+
+    #define ACCEL_X_OFFSET ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f)
+    #define ACCEL_Y_OFFSET ((ACCEL_Y_MIN + ACCEL_Y_MAX) / 2.0f)
+    #define ACCEL_Z_OFFSET ((ACCEL_Z_MIN + ACCEL_Z_MAX) / 2.0f)
+    #define ACCEL_X_SCALE (GRAVITY / (ACCEL_X_MAX - ACCEL_X_OFFSET))
+    #define ACCEL_Y_SCALE (GRAVITY / (ACCEL_Y_MAX - ACCEL_Y_OFFSET))
+    #define ACCEL_Z_SCALE (GRAVITY / (ACCEL_Z_MAX - ACCEL_Z_OFFSET))
+    #define ACCEL_X_MIN ((float) -35)
+    #define ACCEL_X_MAX ((float) 33)
+
+    #define ACCEL_Y_MIN ((float) -34)//267
+    #define ACCEL_Y_MAX ((float) 34)
+
+    #define ACCEL_Z_MIN ((float) -36)
+    #define ACCEL_Z_MAX ((float) 33)
+    
+#define goal_ang_1 75
+#define goal_ang_2 -105
+#define Kp 15
+#define Ki 0
+#define Kd 0
+#define base_speed 50
+#define Kp_ang 15
+#define Ki_ang 0
+#define Kd_ang 0
 
 short rawAccX, rawAccY, rawAccZ, rawGyroX, rawGyroY, rawGyroZ, rawMagX, rawMagY, rawMagZ;
 unsigned short sendAccX, sendAccY, sendAccZ, sendGyroX, sendGyroY, sendGyroZ, sendMagX, sendMagY, sendMagZ;
-char baca;
+unsigned char baca=0;
+
+//variabel homing
+signed short angle;
+signed short GoalAngle;
+signed short delta_angle_1, delta_angle_2;
+signed char delta;
+int servo_left, servo_right;
+int jump;
+float jump_error;
+float e, ea, ea_sum, ea_old;
+long int delta_lat, delta_long;
+float error, error_sum, error_old;
+////=====================================
+
+//variabel DCM
+float MAG_Heading;
+float Accel_Vector[3];//= {0, 0, 0}; // Store the acceleration in a vector
+float Gyro_Vector[3];//= {0, 0, 0}; // Store the gyros turn rate in a vector
+float Omega_Vector[3];//= {0, 0, 0}; // Corrected Gyro_Vector data
+float Omega_P[3];//= {0, 0, 0}; // Omega Proportional correction
+float Omega_I[3];//= {0, 0, 0}; // Omega Integrator
+float Omega[3];//= {0, 0, 0};
+float errorRollPitch[3];// = {0, 0, 0};
+float errorYaw[3];// = {0, 0, 0};
+float DCM_Matrix[3][3];// = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}};
+float Update_Matrix[3][3];// = {{0, 1, 2}, {3, 4, 5}, {6, 7, 8}};
+float Temporary_Matrix[3][3];// = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}};
+float G_Dt;
+int rawMagnet[3];
+int gyroBias[3];
+float accel[3];  // Actually stores the NEGATED acceleration (equals gravity, if board not moving).
+int accelRaw[3];
+float accel_min[3];
+float accel_max[3];
+
+float magnet[3];
+int magnetRaw[3];
+float magnetom_min[3];
+float magnetom_max[3];
+
+int gyro[3];
+int gyroRaw[3];
+float gyro_average[3];
+
+int gyro_num_samples;
+//euler
+float yaw;
+float pitch;
+float roll;
+//=======
+//==================================================================
 
 void bin_dec_conv(unsigned short data)
 {
@@ -105,7 +209,6 @@
     i2c.stop();
     return data;
 }
-
 int baca_adxl()
 {
     unsigned char acc_x_msb, acc_x_lsb;
@@ -129,11 +232,17 @@
         float acc_y = 1*(signed short)((signed short)(acc_y_msb<<8) | acc_y_lsb);//*ADXL345_AXIS_X_SCALE/1000.0f;
         float acc_z = (signed short)((signed short)(acc_z_msb<<8) | acc_z_lsb);//*ADXL345_AXIS_X_SCALE/1000.0f
         
-        rawAccX=-1*acc_y;
-        rawAccY=acc_x;
+//        rawAccX=-1*acc_y;
+//        rawAccY=acc_x;
+//        rawAccZ=acc_z;
+        rawAccX=acc_x;
+        rawAccY=acc_y;
         rawAccZ=acc_z;
+
 }
 
+
+
 void baca_itg()
 {
     tulis_i2c(itg3200_address, itg3200_reg_dlpf_fs, (DLPF_FS_SEL_0|DLPF_FS_SEL_1|DLPF_CFG_0));
@@ -150,9 +259,13 @@
         x=1*(signed short)((signed short)(xl | (xh<<8)));//*0.0695652174;
         y=1*(signed short)((signed short)(yl | (yh<<8)));//*0.0695652174;
         z=1*(signed short)((signed short)(zl | (zh<<8)));
-        
-        rawGyroX=-1*y;
-        rawGyroY=x;
+        //
+//        rawGyroX=-1*y;
+//        rawGyroY=x;
+//        rawGyroZ=z;
+
+        rawGyroX=x;
+        rawGyroY=y;
         rawGyroZ=z;
 }
 
@@ -197,14 +310,458 @@
         x=(signed short)((signed short)(xl | (xh<<8)));
         y=(signed short)((signed short)(yl | (yh<<8)));
         z=(signed short)((signed short)(zl | (zh<<8)));
-//        rawMagX=-1*y;
-//        rawMagY=-1*x;
-//        rawMagZ=-1*z;
         rawMagX=x;
         rawMagY=y;
         rawMagZ=z;
+//        rawMagX=-1*x;
+//        rawMagY=1*y;
+//        rawMagZ=1*z;
 }
 
+void setAccelRaw(int accelRawX,int accelRawY,int accelRawZ)
+{
+    accelRaw[0] = (uint16_t)accelRawX;
+    accelRaw[1] = (uint16_t)accelRawY;
+    accelRaw[2] = (uint16_t)accelRawZ;
+}
+void setGyroRaw(int gyroRawX,int gyroRawY,int gyroRawZ)
+{
+    gyroRaw[0] = gyroRawX;
+    gyroRaw[1] = gyroRawY;
+    gyroRaw[2] = gyroRawZ;
+}
+void setMagnetRaw(int magnetRawX,int magnetRawY,int magnetRawZ)
+{
+    magnetRaw[0] = (uint16_t)magnetRawX;
+    magnetRaw[1] = (uint16_t)magnetRawY;
+    magnetRaw[2] = (uint16_t)magnetRawZ;
+}
+
+void ReadMagnetometer()
+{
+    rawMagnet[0] = ((int16_t)magnetRaw[0]);
+    rawMagnet[1] = ((int16_t)magnetRaw[1]);
+    rawMagnet[2] = ((int16_t)magnetRaw[2]);
+
+    magnet[0] = ((float)(rawMagnet[0] - MAGN_X_OFFSET) * MAGN_X_SCALE);
+    magnet[1] = ((float)(rawMagnet[1] - MAGN_Y_OFFSET) * MAGN_Y_SCALE);
+    magnet[2] = ((float)(rawMagnet[2] - MAGN_Z_OFFSET) * MAGN_Z_SCALE);
+}
+void ReadAccelerometer()
+{
+    accel[0] = (((int16_t)accelRaw[0]) - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
+    accel[1] = (((int16_t)accelRaw[1]) - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE;
+    accel[2] = (((int16_t)accelRaw[2]) - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;
+}
+void ReadGyroscope()
+{
+    gyro[0] = (gyroRaw[0] - GYRO_BIAS_X);
+    gyro[1] = (gyroRaw[1] - GYRO_BIAS_Y);
+    gyro[2] = (gyroRaw[2] - GYRO_BIAS_Z);
+}
+
+void Compass_Heading()
+{
+  float mag_x;
+  float mag_y;
+  float cos_roll;
+  float sin_roll;
+  float cos_pitch;
+  float sin_pitch;
+
+  cos_roll = cos(roll);
+  sin_roll = sin(roll);
+  cos_pitch = cos(pitch);
+  sin_pitch = sin(pitch);
+
+  // Tilt compensated magnetic field X
+  mag_x = magnet[0]*cos_pitch + magnet[1]*sin_roll*sin_pitch + magnet[2]*cos_roll*sin_pitch;
+  // Tilt compensated magnetic field Y
+  mag_y = magnet[1]*cos_roll - magnet[2]*sin_roll;
+  // Magnetic Heading
+  MAG_Heading = atan2(-mag_y, mag_x);
+}
+//=========fungsi matrix======
+void Vector_Cross_Product(float C[3], float A[3], float B[3])
+{
+    C[0] = (A[1] * B[2]) - (A[2] * B[1]);
+    C[1] = (A[2] * B[0]) - (A[0] * B[2]);
+    C[2] = (A[0] * B[1]) - (A[1] * B[0]);
+
+    return;
+}
+
+void Vector_Scale(float C[3], float A[3], float b)
+{
+    int m;
+    for (m = 0; m < 3; m++)
+        C[m] = A[m] * b;
+
+    return;
+}
+
+float Vector_Dot_Product(float A[3], float B[3])
+{
+    float result = 0.0;
+
+    int i;
+    for (i = 0; i < 3; i++) {
+        result += A[i] * B[i];
+    }
+
+    return result;
+}
+
+void Vector_Add1(float C[3], float A[3], float B[3])
+{
+    int m;
+    for (m = 0; m < 3; m++)
+        C[m] = A[m] + B[m];
+    return;
+}
+
+void Vector_Add(float C[3][3], float A[3][3], float B[3][3])
+{
+    int m,n;
+    for (m = 0; m < 3; m++)
+        for (n = 0; n < 3; n++)
+            C[m][n] = A[m][n] + B[m][n];
+}
+
+void Matrix_Multiply(float C[3][3], float A[3][3], float B[3][3])
+{
+    int i,j,k;
+    for (i = 0; i < 3; i++) {
+        for (j = 0; j < 3; j++) {
+            C[i][j] = 0;
+            for (k = 0; k < 3; k++) {
+               C[i][j] += A[i][k] * B[k][j];
+            }
+        }
+    }
+}
+
+//=========end matrix=========
+
+//=====mulai DCM======
+//dari objek terhadap tanah
+void init_rotation_matrix(float m[3][3], float yaw, float pitch, float roll)
+{
+  float c1 = cos(roll);
+  float s1 = sin(roll);
+  float c2 = cos(pitch);
+  float s2 = sin(pitch);
+  float c3 = cos(yaw);
+  float s3 = sin(yaw);
+
+  // Euler angles, right-handed, intrinsic, XYZ convention
+  // (which means: rotate around body axes Z, Y', X'')
+  m[0][0] = c2 * c3;
+  m[0][1] = c3 * s1 * s2 - c1 * s3;
+  m[0][2] = s1 * s3 + c1 * c3 * s2;
+
+  m[1][0] = c2 * s3;
+  m[1][1] = c1 * c3 + s1 * s2 * s3;
+  m[1][2] = c1 * s2 * s3 - c3 * s1;
+
+  m[2][0] = -s2;
+  m[2][1] = c2 * s1;
+  m[2][2] = c1 * c2;
+}
+
+float constrain(float in, float min, float max)
+{
+    in = in > max ? max : in;
+    in = in < min ? min : in;
+    return in;
+}
+
+void Normalize()
+{
+  float error=0;
+  float temporary[3][3];
+  float renorm=0;
+
+  error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19
+
+  Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19
+  Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19
+  Vector_Add1(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19
+  Vector_Add1(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19
+
+  Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20
+
+  renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21
+  Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm);
+
+  renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21
+  Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm);
+
+  renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21
+  Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm);
+}
+
+void Drift_correction()
+{
+  float mag_heading_x;
+  float mag_heading_y;
+  float errorCourse;
+  //Compensation the Roll, Pitch and Yaw drift.
+  static float Scaled_Omega_P[3];
+  static float Scaled_Omega_I[3];
+  float Accel_magnitude;
+  float Accel_weight;
+    //*****Roll and Pitch***************
+
+  // Calculate the magnitude of the accelerometer vector
+  Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]);
+  Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
+  // Dynamic weighting of accelerometer info (reliability filter)
+  // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
+  Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1);  //
+
+  Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
+  Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
+   Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
+  Vector_Add1(Omega_I,Omega_I,Scaled_Omega_I);
+    //*****YAW***************
+  // We make the gyro YAW drift correction based on compass magnetic heading
+
+  mag_heading_x = cos(MAG_Heading);
+  mag_heading_y = sin(MAG_Heading);
+  errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x);  //Calculating YAW error
+  Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
+
+  Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01proportional of YAW.
+  Vector_Add1(Omega_P,Omega_P,Scaled_Omega_P);//Adding  Proportional.
+
+  Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001Integrator
+  Vector_Add1(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
+}
+void Matrix_update()
+{
+  Gyro_Vector[0]=GYRO_SCALED_RAD(gyro[0]); //gyro x roll
+  Gyro_Vector[1]=GYRO_SCALED_RAD(gyro[1]); //gyro y pitch
+  Gyro_Vector[2]=GYRO_SCALED_RAD(gyro[2]); //gyro z yaw
+
+  Accel_Vector[0]=accel[0];
+  Accel_Vector[1]=accel[1];
+  Accel_Vector[2]=accel[2];
+
+  Vector_Add1(&Omega[0], &Gyro_Vector[0], &Omega_I[0]);  //adding proportional term
+  Vector_Add1(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
+
+  Update_Matrix[0][0]=0;
+  Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z
+  Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y
+  Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z
+  Update_Matrix[1][1]=0;
+  Update_Matrix[1][2]=-G_Dt*Omega_Vector[0];
+  Update_Matrix[2][0]=-G_Dt*Omega_Vector[1];
+  Update_Matrix[2][1]=G_Dt*Omega_Vector[0];
+  Update_Matrix[2][2]=0;
+
+  Matrix_Multiply(Temporary_Matrix,DCM_Matrix,Update_Matrix); //a*b=c
+
+  int x,y;
+  for(x=0; x<3; x++) //Matrix Addition (update)
+  {
+    for(y=0; y<3; y++)
+    {
+      DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
+    }
+  }
+}
+void Euler_angles()
+{
+  pitch = -asin(DCM_Matrix[2][0]);
+  roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]);
+  yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]);
+}
+void Sensors()
+{
+    ReadAccelerometer();
+    ReadGyroscope();
+    ReadMagnetometer();
+    Compass_Heading();
+}
+void UpdateFilter()
+{
+    Sensors();
+    Matrix_update();
+    Normalize();
+    Drift_correction();
+    Euler_angles();
+}
+void UpdateFilter2()
+{
+    baca_itg();
+    baca_hmc();
+    baca_adxl();
+
+    setAccelRaw(rawAccX,rawAccY,rawAccZ);
+
+    setGyroRaw(rawGyroX,rawGyroY,rawGyroZ);
+
+    setMagnetRaw(rawMagX,rawMagY,rawMagZ);
+    UpdateFilter();
+}
+void reset_sensor_fusion() {
+  float temp1[3];
+  float temp2[3];
+  float xAxis[] = {1.0f, 0.0f, 0.0f};
+
+  Sensors();
+
+  // GET PITCH
+  // Using y-z-plane-component/x-component of gravity vector
+  pitch = -atan2(accel[0], sqrt(accel[1] * accel[1] + accel[2] * accel[2]));
+
+  Vector_Cross_Product(temp1, accel, xAxis);
+  Vector_Cross_Product(temp2, xAxis, temp1);
+  roll = atan2(temp2[1], temp2[2]);
+
+  // GET YAW
+  Compass_Heading();
+  yaw = MAG_Heading;
+
+  // Init rotation matrix
+    init_rotation_matrix(DCM_Matrix, yaw, pitch, roll);
+}
+float getYaw()
+{
+    return TO_DEG(yaw);
+}
+float getPitch()
+{
+    return TO_DEG(pitch);
+}
+float getRoll()
+{
+    return TO_DEG(roll);
+}
+void data_DCM ()
+{
+    UpdateFilter2();
+    yaw = getYaw();
+    pitch = getPitch();
+    roll = getRoll();
+}
+
+void DCM(float dt)
+{
+    int i,l,j;
+    for(i = 0; i < 3; i++)
+    {
+        Accel_Vector[i] = 0; // Store the acceleration in a vector
+        Gyro_Vector[i] = 0; // Store the gyros turn rate in a vector
+        Omega_Vector[i] = 0; // Corrected Gyro_Vector data
+        Omega_P[i] = 0; // Omega Proportional correction
+        Omega_I[i] = 0; // Omega Integrator
+        Omega[i]= 0;
+        errorRollPitch[i] = 0;
+        errorYaw[i] = 0;
+    }
+
+    int k = 1;
+
+    for(l = 0; l < 3; l++)
+    {
+        for(j = 0; j < 3; j++)
+        {
+            DCM_Matrix[l][j] = 0;
+            Update_Matrix[l][j] = k;
+            Temporary_Matrix[l][j] = 0;
+            k++;
+        }
+        k++;
+    }
+
+    G_Dt = dt;
+
+    reset_sensor_fusion();
+}
+void dataoutdcm2()
+{
+//    unsigned char i;
+    pc.printf("#YPR=%.2f,%.2f,%.2f\n",yaw,pitch,roll);
+//    for(i=0;i<3;i++)
+//    {
+//        sprintf(kirim,"Acc=%d,%d,%d\n",roll,pitch,yaw);
+
+//    }
+
+}
+void FilterInit(int rawAccX, int rawAccY, int rawAccZ, int rawGyroX, int rawGyroY, int rawGyroZ, int rawMagX, int rawMagY, int rawMagZ)
+{
+    baca_itg();
+    baca_hmc();
+    baca_adxl();
+
+   setAccelRaw(rawAccX,rawAccY,rawAccZ);
+
+    setGyroRaw(rawGyroX,rawGyroY,rawGyroZ);
+
+    setMagnetRaw(rawMagX,rawMagY,rawMagZ);
+
+    reset_sensor_fusion();
+}
+//=====end DCM========
+
+
+//=====baca baro
+void baca_baro()
+{
+   unsigned char tekanan;
+    while(1)
+    {
+        tekanan=alt_sensor.get_pressure();
+        pc.printf("Altitude: %f\r\n", alt_sensor.get_altitude_m());
+    }
+}
+
+void Servo()
+{
+       jump = base_speed - delta;
+    if(jump>100) servo_left = 100;
+    else if(jump<0) servo_left = 0;
+    else servo_left = (unsigned char)(jump);
+
+    jump = base_speed + delta;
+    if(jump>100) servo_right = 100;
+    else if(jump<0) servo_right = 0;
+    else servo_right = (unsigned char)(jump);
+    
+    servo_left = 1000 + servo_left*10;
+    servo_right = 1000 + servo_right*10;
+    Servo_1.pulsewidth_us(servo_left);
+    Servo_2.pulsewidth_us(servo_right);
+    pc.printf("%d || %d\n",servo_left,servo_right);
+}
+
+void PID()
+{
+     FilterInit(rawAccX, rawAccY, rawAccZ, rawGyroX, rawGyroY, rawGyroZ, rawMagX, rawMagY, rawMagZ);
+     data_DCM ();
+     angle = (signed short)(yaw);
+    delta_angle_1 = abs(angle-goal_ang_1);
+    if(delta_angle_1>180) delta_angle_1 = 360 - delta_angle_1;
+    delta_angle_2 = abs(angle-goal_ang_2);
+    if(delta_angle_2>180) delta_angle_2 = 360 - delta_angle_2;
+    if(delta_angle_1<delta_angle_2) GoalAngle = goal_ang_1;
+    else GoalAngle = goal_ang_2;
+        jump = (angle - GoalAngle);
+    error = atan2(sin(TO_RAD(jump)),cos(TO_RAD(jump)));
+        error_sum = error_sum + error;
+         jump_error = error_old;
+    error_old=error;
+    delta = (signed char) (Kp_ang*error + Ki_ang*error_sum + Kd_ang*(error-error_old));
+    
+
+     
+}
+
+
 void raw_to_send()
 {
     sendAccX = (unsigned short) (rawAccX + 512);//if(sendAccX>999) sendAccX=999;
@@ -220,17 +777,23 @@
 
 int main()
 {
+    //baca = 1;
+    int count = 0;
     pc.baud(57600);
+    Servo_1.period_ms(20);
+    Servo_2.period_ms(20);
     while(1)
     {
         if(kirim.readable())
           {
               baca=kirim.getc();
               if(baca=='s')
-              baca='0';
-              while(1)
               {
-                  //pc.printf("ivan\n");
+                  baca='0';
+                  while(1)
+                  {
+                      
+                      count=count++;
                   baca_adxl();
                   baca_itg();
                   baca_hmc();
@@ -278,12 +841,77 @@
                   wait_ms(75); 
                   
                   baca=kirim.getc();
+                  
+                  //if(count ==4)
+//                  {
+//                    Servo_1.pulsewidth_us(1000);
+//                    Servo_2.pulsewidth_us(2000);  
+//                  }
+//                  if(count ==8)
+//                  {
+//                     Servo_1.pulsewidth_us(2000); 
+//                     Servo_2.pulsewidth_us(1000); 
+//                     count = 0;
+//                  }
+                  
                   if(baca=='x')
                   {
                       baca='0';
                       break;
                   }
+                  }
               }
-          }
+              //baca=kirim.getc();
+              if(baca=='d')
+              {
+                  baca='0';
+                  while(1)
+                  {
+                    FilterInit(rawAccX, rawAccY, rawAccZ, rawGyroX, rawGyroY, rawGyroZ, rawMagX, rawMagY, rawMagZ);
+                   data_DCM ();
+
+                      dataoutdcm2();
+                      PID();
+                      Servo();
+                      wait_ms(10);
+                      baca=kirim.getc();
+                  if(baca=='x')
+                  {
+                     baca='0';
+                     break; 
+                  }
+                  }
+              }
+              if(baca=='b')
+              {
+                  baca='0';
+                  while(1)
+                  {
+                      PID();
+                      baca=kirim.getc();
+                  if(baca=='x')
+                  {
+                     baca='0';
+                     break; 
+                  }
+                  }  
+              }
+              if(baca=='p')
+              {
+                    baca='0';
+                    while(1)
+                    {
+                         pc.printf("IVAN JELEK IVAN KELEKEJFKDFJKJ \n");
+                         baca=kirim.getc();
+                         if(baca=='x')
+                         {
+                             baca='0';
+                             break;
+                         }
+                    }    
+              }
+              
+              
+          }        
     }
 }

--- a/mbed.bld	Wed Mar 12 13:42:11 2014 +0000
+++ b/mbed.bld	Wed May 14 16:31:24 2014 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/9114680c05da
+http://world3.dev.mbed.org/users/mbed_official/code/mbed/builds/824293ae5e43
\ No newline at end of file