gmat juara
Dependencies: BufferSerial mbed BMP085_lib
main.cpp
- Committer:
- ivanff15
- Date:
- 2014-05-14
- Revision:
- 1:ea4efc600a1e
- Parent:
- 0:5af6fad57e1a
File content as of revision 1:ea4efc600a1e:
#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 #define ID_GATHOTKACA 106 //initialize accel #define ADXL345_AXIS_X_SCALE 4 #define adxl345_address (0xA6>>1) #define adxl345_reg_data_format (0x31) #define adxl345_reg_pwr_ctl (0x2D) #define adxl345_reg_xlsb (0x32) //initialize gyro #define ITG3200_R 0x69 #define ITG3200_W 0x68 #define WHO 0x00 #define SMPL 0x15 #define INT_C 0x17 #define TMP_H 0x1B #define GX_H 0x1D #define GY_H 0x1F #define GZ_H 0x21 #define PWR_M 0x3E #define DLPF 0x16 #define INT_S 0x1A #define TMP_L 0x1C #define GX_L 0x1E #define GY_L 0x20 #define GZ_L 0x22 #define itg3200_address (0xD0) #define itg3200_reg_xmsb (0x1D) #define itg3200_reg_who_am_I (0x00) #define itg3200_reg_smplrt_div (0x15) #define itg3200_reg_dlpf_fs (0x16) #define DLPF_CFG_0 (1<<0) #define DLPF_CFG_1 (1<<1) #define DLPF_CFG_2 (1<<2) #define DLPF_FS_SEL_0 (1<<3) #define DLPF_FS_SEL_1 (1<<4) //initialize magneto #define HMC5843_W 0x3C #define HMC5843_R 0x3D #define PI 3.14159265 #define GRAVITY 256.0f // "1G reference" used for DCM filter and accelerometer calibration #define MAGN_X_MAX 465 #define MAGN_Y_MAX 475 #define MAGN_Z_MAX 600 #define MAGN_X_MIN -561 #define MAGN_Y_MIN -547 #define MAGN_Z_MIN -379 #define MAGN_X_OFFSET ((MAGN_X_MIN + MAGN_X_MAX) / 2.0f) #define MAGN_Y_OFFSET ((MAGN_Y_MIN + MAGN_Y_MAX) / 2.0f) #define MAGN_Z_OFFSET ((MAGN_Z_MIN + MAGN_Z_MAX) / 2.0f) #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; 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) { unsigned short hasil; // unsigned char kirim[16]; hasil = data%100; pc.printf("%d%d%d",(data/100),(hasil/10),(hasil%10)); } void tulis_i2c(unsigned char devadd, unsigned char regadd, unsigned char data) { i2c.start(); i2c.write(devadd); i2c.write(regadd); i2c.write(data); i2c.stop(); } unsigned char baca_i2c(unsigned char devadd, unsigned char regadd) { unsigned char data; i2c.start(); i2c.write(devadd); i2c.write(regadd); i2c.start(); i2c.write(devadd|1); data=i2c.read(0); i2c.stop(); return data; } int baca_adxl() { unsigned char acc_x_msb, acc_x_lsb; unsigned char acc_y_msb, acc_y_lsb; unsigned char acc_z_msb, acc_z_lsb; tulis_i2c(ACCEL, adxl345_reg_pwr_ctl, 0); tulis_i2c(ACCEL, adxl345_reg_pwr_ctl, 16); tulis_i2c(ACCEL, adxl345_reg_pwr_ctl, 8); tulis_i2c(ACCEL, adxl345_reg_data_format, 0x03); acc_x_lsb = baca_i2c(ACCEL,0x32); acc_x_msb = baca_i2c(ACCEL,0x33); acc_y_lsb = baca_i2c(ACCEL,0x34); acc_y_msb = baca_i2c(ACCEL,0x35); acc_z_lsb = baca_i2c(ACCEL,0x36); acc_z_msb = baca_i2c(ACCEL,0x37); float acc_x = (signed short)((signed short)(acc_x_msb<<8) | acc_x_lsb);//*ADXL345_AXIS_X_SCALE/1000.0f; 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; // 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)); tulis_i2c(itg3200_address, itg3200_reg_smplrt_div, 9); char xh,xl,yh,yl,zh,zl; short x,y,z; xl = baca_i2c(itg3200_address,0x1E); xh = baca_i2c(itg3200_address,0x1D); yh = baca_i2c(itg3200_address,0x1F); yl = baca_i2c(itg3200_address,0x20); zh = baca_i2c(itg3200_address,0x21); zl = baca_i2c(itg3200_address,0x22); 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; // rawGyroZ=z; rawGyroX=x; rawGyroY=y; rawGyroZ=z; } void i2c_tulis_hmc(unsigned char address, unsigned char data) { i2c.start(); i2c.write(HMC5843_W); // write 0x i2c.write(address); // write register address i2c.write(data); // write register address i2c.stop(); //__delay_ms(10); } unsigned char i2c_baca_hmc(unsigned char address) { unsigned char data; i2c.start(); i2c.write(HMC5843_W); // write 0x i2c.write(address); // write register address i2c.start(); i2c.write(HMC5843_R); // write 0x data = i2c.read(0); // Get MSB result i2c.stop(); //__delay_ms(10); return data; } int baca_hmc() // baca_itg(raw) untuk ambil data raw : baca_itg(scaled) untuk ambil data terskala { // char xh, xl, yh, yl, zh, zl; // short x, y, z; int xh, xl, yh, yl, zh, zl; float x, y, z; i2c_tulis_hmc(0x02,0x01); xh=i2c_baca_hmc(0x03); xl=i2c_baca_hmc(0x04); zh=i2c_baca_hmc(0x05); zl=i2c_baca_hmc(0x06); yh=i2c_baca_hmc(0x07); yl=i2c_baca_hmc(0x08); 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=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; sendAccY = (unsigned short) (rawAccY + 512);//if(sendAccY>999) sendAccY=999; sendAccZ = (unsigned short) (rawAccZ + 512);//if(sendAccZ>999) sendAccZ=999; sendGyroX = (unsigned short)((rawGyroX*0.06956+2400)*0.2083);//if(sendGyroX>999) sendGyroX=999; sendGyroY = (unsigned short)((rawGyroY*0.06956+2400)*0.2083);//if(sendGyroY>999) sendGyroY=999; sendGyroZ = (unsigned short)((rawGyroZ*0.06956+2400)*0.2083);//if(sendGyroZ>999) sendGyroZ=999; sendMagX = (unsigned short)((rawMagX+2048)*0.25);//if(sendMagX>999) sendMagX=999; sendMagY = (unsigned short)((rawMagY+2048)*0.25);//if(sendMagY>999) sendMagY=999; sendMagZ = (unsigned short)((rawMagZ+2048)*0.25);//if(sendMagZ>999) sendMagZ=999; } 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) { count=count++; baca_adxl(); baca_itg(); baca_hmc(); raw_to_send(); pc.putc(0x0D); bin_dec_conv(ID_GATHOTKACA); pc.putc(0x20); bin_dec_conv(sendAccX); pc.putc(0x20); bin_dec_conv(sendAccY); pc.putc(0x20); bin_dec_conv(sendAccZ); pc.putc(0x20); bin_dec_conv(sendGyroX); pc.putc(0x20); bin_dec_conv(sendGyroY); pc.putc(0x20); bin_dec_conv(sendGyroZ); pc.putc(0x20); bin_dec_conv(sendMagX); pc.putc(0x20); bin_dec_conv(sendMagY); pc.putc(0x20); bin_dec_conv(sendMagZ); 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; } } } } } }