Smart Car Learning (13)--angle control

I. Manual code and illustrations

Ii. Process description 1, Angle calculation function description

//============================================================================//function name: void Anglecalculate (void)//function return: none//parameter description: None//Function Summary: Body angle calculation. //============================================================================voidAnglecalculate (void) {  //Reading AccelerometerMma8451_read (); //Read GyroscopeLs3g4200d_read (); //take the x-axis data of the gyroscopeVoltage_gyro =gyro_x; //take the y-axis data of the accelerometer readingVoltage_gravity =gray_y; //Gyroscope 0 Partial followgyro_offset_calculate (); if(Voltage_gravity >1050)//Limit Accelerometer readings -1050 ~1050{voltage_gravity=1050; }    if(Voltage_gravity <=-1050) {voltage_gravity= -1050; }      //the y-axis value of the accelerometer read is converted to an angleG_fgravityangle = (voltage_gravity-gravity_offset) *Gravity_angle_ratio; //gyroscope extraction angular velocityG_fgyroscopeanglespeed = (voltage_gyro-gyroscope_offset) *Gyroscope_angle_ratio; //Body Angle CalculationG_fcarangle =g_fgyroscopeangleintegral; Fdeltavalue= (G_fgravityangle-g_fcarangle)/gravity_adjust_time_constant; G_fgyroscopeangleintegral+ = (g_fgyroscopeanglespeed + fdeltavalue)/gyroscope_angle_sigma_frequency;}

The first is to measure the gravitational acceleration by means of an accelerometer and then to calculate the angle by normalization (as the G is proportional to the angle). Then, because the car in the course of movement, the acceleration of the car will have a certain impact, resulting in not proportional relations, so the use of gyroscope correction, gyroscope measured angular velocity, after the integration is the angle, and then the accelerometer measured angle fusion, and then feedback back, so that the system balance.

2, Gyro 0-point follow

/============================================================================//function name: void Gyro_offset_calculate ()//function return: none//parameter description: None//Feature Summary://Gyro Three axis 0 points follow cut_gyro_x for the treated value, gyro_x is the actual read value, Gyro_x_offset is 0 points//============================================================================voidgyro_offset_calculate () {floatcut_gyro_x; //subtract real-time values that are worth the offsetcut_gyro_x = gyro_x-Gyroscope_offset; //Modify the 0 bias based on real-time values  if(cut_gyro_x>=2) {Gyroscope_offset+=0.001; }   Else if(cut_gyro_x<=-2) {Gyroscope_offset-=0.001; }   Else{Gyroscope_offset= Gyroscope_offset *0.99+0.01*gyro_x; }

  //boot 2 seconds to follow          if-0.50.5 * gyro_ X;  }  }

Modify the 0 offset according to the real-time value.

3, some of the parameters set:

/*************** Upright Control *****************/floatG_fgravityangle =0;//the angle that the accelerometer converts tofloatG_fgyroscopeanglespeed =0;//Gyro-Converted angular velocityfloatG_fcarangle =0;//Body AnglefloatVoltage_gyro =0;//Defining a gyroscope (select X, Y, z)floatVoltage_gravity =0;//Defining an accelerometer (select X, Y, z)floatGravity_offset = -;//Accelerometer 0 OffsetfloatGravity_angle_ratio = ( -/(1050.0- -)) ;//The normalized factor of an accelerometerfloatGyroscope_offset =0;//Gyroscope 0 OffsetfloatGyroscope_angle_ratio =1.780;//Normalization FactorfloatGravity_adjust_time_constant =2;//the scale factor of gyroscope acceleration JigafloatGyroscope_angle_sigma_frequency = $;//Frequency of pointsfloatClocks_gyro =0;//Gyroscope 0-point ratio in power-on 2s

Smart Car Learning (13)--angle control