gmat juara
Dependencies: BufferSerial mbed BMP085_lib
main.cpp@1:ea4efc600a1e, 2014-05-14 (annotated)
- Committer:
- ivanff15
- Date:
- Wed May 14 16:31:24 2014 +0000
- Revision:
- 1:ea4efc600a1e
- Parent:
- 0:5af6fad57e1a
hhhh
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
ivanff15 | 0:5af6fad57e1a | 1 | #include "mbed.h" |
ivanff15 | 0:5af6fad57e1a | 2 | #include <stdio.h> |
ivanff15 | 0:5af6fad57e1a | 3 | #include "BufferSerial.h" |
ivanff15 | 1:ea4efc600a1e | 4 | #include "BMP085.h" |
ivanff15 | 1:ea4efc600a1e | 5 | //#include "MODSERIAL.h" |
ivanff15 | 0:5af6fad57e1a | 6 | |
ivanff15 | 0:5af6fad57e1a | 7 | Serial pc(USBTX,USBRX); |
ivanff15 | 0:5af6fad57e1a | 8 | I2C i2c(p28,p27); |
ivanff15 | 1:ea4efc600a1e | 9 | BMP085 alt_sensor(i2c); |
ivanff15 | 0:5af6fad57e1a | 10 | BufferSerial kirim(USBTX,USBRX,1); |
ivanff15 | 1:ea4efc600a1e | 11 | //BufferSerial kirim(p9, p10, 1); |
ivanff15 | 1:ea4efc600a1e | 12 | PwmOut Servo_1(p21); |
ivanff15 | 1:ea4efc600a1e | 13 | PwmOut Servo_2(p22); |
ivanff15 | 1:ea4efc600a1e | 14 | |
ivanff15 | 1:ea4efc600a1e | 15 | //const int SlaveAddressI2C = 0xEE; |
ivanff15 | 1:ea4efc600a1e | 16 | |
ivanff15 | 0:5af6fad57e1a | 17 | |
ivanff15 | 0:5af6fad57e1a | 18 | #define ACCEL 0xA6 |
ivanff15 | 0:5af6fad57e1a | 19 | #define MAGNET 0x3C |
ivanff15 | 0:5af6fad57e1a | 20 | #define ID_GATHOTKACA 106 |
ivanff15 | 0:5af6fad57e1a | 21 | |
ivanff15 | 0:5af6fad57e1a | 22 | //initialize accel |
ivanff15 | 0:5af6fad57e1a | 23 | #define ADXL345_AXIS_X_SCALE 4 |
ivanff15 | 0:5af6fad57e1a | 24 | |
ivanff15 | 0:5af6fad57e1a | 25 | #define adxl345_address (0xA6>>1) |
ivanff15 | 0:5af6fad57e1a | 26 | #define adxl345_reg_data_format (0x31) |
ivanff15 | 0:5af6fad57e1a | 27 | #define adxl345_reg_pwr_ctl (0x2D) |
ivanff15 | 0:5af6fad57e1a | 28 | #define adxl345_reg_xlsb (0x32) |
ivanff15 | 0:5af6fad57e1a | 29 | |
ivanff15 | 0:5af6fad57e1a | 30 | //initialize gyro |
ivanff15 | 0:5af6fad57e1a | 31 | #define ITG3200_R 0x69 |
ivanff15 | 0:5af6fad57e1a | 32 | #define ITG3200_W 0x68 |
ivanff15 | 0:5af6fad57e1a | 33 | #define WHO 0x00 |
ivanff15 | 0:5af6fad57e1a | 34 | #define SMPL 0x15 |
ivanff15 | 0:5af6fad57e1a | 35 | #define INT_C 0x17 |
ivanff15 | 0:5af6fad57e1a | 36 | #define TMP_H 0x1B |
ivanff15 | 0:5af6fad57e1a | 37 | #define GX_H 0x1D |
ivanff15 | 0:5af6fad57e1a | 38 | #define GY_H 0x1F |
ivanff15 | 0:5af6fad57e1a | 39 | #define GZ_H 0x21 |
ivanff15 | 0:5af6fad57e1a | 40 | #define PWR_M 0x3E |
ivanff15 | 0:5af6fad57e1a | 41 | #define DLPF 0x16 |
ivanff15 | 0:5af6fad57e1a | 42 | #define INT_S 0x1A |
ivanff15 | 0:5af6fad57e1a | 43 | #define TMP_L 0x1C |
ivanff15 | 0:5af6fad57e1a | 44 | #define GX_L 0x1E |
ivanff15 | 0:5af6fad57e1a | 45 | #define GY_L 0x20 |
ivanff15 | 0:5af6fad57e1a | 46 | #define GZ_L 0x22 |
ivanff15 | 0:5af6fad57e1a | 47 | |
ivanff15 | 0:5af6fad57e1a | 48 | #define itg3200_address (0xD0) |
ivanff15 | 0:5af6fad57e1a | 49 | #define itg3200_reg_xmsb (0x1D) |
ivanff15 | 0:5af6fad57e1a | 50 | #define itg3200_reg_who_am_I (0x00) |
ivanff15 | 0:5af6fad57e1a | 51 | #define itg3200_reg_smplrt_div (0x15) |
ivanff15 | 0:5af6fad57e1a | 52 | #define itg3200_reg_dlpf_fs (0x16) |
ivanff15 | 0:5af6fad57e1a | 53 | #define DLPF_CFG_0 (1<<0) |
ivanff15 | 0:5af6fad57e1a | 54 | #define DLPF_CFG_1 (1<<1) |
ivanff15 | 0:5af6fad57e1a | 55 | #define DLPF_CFG_2 (1<<2) |
ivanff15 | 0:5af6fad57e1a | 56 | #define DLPF_FS_SEL_0 (1<<3) |
ivanff15 | 0:5af6fad57e1a | 57 | #define DLPF_FS_SEL_1 (1<<4) |
ivanff15 | 0:5af6fad57e1a | 58 | |
ivanff15 | 0:5af6fad57e1a | 59 | |
ivanff15 | 0:5af6fad57e1a | 60 | //initialize magneto |
ivanff15 | 0:5af6fad57e1a | 61 | #define HMC5843_W 0x3C |
ivanff15 | 0:5af6fad57e1a | 62 | #define HMC5843_R 0x3D |
ivanff15 | 0:5af6fad57e1a | 63 | #define PI 3.14159265 |
ivanff15 | 0:5af6fad57e1a | 64 | |
ivanff15 | 0:5af6fad57e1a | 65 | #define GRAVITY 256.0f // "1G reference" used for DCM filter and accelerometer calibration |
ivanff15 | 0:5af6fad57e1a | 66 | |
ivanff15 | 0:5af6fad57e1a | 67 | #define MAGN_X_MAX 465 |
ivanff15 | 0:5af6fad57e1a | 68 | #define MAGN_Y_MAX 475 |
ivanff15 | 0:5af6fad57e1a | 69 | #define MAGN_Z_MAX 600 |
ivanff15 | 0:5af6fad57e1a | 70 | |
ivanff15 | 0:5af6fad57e1a | 71 | #define MAGN_X_MIN -561 |
ivanff15 | 0:5af6fad57e1a | 72 | #define MAGN_Y_MIN -547 |
ivanff15 | 0:5af6fad57e1a | 73 | #define MAGN_Z_MIN -379 |
ivanff15 | 0:5af6fad57e1a | 74 | #define MAGN_X_OFFSET ((MAGN_X_MIN + MAGN_X_MAX) / 2.0f) |
ivanff15 | 0:5af6fad57e1a | 75 | #define MAGN_Y_OFFSET ((MAGN_Y_MIN + MAGN_Y_MAX) / 2.0f) |
ivanff15 | 0:5af6fad57e1a | 76 | #define MAGN_Z_OFFSET ((MAGN_Z_MIN + MAGN_Z_MAX) / 2.0f) |
ivanff15 | 0:5af6fad57e1a | 77 | |
ivanff15 | 0:5af6fad57e1a | 78 | #define MAGN_X_SCALE (100.0f / (MAGN_X_MAX - MAGN_X_OFFSET)) |
ivanff15 | 0:5af6fad57e1a | 79 | #define MAGN_Y_SCALE (100.0f / (MAGN_Y_MAX - MAGN_Y_OFFSET)) |
ivanff15 | 0:5af6fad57e1a | 80 | #define MAGN_Z_SCALE (100.0f / (MAGN_Z_MAX - MAGN_Z_OFFSET)) |
ivanff15 | 1:ea4efc600a1e | 81 | |
ivanff15 | 1:ea4efc600a1e | 82 | #define DEBUG__NO_DRIFT_CORRECTION true |
ivanff15 | 1:ea4efc600a1e | 83 | |
ivanff15 | 1:ea4efc600a1e | 84 | //Change this parameter if you wanna use another gyroscope. |
ivanff15 | 1:ea4efc600a1e | 85 | //by default it is ITG3200 gain. |
ivanff15 | 1:ea4efc600a1e | 86 | #define GYRO_SCALED_RAD(x) (x * TO_RAD(GYRO_GAIN)) // Calculate the scaled gyro readings in radians per second |
ivanff15 | 1:ea4efc600a1e | 87 | #define GYRO_GAIN (0.06956521739130434782608695652174) |
ivanff15 | 1:ea4efc600a1e | 88 | // DCM parameters |
ivanff15 | 1:ea4efc600a1e | 89 | #define Kp_ROLLPITCH 0.02f |
ivanff15 | 1:ea4efc600a1e | 90 | #define Ki_ROLLPITCH 0.00002f |
ivanff15 | 1:ea4efc600a1e | 91 | #define Kp_YAW 1.2f |
ivanff15 | 1:ea4efc600a1e | 92 | #define Ki_YAW 0.00002f |
ivanff15 | 1:ea4efc600a1e | 93 | |
ivanff15 | 1:ea4efc600a1e | 94 | #define GRAVITY 256.0f // "1G reference" used for DCM filter and accelerometer calibration |
ivanff15 | 1:ea4efc600a1e | 95 | #define TO_RAD(x) (x * 0.01745329252) // *pi/180 |
ivanff15 | 1:ea4efc600a1e | 96 | #define TO_DEG(x) (x * 57.2957795131) |
ivanff15 | 1:ea4efc600a1e | 97 | #define GYRO_BIAS_X -74.49 |
ivanff15 | 1:ea4efc600a1e | 98 | #define GYRO_BIAS_Y -49.43 |
ivanff15 | 1:ea4efc600a1e | 99 | #define GYRO_BIAS_Z -17.06 |
ivanff15 | 1:ea4efc600a1e | 100 | |
ivanff15 | 1:ea4efc600a1e | 101 | #define ACCEL_X_OFFSET ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f) |
ivanff15 | 1:ea4efc600a1e | 102 | #define ACCEL_Y_OFFSET ((ACCEL_Y_MIN + ACCEL_Y_MAX) / 2.0f) |
ivanff15 | 1:ea4efc600a1e | 103 | #define ACCEL_Z_OFFSET ((ACCEL_Z_MIN + ACCEL_Z_MAX) / 2.0f) |
ivanff15 | 1:ea4efc600a1e | 104 | #define ACCEL_X_SCALE (GRAVITY / (ACCEL_X_MAX - ACCEL_X_OFFSET)) |
ivanff15 | 1:ea4efc600a1e | 105 | #define ACCEL_Y_SCALE (GRAVITY / (ACCEL_Y_MAX - ACCEL_Y_OFFSET)) |
ivanff15 | 1:ea4efc600a1e | 106 | #define ACCEL_Z_SCALE (GRAVITY / (ACCEL_Z_MAX - ACCEL_Z_OFFSET)) |
ivanff15 | 1:ea4efc600a1e | 107 | #define ACCEL_X_MIN ((float) -35) |
ivanff15 | 1:ea4efc600a1e | 108 | #define ACCEL_X_MAX ((float) 33) |
ivanff15 | 1:ea4efc600a1e | 109 | |
ivanff15 | 1:ea4efc600a1e | 110 | #define ACCEL_Y_MIN ((float) -34)//267 |
ivanff15 | 1:ea4efc600a1e | 111 | #define ACCEL_Y_MAX ((float) 34) |
ivanff15 | 1:ea4efc600a1e | 112 | |
ivanff15 | 1:ea4efc600a1e | 113 | #define ACCEL_Z_MIN ((float) -36) |
ivanff15 | 1:ea4efc600a1e | 114 | #define ACCEL_Z_MAX ((float) 33) |
ivanff15 | 1:ea4efc600a1e | 115 | |
ivanff15 | 1:ea4efc600a1e | 116 | #define goal_ang_1 75 |
ivanff15 | 1:ea4efc600a1e | 117 | #define goal_ang_2 -105 |
ivanff15 | 1:ea4efc600a1e | 118 | #define Kp 15 |
ivanff15 | 1:ea4efc600a1e | 119 | #define Ki 0 |
ivanff15 | 1:ea4efc600a1e | 120 | #define Kd 0 |
ivanff15 | 1:ea4efc600a1e | 121 | #define base_speed 50 |
ivanff15 | 1:ea4efc600a1e | 122 | #define Kp_ang 15 |
ivanff15 | 1:ea4efc600a1e | 123 | #define Ki_ang 0 |
ivanff15 | 1:ea4efc600a1e | 124 | #define Kd_ang 0 |
ivanff15 | 0:5af6fad57e1a | 125 | |
ivanff15 | 0:5af6fad57e1a | 126 | short rawAccX, rawAccY, rawAccZ, rawGyroX, rawGyroY, rawGyroZ, rawMagX, rawMagY, rawMagZ; |
ivanff15 | 0:5af6fad57e1a | 127 | unsigned short sendAccX, sendAccY, sendAccZ, sendGyroX, sendGyroY, sendGyroZ, sendMagX, sendMagY, sendMagZ; |
ivanff15 | 1:ea4efc600a1e | 128 | unsigned char baca=0; |
ivanff15 | 1:ea4efc600a1e | 129 | |
ivanff15 | 1:ea4efc600a1e | 130 | //variabel homing |
ivanff15 | 1:ea4efc600a1e | 131 | signed short angle; |
ivanff15 | 1:ea4efc600a1e | 132 | signed short GoalAngle; |
ivanff15 | 1:ea4efc600a1e | 133 | signed short delta_angle_1, delta_angle_2; |
ivanff15 | 1:ea4efc600a1e | 134 | signed char delta; |
ivanff15 | 1:ea4efc600a1e | 135 | int servo_left, servo_right; |
ivanff15 | 1:ea4efc600a1e | 136 | int jump; |
ivanff15 | 1:ea4efc600a1e | 137 | float jump_error; |
ivanff15 | 1:ea4efc600a1e | 138 | float e, ea, ea_sum, ea_old; |
ivanff15 | 1:ea4efc600a1e | 139 | long int delta_lat, delta_long; |
ivanff15 | 1:ea4efc600a1e | 140 | float error, error_sum, error_old; |
ivanff15 | 1:ea4efc600a1e | 141 | ////===================================== |
ivanff15 | 1:ea4efc600a1e | 142 | |
ivanff15 | 1:ea4efc600a1e | 143 | //variabel DCM |
ivanff15 | 1:ea4efc600a1e | 144 | float MAG_Heading; |
ivanff15 | 1:ea4efc600a1e | 145 | float Accel_Vector[3];//= {0, 0, 0}; // Store the acceleration in a vector |
ivanff15 | 1:ea4efc600a1e | 146 | float Gyro_Vector[3];//= {0, 0, 0}; // Store the gyros turn rate in a vector |
ivanff15 | 1:ea4efc600a1e | 147 | float Omega_Vector[3];//= {0, 0, 0}; // Corrected Gyro_Vector data |
ivanff15 | 1:ea4efc600a1e | 148 | float Omega_P[3];//= {0, 0, 0}; // Omega Proportional correction |
ivanff15 | 1:ea4efc600a1e | 149 | float Omega_I[3];//= {0, 0, 0}; // Omega Integrator |
ivanff15 | 1:ea4efc600a1e | 150 | float Omega[3];//= {0, 0, 0}; |
ivanff15 | 1:ea4efc600a1e | 151 | float errorRollPitch[3];// = {0, 0, 0}; |
ivanff15 | 1:ea4efc600a1e | 152 | float errorYaw[3];// = {0, 0, 0}; |
ivanff15 | 1:ea4efc600a1e | 153 | float DCM_Matrix[3][3];// = {{1, 0, 0}, {0, 1, 0}, {0, 0, 1}}; |
ivanff15 | 1:ea4efc600a1e | 154 | float Update_Matrix[3][3];// = {{0, 1, 2}, {3, 4, 5}, {6, 7, 8}}; |
ivanff15 | 1:ea4efc600a1e | 155 | float Temporary_Matrix[3][3];// = {{0, 0, 0}, {0, 0, 0}, {0, 0, 0}}; |
ivanff15 | 1:ea4efc600a1e | 156 | float G_Dt; |
ivanff15 | 1:ea4efc600a1e | 157 | int rawMagnet[3]; |
ivanff15 | 1:ea4efc600a1e | 158 | int gyroBias[3]; |
ivanff15 | 1:ea4efc600a1e | 159 | float accel[3]; // Actually stores the NEGATED acceleration (equals gravity, if board not moving). |
ivanff15 | 1:ea4efc600a1e | 160 | int accelRaw[3]; |
ivanff15 | 1:ea4efc600a1e | 161 | float accel_min[3]; |
ivanff15 | 1:ea4efc600a1e | 162 | float accel_max[3]; |
ivanff15 | 1:ea4efc600a1e | 163 | |
ivanff15 | 1:ea4efc600a1e | 164 | float magnet[3]; |
ivanff15 | 1:ea4efc600a1e | 165 | int magnetRaw[3]; |
ivanff15 | 1:ea4efc600a1e | 166 | float magnetom_min[3]; |
ivanff15 | 1:ea4efc600a1e | 167 | float magnetom_max[3]; |
ivanff15 | 1:ea4efc600a1e | 168 | |
ivanff15 | 1:ea4efc600a1e | 169 | int gyro[3]; |
ivanff15 | 1:ea4efc600a1e | 170 | int gyroRaw[3]; |
ivanff15 | 1:ea4efc600a1e | 171 | float gyro_average[3]; |
ivanff15 | 1:ea4efc600a1e | 172 | |
ivanff15 | 1:ea4efc600a1e | 173 | int gyro_num_samples; |
ivanff15 | 1:ea4efc600a1e | 174 | //euler |
ivanff15 | 1:ea4efc600a1e | 175 | float yaw; |
ivanff15 | 1:ea4efc600a1e | 176 | float pitch; |
ivanff15 | 1:ea4efc600a1e | 177 | float roll; |
ivanff15 | 1:ea4efc600a1e | 178 | //======= |
ivanff15 | 1:ea4efc600a1e | 179 | //================================================================== |
ivanff15 | 0:5af6fad57e1a | 180 | |
ivanff15 | 0:5af6fad57e1a | 181 | void bin_dec_conv(unsigned short data) |
ivanff15 | 0:5af6fad57e1a | 182 | { |
ivanff15 | 0:5af6fad57e1a | 183 | unsigned short hasil; |
ivanff15 | 0:5af6fad57e1a | 184 | // unsigned char kirim[16]; |
ivanff15 | 0:5af6fad57e1a | 185 | |
ivanff15 | 0:5af6fad57e1a | 186 | hasil = data%100; |
ivanff15 | 0:5af6fad57e1a | 187 | pc.printf("%d%d%d",(data/100),(hasil/10),(hasil%10)); |
ivanff15 | 0:5af6fad57e1a | 188 | |
ivanff15 | 0:5af6fad57e1a | 189 | } |
ivanff15 | 0:5af6fad57e1a | 190 | |
ivanff15 | 0:5af6fad57e1a | 191 | void tulis_i2c(unsigned char devadd, unsigned char regadd, unsigned char data) |
ivanff15 | 0:5af6fad57e1a | 192 | { |
ivanff15 | 0:5af6fad57e1a | 193 | i2c.start(); |
ivanff15 | 0:5af6fad57e1a | 194 | i2c.write(devadd); |
ivanff15 | 0:5af6fad57e1a | 195 | i2c.write(regadd); |
ivanff15 | 0:5af6fad57e1a | 196 | i2c.write(data); |
ivanff15 | 0:5af6fad57e1a | 197 | i2c.stop(); |
ivanff15 | 0:5af6fad57e1a | 198 | |
ivanff15 | 0:5af6fad57e1a | 199 | } |
ivanff15 | 0:5af6fad57e1a | 200 | unsigned char baca_i2c(unsigned char devadd, unsigned char regadd) |
ivanff15 | 0:5af6fad57e1a | 201 | { |
ivanff15 | 0:5af6fad57e1a | 202 | unsigned char data; |
ivanff15 | 0:5af6fad57e1a | 203 | i2c.start(); |
ivanff15 | 0:5af6fad57e1a | 204 | i2c.write(devadd); |
ivanff15 | 0:5af6fad57e1a | 205 | i2c.write(regadd); |
ivanff15 | 0:5af6fad57e1a | 206 | i2c.start(); |
ivanff15 | 0:5af6fad57e1a | 207 | i2c.write(devadd|1); |
ivanff15 | 0:5af6fad57e1a | 208 | data=i2c.read(0); |
ivanff15 | 0:5af6fad57e1a | 209 | i2c.stop(); |
ivanff15 | 0:5af6fad57e1a | 210 | return data; |
ivanff15 | 0:5af6fad57e1a | 211 | } |
ivanff15 | 0:5af6fad57e1a | 212 | int baca_adxl() |
ivanff15 | 0:5af6fad57e1a | 213 | { |
ivanff15 | 0:5af6fad57e1a | 214 | unsigned char acc_x_msb, acc_x_lsb; |
ivanff15 | 0:5af6fad57e1a | 215 | unsigned char acc_y_msb, acc_y_lsb; |
ivanff15 | 0:5af6fad57e1a | 216 | unsigned char acc_z_msb, acc_z_lsb; |
ivanff15 | 0:5af6fad57e1a | 217 | |
ivanff15 | 0:5af6fad57e1a | 218 | tulis_i2c(ACCEL, adxl345_reg_pwr_ctl, 0); |
ivanff15 | 0:5af6fad57e1a | 219 | tulis_i2c(ACCEL, adxl345_reg_pwr_ctl, 16); |
ivanff15 | 0:5af6fad57e1a | 220 | tulis_i2c(ACCEL, adxl345_reg_pwr_ctl, 8); |
ivanff15 | 0:5af6fad57e1a | 221 | |
ivanff15 | 0:5af6fad57e1a | 222 | tulis_i2c(ACCEL, adxl345_reg_data_format, 0x03); |
ivanff15 | 0:5af6fad57e1a | 223 | |
ivanff15 | 0:5af6fad57e1a | 224 | acc_x_lsb = baca_i2c(ACCEL,0x32); |
ivanff15 | 0:5af6fad57e1a | 225 | acc_x_msb = baca_i2c(ACCEL,0x33); |
ivanff15 | 0:5af6fad57e1a | 226 | acc_y_lsb = baca_i2c(ACCEL,0x34); |
ivanff15 | 0:5af6fad57e1a | 227 | acc_y_msb = baca_i2c(ACCEL,0x35); |
ivanff15 | 0:5af6fad57e1a | 228 | acc_z_lsb = baca_i2c(ACCEL,0x36); |
ivanff15 | 0:5af6fad57e1a | 229 | acc_z_msb = baca_i2c(ACCEL,0x37); |
ivanff15 | 0:5af6fad57e1a | 230 | |
ivanff15 | 0:5af6fad57e1a | 231 | float acc_x = (signed short)((signed short)(acc_x_msb<<8) | acc_x_lsb);//*ADXL345_AXIS_X_SCALE/1000.0f; |
ivanff15 | 0:5af6fad57e1a | 232 | float acc_y = 1*(signed short)((signed short)(acc_y_msb<<8) | acc_y_lsb);//*ADXL345_AXIS_X_SCALE/1000.0f; |
ivanff15 | 0:5af6fad57e1a | 233 | float acc_z = (signed short)((signed short)(acc_z_msb<<8) | acc_z_lsb);//*ADXL345_AXIS_X_SCALE/1000.0f |
ivanff15 | 0:5af6fad57e1a | 234 | |
ivanff15 | 1:ea4efc600a1e | 235 | // rawAccX=-1*acc_y; |
ivanff15 | 1:ea4efc600a1e | 236 | // rawAccY=acc_x; |
ivanff15 | 1:ea4efc600a1e | 237 | // rawAccZ=acc_z; |
ivanff15 | 1:ea4efc600a1e | 238 | rawAccX=acc_x; |
ivanff15 | 1:ea4efc600a1e | 239 | rawAccY=acc_y; |
ivanff15 | 0:5af6fad57e1a | 240 | rawAccZ=acc_z; |
ivanff15 | 1:ea4efc600a1e | 241 | |
ivanff15 | 0:5af6fad57e1a | 242 | } |
ivanff15 | 0:5af6fad57e1a | 243 | |
ivanff15 | 1:ea4efc600a1e | 244 | |
ivanff15 | 1:ea4efc600a1e | 245 | |
ivanff15 | 0:5af6fad57e1a | 246 | void baca_itg() |
ivanff15 | 0:5af6fad57e1a | 247 | { |
ivanff15 | 0:5af6fad57e1a | 248 | tulis_i2c(itg3200_address, itg3200_reg_dlpf_fs, (DLPF_FS_SEL_0|DLPF_FS_SEL_1|DLPF_CFG_0)); |
ivanff15 | 0:5af6fad57e1a | 249 | tulis_i2c(itg3200_address, itg3200_reg_smplrt_div, 9); |
ivanff15 | 0:5af6fad57e1a | 250 | char xh,xl,yh,yl,zh,zl; |
ivanff15 | 0:5af6fad57e1a | 251 | short x,y,z; |
ivanff15 | 0:5af6fad57e1a | 252 | xl = baca_i2c(itg3200_address,0x1E); |
ivanff15 | 0:5af6fad57e1a | 253 | xh = baca_i2c(itg3200_address,0x1D); |
ivanff15 | 0:5af6fad57e1a | 254 | yh = baca_i2c(itg3200_address,0x1F); |
ivanff15 | 0:5af6fad57e1a | 255 | yl = baca_i2c(itg3200_address,0x20); |
ivanff15 | 0:5af6fad57e1a | 256 | zh = baca_i2c(itg3200_address,0x21); |
ivanff15 | 0:5af6fad57e1a | 257 | zl = baca_i2c(itg3200_address,0x22); |
ivanff15 | 0:5af6fad57e1a | 258 | |
ivanff15 | 0:5af6fad57e1a | 259 | x=1*(signed short)((signed short)(xl | (xh<<8)));//*0.0695652174; |
ivanff15 | 0:5af6fad57e1a | 260 | y=1*(signed short)((signed short)(yl | (yh<<8)));//*0.0695652174; |
ivanff15 | 0:5af6fad57e1a | 261 | z=1*(signed short)((signed short)(zl | (zh<<8))); |
ivanff15 | 1:ea4efc600a1e | 262 | // |
ivanff15 | 1:ea4efc600a1e | 263 | // rawGyroX=-1*y; |
ivanff15 | 1:ea4efc600a1e | 264 | // rawGyroY=x; |
ivanff15 | 1:ea4efc600a1e | 265 | // rawGyroZ=z; |
ivanff15 | 1:ea4efc600a1e | 266 | |
ivanff15 | 1:ea4efc600a1e | 267 | rawGyroX=x; |
ivanff15 | 1:ea4efc600a1e | 268 | rawGyroY=y; |
ivanff15 | 0:5af6fad57e1a | 269 | rawGyroZ=z; |
ivanff15 | 0:5af6fad57e1a | 270 | } |
ivanff15 | 0:5af6fad57e1a | 271 | |
ivanff15 | 0:5af6fad57e1a | 272 | void i2c_tulis_hmc(unsigned char address, unsigned char data) |
ivanff15 | 0:5af6fad57e1a | 273 | { |
ivanff15 | 0:5af6fad57e1a | 274 | i2c.start(); |
ivanff15 | 0:5af6fad57e1a | 275 | i2c.write(HMC5843_W); // write 0x |
ivanff15 | 0:5af6fad57e1a | 276 | i2c.write(address); // write register address |
ivanff15 | 0:5af6fad57e1a | 277 | i2c.write(data); // write register address |
ivanff15 | 0:5af6fad57e1a | 278 | i2c.stop(); |
ivanff15 | 0:5af6fad57e1a | 279 | //__delay_ms(10); |
ivanff15 | 0:5af6fad57e1a | 280 | } |
ivanff15 | 0:5af6fad57e1a | 281 | |
ivanff15 | 0:5af6fad57e1a | 282 | unsigned char i2c_baca_hmc(unsigned char address) |
ivanff15 | 0:5af6fad57e1a | 283 | { |
ivanff15 | 0:5af6fad57e1a | 284 | unsigned char data; |
ivanff15 | 0:5af6fad57e1a | 285 | i2c.start(); |
ivanff15 | 0:5af6fad57e1a | 286 | i2c.write(HMC5843_W); // write 0x |
ivanff15 | 0:5af6fad57e1a | 287 | i2c.write(address); // write register address |
ivanff15 | 0:5af6fad57e1a | 288 | i2c.start(); |
ivanff15 | 0:5af6fad57e1a | 289 | i2c.write(HMC5843_R); // write 0x |
ivanff15 | 0:5af6fad57e1a | 290 | data = i2c.read(0); // Get MSB result |
ivanff15 | 0:5af6fad57e1a | 291 | i2c.stop(); |
ivanff15 | 0:5af6fad57e1a | 292 | //__delay_ms(10); |
ivanff15 | 0:5af6fad57e1a | 293 | return data; |
ivanff15 | 0:5af6fad57e1a | 294 | } |
ivanff15 | 0:5af6fad57e1a | 295 | |
ivanff15 | 0:5af6fad57e1a | 296 | int baca_hmc() // baca_itg(raw) untuk ambil data raw : baca_itg(scaled) untuk ambil data terskala |
ivanff15 | 0:5af6fad57e1a | 297 | { |
ivanff15 | 0:5af6fad57e1a | 298 | // char xh, xl, yh, yl, zh, zl; |
ivanff15 | 0:5af6fad57e1a | 299 | // short x, y, z; |
ivanff15 | 0:5af6fad57e1a | 300 | int xh, xl, yh, yl, zh, zl; |
ivanff15 | 0:5af6fad57e1a | 301 | float x, y, z; |
ivanff15 | 0:5af6fad57e1a | 302 | |
ivanff15 | 0:5af6fad57e1a | 303 | i2c_tulis_hmc(0x02,0x01); |
ivanff15 | 0:5af6fad57e1a | 304 | xh=i2c_baca_hmc(0x03); |
ivanff15 | 0:5af6fad57e1a | 305 | xl=i2c_baca_hmc(0x04); |
ivanff15 | 0:5af6fad57e1a | 306 | zh=i2c_baca_hmc(0x05); |
ivanff15 | 0:5af6fad57e1a | 307 | zl=i2c_baca_hmc(0x06); |
ivanff15 | 0:5af6fad57e1a | 308 | yh=i2c_baca_hmc(0x07); |
ivanff15 | 0:5af6fad57e1a | 309 | yl=i2c_baca_hmc(0x08); |
ivanff15 | 0:5af6fad57e1a | 310 | x=(signed short)((signed short)(xl | (xh<<8))); |
ivanff15 | 0:5af6fad57e1a | 311 | y=(signed short)((signed short)(yl | (yh<<8))); |
ivanff15 | 0:5af6fad57e1a | 312 | z=(signed short)((signed short)(zl | (zh<<8))); |
ivanff15 | 0:5af6fad57e1a | 313 | rawMagX=x; |
ivanff15 | 0:5af6fad57e1a | 314 | rawMagY=y; |
ivanff15 | 0:5af6fad57e1a | 315 | rawMagZ=z; |
ivanff15 | 1:ea4efc600a1e | 316 | // rawMagX=-1*x; |
ivanff15 | 1:ea4efc600a1e | 317 | // rawMagY=1*y; |
ivanff15 | 1:ea4efc600a1e | 318 | // rawMagZ=1*z; |
ivanff15 | 0:5af6fad57e1a | 319 | } |
ivanff15 | 0:5af6fad57e1a | 320 | |
ivanff15 | 1:ea4efc600a1e | 321 | void setAccelRaw(int accelRawX,int accelRawY,int accelRawZ) |
ivanff15 | 1:ea4efc600a1e | 322 | { |
ivanff15 | 1:ea4efc600a1e | 323 | accelRaw[0] = (uint16_t)accelRawX; |
ivanff15 | 1:ea4efc600a1e | 324 | accelRaw[1] = (uint16_t)accelRawY; |
ivanff15 | 1:ea4efc600a1e | 325 | accelRaw[2] = (uint16_t)accelRawZ; |
ivanff15 | 1:ea4efc600a1e | 326 | } |
ivanff15 | 1:ea4efc600a1e | 327 | void setGyroRaw(int gyroRawX,int gyroRawY,int gyroRawZ) |
ivanff15 | 1:ea4efc600a1e | 328 | { |
ivanff15 | 1:ea4efc600a1e | 329 | gyroRaw[0] = gyroRawX; |
ivanff15 | 1:ea4efc600a1e | 330 | gyroRaw[1] = gyroRawY; |
ivanff15 | 1:ea4efc600a1e | 331 | gyroRaw[2] = gyroRawZ; |
ivanff15 | 1:ea4efc600a1e | 332 | } |
ivanff15 | 1:ea4efc600a1e | 333 | void setMagnetRaw(int magnetRawX,int magnetRawY,int magnetRawZ) |
ivanff15 | 1:ea4efc600a1e | 334 | { |
ivanff15 | 1:ea4efc600a1e | 335 | magnetRaw[0] = (uint16_t)magnetRawX; |
ivanff15 | 1:ea4efc600a1e | 336 | magnetRaw[1] = (uint16_t)magnetRawY; |
ivanff15 | 1:ea4efc600a1e | 337 | magnetRaw[2] = (uint16_t)magnetRawZ; |
ivanff15 | 1:ea4efc600a1e | 338 | } |
ivanff15 | 1:ea4efc600a1e | 339 | |
ivanff15 | 1:ea4efc600a1e | 340 | void ReadMagnetometer() |
ivanff15 | 1:ea4efc600a1e | 341 | { |
ivanff15 | 1:ea4efc600a1e | 342 | rawMagnet[0] = ((int16_t)magnetRaw[0]); |
ivanff15 | 1:ea4efc600a1e | 343 | rawMagnet[1] = ((int16_t)magnetRaw[1]); |
ivanff15 | 1:ea4efc600a1e | 344 | rawMagnet[2] = ((int16_t)magnetRaw[2]); |
ivanff15 | 1:ea4efc600a1e | 345 | |
ivanff15 | 1:ea4efc600a1e | 346 | magnet[0] = ((float)(rawMagnet[0] - MAGN_X_OFFSET) * MAGN_X_SCALE); |
ivanff15 | 1:ea4efc600a1e | 347 | magnet[1] = ((float)(rawMagnet[1] - MAGN_Y_OFFSET) * MAGN_Y_SCALE); |
ivanff15 | 1:ea4efc600a1e | 348 | magnet[2] = ((float)(rawMagnet[2] - MAGN_Z_OFFSET) * MAGN_Z_SCALE); |
ivanff15 | 1:ea4efc600a1e | 349 | } |
ivanff15 | 1:ea4efc600a1e | 350 | void ReadAccelerometer() |
ivanff15 | 1:ea4efc600a1e | 351 | { |
ivanff15 | 1:ea4efc600a1e | 352 | accel[0] = (((int16_t)accelRaw[0]) - ACCEL_X_OFFSET) * ACCEL_X_SCALE; |
ivanff15 | 1:ea4efc600a1e | 353 | accel[1] = (((int16_t)accelRaw[1]) - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE; |
ivanff15 | 1:ea4efc600a1e | 354 | accel[2] = (((int16_t)accelRaw[2]) - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE; |
ivanff15 | 1:ea4efc600a1e | 355 | } |
ivanff15 | 1:ea4efc600a1e | 356 | void ReadGyroscope() |
ivanff15 | 1:ea4efc600a1e | 357 | { |
ivanff15 | 1:ea4efc600a1e | 358 | gyro[0] = (gyroRaw[0] - GYRO_BIAS_X); |
ivanff15 | 1:ea4efc600a1e | 359 | gyro[1] = (gyroRaw[1] - GYRO_BIAS_Y); |
ivanff15 | 1:ea4efc600a1e | 360 | gyro[2] = (gyroRaw[2] - GYRO_BIAS_Z); |
ivanff15 | 1:ea4efc600a1e | 361 | } |
ivanff15 | 1:ea4efc600a1e | 362 | |
ivanff15 | 1:ea4efc600a1e | 363 | void Compass_Heading() |
ivanff15 | 1:ea4efc600a1e | 364 | { |
ivanff15 | 1:ea4efc600a1e | 365 | float mag_x; |
ivanff15 | 1:ea4efc600a1e | 366 | float mag_y; |
ivanff15 | 1:ea4efc600a1e | 367 | float cos_roll; |
ivanff15 | 1:ea4efc600a1e | 368 | float sin_roll; |
ivanff15 | 1:ea4efc600a1e | 369 | float cos_pitch; |
ivanff15 | 1:ea4efc600a1e | 370 | float sin_pitch; |
ivanff15 | 1:ea4efc600a1e | 371 | |
ivanff15 | 1:ea4efc600a1e | 372 | cos_roll = cos(roll); |
ivanff15 | 1:ea4efc600a1e | 373 | sin_roll = sin(roll); |
ivanff15 | 1:ea4efc600a1e | 374 | cos_pitch = cos(pitch); |
ivanff15 | 1:ea4efc600a1e | 375 | sin_pitch = sin(pitch); |
ivanff15 | 1:ea4efc600a1e | 376 | |
ivanff15 | 1:ea4efc600a1e | 377 | // Tilt compensated magnetic field X |
ivanff15 | 1:ea4efc600a1e | 378 | mag_x = magnet[0]*cos_pitch + magnet[1]*sin_roll*sin_pitch + magnet[2]*cos_roll*sin_pitch; |
ivanff15 | 1:ea4efc600a1e | 379 | // Tilt compensated magnetic field Y |
ivanff15 | 1:ea4efc600a1e | 380 | mag_y = magnet[1]*cos_roll - magnet[2]*sin_roll; |
ivanff15 | 1:ea4efc600a1e | 381 | // Magnetic Heading |
ivanff15 | 1:ea4efc600a1e | 382 | MAG_Heading = atan2(-mag_y, mag_x); |
ivanff15 | 1:ea4efc600a1e | 383 | } |
ivanff15 | 1:ea4efc600a1e | 384 | //=========fungsi matrix====== |
ivanff15 | 1:ea4efc600a1e | 385 | void Vector_Cross_Product(float C[3], float A[3], float B[3]) |
ivanff15 | 1:ea4efc600a1e | 386 | { |
ivanff15 | 1:ea4efc600a1e | 387 | C[0] = (A[1] * B[2]) - (A[2] * B[1]); |
ivanff15 | 1:ea4efc600a1e | 388 | C[1] = (A[2] * B[0]) - (A[0] * B[2]); |
ivanff15 | 1:ea4efc600a1e | 389 | C[2] = (A[0] * B[1]) - (A[1] * B[0]); |
ivanff15 | 1:ea4efc600a1e | 390 | |
ivanff15 | 1:ea4efc600a1e | 391 | return; |
ivanff15 | 1:ea4efc600a1e | 392 | } |
ivanff15 | 1:ea4efc600a1e | 393 | |
ivanff15 | 1:ea4efc600a1e | 394 | void Vector_Scale(float C[3], float A[3], float b) |
ivanff15 | 1:ea4efc600a1e | 395 | { |
ivanff15 | 1:ea4efc600a1e | 396 | int m; |
ivanff15 | 1:ea4efc600a1e | 397 | for (m = 0; m < 3; m++) |
ivanff15 | 1:ea4efc600a1e | 398 | C[m] = A[m] * b; |
ivanff15 | 1:ea4efc600a1e | 399 | |
ivanff15 | 1:ea4efc600a1e | 400 | return; |
ivanff15 | 1:ea4efc600a1e | 401 | } |
ivanff15 | 1:ea4efc600a1e | 402 | |
ivanff15 | 1:ea4efc600a1e | 403 | float Vector_Dot_Product(float A[3], float B[3]) |
ivanff15 | 1:ea4efc600a1e | 404 | { |
ivanff15 | 1:ea4efc600a1e | 405 | float result = 0.0; |
ivanff15 | 1:ea4efc600a1e | 406 | |
ivanff15 | 1:ea4efc600a1e | 407 | int i; |
ivanff15 | 1:ea4efc600a1e | 408 | for (i = 0; i < 3; i++) { |
ivanff15 | 1:ea4efc600a1e | 409 | result += A[i] * B[i]; |
ivanff15 | 1:ea4efc600a1e | 410 | } |
ivanff15 | 1:ea4efc600a1e | 411 | |
ivanff15 | 1:ea4efc600a1e | 412 | return result; |
ivanff15 | 1:ea4efc600a1e | 413 | } |
ivanff15 | 1:ea4efc600a1e | 414 | |
ivanff15 | 1:ea4efc600a1e | 415 | void Vector_Add1(float C[3], float A[3], float B[3]) |
ivanff15 | 1:ea4efc600a1e | 416 | { |
ivanff15 | 1:ea4efc600a1e | 417 | int m; |
ivanff15 | 1:ea4efc600a1e | 418 | for (m = 0; m < 3; m++) |
ivanff15 | 1:ea4efc600a1e | 419 | C[m] = A[m] + B[m]; |
ivanff15 | 1:ea4efc600a1e | 420 | return; |
ivanff15 | 1:ea4efc600a1e | 421 | } |
ivanff15 | 1:ea4efc600a1e | 422 | |
ivanff15 | 1:ea4efc600a1e | 423 | void Vector_Add(float C[3][3], float A[3][3], float B[3][3]) |
ivanff15 | 1:ea4efc600a1e | 424 | { |
ivanff15 | 1:ea4efc600a1e | 425 | int m,n; |
ivanff15 | 1:ea4efc600a1e | 426 | for (m = 0; m < 3; m++) |
ivanff15 | 1:ea4efc600a1e | 427 | for (n = 0; n < 3; n++) |
ivanff15 | 1:ea4efc600a1e | 428 | C[m][n] = A[m][n] + B[m][n]; |
ivanff15 | 1:ea4efc600a1e | 429 | } |
ivanff15 | 1:ea4efc600a1e | 430 | |
ivanff15 | 1:ea4efc600a1e | 431 | void Matrix_Multiply(float C[3][3], float A[3][3], float B[3][3]) |
ivanff15 | 1:ea4efc600a1e | 432 | { |
ivanff15 | 1:ea4efc600a1e | 433 | int i,j,k; |
ivanff15 | 1:ea4efc600a1e | 434 | for (i = 0; i < 3; i++) { |
ivanff15 | 1:ea4efc600a1e | 435 | for (j = 0; j < 3; j++) { |
ivanff15 | 1:ea4efc600a1e | 436 | C[i][j] = 0; |
ivanff15 | 1:ea4efc600a1e | 437 | for (k = 0; k < 3; k++) { |
ivanff15 | 1:ea4efc600a1e | 438 | C[i][j] += A[i][k] * B[k][j]; |
ivanff15 | 1:ea4efc600a1e | 439 | } |
ivanff15 | 1:ea4efc600a1e | 440 | } |
ivanff15 | 1:ea4efc600a1e | 441 | } |
ivanff15 | 1:ea4efc600a1e | 442 | } |
ivanff15 | 1:ea4efc600a1e | 443 | |
ivanff15 | 1:ea4efc600a1e | 444 | //=========end matrix========= |
ivanff15 | 1:ea4efc600a1e | 445 | |
ivanff15 | 1:ea4efc600a1e | 446 | //=====mulai DCM====== |
ivanff15 | 1:ea4efc600a1e | 447 | //dari objek terhadap tanah |
ivanff15 | 1:ea4efc600a1e | 448 | void init_rotation_matrix(float m[3][3], float yaw, float pitch, float roll) |
ivanff15 | 1:ea4efc600a1e | 449 | { |
ivanff15 | 1:ea4efc600a1e | 450 | float c1 = cos(roll); |
ivanff15 | 1:ea4efc600a1e | 451 | float s1 = sin(roll); |
ivanff15 | 1:ea4efc600a1e | 452 | float c2 = cos(pitch); |
ivanff15 | 1:ea4efc600a1e | 453 | float s2 = sin(pitch); |
ivanff15 | 1:ea4efc600a1e | 454 | float c3 = cos(yaw); |
ivanff15 | 1:ea4efc600a1e | 455 | float s3 = sin(yaw); |
ivanff15 | 1:ea4efc600a1e | 456 | |
ivanff15 | 1:ea4efc600a1e | 457 | // Euler angles, right-handed, intrinsic, XYZ convention |
ivanff15 | 1:ea4efc600a1e | 458 | // (which means: rotate around body axes Z, Y', X'') |
ivanff15 | 1:ea4efc600a1e | 459 | m[0][0] = c2 * c3; |
ivanff15 | 1:ea4efc600a1e | 460 | m[0][1] = c3 * s1 * s2 - c1 * s3; |
ivanff15 | 1:ea4efc600a1e | 461 | m[0][2] = s1 * s3 + c1 * c3 * s2; |
ivanff15 | 1:ea4efc600a1e | 462 | |
ivanff15 | 1:ea4efc600a1e | 463 | m[1][0] = c2 * s3; |
ivanff15 | 1:ea4efc600a1e | 464 | m[1][1] = c1 * c3 + s1 * s2 * s3; |
ivanff15 | 1:ea4efc600a1e | 465 | m[1][2] = c1 * s2 * s3 - c3 * s1; |
ivanff15 | 1:ea4efc600a1e | 466 | |
ivanff15 | 1:ea4efc600a1e | 467 | m[2][0] = -s2; |
ivanff15 | 1:ea4efc600a1e | 468 | m[2][1] = c2 * s1; |
ivanff15 | 1:ea4efc600a1e | 469 | m[2][2] = c1 * c2; |
ivanff15 | 1:ea4efc600a1e | 470 | } |
ivanff15 | 1:ea4efc600a1e | 471 | |
ivanff15 | 1:ea4efc600a1e | 472 | float constrain(float in, float min, float max) |
ivanff15 | 1:ea4efc600a1e | 473 | { |
ivanff15 | 1:ea4efc600a1e | 474 | in = in > max ? max : in; |
ivanff15 | 1:ea4efc600a1e | 475 | in = in < min ? min : in; |
ivanff15 | 1:ea4efc600a1e | 476 | return in; |
ivanff15 | 1:ea4efc600a1e | 477 | } |
ivanff15 | 1:ea4efc600a1e | 478 | |
ivanff15 | 1:ea4efc600a1e | 479 | void Normalize() |
ivanff15 | 1:ea4efc600a1e | 480 | { |
ivanff15 | 1:ea4efc600a1e | 481 | float error=0; |
ivanff15 | 1:ea4efc600a1e | 482 | float temporary[3][3]; |
ivanff15 | 1:ea4efc600a1e | 483 | float renorm=0; |
ivanff15 | 1:ea4efc600a1e | 484 | |
ivanff15 | 1:ea4efc600a1e | 485 | error= -Vector_Dot_Product(&DCM_Matrix[0][0],&DCM_Matrix[1][0])*.5; //eq.19 |
ivanff15 | 1:ea4efc600a1e | 486 | |
ivanff15 | 1:ea4efc600a1e | 487 | Vector_Scale(&temporary[0][0], &DCM_Matrix[1][0], error); //eq.19 |
ivanff15 | 1:ea4efc600a1e | 488 | Vector_Scale(&temporary[1][0], &DCM_Matrix[0][0], error); //eq.19 |
ivanff15 | 1:ea4efc600a1e | 489 | Vector_Add1(&temporary[0][0], &temporary[0][0], &DCM_Matrix[0][0]);//eq.19 |
ivanff15 | 1:ea4efc600a1e | 490 | Vector_Add1(&temporary[1][0], &temporary[1][0], &DCM_Matrix[1][0]);//eq.19 |
ivanff15 | 1:ea4efc600a1e | 491 | |
ivanff15 | 1:ea4efc600a1e | 492 | Vector_Cross_Product(&temporary[2][0],&temporary[0][0],&temporary[1][0]); // c= a x b //eq.20 |
ivanff15 | 1:ea4efc600a1e | 493 | |
ivanff15 | 1:ea4efc600a1e | 494 | renorm= .5 *(3 - Vector_Dot_Product(&temporary[0][0],&temporary[0][0])); //eq.21 |
ivanff15 | 1:ea4efc600a1e | 495 | Vector_Scale(&DCM_Matrix[0][0], &temporary[0][0], renorm); |
ivanff15 | 1:ea4efc600a1e | 496 | |
ivanff15 | 1:ea4efc600a1e | 497 | renorm= .5 *(3 - Vector_Dot_Product(&temporary[1][0],&temporary[1][0])); //eq.21 |
ivanff15 | 1:ea4efc600a1e | 498 | Vector_Scale(&DCM_Matrix[1][0], &temporary[1][0], renorm); |
ivanff15 | 1:ea4efc600a1e | 499 | |
ivanff15 | 1:ea4efc600a1e | 500 | renorm= .5 *(3 - Vector_Dot_Product(&temporary[2][0],&temporary[2][0])); //eq.21 |
ivanff15 | 1:ea4efc600a1e | 501 | Vector_Scale(&DCM_Matrix[2][0], &temporary[2][0], renorm); |
ivanff15 | 1:ea4efc600a1e | 502 | } |
ivanff15 | 1:ea4efc600a1e | 503 | |
ivanff15 | 1:ea4efc600a1e | 504 | void Drift_correction() |
ivanff15 | 1:ea4efc600a1e | 505 | { |
ivanff15 | 1:ea4efc600a1e | 506 | float mag_heading_x; |
ivanff15 | 1:ea4efc600a1e | 507 | float mag_heading_y; |
ivanff15 | 1:ea4efc600a1e | 508 | float errorCourse; |
ivanff15 | 1:ea4efc600a1e | 509 | //Compensation the Roll, Pitch and Yaw drift. |
ivanff15 | 1:ea4efc600a1e | 510 | static float Scaled_Omega_P[3]; |
ivanff15 | 1:ea4efc600a1e | 511 | static float Scaled_Omega_I[3]; |
ivanff15 | 1:ea4efc600a1e | 512 | float Accel_magnitude; |
ivanff15 | 1:ea4efc600a1e | 513 | float Accel_weight; |
ivanff15 | 1:ea4efc600a1e | 514 | //*****Roll and Pitch*************** |
ivanff15 | 1:ea4efc600a1e | 515 | |
ivanff15 | 1:ea4efc600a1e | 516 | // Calculate the magnitude of the accelerometer vector |
ivanff15 | 1:ea4efc600a1e | 517 | Accel_magnitude = sqrt(Accel_Vector[0]*Accel_Vector[0] + Accel_Vector[1]*Accel_Vector[1] + Accel_Vector[2]*Accel_Vector[2]); |
ivanff15 | 1:ea4efc600a1e | 518 | Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity. |
ivanff15 | 1:ea4efc600a1e | 519 | // Dynamic weighting of accelerometer info (reliability filter) |
ivanff15 | 1:ea4efc600a1e | 520 | // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0) |
ivanff15 | 1:ea4efc600a1e | 521 | Accel_weight = constrain(1 - 2*abs(1 - Accel_magnitude),0,1); // |
ivanff15 | 1:ea4efc600a1e | 522 | |
ivanff15 | 1:ea4efc600a1e | 523 | Vector_Cross_Product(&errorRollPitch[0],&Accel_Vector[0],&DCM_Matrix[2][0]); //adjust the ground of reference |
ivanff15 | 1:ea4efc600a1e | 524 | Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight); |
ivanff15 | 1:ea4efc600a1e | 525 | Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight); |
ivanff15 | 1:ea4efc600a1e | 526 | Vector_Add1(Omega_I,Omega_I,Scaled_Omega_I); |
ivanff15 | 1:ea4efc600a1e | 527 | //*****YAW*************** |
ivanff15 | 1:ea4efc600a1e | 528 | // We make the gyro YAW drift correction based on compass magnetic heading |
ivanff15 | 1:ea4efc600a1e | 529 | |
ivanff15 | 1:ea4efc600a1e | 530 | mag_heading_x = cos(MAG_Heading); |
ivanff15 | 1:ea4efc600a1e | 531 | mag_heading_y = sin(MAG_Heading); |
ivanff15 | 1:ea4efc600a1e | 532 | errorCourse=(DCM_Matrix[0][0]*mag_heading_y) - (DCM_Matrix[1][0]*mag_heading_x); //Calculating YAW error |
ivanff15 | 1:ea4efc600a1e | 533 | Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position. |
ivanff15 | 1:ea4efc600a1e | 534 | |
ivanff15 | 1:ea4efc600a1e | 535 | Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);//.01proportional of YAW. |
ivanff15 | 1:ea4efc600a1e | 536 | Vector_Add1(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional. |
ivanff15 | 1:ea4efc600a1e | 537 | |
ivanff15 | 1:ea4efc600a1e | 538 | Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);//.00001Integrator |
ivanff15 | 1:ea4efc600a1e | 539 | Vector_Add1(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I |
ivanff15 | 1:ea4efc600a1e | 540 | } |
ivanff15 | 1:ea4efc600a1e | 541 | void Matrix_update() |
ivanff15 | 1:ea4efc600a1e | 542 | { |
ivanff15 | 1:ea4efc600a1e | 543 | Gyro_Vector[0]=GYRO_SCALED_RAD(gyro[0]); //gyro x roll |
ivanff15 | 1:ea4efc600a1e | 544 | Gyro_Vector[1]=GYRO_SCALED_RAD(gyro[1]); //gyro y pitch |
ivanff15 | 1:ea4efc600a1e | 545 | Gyro_Vector[2]=GYRO_SCALED_RAD(gyro[2]); //gyro z yaw |
ivanff15 | 1:ea4efc600a1e | 546 | |
ivanff15 | 1:ea4efc600a1e | 547 | Accel_Vector[0]=accel[0]; |
ivanff15 | 1:ea4efc600a1e | 548 | Accel_Vector[1]=accel[1]; |
ivanff15 | 1:ea4efc600a1e | 549 | Accel_Vector[2]=accel[2]; |
ivanff15 | 1:ea4efc600a1e | 550 | |
ivanff15 | 1:ea4efc600a1e | 551 | Vector_Add1(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term |
ivanff15 | 1:ea4efc600a1e | 552 | Vector_Add1(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term |
ivanff15 | 1:ea4efc600a1e | 553 | |
ivanff15 | 1:ea4efc600a1e | 554 | Update_Matrix[0][0]=0; |
ivanff15 | 1:ea4efc600a1e | 555 | Update_Matrix[0][1]=-G_Dt*Omega_Vector[2];//-z |
ivanff15 | 1:ea4efc600a1e | 556 | Update_Matrix[0][2]=G_Dt*Omega_Vector[1];//y |
ivanff15 | 1:ea4efc600a1e | 557 | Update_Matrix[1][0]=G_Dt*Omega_Vector[2];//z |
ivanff15 | 1:ea4efc600a1e | 558 | Update_Matrix[1][1]=0; |
ivanff15 | 1:ea4efc600a1e | 559 | Update_Matrix[1][2]=-G_Dt*Omega_Vector[0]; |
ivanff15 | 1:ea4efc600a1e | 560 | Update_Matrix[2][0]=-G_Dt*Omega_Vector[1]; |
ivanff15 | 1:ea4efc600a1e | 561 | Update_Matrix[2][1]=G_Dt*Omega_Vector[0]; |
ivanff15 | 1:ea4efc600a1e | 562 | Update_Matrix[2][2]=0; |
ivanff15 | 1:ea4efc600a1e | 563 | |
ivanff15 | 1:ea4efc600a1e | 564 | Matrix_Multiply(Temporary_Matrix,DCM_Matrix,Update_Matrix); //a*b=c |
ivanff15 | 1:ea4efc600a1e | 565 | |
ivanff15 | 1:ea4efc600a1e | 566 | int x,y; |
ivanff15 | 1:ea4efc600a1e | 567 | for(x=0; x<3; x++) //Matrix Addition (update) |
ivanff15 | 1:ea4efc600a1e | 568 | { |
ivanff15 | 1:ea4efc600a1e | 569 | for(y=0; y<3; y++) |
ivanff15 | 1:ea4efc600a1e | 570 | { |
ivanff15 | 1:ea4efc600a1e | 571 | DCM_Matrix[x][y]+=Temporary_Matrix[x][y]; |
ivanff15 | 1:ea4efc600a1e | 572 | } |
ivanff15 | 1:ea4efc600a1e | 573 | } |
ivanff15 | 1:ea4efc600a1e | 574 | } |
ivanff15 | 1:ea4efc600a1e | 575 | void Euler_angles() |
ivanff15 | 1:ea4efc600a1e | 576 | { |
ivanff15 | 1:ea4efc600a1e | 577 | pitch = -asin(DCM_Matrix[2][0]); |
ivanff15 | 1:ea4efc600a1e | 578 | roll = atan2(DCM_Matrix[2][1],DCM_Matrix[2][2]); |
ivanff15 | 1:ea4efc600a1e | 579 | yaw = atan2(DCM_Matrix[1][0],DCM_Matrix[0][0]); |
ivanff15 | 1:ea4efc600a1e | 580 | } |
ivanff15 | 1:ea4efc600a1e | 581 | void Sensors() |
ivanff15 | 1:ea4efc600a1e | 582 | { |
ivanff15 | 1:ea4efc600a1e | 583 | ReadAccelerometer(); |
ivanff15 | 1:ea4efc600a1e | 584 | ReadGyroscope(); |
ivanff15 | 1:ea4efc600a1e | 585 | ReadMagnetometer(); |
ivanff15 | 1:ea4efc600a1e | 586 | Compass_Heading(); |
ivanff15 | 1:ea4efc600a1e | 587 | } |
ivanff15 | 1:ea4efc600a1e | 588 | void UpdateFilter() |
ivanff15 | 1:ea4efc600a1e | 589 | { |
ivanff15 | 1:ea4efc600a1e | 590 | Sensors(); |
ivanff15 | 1:ea4efc600a1e | 591 | Matrix_update(); |
ivanff15 | 1:ea4efc600a1e | 592 | Normalize(); |
ivanff15 | 1:ea4efc600a1e | 593 | Drift_correction(); |
ivanff15 | 1:ea4efc600a1e | 594 | Euler_angles(); |
ivanff15 | 1:ea4efc600a1e | 595 | } |
ivanff15 | 1:ea4efc600a1e | 596 | void UpdateFilter2() |
ivanff15 | 1:ea4efc600a1e | 597 | { |
ivanff15 | 1:ea4efc600a1e | 598 | baca_itg(); |
ivanff15 | 1:ea4efc600a1e | 599 | baca_hmc(); |
ivanff15 | 1:ea4efc600a1e | 600 | baca_adxl(); |
ivanff15 | 1:ea4efc600a1e | 601 | |
ivanff15 | 1:ea4efc600a1e | 602 | setAccelRaw(rawAccX,rawAccY,rawAccZ); |
ivanff15 | 1:ea4efc600a1e | 603 | |
ivanff15 | 1:ea4efc600a1e | 604 | setGyroRaw(rawGyroX,rawGyroY,rawGyroZ); |
ivanff15 | 1:ea4efc600a1e | 605 | |
ivanff15 | 1:ea4efc600a1e | 606 | setMagnetRaw(rawMagX,rawMagY,rawMagZ); |
ivanff15 | 1:ea4efc600a1e | 607 | UpdateFilter(); |
ivanff15 | 1:ea4efc600a1e | 608 | } |
ivanff15 | 1:ea4efc600a1e | 609 | void reset_sensor_fusion() { |
ivanff15 | 1:ea4efc600a1e | 610 | float temp1[3]; |
ivanff15 | 1:ea4efc600a1e | 611 | float temp2[3]; |
ivanff15 | 1:ea4efc600a1e | 612 | float xAxis[] = {1.0f, 0.0f, 0.0f}; |
ivanff15 | 1:ea4efc600a1e | 613 | |
ivanff15 | 1:ea4efc600a1e | 614 | Sensors(); |
ivanff15 | 1:ea4efc600a1e | 615 | |
ivanff15 | 1:ea4efc600a1e | 616 | // GET PITCH |
ivanff15 | 1:ea4efc600a1e | 617 | // Using y-z-plane-component/x-component of gravity vector |
ivanff15 | 1:ea4efc600a1e | 618 | pitch = -atan2(accel[0], sqrt(accel[1] * accel[1] + accel[2] * accel[2])); |
ivanff15 | 1:ea4efc600a1e | 619 | |
ivanff15 | 1:ea4efc600a1e | 620 | Vector_Cross_Product(temp1, accel, xAxis); |
ivanff15 | 1:ea4efc600a1e | 621 | Vector_Cross_Product(temp2, xAxis, temp1); |
ivanff15 | 1:ea4efc600a1e | 622 | roll = atan2(temp2[1], temp2[2]); |
ivanff15 | 1:ea4efc600a1e | 623 | |
ivanff15 | 1:ea4efc600a1e | 624 | // GET YAW |
ivanff15 | 1:ea4efc600a1e | 625 | Compass_Heading(); |
ivanff15 | 1:ea4efc600a1e | 626 | yaw = MAG_Heading; |
ivanff15 | 1:ea4efc600a1e | 627 | |
ivanff15 | 1:ea4efc600a1e | 628 | // Init rotation matrix |
ivanff15 | 1:ea4efc600a1e | 629 | init_rotation_matrix(DCM_Matrix, yaw, pitch, roll); |
ivanff15 | 1:ea4efc600a1e | 630 | } |
ivanff15 | 1:ea4efc600a1e | 631 | float getYaw() |
ivanff15 | 1:ea4efc600a1e | 632 | { |
ivanff15 | 1:ea4efc600a1e | 633 | return TO_DEG(yaw); |
ivanff15 | 1:ea4efc600a1e | 634 | } |
ivanff15 | 1:ea4efc600a1e | 635 | float getPitch() |
ivanff15 | 1:ea4efc600a1e | 636 | { |
ivanff15 | 1:ea4efc600a1e | 637 | return TO_DEG(pitch); |
ivanff15 | 1:ea4efc600a1e | 638 | } |
ivanff15 | 1:ea4efc600a1e | 639 | float getRoll() |
ivanff15 | 1:ea4efc600a1e | 640 | { |
ivanff15 | 1:ea4efc600a1e | 641 | return TO_DEG(roll); |
ivanff15 | 1:ea4efc600a1e | 642 | } |
ivanff15 | 1:ea4efc600a1e | 643 | void data_DCM () |
ivanff15 | 1:ea4efc600a1e | 644 | { |
ivanff15 | 1:ea4efc600a1e | 645 | UpdateFilter2(); |
ivanff15 | 1:ea4efc600a1e | 646 | yaw = getYaw(); |
ivanff15 | 1:ea4efc600a1e | 647 | pitch = getPitch(); |
ivanff15 | 1:ea4efc600a1e | 648 | roll = getRoll(); |
ivanff15 | 1:ea4efc600a1e | 649 | } |
ivanff15 | 1:ea4efc600a1e | 650 | |
ivanff15 | 1:ea4efc600a1e | 651 | void DCM(float dt) |
ivanff15 | 1:ea4efc600a1e | 652 | { |
ivanff15 | 1:ea4efc600a1e | 653 | int i,l,j; |
ivanff15 | 1:ea4efc600a1e | 654 | for(i = 0; i < 3; i++) |
ivanff15 | 1:ea4efc600a1e | 655 | { |
ivanff15 | 1:ea4efc600a1e | 656 | Accel_Vector[i] = 0; // Store the acceleration in a vector |
ivanff15 | 1:ea4efc600a1e | 657 | Gyro_Vector[i] = 0; // Store the gyros turn rate in a vector |
ivanff15 | 1:ea4efc600a1e | 658 | Omega_Vector[i] = 0; // Corrected Gyro_Vector data |
ivanff15 | 1:ea4efc600a1e | 659 | Omega_P[i] = 0; // Omega Proportional correction |
ivanff15 | 1:ea4efc600a1e | 660 | Omega_I[i] = 0; // Omega Integrator |
ivanff15 | 1:ea4efc600a1e | 661 | Omega[i]= 0; |
ivanff15 | 1:ea4efc600a1e | 662 | errorRollPitch[i] = 0; |
ivanff15 | 1:ea4efc600a1e | 663 | errorYaw[i] = 0; |
ivanff15 | 1:ea4efc600a1e | 664 | } |
ivanff15 | 1:ea4efc600a1e | 665 | |
ivanff15 | 1:ea4efc600a1e | 666 | int k = 1; |
ivanff15 | 1:ea4efc600a1e | 667 | |
ivanff15 | 1:ea4efc600a1e | 668 | for(l = 0; l < 3; l++) |
ivanff15 | 1:ea4efc600a1e | 669 | { |
ivanff15 | 1:ea4efc600a1e | 670 | for(j = 0; j < 3; j++) |
ivanff15 | 1:ea4efc600a1e | 671 | { |
ivanff15 | 1:ea4efc600a1e | 672 | DCM_Matrix[l][j] = 0; |
ivanff15 | 1:ea4efc600a1e | 673 | Update_Matrix[l][j] = k; |
ivanff15 | 1:ea4efc600a1e | 674 | Temporary_Matrix[l][j] = 0; |
ivanff15 | 1:ea4efc600a1e | 675 | k++; |
ivanff15 | 1:ea4efc600a1e | 676 | } |
ivanff15 | 1:ea4efc600a1e | 677 | k++; |
ivanff15 | 1:ea4efc600a1e | 678 | } |
ivanff15 | 1:ea4efc600a1e | 679 | |
ivanff15 | 1:ea4efc600a1e | 680 | G_Dt = dt; |
ivanff15 | 1:ea4efc600a1e | 681 | |
ivanff15 | 1:ea4efc600a1e | 682 | reset_sensor_fusion(); |
ivanff15 | 1:ea4efc600a1e | 683 | } |
ivanff15 | 1:ea4efc600a1e | 684 | void dataoutdcm2() |
ivanff15 | 1:ea4efc600a1e | 685 | { |
ivanff15 | 1:ea4efc600a1e | 686 | // unsigned char i; |
ivanff15 | 1:ea4efc600a1e | 687 | pc.printf("#YPR=%.2f,%.2f,%.2f\n",yaw,pitch,roll); |
ivanff15 | 1:ea4efc600a1e | 688 | // for(i=0;i<3;i++) |
ivanff15 | 1:ea4efc600a1e | 689 | // { |
ivanff15 | 1:ea4efc600a1e | 690 | // sprintf(kirim,"Acc=%d,%d,%d\n",roll,pitch,yaw); |
ivanff15 | 1:ea4efc600a1e | 691 | |
ivanff15 | 1:ea4efc600a1e | 692 | // } |
ivanff15 | 1:ea4efc600a1e | 693 | |
ivanff15 | 1:ea4efc600a1e | 694 | } |
ivanff15 | 1:ea4efc600a1e | 695 | void FilterInit(int rawAccX, int rawAccY, int rawAccZ, int rawGyroX, int rawGyroY, int rawGyroZ, int rawMagX, int rawMagY, int rawMagZ) |
ivanff15 | 1:ea4efc600a1e | 696 | { |
ivanff15 | 1:ea4efc600a1e | 697 | baca_itg(); |
ivanff15 | 1:ea4efc600a1e | 698 | baca_hmc(); |
ivanff15 | 1:ea4efc600a1e | 699 | baca_adxl(); |
ivanff15 | 1:ea4efc600a1e | 700 | |
ivanff15 | 1:ea4efc600a1e | 701 | setAccelRaw(rawAccX,rawAccY,rawAccZ); |
ivanff15 | 1:ea4efc600a1e | 702 | |
ivanff15 | 1:ea4efc600a1e | 703 | setGyroRaw(rawGyroX,rawGyroY,rawGyroZ); |
ivanff15 | 1:ea4efc600a1e | 704 | |
ivanff15 | 1:ea4efc600a1e | 705 | setMagnetRaw(rawMagX,rawMagY,rawMagZ); |
ivanff15 | 1:ea4efc600a1e | 706 | |
ivanff15 | 1:ea4efc600a1e | 707 | reset_sensor_fusion(); |
ivanff15 | 1:ea4efc600a1e | 708 | } |
ivanff15 | 1:ea4efc600a1e | 709 | //=====end DCM======== |
ivanff15 | 1:ea4efc600a1e | 710 | |
ivanff15 | 1:ea4efc600a1e | 711 | |
ivanff15 | 1:ea4efc600a1e | 712 | //=====baca baro |
ivanff15 | 1:ea4efc600a1e | 713 | void baca_baro() |
ivanff15 | 1:ea4efc600a1e | 714 | { |
ivanff15 | 1:ea4efc600a1e | 715 | unsigned char tekanan; |
ivanff15 | 1:ea4efc600a1e | 716 | while(1) |
ivanff15 | 1:ea4efc600a1e | 717 | { |
ivanff15 | 1:ea4efc600a1e | 718 | tekanan=alt_sensor.get_pressure(); |
ivanff15 | 1:ea4efc600a1e | 719 | pc.printf("Altitude: %f\r\n", alt_sensor.get_altitude_m()); |
ivanff15 | 1:ea4efc600a1e | 720 | } |
ivanff15 | 1:ea4efc600a1e | 721 | } |
ivanff15 | 1:ea4efc600a1e | 722 | |
ivanff15 | 1:ea4efc600a1e | 723 | void Servo() |
ivanff15 | 1:ea4efc600a1e | 724 | { |
ivanff15 | 1:ea4efc600a1e | 725 | jump = base_speed - delta; |
ivanff15 | 1:ea4efc600a1e | 726 | if(jump>100) servo_left = 100; |
ivanff15 | 1:ea4efc600a1e | 727 | else if(jump<0) servo_left = 0; |
ivanff15 | 1:ea4efc600a1e | 728 | else servo_left = (unsigned char)(jump); |
ivanff15 | 1:ea4efc600a1e | 729 | |
ivanff15 | 1:ea4efc600a1e | 730 | jump = base_speed + delta; |
ivanff15 | 1:ea4efc600a1e | 731 | if(jump>100) servo_right = 100; |
ivanff15 | 1:ea4efc600a1e | 732 | else if(jump<0) servo_right = 0; |
ivanff15 | 1:ea4efc600a1e | 733 | else servo_right = (unsigned char)(jump); |
ivanff15 | 1:ea4efc600a1e | 734 | |
ivanff15 | 1:ea4efc600a1e | 735 | servo_left = 1000 + servo_left*10; |
ivanff15 | 1:ea4efc600a1e | 736 | servo_right = 1000 + servo_right*10; |
ivanff15 | 1:ea4efc600a1e | 737 | Servo_1.pulsewidth_us(servo_left); |
ivanff15 | 1:ea4efc600a1e | 738 | Servo_2.pulsewidth_us(servo_right); |
ivanff15 | 1:ea4efc600a1e | 739 | pc.printf("%d || %d\n",servo_left,servo_right); |
ivanff15 | 1:ea4efc600a1e | 740 | } |
ivanff15 | 1:ea4efc600a1e | 741 | |
ivanff15 | 1:ea4efc600a1e | 742 | void PID() |
ivanff15 | 1:ea4efc600a1e | 743 | { |
ivanff15 | 1:ea4efc600a1e | 744 | FilterInit(rawAccX, rawAccY, rawAccZ, rawGyroX, rawGyroY, rawGyroZ, rawMagX, rawMagY, rawMagZ); |
ivanff15 | 1:ea4efc600a1e | 745 | data_DCM (); |
ivanff15 | 1:ea4efc600a1e | 746 | angle = (signed short)(yaw); |
ivanff15 | 1:ea4efc600a1e | 747 | delta_angle_1 = abs(angle-goal_ang_1); |
ivanff15 | 1:ea4efc600a1e | 748 | if(delta_angle_1>180) delta_angle_1 = 360 - delta_angle_1; |
ivanff15 | 1:ea4efc600a1e | 749 | delta_angle_2 = abs(angle-goal_ang_2); |
ivanff15 | 1:ea4efc600a1e | 750 | if(delta_angle_2>180) delta_angle_2 = 360 - delta_angle_2; |
ivanff15 | 1:ea4efc600a1e | 751 | if(delta_angle_1<delta_angle_2) GoalAngle = goal_ang_1; |
ivanff15 | 1:ea4efc600a1e | 752 | else GoalAngle = goal_ang_2; |
ivanff15 | 1:ea4efc600a1e | 753 | jump = (angle - GoalAngle); |
ivanff15 | 1:ea4efc600a1e | 754 | error = atan2(sin(TO_RAD(jump)),cos(TO_RAD(jump))); |
ivanff15 | 1:ea4efc600a1e | 755 | error_sum = error_sum + error; |
ivanff15 | 1:ea4efc600a1e | 756 | jump_error = error_old; |
ivanff15 | 1:ea4efc600a1e | 757 | error_old=error; |
ivanff15 | 1:ea4efc600a1e | 758 | delta = (signed char) (Kp_ang*error + Ki_ang*error_sum + Kd_ang*(error-error_old)); |
ivanff15 | 1:ea4efc600a1e | 759 | |
ivanff15 | 1:ea4efc600a1e | 760 | |
ivanff15 | 1:ea4efc600a1e | 761 | |
ivanff15 | 1:ea4efc600a1e | 762 | } |
ivanff15 | 1:ea4efc600a1e | 763 | |
ivanff15 | 1:ea4efc600a1e | 764 | |
ivanff15 | 0:5af6fad57e1a | 765 | void raw_to_send() |
ivanff15 | 0:5af6fad57e1a | 766 | { |
ivanff15 | 0:5af6fad57e1a | 767 | sendAccX = (unsigned short) (rawAccX + 512);//if(sendAccX>999) sendAccX=999; |
ivanff15 | 0:5af6fad57e1a | 768 | sendAccY = (unsigned short) (rawAccY + 512);//if(sendAccY>999) sendAccY=999; |
ivanff15 | 0:5af6fad57e1a | 769 | sendAccZ = (unsigned short) (rawAccZ + 512);//if(sendAccZ>999) sendAccZ=999; |
ivanff15 | 0:5af6fad57e1a | 770 | sendGyroX = (unsigned short)((rawGyroX*0.06956+2400)*0.2083);//if(sendGyroX>999) sendGyroX=999; |
ivanff15 | 0:5af6fad57e1a | 771 | sendGyroY = (unsigned short)((rawGyroY*0.06956+2400)*0.2083);//if(sendGyroY>999) sendGyroY=999; |
ivanff15 | 0:5af6fad57e1a | 772 | sendGyroZ = (unsigned short)((rawGyroZ*0.06956+2400)*0.2083);//if(sendGyroZ>999) sendGyroZ=999; |
ivanff15 | 0:5af6fad57e1a | 773 | sendMagX = (unsigned short)((rawMagX+2048)*0.25);//if(sendMagX>999) sendMagX=999; |
ivanff15 | 0:5af6fad57e1a | 774 | sendMagY = (unsigned short)((rawMagY+2048)*0.25);//if(sendMagY>999) sendMagY=999; |
ivanff15 | 0:5af6fad57e1a | 775 | sendMagZ = (unsigned short)((rawMagZ+2048)*0.25);//if(sendMagZ>999) sendMagZ=999; |
ivanff15 | 0:5af6fad57e1a | 776 | } |
ivanff15 | 0:5af6fad57e1a | 777 | |
ivanff15 | 0:5af6fad57e1a | 778 | int main() |
ivanff15 | 0:5af6fad57e1a | 779 | { |
ivanff15 | 1:ea4efc600a1e | 780 | //baca = 1; |
ivanff15 | 1:ea4efc600a1e | 781 | int count = 0; |
ivanff15 | 0:5af6fad57e1a | 782 | pc.baud(57600); |
ivanff15 | 1:ea4efc600a1e | 783 | Servo_1.period_ms(20); |
ivanff15 | 1:ea4efc600a1e | 784 | Servo_2.period_ms(20); |
ivanff15 | 0:5af6fad57e1a | 785 | while(1) |
ivanff15 | 0:5af6fad57e1a | 786 | { |
ivanff15 | 0:5af6fad57e1a | 787 | if(kirim.readable()) |
ivanff15 | 0:5af6fad57e1a | 788 | { |
ivanff15 | 0:5af6fad57e1a | 789 | baca=kirim.getc(); |
ivanff15 | 0:5af6fad57e1a | 790 | if(baca=='s') |
ivanff15 | 0:5af6fad57e1a | 791 | { |
ivanff15 | 1:ea4efc600a1e | 792 | baca='0'; |
ivanff15 | 1:ea4efc600a1e | 793 | while(1) |
ivanff15 | 1:ea4efc600a1e | 794 | { |
ivanff15 | 1:ea4efc600a1e | 795 | |
ivanff15 | 1:ea4efc600a1e | 796 | count=count++; |
ivanff15 | 0:5af6fad57e1a | 797 | baca_adxl(); |
ivanff15 | 0:5af6fad57e1a | 798 | baca_itg(); |
ivanff15 | 0:5af6fad57e1a | 799 | baca_hmc(); |
ivanff15 | 0:5af6fad57e1a | 800 | raw_to_send(); |
ivanff15 | 0:5af6fad57e1a | 801 | |
ivanff15 | 0:5af6fad57e1a | 802 | pc.putc(0x0D); |
ivanff15 | 0:5af6fad57e1a | 803 | bin_dec_conv(ID_GATHOTKACA); |
ivanff15 | 0:5af6fad57e1a | 804 | pc.putc(0x20); |
ivanff15 | 0:5af6fad57e1a | 805 | |
ivanff15 | 0:5af6fad57e1a | 806 | |
ivanff15 | 0:5af6fad57e1a | 807 | bin_dec_conv(sendAccX); |
ivanff15 | 0:5af6fad57e1a | 808 | pc.putc(0x20); |
ivanff15 | 0:5af6fad57e1a | 809 | |
ivanff15 | 0:5af6fad57e1a | 810 | |
ivanff15 | 0:5af6fad57e1a | 811 | bin_dec_conv(sendAccY); |
ivanff15 | 0:5af6fad57e1a | 812 | pc.putc(0x20); |
ivanff15 | 0:5af6fad57e1a | 813 | |
ivanff15 | 0:5af6fad57e1a | 814 | |
ivanff15 | 0:5af6fad57e1a | 815 | bin_dec_conv(sendAccZ); |
ivanff15 | 0:5af6fad57e1a | 816 | pc.putc(0x20); |
ivanff15 | 0:5af6fad57e1a | 817 | |
ivanff15 | 0:5af6fad57e1a | 818 | |
ivanff15 | 0:5af6fad57e1a | 819 | bin_dec_conv(sendGyroX); |
ivanff15 | 0:5af6fad57e1a | 820 | pc.putc(0x20); |
ivanff15 | 0:5af6fad57e1a | 821 | |
ivanff15 | 0:5af6fad57e1a | 822 | |
ivanff15 | 0:5af6fad57e1a | 823 | bin_dec_conv(sendGyroY); |
ivanff15 | 0:5af6fad57e1a | 824 | pc.putc(0x20); |
ivanff15 | 0:5af6fad57e1a | 825 | |
ivanff15 | 0:5af6fad57e1a | 826 | |
ivanff15 | 0:5af6fad57e1a | 827 | bin_dec_conv(sendGyroZ); |
ivanff15 | 0:5af6fad57e1a | 828 | pc.putc(0x20); |
ivanff15 | 0:5af6fad57e1a | 829 | |
ivanff15 | 0:5af6fad57e1a | 830 | |
ivanff15 | 0:5af6fad57e1a | 831 | bin_dec_conv(sendMagX); |
ivanff15 | 0:5af6fad57e1a | 832 | pc.putc(0x20); |
ivanff15 | 0:5af6fad57e1a | 833 | |
ivanff15 | 0:5af6fad57e1a | 834 | |
ivanff15 | 0:5af6fad57e1a | 835 | bin_dec_conv(sendMagY); |
ivanff15 | 0:5af6fad57e1a | 836 | pc.putc(0x20); |
ivanff15 | 0:5af6fad57e1a | 837 | |
ivanff15 | 0:5af6fad57e1a | 838 | |
ivanff15 | 0:5af6fad57e1a | 839 | bin_dec_conv(sendMagZ); |
ivanff15 | 0:5af6fad57e1a | 840 | |
ivanff15 | 0:5af6fad57e1a | 841 | wait_ms(75); |
ivanff15 | 0:5af6fad57e1a | 842 | |
ivanff15 | 0:5af6fad57e1a | 843 | baca=kirim.getc(); |
ivanff15 | 1:ea4efc600a1e | 844 | |
ivanff15 | 1:ea4efc600a1e | 845 | //if(count ==4) |
ivanff15 | 1:ea4efc600a1e | 846 | // { |
ivanff15 | 1:ea4efc600a1e | 847 | // Servo_1.pulsewidth_us(1000); |
ivanff15 | 1:ea4efc600a1e | 848 | // Servo_2.pulsewidth_us(2000); |
ivanff15 | 1:ea4efc600a1e | 849 | // } |
ivanff15 | 1:ea4efc600a1e | 850 | // if(count ==8) |
ivanff15 | 1:ea4efc600a1e | 851 | // { |
ivanff15 | 1:ea4efc600a1e | 852 | // Servo_1.pulsewidth_us(2000); |
ivanff15 | 1:ea4efc600a1e | 853 | // Servo_2.pulsewidth_us(1000); |
ivanff15 | 1:ea4efc600a1e | 854 | // count = 0; |
ivanff15 | 1:ea4efc600a1e | 855 | // } |
ivanff15 | 1:ea4efc600a1e | 856 | |
ivanff15 | 0:5af6fad57e1a | 857 | if(baca=='x') |
ivanff15 | 0:5af6fad57e1a | 858 | { |
ivanff15 | 0:5af6fad57e1a | 859 | baca='0'; |
ivanff15 | 0:5af6fad57e1a | 860 | break; |
ivanff15 | 0:5af6fad57e1a | 861 | } |
ivanff15 | 1:ea4efc600a1e | 862 | } |
ivanff15 | 0:5af6fad57e1a | 863 | } |
ivanff15 | 1:ea4efc600a1e | 864 | //baca=kirim.getc(); |
ivanff15 | 1:ea4efc600a1e | 865 | if(baca=='d') |
ivanff15 | 1:ea4efc600a1e | 866 | { |
ivanff15 | 1:ea4efc600a1e | 867 | baca='0'; |
ivanff15 | 1:ea4efc600a1e | 868 | while(1) |
ivanff15 | 1:ea4efc600a1e | 869 | { |
ivanff15 | 1:ea4efc600a1e | 870 | FilterInit(rawAccX, rawAccY, rawAccZ, rawGyroX, rawGyroY, rawGyroZ, rawMagX, rawMagY, rawMagZ); |
ivanff15 | 1:ea4efc600a1e | 871 | data_DCM (); |
ivanff15 | 1:ea4efc600a1e | 872 | |
ivanff15 | 1:ea4efc600a1e | 873 | dataoutdcm2(); |
ivanff15 | 1:ea4efc600a1e | 874 | PID(); |
ivanff15 | 1:ea4efc600a1e | 875 | Servo(); |
ivanff15 | 1:ea4efc600a1e | 876 | wait_ms(10); |
ivanff15 | 1:ea4efc600a1e | 877 | baca=kirim.getc(); |
ivanff15 | 1:ea4efc600a1e | 878 | if(baca=='x') |
ivanff15 | 1:ea4efc600a1e | 879 | { |
ivanff15 | 1:ea4efc600a1e | 880 | baca='0'; |
ivanff15 | 1:ea4efc600a1e | 881 | break; |
ivanff15 | 1:ea4efc600a1e | 882 | } |
ivanff15 | 1:ea4efc600a1e | 883 | } |
ivanff15 | 1:ea4efc600a1e | 884 | } |
ivanff15 | 1:ea4efc600a1e | 885 | if(baca=='b') |
ivanff15 | 1:ea4efc600a1e | 886 | { |
ivanff15 | 1:ea4efc600a1e | 887 | baca='0'; |
ivanff15 | 1:ea4efc600a1e | 888 | while(1) |
ivanff15 | 1:ea4efc600a1e | 889 | { |
ivanff15 | 1:ea4efc600a1e | 890 | PID(); |
ivanff15 | 1:ea4efc600a1e | 891 | baca=kirim.getc(); |
ivanff15 | 1:ea4efc600a1e | 892 | if(baca=='x') |
ivanff15 | 1:ea4efc600a1e | 893 | { |
ivanff15 | 1:ea4efc600a1e | 894 | baca='0'; |
ivanff15 | 1:ea4efc600a1e | 895 | break; |
ivanff15 | 1:ea4efc600a1e | 896 | } |
ivanff15 | 1:ea4efc600a1e | 897 | } |
ivanff15 | 1:ea4efc600a1e | 898 | } |
ivanff15 | 1:ea4efc600a1e | 899 | if(baca=='p') |
ivanff15 | 1:ea4efc600a1e | 900 | { |
ivanff15 | 1:ea4efc600a1e | 901 | baca='0'; |
ivanff15 | 1:ea4efc600a1e | 902 | while(1) |
ivanff15 | 1:ea4efc600a1e | 903 | { |
ivanff15 | 1:ea4efc600a1e | 904 | pc.printf("IVAN JELEK IVAN KELEKEJFKDFJKJ \n"); |
ivanff15 | 1:ea4efc600a1e | 905 | baca=kirim.getc(); |
ivanff15 | 1:ea4efc600a1e | 906 | if(baca=='x') |
ivanff15 | 1:ea4efc600a1e | 907 | { |
ivanff15 | 1:ea4efc600a1e | 908 | baca='0'; |
ivanff15 | 1:ea4efc600a1e | 909 | break; |
ivanff15 | 1:ea4efc600a1e | 910 | } |
ivanff15 | 1:ea4efc600a1e | 911 | } |
ivanff15 | 1:ea4efc600a1e | 912 | } |
ivanff15 | 1:ea4efc600a1e | 913 | |
ivanff15 | 1:ea4efc600a1e | 914 | |
ivanff15 | 1:ea4efc600a1e | 915 | } |
ivanff15 | 0:5af6fad57e1a | 916 | } |
ivanff15 | 0:5af6fad57e1a | 917 | } |