ivan fajar / Mbed 2 deprecated 10dof2

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?

UserRevisionLine numberNew 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 }