コード例 #1
0
/*
 * 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
}
コード例 #2
0
// 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
}
コード例 #3
0
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);
}
コード例 #4
0
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 

	}

}
コード例 #5
0
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

	}

}
コード例 #6
0
ファイル: main.c プロジェクト: Ribster/AVR-Projects
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

	}

}