/*
* get raw data converted to g and deg/sec values
*/
void mpu6050_getConvData(char address, double* axg, double* ayg, double* azg, double* gxds, double* gyds, double* gzds) {
short ax = 0;
short ay = 0;
short az = 0;
short gx = 0;
short gy = 0;
short gz = 0;
mpu6050_getRawData(address, &ax, &ay, &az, &gx, &gy, &gz);
#if MPU6050_CALIBRATEDACCGYRO == 1
*axg = (double)(ax-MPU6050_AXOFFSET)/MPU6050_AXGAIN;
*ayg = (double)(ay-MPU6050_AYOFFSET)/MPU6050_AYGAIN;
*azg = (double)(az-MPU6050_AZOFFSET)/MPU6050_AZGAIN;
*gxds = (double)(gx-MPU6050_GXOFFSET)/MPU6050_GXGAIN;
*gyds = (double)(gy-MPU6050_GYOFFSET)/MPU6050_GYGAIN;
*gzds = (double)(gz-MPU6050_GZOFFSET)/MPU6050_GZGAIN;
#else
*axg = (double)(ax)/MPU6050_AGAIN;
*ayg = (double)(ay)/MPU6050_AGAIN;
*azg = (double)(az)/MPU6050_AGAIN;
*gxds = (double)(gx)/MPU6050_GGAIN;
*gyds = (double)(gy)/MPU6050_GGAIN;
*gzds = (double)(gz)/MPU6050_GGAIN;
#endif
}
// Edit do read data from MPU6050 [10/14/2015 QuocTuanIT]
void sensorsRead()
{
int16_t ax = 0;
int16_t ay = 0;
int16_t az = 0;
int16_t gx = 0;
int16_t gy = 0;
int16_t gz = 0;
mpu6050_getRawData(&ax, &ay, &az, &gx, &gy, &gz);
GYRO_raw[PIT] = gx;//adcGet(ADC_GYR_X);
GYRO[PIT] = -(int16_t)(GYRO_raw[PIT] - Config.GYRO_zero[PIT]);
#if GYRO_DEADBAND > 0
if (abs(GYRO[PIT]) <= GYRO_DEADBAND) GYRO[PIT] = 0;
#endif
GYRO_raw[ROL] = gy;//adcGet(ADC_GYR_Y);
GYRO[ROL] = -(int16_t)(GYRO_raw[ROL] - Config.GYRO_zero[ROL]);
#if GYRO_DEADBAND > 0
if (abs(GYRO[ROL]) <= GYRO_DEADBAND) GYRO[ROL] = 0;
#endif
GYRO_raw[YAW] = gz;//adcGet(ADC_GYR_Z);
GYRO[YAW] = (int16_t)(GYRO_raw[YAW] - Config.GYRO_zero[YAW]);
#if GYRO_DEADBAND > 0
if (abs(GYRO[YAW]) <= GYRO_DEADBAND) GYRO[YAW] = 0;
#endif
ACC_raw[PIT] = ax;//adcGet(ADC_ACC_X);
ACC[PIT] = (int16_t)(ACC_raw[PIT] - Config.ACC_zero[PIT]);
ACC_raw[ROL] = ay;//adcGet(ADC_ACC_Y);
ACC[ROL] = (int16_t)(ACC_raw[ROL] - Config.ACC_zero[ROL]);
ACC_raw[YAW] = az;//adcGet(ADC_ACC_Z);
ACC[YAW] = (int16_t)(ACC_raw[YAW] - Config.ACC_zero[YAW]);
BATT = adcGet(ADC_VBAT) * 100 / 376;
#ifdef SIMULATOR
GYRO[0] = 100;
GYRO[1] = 100;
GYRO[2] = 100;
ACC[0] = 100;
ACC[1] = 100;
ACC[2] = 100;
#endif
}
void mpu6050_calibrate(char address)
{
short ax = 0;
short ay = 0;
short az = 0;
short gx = 0;
short gy = 0;
short gz = 0;
int axs = 0;
int ays = 0;
int azs = 0;
int gxs = 0;
int gys = 0;
int gzs = 0;
short axOffset = 0;
short ayOffset = 0;
short azOffset = 0;
short gxOffset = 0;
short gyOffset = 0;
short gzOffset = 0;
int i;
for(i = 0; i < 20; i++) {
mpu6050_getRawData(address, &ax, &ay, &az, &gx, &gy, &gz);
axs += ax; ays += ay; azs += az, gxs += gx, gys += gy, gzs += gz;
}
axOffset = axs/20; ayOffset = ays/20; azOffset = azs/20; gxOffset = gxs/20; gyOffset = gys/20; gzOffset = gzs/20;
azOffset -= 16384;
ayOffset = 0xFFFF;
i2cWriteWord(address, MPU6050_XA_OFFS_H, &axOffset);
unsigned char read_h, read_l; i2cRead(address, MPU6050_XA_OFFS_H, &read_h, 1);
i2cRead(address, MPU6050_XA_OFFS_L_TC, &read_l, 1);
i2cWriteWord(address, MPU6050_YA_OFFS_H, &ayOffset);
i2cWriteWord(address, MPU6050_ZA_OFFS_H, &azOffset);
i2cWriteWord(address, MPU6050_XG_OFFS_USRH, &gxOffset);
i2cWriteWord(address, MPU6050_YG_OFFS_USRH, &gyOffset);
i2cWriteWord(address, MPU6050_ZG_OFFS_USRH, &gzOffset);
}
int main(void) {
DDRB=0x08;
/* PB3 pin of PORTB is declared output (PWM1 pin of DC Motor Driver is connected) */
DDRD=0x80;
/* PD7 pin of PORTD is declared output (PWM2 pin of DC Motor Driver is connected) */
DDRA=0x0f;
/*PA0,PA1,PA2 and PA3 pins of PortC are declared output ( i/p1,i/p2,i/p3 and i/p4 pins of DC Motor Driver are connected)*/
set_timercounter0_mode(1);
/*Timer counter 0 is set to Phase Correct pwm mode*/
set_timercounter0_prescaler(4);
/*Timer counter 0 frequency is set to 3.90625KHz*/
set_timercounter0_output_mode(2);
/*Timer counter 0 output mode is set to non-inverting mode*/
set_timercounter2_mode(1);
/*Timer counter 2 is set to Phase Correct pwm mode*/
set_timercounter2_prescaler(4);
/*Timer counter 2 frequency is set to 3.90625KHz*/
set_timercounter2_output_mode(2);
/*Timer counter 2 output mode is set to non-inverting mode*/
#if MPU6050_GETATTITUDE == 0
int16_t ax = 0;
int16_t ay = 0;
int16_t az = 0;
int16_t gx = 0;
int16_t gy = 0;
int16_t gz = 0;
double axg = 0;
double ayg = 0;
double azg = 0;
double gxds = 0;
double gyds = 0;
double gzds = 0;
double accXangle = 0;
double gyroXangle = 0;
double Xangle = 0 ;
double error = 0;
double I_error = 0;
double D_error = 0;
double previous_error = 0 ;
double outputspeed = 0;
/*double initangle = 0;*/
#endif
//init uart
uart_init(UART_BAUD_SELECT(UART_BAUD_RATE,F_CPU));
//init interrupt
sei();
//init mpu6050
mpu6050_init();
_delay_ms(50);
#if MPU6050_GETATTITUDE == 0
mpu6050_getRawData(&ax, &ay, &az, &gx, &gy, &gz);
mpu6050_getConvData(&axg, &ayg, &azg, &gxds, &gyds, &gzds);
accXangle = (atan2(ayg,azg)+PI)*RAD_TO_DEG;
gyroXangle = accXangle;
#endif
for(;;) {
#if MPU6050_GETATTITUDE == 0
mpu6050_getRawData(&ax, &ay, &az, &gx, &gy, &gz);
mpu6050_getConvData(&axg, &ayg, &azg, &gxds, &gyds, &gzds);
#endif
accXangle = (atan2(ayg,azg)+PI)*RAD_TO_DEG;
gyroXangle = accXangle + gxds*dt;
Xangle = 0.98*gyroXangle + 0.02*accXangle;
error = 180 - Xangle;
I_error += (error)*dt;
D_error = (error - previous_error)/*/dt*/;
outputspeed = (P_GAIN * error) + (I_GAIN * I_error) + (D_GAIN * D_error);
previous_error = error;
/*Bang Bang Controller
if((Xangle<=(180.01))&&(Xangle>=179.99))
{
PORTA = 0x00;
}
else if (Xangle>(180.01))
{
set_timercounter0_compare_value(255);
set_timercounter2_compare_value(255);
PORTA = 0x0a;
}
else if(Xangle<(179.99))
{
set_timercounter0_compare_value(255);
set_timercounter2_compare_value(255);
PORTA = 0x05;
}
Bang Bang Controller*/
if((Xangle<=(180.1))&&(Xangle>=179.9))
{
PORTA = 0x00;
}
else if (Xangle>(180.1))
{
set_timercounter0_compare_value(abs(outputspeed));
set_timercounter2_compare_value(abs(outputspeed));
PORTA = 0x0a;
}
else if(Xangle<(179.9))
{
set_timercounter0_compare_value(abs(outputspeed));
set_timercounter2_compare_value(abs(outputspeed));
PORTA = 0x05;
}
#if MPU6050_GETATTITUDE == 0
char itmp[10];
/*dtostrf(ax, 3, 5, itmp); uart_puts(itmp); uart_putc(' ');
dtostrf(ay, 3, 5, itmp); uart_puts(itmp); uart_putc(' ');
dtostrf(az, 3, 5, itmp); uart_puts(itmp); uart_putc(' ');
dtostrf(gx, 3, 5, itmp); uart_puts(itmp); uart_putc(' ');
dtostrf(gy, 3, 5, itmp); uart_puts(itmp); uart_putc(' ');
dtostrf(gz, 3, 5, itmp); uart_puts(itmp); uart_putc(' ');
dtostrf(axg, 3, 5, itmp); uart_puts(itmp); uart_putc(' ');
dtostrf(ayg, 3, 5, itmp); uart_puts(itmp); uart_putc(' ');
dtostrf(azg, 3, 5, itmp); uart_puts(itmp); uart_putc(' ');
dtostrf(gxds, 3, 5, itmp); uart_puts(itmp); uart_putc(' ');
dtostrf(gyds, 3, 5, itmp); uart_puts(itmp); uart_putc(' ');
dtostrf(gzds, 3, 5, itmp); uart_puts(itmp); uart_putc(' ');
dtostrf(accXangle, 3, 5, itmp); uart_puts(itmp); uart_putc(' ');
dtostrf(gyroXangle, 3, 5, itmp); uart_puts(itmp); uart_putc(' ');
dtostrf(initangle, 3, 5, itmp); uart_puts(itmp); uart_putc(' ');
dtostrf(Xangle, 3, 5, itmp); uart_puts(itmp); uart_putc(' ');
dtostrf(error, 3, 5, itmp); uart_puts(itmp); uart_putc(' ');
dtostrf(I_error, 3, 5, itmp); uart_puts(itmp); uart_putc(' ');
dtostrf(D_error, 3, 5, itmp); uart_puts(itmp); uart_putc(' ');*/
dtostrf(outputspeed, 3, 5, itmp); uart_puts(itmp); uart_putc(' ');
uart_puts("\r\n");
uart_puts("\r\n");
#endif
}
}
int main(void) {
//programowy pwm
DDRC |= (1<<PC0)|(1<<PC1);
PORTC |= (1<<PC0);
PORTC |= (1<<PC1);
TCCR2B |= (1<<WGM21); // tryb CTC
TCCR2B |= (1<<CS20); // preskaler = 1
OCR2B = 199; // dodatkowy podzia³ czêsttotliwoœci przez 200
TIMSK2 |= (1<<OCIE2B);
/*TCCR2 |= (1<<WGM21); // tryb CTC
TCCR2 |= (1<<CS20); // preskaler = 1
OCR2 = 199; // dodatkowy podzia³ czêsttotliwoœci przez 200
TIMSK |= (1<<OCIE2);*/
#if MPU6050_GETATTITUDE == 0
int16_t ax = 0;
int16_t ay = 0;
int16_t az = 0;
int16_t gx = 0;
int16_t gy = 0;
int16_t gz = 0;
double axg = 0;
double ayg = 0;
double azg = 0;
double gxds = 0;
double gyds = 0;
double gzds = 0;
#endif
#if MPU6050_GETATTITUDE == 1 || MPU6050_GETATTITUDE == 2
//long *ptr = 0;
double qw = 1.0f;
double qx = 0.0f;
double qy = 0.0f;
double qz = 0.0f;
double roll = 0.0f;
double pitch = 0.0f;
double yaw = 0.0f;
#endif
//uart_init(UART_BAUD_SELECT(UART_BAUD_RATE,F_CPU));
sei();
mpu6050_init();
_delay_ms(50);
int maxa=0,maxg=0,aax,pom;
//init mpu6050 dmp processor
#if MPU6050_GETATTITUDE == 2
mpu6050_dmpInitialize();
mpu6050_dmpEnable();
_delay_ms(10);
#endif
DDRD |= (1<<PD0);
PORTD |= (1<<PD0);
lcd_init();
int licz=1,suma=0,_ax,fi,bak;
int i;
int ii,jj;
int sprawdz;
//pid
/*float wzmocnienieP=10; //Wzmocnienie
float stalaI=0.4; //Sta³a czasowa ca³kowania
float stalaD=12; //Sta³a czasowa ró¿niczkowania
*/
float wzmocnienieP=14; //Wzmocnienie
float stalaI=0; //Sta³a czasowa ca³kowania
float stalaD=0;
float czas=0.15; //Czas zmian wielkoœci
int predkosc=127; //Prêdkoœæ silników
int predkosc_k=127;
int sterowanie,uchyb,uchyb_pop=0;
int calka=0;
int lqr;
//KF
/*Matrix *x_post = matrix_alloc(4,1);
for(ii=0;ii<4;ii++) {
for(jj=0;jj<1;jj++) {
x_post->matrix_entry[ii][jj]=0;
}
}
Matrix *P_post = matrix_alloc(4,4);
for(ii=0;ii<4;ii++) {
for(jj=0;jj<4;jj++) {
P_post->matrix_entry[ii][jj]=1;
}
}
Matrix *V = matrix_alloc(4,4);
for(ii=0;ii<4;ii++) {
for(jj=0;jj<4;jj++) {
if(ii==jj) V->matrix_entry[ii][ii]=0.8;
else V->matrix_entry[ii][jj]=0;
}
}
Matrix *W = matrix_alloc(2,2);
W->matrix_entry[0][0]=0.02;
W->matrix_entry[0][1]=0;
W->matrix_entry[1][0]=0;
W->matrix_entry[1][1]=4;
//LQR
Matrix *K_C = matrix_alloc(1,4);
K_C->matrix_entry[0][0]=80;
K_C->matrix_entry[0][1]=28.6388;
K_C->matrix_entry[0][2]=-63.7184;
K_C->matrix_entry[0][3]=-5.6401;
*/
for(;;) {
lcd_cls();
#if MPU6050_GETATTITUDE == 0
mpu6050_getRawData(&ax, &ay, &az, &gx, &gy, &gz);
mpu6050_getConvData(&axg, &ayg, &azg, &gxds, &gyds, &gzds);
#endif
#if MPU6050_GETATTITUDE == 1
mpu6050_getQuaternion(&qw, &qx, &qy, &qz);
mpu6050_getRollPitchYaw(&roll, &pitch, &yaw);
_delay_ms(10);
#endif
#if MPU6050_GETATTITUDE == 2
if(mpu6050_getQuaternionWait(&qw, &qx, &qy, &qz)) {
mpu6050_getRollPitchYaw(qw, qx, qy, qz, &roll, &pitch, &yaw);
}
_delay_ms(10);
#endif
#if MPU6050_GETATTITUDE == 0
//char itmp[10];
/*ltoa(ax, itmp, 10); uart_putc(' '); uart_puts(itmp); uart_putc(' ');
ltoa(ay, itmp, 10); uart_putc(' '); uart_puts(itmp); uart_putc(' ');
ltoa(az, itmp, 10); uart_putc(' '); uart_puts(itmp); uart_putc(' ');
ltoa(gx, itmp, 10); uart_putc(' '); uart_puts(itmp); uart_putc(' ');
ltoa(gy, itmp, 10); uart_putc(' '); uart_puts(itmp); uart_putc(' ');
ltoa(gz, itmp, 10); uart_putc(' '); uart_puts(itmp); uart_putc(' ');
uart_puts("\r\n");
dtostrf(axg, 3, 5, itmp); uart_puts(itmp); uart_putc(' ');
dtostrf(ayg, 3, 5, itmp); uart_puts(itmp); uart_putc(' ');
dtostrf(azg, 3, 5, itmp); uart_puts(itmp); uart_putc(' ');
dtostrf(gxds, 3, 5, itmp); uart_puts(itmp); uart_putc(' ');
dtostrf(gyds, 3, 5, itmp); uart_puts(itmp); uart_putc(' ');
dtostrf(gzds, 3, 5, itmp); uart_puts(itmp); uart_putc(' ');
uart_puts("\r\n");
uart_puts("\r\n");*/
/*lcd_cls();
lcd_str("a=(");
lcd_locate(0,3);
lcd_int(axg);
lcd_locate(0,6);
lcd_str(",");
lcd_locate(0,7);
lcd_int(ayg);
lcd_locate(0,10);
lcd_str(",");
lcd_locate(0,11);
lcd_int(azg);
lcd_locate(0,14);
lcd_str(")");
lcd_locate(1,0);
lcd_str("g=(");
lcd_locate(1,3);
lcd_int(gxds);
lcd_locate(1,6);
lcd_str(",");
lcd_locate(1,7);
lcd_int(gyds);
lcd_locate(1,10);
lcd_str(",");
lcd_locate(1,11);
lcd_int(gzds);
lcd_locate(1,14);
lcd_str(")");
_delay_ms(100);*/
/*if(ax<0) ax*=-1;
if(gx<0) gx*=-1;
if(ax>maxa) maxa=ax;
if(gx>maxg) maxg=gx;
lcd_int(maxa);
lcd_locate(1,0);
lcd_int(maxg);
_delay_ms(100);*/
/*
lcd_int(ax);
lcd_locate(1,0);
lcd_int(gx);
_delay_ms(100);*/
//REGULATOR PROPORCJONALNY
/*int K=26;
ax/=100;
if(ax<0) ax *= -1;
pom=-K*ax+255;
if(pom<0) pom = 0;
pwm1=pom;
if(ax>=0) PORTC &= ~(1<<PC1);
else PORTC |= (1<<PC1);
licz++;*/
//if((licz%10000)==0)
//{
/*lcd_cls();
lcd_int(pwm1);
_delay_ms(1000);
//}
*
*/
//############################33
ax /= 100;
suma+=ax;
licz++;
if(licz == 20) {
suma /= 20;
fi=suma;
//lcd_cls();
//lcd_int(fi);
//_delay_ms(500);
//
if(fi>=0) {
PORTC &= ~(1<<PC1);
}
if(fi<0) {
PORTC |= (1<<PC1);
fi *= -1;
}
//REGULATOR PROPORCJONALNY
//int K=80;
//pom=K*fi;
//if(pom>255) pom = 255;
//pwm1=pom;
//PID
if(fi<0) {
uchyb = -1*fi;
} else {
uchyb = fi;
}
calka += stalaI*fi;
sterowanie = (int)(wzmocnienieP*fi)/* - (int)(stalaD*((fi - uchyb_pop)))*/ + (int)(calka);
sprawdz = predkosc - sterowanie;
if(sterowanie>255) sterowanie = 255;
if(sterowanie<0) sterowanie = 0-sterowanie;
if(sterowanie<-255) sterowanie = 255;
//lcd_int(sterowanie);
//_delay_ms(1000);
pwm1=sterowanie;
uchyb_pop=fi;
suma=0;
licz=1;
}
//LQR
//Wypelnianie macierzy A
/*Matrix *A;
A = matrix_alloc(4,4);
for(ii=0;ii<4;ii++) {
for(jj=0;jj<4;jj++) {
A->matrix_entry[ii][jj]=0;
}
}
A->matrix_entry[0][1]=1; A->matrix_entry[2][3]=1;
A->matrix_entry[1][1]=-0.1192; A->matrix_entry[1][2]=6.7359;
A->matrix_entry[3][1]=-1.2764; A->matrix_entry[3][2]=177.1575;
//Wypelnianie macierzy B
Matrix *B;
B = matrix_alloc(4,1);
for(ii=0;ii<4;ii++) {
B->matrix_entry[ii][0]=0;
}
B->matrix_entry[1][0]=1.1923; B->matrix_entry[3][0]=12.7640;
//Wypelnianie macierzy C
Matrix *C;
C = matrix_alloc(2,4);
for(ii=0;ii<2;ii++) {
for(jj=0;jj<4;jj++) {
C->matrix_entry[ii][jj]=0;
}
}
C->matrix_entry[0][0]=1; C->matrix_entry[1][2]=1;
//u=pwm1
Matrix *x;
x = matrix_alloc(4,1);
x->matrix_entry[0][0]=0.000628*licznik; //m #################################
//x->matrix_entry[1][0]=6.28/(-0.0588*pwm1+20); //m/s
x->matrix_entry[1][0]=3.1416/(5000*pwm1); //m/s
x->matrix_entry[2][0]=(ax*9)/1600; //16000 - 90 stopni
x->matrix_entry[3][0]=gy; //
Matrix *y = matrix_alloc(2,1);
y->matrix_entry[0][0]=x->matrix_entry[0][0];
y->matrix_entry[1][0]=x->matrix_entry[2][0];
//lcd_cls();
//lcd_int((int)x->matrix_entry[2][0]);
//_delay_ms(500);
//FILTR KALMANA
Matrix *Ax = matrix_alloc(4,1);
Matrix *Bu = matrix_alloc(4,1);
Matrix *x_pri = matrix_alloc(4,1);
Matrix *AP = matrix_alloc(4,4);
Matrix *AT = matrix_alloc(4,4);
Matrix *APAT = matrix_alloc(4,4);
Matrix *P_pri = matrix_alloc(4,4);
Matrix *eps = matrix_alloc(2,1);
Matrix *CX = matrix_alloc(2,1);
// x(t+1|t) = Ax(t|t) + Bu(t)
Ax=matrix_multiply(A, x_post);
Bu->matrix_entry[0][0]=0;
Bu->matrix_entry[1][0]=pwm1*B->matrix_entry[1][0];
Bu->matrix_entry[2][0]=0;
Bu->matrix_entry[3][0]=pwm1*B->matrix_entry[3][0];
matrix_add(x_pri, Ax, Bu);
// P(t+1|t) = AP(t|t)A^T + V
AP=matrix_multiply(A, P_post);
AT=matrix_transpose(A);
APAT=matrix_multiply(AP, AT);
matrix_add(P_pri, APAT, V);
// eps(t) = y(t) - Cx(t|t-1)
CX=matrix_multiply(C, x_pri);
matrix_subtract(eps,y,CX);
Matrix *CP = matrix_alloc(2,4);
Matrix *CPCT = matrix_alloc(2,2);
Matrix *CT = matrix_alloc(4,2);
Matrix *S = matrix_alloc(2,2);
Matrix *PCT = matrix_alloc(4,2);
//Matrix *S1 = matrix_alloc(2,2);
Matrix *K = matrix_alloc(4,2);
Matrix *Keps = matrix_alloc(4,1);
Matrix *KS = matrix_alloc(4,2);
Matrix *KT = matrix_alloc(2,4);
Matrix *KSKT = matrix_alloc(4,4);
// S(t) = CP(t|t-1)C^T + W
CP=matrix_multiply(C, P_pri);
CT=matrix_transpose(C);
CPCT=matrix_multiply(CP, CT);
matrix_add(S, CPCT, W);
// K(t) = P(t|t-1)C^TS(t)^-1
PCT=matrix_multiply(P_pri, CT);
matrix_invert(S);
K=matrix_multiply(PCT, S); //S^-1
// x(t|t) = x(t|t-1) + K(t)eps(t)
Keps=matrix_multiply(K, eps);
matrix_add(x_post, x_pri, Keps);
// P(t|t) = P(t|t-1) - K(t)S(t)K(t)^T
matrix_invert(S);
KS=matrix_multiply(K, S);//S
KT=matrix_transpose(K);
KSKT=matrix_multiply(KS, KT);
matrix_subtract(P_post, P_pri, KSKT);
//LQR
Matrix *KX = matrix_alloc(1,1);
KX=matrix_multiply(K, x_post);
lqr=(int)(KX->matrix_entry[0][0]);
sprawdz=predkosc + lqr;
if(sprawdz>255) sprawdz = 255;
if(sprawdz<0) sprawdz = 0;
pwm1=sprawdz;
pwm1 = predkosc
*/
//############
//matrix_t *A = make_matrix( 4, 4 );
//put_entry_matrix( eqs, if1, if1, dx );
/*lcd_cls();
lcd_int(fi);
lcd_locate(0,7);
lcd_int(pwm1);
_delay_ms(500);*/
//#################################
/*for(i=255;i>=0;i--) {
pwm1=i;
_delay_ms(30);
if((i%10)==0) {
lcd_cls();
lcd_int(pwm1);
}
}
for(i=0;i<=255;i++) {
pwm1=i;
_delay_ms(30);
if((i%10)==0) {
lcd_cls();
lcd_int(pwm1);
}
}*/
#endif
#if MPU6050_GETATTITUDE == 1 || MPU6050_GETATTITUDE == 2
//quaternion
/*ptr = (long *)(&qw);
uart_putc(*ptr);
uart_putc(*ptr>>8);
uart_putc(*ptr>>16);
uart_putc(*ptr>>24);
ptr = (long *)(&qx);
uart_putc(*ptr);
uart_putc(*ptr>>8);
uart_putc(*ptr>>16);
uart_putc(*ptr>>24);
ptr = (long *)(&qy);
uart_putc(*ptr);
uart_putc(*ptr>>8);
uart_putc(*ptr>>16);
uart_putc(*ptr>>24);
ptr = (long *)(&qz);
uart_putc(*ptr);
uart_putc(*ptr>>8);
uart_putc(*ptr>>16);
uart_putc(*ptr>>24);
//roll pitch yaw
ptr = (long *)(&roll);
uart_putc(*ptr);
uart_putc(*ptr>>8);
uart_putc(*ptr>>16);
uart_putc(*ptr>>24);
ptr = (long *)(&pitch);
uart_putc(*ptr);
uart_putc(*ptr>>8);
uart_putc(*ptr>>16);
uart_putc(*ptr>>24);
ptr = (long *)(&yaw);
uart_putc(*ptr);
uart_putc(*ptr>>8);
uart_putc(*ptr>>16);
uart_putc(*ptr>>24);
uart_putc('\n');*/
//lcd_int((int)qw);
//_delay_ms(1000);
#endif
}
}
int main(void) {
#if MPU6050_GETATTITUDE == 0
int16_t ax = 0;
int16_t ay = 0;
int16_t az = 0;
int16_t gx = 0;
int16_t gy = 0;
int16_t gz = 0;
double axg = 0;
double ayg = 0;
double azg = 0;
double gxds = 0;
double gyds = 0;
double gzds = 0;
#endif
#if MPU6050_GETATTITUDE == 1 || MPU6050_GETATTITUDE == 2
long *ptr = 0;
double qw = 1.0f;
double qx = 0.0f;
double qy = 0.0f;
double qz = 0.0f;
double roll = 0.0f;
double pitch = 0.0f;
double yaw = 0.0f;
#endif
//init uart
uart_init(UART_BAUD_SELECT(UART_BAUD_RATE,F_CPU));
//init interrupt
sei();
//init mpu6050
mpu6050_init();
_delay_ms(50);
//init mpu6050 dmp processor
#if MPU6050_GETATTITUDE == 2
mpu6050_dmpInitialize();
mpu6050_dmpEnable();
_delay_ms(10);
#endif
for(;;) {
#if MPU6050_GETATTITUDE == 0
mpu6050_getRawData(&ax, &ay, &az, &gx, &gy, &gz);
mpu6050_getConvData(&axg, &ayg, &azg, &gxds, &gyds, &gzds);
#endif
#if MPU6050_GETATTITUDE == 1
mpu6050_getQuaternion(&qw, &qx, &qy, &qz);
mpu6050_getRollPitchYaw(&roll, &pitch, &yaw);
_delay_ms(10);
#endif
#if MPU6050_GETATTITUDE == 2
if(mpu6050_getQuaternionWait(&qw, &qx, &qy, &qz)) {
mpu6050_getRollPitchYaw(qw, qx, qy, qz, &roll, &pitch, &yaw);
}
_delay_ms(10);
#endif
#if MPU6050_GETATTITUDE == 0
char itmp[10];
ltoa(ax, itmp, 10); uart_putc(' '); uart_puts(itmp); uart_putc(' ');
ltoa(ay, itmp, 10); uart_putc(' '); uart_puts(itmp); uart_putc(' ');
ltoa(az, itmp, 10); uart_putc(' '); uart_puts(itmp); uart_putc(' ');
ltoa(gx, itmp, 10); uart_putc(' '); uart_puts(itmp); uart_putc(' ');
ltoa(gy, itmp, 10); uart_putc(' '); uart_puts(itmp); uart_putc(' ');
ltoa(gz, itmp, 10); uart_putc(' '); uart_puts(itmp); uart_putc(' ');
uart_puts("\r\n");
dtostrf(axg, 3, 5, itmp); uart_puts(itmp); uart_putc(' ');
dtostrf(ayg, 3, 5, itmp); uart_puts(itmp); uart_putc(' ');
dtostrf(azg, 3, 5, itmp); uart_puts(itmp); uart_putc(' ');
dtostrf(gxds, 3, 5, itmp); uart_puts(itmp); uart_putc(' ');
dtostrf(gyds, 3, 5, itmp); uart_puts(itmp); uart_putc(' ');
dtostrf(gzds, 3, 5, itmp); uart_puts(itmp); uart_putc(' ');
uart_puts("\r\n");
uart_puts("\r\n");
_delay_ms(1000);
#endif
#if MPU6050_GETATTITUDE == 1 || MPU6050_GETATTITUDE == 2
//quaternion
ptr = (long *)(&qw);
uart_putc(*ptr);
uart_putc(*ptr>>8);
uart_putc(*ptr>>16);
uart_putc(*ptr>>24);
ptr = (long *)(&qx);
uart_putc(*ptr);
uart_putc(*ptr>>8);
uart_putc(*ptr>>16);
uart_putc(*ptr>>24);
ptr = (long *)(&qy);
uart_putc(*ptr);
uart_putc(*ptr>>8);
uart_putc(*ptr>>16);
uart_putc(*ptr>>24);
ptr = (long *)(&qz);
uart_putc(*ptr);
uart_putc(*ptr>>8);
uart_putc(*ptr>>16);
uart_putc(*ptr>>24);
//roll pitch yaw
ptr = (long *)(&roll);
uart_putc(*ptr);
uart_putc(*ptr>>8);
uart_putc(*ptr>>16);
uart_putc(*ptr>>24);
ptr = (long *)(&pitch);
uart_putc(*ptr);
uart_putc(*ptr>>8);
uart_putc(*ptr>>16);
uart_putc(*ptr>>24);
ptr = (long *)(&yaw);
uart_putc(*ptr);
uart_putc(*ptr>>8);
uart_putc(*ptr>>16);
uart_putc(*ptr>>24);
uart_putc('\n');
#endif
}
}
