void gyro_sample()
{
/* read the rate from the register */
gyrox_l_temp=I2C_readreg(L3G4200D_ADDR,OUT_X_L);
gyrox_h_temp=I2C_readreg(L3G4200D_ADDR,OUT_X_H);
gyrox_temp=((gyrox_h_temp << 8) | gyrox_l_temp);
gyroy_l_temp=I2C_readreg(L3G4200D_ADDR,OUT_Y_L);
gyroy_h_temp=I2C_readreg(L3G4200D_ADDR,OUT_Y_H);
gyroy_temp=((gyroy_h_temp << 8) | gyroy_l_temp);
gyroz_l_temp=I2C_readreg(L3G4200D_ADDR,OUT_Z_L);
gyroz_h_temp=I2C_readreg(L3G4200D_ADDR,OUT_Z_H);
gyroz_temp=((gyroz_h_temp << 8) | gyroz_l_temp);
gyrox = gyrox_temp * gyro_sensitivity_250 /1000 ;
gyroy = gyroy_temp * gyro_sensitivity_250 /1000 ;
gyroz = gyroz_temp * gyro_sensitivity_250 /1000 ;
gyrox_angle=gyrox_angle+gyrox*0.01;
gyroy_angle=gyroy_angle+gyroy*0.01;
gyroz_angle=gyroz_angle+gyroz*0.01;
Delay(10);
}
uint32_t ov7670_init()
{
if (I2C_readreg(REG_PID) != 0x76) {
return 1;
}
I2C_writereg(REG_COM7, COM7_RESET); /* reset to default values */
I2C_writereg(REG_CLKRC, CLK_EXT);
I2C_writereg(REG_COM7, COM7_FMT_VGA | COM7_RGB); /* output format: RGB */
I2C_writereg(REG_COM10, COM10_PCLK_HB); /* set pclk to switch only when data is valid */
return 0;
}
int main(void)
{
int i=1;
USART3_Config();
SysTick_Init();
init_I2C1();
sensor_ayarla();
printf("l3g4200d\r\n");
while (1){
if((I2C_readreg(L3G4200D_ADDR,STATUS_REG)&0x08)==0x08)
{
/* read the rate from the register */
gyrox_l_temp=I2C_readreg(L3G4200D_ADDR,OUT_X_L);
gyrox_h_temp=I2C_readreg(L3G4200D_ADDR,OUT_X_H);
gyrox_temp=((gyrox_h_temp << 8) | gyrox_l_temp);
gyroy_l_temp=I2C_readreg(L3G4200D_ADDR,OUT_Y_L);
gyroy_h_temp=I2C_readreg(L3G4200D_ADDR,OUT_Y_H);
gyroy_temp=((gyroy_h_temp << 8) | gyroy_l_temp);
gyroz_l_temp=I2C_readreg(L3G4200D_ADDR,OUT_Z_L);
gyroz_h_temp=I2C_readreg(L3G4200D_ADDR,OUT_Z_H);
gyroz_temp=((gyroz_h_temp << 8) | gyroz_l_temp);
printf("gyrox_temp=%d gyroy_temp=%d gyroz_temp=%d\r\n",gyrox_temp,gyroy_temp,gyroz_temp);
/* compute the value of rate */
gyrox = gyrox_temp * gyro_sensitivity_250 /1000 ;
gyroy = gyroy_temp * gyro_sensitivity_250 /1000 ;
gyroz = gyroz_temp * gyro_sensitivity_250 /1000 ;
/* print the value of rate*/
gyrox_int = (int16_t)gyrox;
gyrox_decimal = (gyrox - gyrox_int)*1000;
if(gyrox_decimal < 0)
gyrox_decimal=-gyrox_decimal;
gyroy_int = (int16_t)gyroy;
gyroy_decimal = (gyroy - gyroy_int)*1000;
if(gyroy_decimal < 0)
gyroy_decimal=-gyroy_decimal;
gyroz_int = (int16_t)gyroz;
gyroz_decimal = (gyroz - gyroz_int)*1000;
if(gyroz_decimal < 0)
gyroz_decimal=-gyroz_decimal;
printf("gyrox_float=%d.%d\r\n",gyrox_int,gyrox_decimal);
printf("gyroy_float=%d.%d\r\n",gyroy_int,gyroy_decimal);
printf("gyroz_float=%d.%d\r\n",gyroz_int,gyroz_decimal);
/* 頃角計算 */
i=1;
for(i;i<=50;i++)
gyro_sample();
//gyrox_angle=gyrox_angle+gyrox*0.5;
//gyroy_angle=gyroy_angle+gyroy*0.5;
//gyroz_angle=gyroz_angle+gyroz*0.5;
/* print the value of angle */
gyrox_angle_int = (int16_t)gyrox_angle;
gyrox_angle_decimal = (gyrox_angle - gyrox_angle_int)*1000;
if(gyrox_angle_decimal < 0)
gyrox_angle_decimal=-gyrox_angle_decimal;
gyroy_angle_int = (int16_t)gyroy_angle;
gyroy_angle_decimal = (gyroy_angle - gyroy_angle_int)*1000;
if(gyroy_angle_decimal < 0)
gyroy_angle_decimal=-gyroy_angle_decimal;
gyroz_angle_int = (int16_t)gyroz_angle;
gyroz_angle_decimal = (gyroz_angle - gyroz_angle_int)*1000;
if(gyroz_angle_decimal < 0)
gyroz_angle_decimal=-gyroz_angle_decimal;
printf("gyrox_angle_float=%d.%d\r\n",gyrox_angle_int,gyrox_angle_decimal);
printf("gyroy_angle_float=%d.%d\r\n",gyroy_angle_int,gyroy_angle_decimal);
printf("gyroz_angle_float=%d.%d\r\n",gyroz_angle_int,gyroz_angle_decimal);
printf("\r\n-------------------------------------------------------\r\n");
Delay(500);
}
}
}
void vBalanceTask(void *pvParameters)
{
const portTickType twosecDelay = 2000;
const portTickType xDelay = 3000;
vTaskDelay( twosecDelay );
PWM_Motor1 = PWM_MOTOR_INIT_MAX;
PWM_Motor2 = PWM_MOTOR_INIT_MAX;
PWM_Motor3 = PWM_MOTOR_INIT_MAX;
PWM_Motor4 = PWM_MOTOR_INIT_MAX;
vTaskDelay( xDelay ); //6S
PWM_Motor1 = PWM_MOTOR_INIT_MIN;
PWM_Motor2 = PWM_MOTOR_INIT_MIN;
PWM_Motor3 = PWM_MOTOR_INIT_MIN;
PWM_Motor4 = PWM_MOTOR_INIT_MIN;
/*inital Offset value of Gryo. and Acce.*/
LIS3DSH_Read(Buffer_Hx, LIS3DSH_OUT_X_H_REG_ADDR, 1);
LIS3DSH_Read(Buffer_Hy, LIS3DSH_OUT_Y_H_REG_ADDR, 1);
LIS3DSH_Read(Buffer_Lx, LIS3DSH_OUT_X_L_REG_ADDR, 1);
LIS3DSH_Read(Buffer_Ly, LIS3DSH_OUT_Y_L_REG_ADDR, 1);
XOffset = (int16_t)(Buffer_Hx[0] << 8 | Buffer_Lx[0]);
YOffset = (int16_t)(Buffer_Hy[0] << 8 | Buffer_Ly[0]);
/* reset gyro offset */
Buffer_GHx[0]=I2C_readreg(L3G4200D_ADDR,OUT_X_H);
Buffer_GHy[0]=I2C_readreg(L3G4200D_ADDR,OUT_Y_H);
Buffer_GHz[0]=I2C_readreg(L3G4200D_ADDR,OUT_Z_H);
Buffer_GLx[0]=I2C_readreg(L3G4200D_ADDR,OUT_X_L);
Buffer_GLy[0]=I2C_readreg(L3G4200D_ADDR,OUT_Y_L);
Buffer_GLz[0]=I2C_readreg(L3G4200D_ADDR,OUT_Z_L);
GXOffset = (int16_t)(Buffer_GHx[0] << 8 | Buffer_GLx[0]);
GYOffset = (int16_t)(Buffer_GHy[0] << 8 | Buffer_GLy[0]);
GZOffset = (int16_t)(Buffer_GHz[0] << 8 | Buffer_GLz[0]);
int i;
for(i = 1;i<128;i++) {
Buffer_GHx[0]=I2C_readreg(L3G4200D_ADDR,OUT_X_H);
Buffer_GHy[0]=I2C_readreg(L3G4200D_ADDR,OUT_Y_H);
Buffer_GHz[0]=I2C_readreg(L3G4200D_ADDR,OUT_Z_H);
Buffer_GLx[0]=I2C_readreg(L3G4200D_ADDR,OUT_X_L);
Buffer_GLy[0]=I2C_readreg(L3G4200D_ADDR,OUT_Y_L);
Buffer_GLz[0]=I2C_readreg(L3G4200D_ADDR,OUT_Z_L);
GXOffset = (int16_t)(Buffer_GHx[0] << 8 | Buffer_GLx[0]);
GYOffset = (int16_t)(Buffer_GHy[0] << 8 | Buffer_GLy[0]);
GZOffset = (int16_t)(Buffer_GHz[0] << 8 | Buffer_GLz[0]);
}
GXOffset = GXOffset / 128;
GYOffset = GYOffset / 128;
GZOffset = GZOffset / 128;
angle_x = 0;
angle_y = 0;
pwm_flag = 0;
argv.PitchP = 5.5f;
argv.PitchD = 1.2f;
argv.RollP = 5.5f;
argv.RollD = 1.2f;
argv.YawD = 4.1f;
Pitch_desire = 0.0f; //Desire angle of Pitch
Roll_desire = 0.0f; //Desire angle of Roll
xTimerStart(xTimerSampleRate, 0);
while(1){
//qprintf(xQueueUARTSend, "P12:%d ,P13:%d ,P14:%d ,P15:%d ,angle_x: %d,angle_y: %d\n\r", PWM_Motor1, PWM_Motor2, PWM_Motor3, PWM_Motor4, (int)angle_x, (int)angle_y);
}
}
/* sample rate and calculate rate (4ms,250HZ) */
void vTimerSample(xTimerHandle pxTimer)
{
LIS3DSH_Read(Buffer_Hx, LIS3DSH_OUT_X_H_REG_ADDR, 1);
LIS3DSH_Read(Buffer_Hy, LIS3DSH_OUT_Y_H_REG_ADDR, 1);
LIS3DSH_Read(Buffer_Lx, LIS3DSH_OUT_X_L_REG_ADDR, 1);
LIS3DSH_Read(Buffer_Ly, LIS3DSH_OUT_Y_L_REG_ADDR, 1);
x_acc = (float)((int16_t)(Buffer_Hx[0] << 8 | Buffer_Lx[0]) - XOffset) * Sensitivity_2G / 1000 * 180 / 3.14159f;
y_acc = (float)((int16_t)(Buffer_Hy[0] << 8 | Buffer_Ly[0]) - YOffset) * Sensitivity_2G / 1000 * 180 / 3.14159f;
Buffer_GHx[0]=I2C_readreg(L3G4200D_ADDR,OUT_X_H);
Buffer_GHy[0]=I2C_readreg(L3G4200D_ADDR,OUT_Y_H);
Buffer_GHz[0]=I2C_readreg(L3G4200D_ADDR,OUT_Z_H);
Buffer_GLx[0]=I2C_readreg(L3G4200D_ADDR,OUT_X_L);
Buffer_GLy[0]=I2C_readreg(L3G4200D_ADDR,OUT_Y_L);
Buffer_GLz[0]=I2C_readreg(L3G4200D_ADDR,OUT_Z_L);
x_gyro = (float)((int16_t)(Buffer_GHx[0] << 8 | Buffer_GLx[0]) - GXOffset) * Sensitivity_250 / 1000;
y_gyro = (float)((int16_t)(Buffer_GHy[0] << 8 | Buffer_GLy[0]) - GYOffset) * Sensitivity_250 / 1000;
z_gyro = (float)((int16_t)(Buffer_GHz[0] << 8 | Buffer_GLz[0]) - GZOffset) * Sensitivity_250 / 1000;
angle_x = (0.99f) * (angle_x + y_gyro * 0.004f) - (0.01f) * (x_acc);
angle_y = (0.99f) * (angle_y + x_gyro * 0.004f) + (0.01f) * (y_acc);
argv.Pitch = angle_y; //pitch degree
argv.Roll = angle_x; //roll degree
argv.Pitch_v = x_gyro; //pitch velocity
argv.Roll_v = y_gyro; //Roll velocity
argv.Pitch_err = Pitch_desire - argv.Pitch;
argv.Roll_err = Roll_desire - argv.Roll;
ROLL = (int)(argv.RollP * argv.Roll_err - argv.RollD * argv.Roll_v);
PITCH = (int)(argv.PitchP * argv.Pitch_err - argv.PitchD * argv.Pitch_v);
YAW = (int)(argv.YawD * z_gyro);
if (PITCH > MAXNUM) {
PITCH = MAXNUM;
}else if (PITCH < MINNUM) {
PITCH = MINNUM;
}else{
PITCH = PITCH;
}
if (ROLL > MAXNUM) {
ROLL = MAXNUM;
}else if (ROLL < MINNUM) {
ROLL = MINNUM;
}else{
ROLL = ROLL;
}
if(argv.Roll > 35 || argv.Roll < -35){
pwm_flag = 0;
Motor_Control(PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN);
}
if(argv.Pitch > 35 || argv.Pitch < -35){
pwm_flag = 0;
Motor_Control(PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN, PWM_MOTOR_MIN);
}
if(pwm_flag == 0){
}else{
Motor1 = PWM_Motor1_tmp + PITCH - ROLL - YAW; //LD4
Motor2 = PWM_Motor2_tmp + PITCH + ROLL + YAW; //LD3
Motor3 = PWM_Motor3_tmp - PITCH + ROLL - YAW; //LD5
Motor4 = PWM_Motor4_tmp - PITCH - ROLL + YAW; //LD6
Motor_Control(Motor1, Motor2, Motor3, Motor4);
}
}
