ivan fajar
gmat juara
Dependencies: BufferSerial mbed BMP085_lib
Diff: main.cpp
- Revision:
- 1:ea4efc600a1e
- Parent:
- 0:5af6fad57e1a
--- 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;
+ }
+ }
+ }
+
+
+ }
}
}