move gyroscope code into seperate file, add lqr code file
This commit is contained in:
Thefeli73
parent
948e704aa6
commit
3eaf27d29e
@ -1,13 +1,25 @@
|
||||
//gyroscope stuff
|
||||
#include <Adafruit_MPU6050.h>
|
||||
#include <Adafruit_Sensor.h>
|
||||
#include <Wire.h>
|
||||
|
||||
Adafruit_MPU6050 mpu;
|
||||
|
||||
//temporary variable to measure main loops
|
||||
int temp_loops;
|
||||
|
||||
//lqr stuff
|
||||
const uint8_t statesNumber = 4;
|
||||
/** Low pass filter angular Position*/
|
||||
float angularPositionLP = 0;
|
||||
/** Zumo's angular position */
|
||||
float angularPosition = 0;
|
||||
/** Corrected angular position */
|
||||
float correctedAngularPosition = 0;
|
||||
/** Zumo's angular speed */
|
||||
float angularSpeed = 0;
|
||||
/** Motor's angular position */
|
||||
float motorAngularPosition = 0;
|
||||
/** Motor's angular speed */
|
||||
float motorAngularSpeed = 0;
|
||||
/** PWM signal applied to the motor's driver 400 is 100% cicle and -400 100% but inverse direction */
|
||||
int32_t speed;
|
||||
|
||||
|
||||
//gyro stuff
|
||||
float AccX, AccY, AccZ;
|
||||
float GyroX, GyroY, GyroZ;
|
||||
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
|
||||
@ -48,85 +60,11 @@ unsigned long timeoldA;
|
||||
unsigned long timeoldB;
|
||||
|
||||
void setup() {
|
||||
//gyroscope
|
||||
Serial.begin(115200);
|
||||
while (!Serial)
|
||||
delay(10); // will pause Zero, Leonardo, etc until serial console opens
|
||||
|
||||
Serial.println("Adafruit MPU6050 test!");
|
||||
|
||||
// Try to initialize!
|
||||
if (!mpu.begin()) {
|
||||
Serial.println("Failed to find MPU6050 chip");
|
||||
while (1) {
|
||||
delay(10);
|
||||
}
|
||||
}
|
||||
Serial.println("MPU6050 Found!");
|
||||
|
||||
mpu.setAccelerometerRange(MPU6050_RANGE_4_G);
|
||||
Serial.print("Accelerometer range set to: ");
|
||||
switch (mpu.getAccelerometerRange()) {
|
||||
case MPU6050_RANGE_2_G:
|
||||
Serial.println("+-2G");
|
||||
break;
|
||||
case MPU6050_RANGE_4_G:
|
||||
Serial.println("+-4G");
|
||||
break;
|
||||
case MPU6050_RANGE_8_G:
|
||||
Serial.println("+-8G");
|
||||
break;
|
||||
case MPU6050_RANGE_16_G:
|
||||
Serial.println("+-16G");
|
||||
break;
|
||||
}
|
||||
mpu.setGyroRange(MPU6050_RANGE_500_DEG);
|
||||
Serial.print("Gyro range set to: ");
|
||||
switch (mpu.getGyroRange()) {
|
||||
case MPU6050_RANGE_250_DEG:
|
||||
Serial.println("+- 250 deg/s");
|
||||
break;
|
||||
case MPU6050_RANGE_500_DEG:
|
||||
Serial.println("+- 500 deg/s");
|
||||
break;
|
||||
case MPU6050_RANGE_1000_DEG:
|
||||
Serial.println("+- 1000 deg/s");
|
||||
break;
|
||||
case MPU6050_RANGE_2000_DEG:
|
||||
Serial.println("+- 2000 deg/s");
|
||||
break;
|
||||
}
|
||||
|
||||
mpu.setFilterBandwidth(MPU6050_BAND_184_HZ);
|
||||
Serial.print("Filter bandwidth set to: ");
|
||||
switch (mpu.getFilterBandwidth()) {
|
||||
case MPU6050_BAND_260_HZ:
|
||||
Serial.println("260 Hz");
|
||||
break;
|
||||
case MPU6050_BAND_184_HZ:
|
||||
Serial.println("184 Hz");
|
||||
break;
|
||||
case MPU6050_BAND_94_HZ:
|
||||
Serial.println("94 Hz");
|
||||
break;
|
||||
case MPU6050_BAND_44_HZ:
|
||||
Serial.println("44 Hz");
|
||||
break;
|
||||
case MPU6050_BAND_21_HZ:
|
||||
Serial.println("21 Hz");
|
||||
break;
|
||||
case MPU6050_BAND_10_HZ:
|
||||
Serial.println("10 Hz");
|
||||
break;
|
||||
case MPU6050_BAND_5_HZ:
|
||||
Serial.println("5 Hz");
|
||||
break;
|
||||
}
|
||||
|
||||
Serial.println("");
|
||||
delay(100);
|
||||
|
||||
calibrateGyro();
|
||||
|
||||
gyro_setup();
|
||||
|
||||
//motor
|
||||
pinMode(MotorPinA, OUTPUT);
|
||||
@ -149,58 +87,8 @@ void setup() {
|
||||
}
|
||||
|
||||
void loop() {
|
||||
gyro_loop();
|
||||
|
||||
/* Get new sensor events with the readings */
|
||||
sensors_event_t a, g, temp;
|
||||
mpu.getEvent(&a, &g, &temp);
|
||||
|
||||
|
||||
//Subtract the offset values from the raw gyro values
|
||||
gyro_x = g.gyro.x;
|
||||
gyro_y = g.gyro.y;
|
||||
gyro_z = g.gyro.z;
|
||||
|
||||
acc_x = a.acceleration.x;
|
||||
acc_y = a.acceleration.y;
|
||||
acc_z = a.acceleration.z;
|
||||
|
||||
gyro_x -= gyro_x_cal;
|
||||
gyro_y -= gyro_y_cal;
|
||||
gyro_z -= gyro_z_cal;
|
||||
previousTime = currentTime; // Previous time is stored before the actual time read
|
||||
currentTime = millis(); // Current time actual time read
|
||||
elapsedTime = (currentTime - previousTime) / 1000; // Divide by 1000 to get seconds
|
||||
|
||||
//Gyro angle calculations
|
||||
angle_pitch += gyro_x * elapsedTime * 180/PI; //Calculate the traveled pitch angle and add this to the angle_pitch variable, rad/s ---> degrees
|
||||
angle_roll += gyro_y * elapsedTime * 180/PI ; //Calculate the traveled roll angle and add this to the angle_roll variable
|
||||
//0.000001066 = 0.0000611 * (3.142(PI) / 180degr) sin function is in radians
|
||||
angle_pitch += angle_roll * sin(gyro_z * 0.000001066); //If the IMU has yawed transfer the roll angle to the pitch angel
|
||||
angle_roll -= angle_pitch * sin(gyro_z * 0.000001066); //If the IMU has yawed transfer the pitch angle to the roll angel
|
||||
|
||||
//Accelerometer angle calculations
|
||||
acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z)); //Calculate the total accelerometer vector
|
||||
//57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
|
||||
angle_pitch_acc = asin((float)acc_y/acc_total_vector)* 57.296; //Calculate the pitch angle
|
||||
angle_roll_acc = asin((float)acc_x/acc_total_vector)* -57.296; //Calculate the roll angle
|
||||
|
||||
angle_pitch_acc -= 0.0; //Accelerometer calibration value for pitch
|
||||
angle_roll_acc -= 0.0; //Accelerometer calibration value for roll
|
||||
|
||||
if(set_gyro_angles){ //If the IMU is already started
|
||||
angle_pitch = angle_pitch * 0.96 + angle_pitch_acc * 0.04; //Correct the drift of the gyro pitch angle with the accelerometer pitch angle
|
||||
angle_roll = angle_roll * 0.96 + angle_roll_acc * 0.04; //Correct the drift of the gyro roll angle with the accelerometer roll angle
|
||||
}
|
||||
else{ //At first start
|
||||
angle_pitch = angle_pitch_acc; //Set the gyro pitch angle equal to the accelerometer pitch angle
|
||||
angle_roll = angle_roll_acc; //Set the gyro roll angle equal to the accelerometer roll angle
|
||||
set_gyro_angles = true; //IMU started flag
|
||||
}
|
||||
|
||||
// complementary filter
|
||||
angle_pitch_output = angle_pitch_output * 0.8 + angle_pitch * 0.2; //Take 90% of the output pitch value and add 10% of the raw pitch value
|
||||
angle_roll_output = angle_roll_output * 0.8 + angle_roll * 0.2; //Take 90% of the output roll value and add 10% of the raw roll value
|
||||
|
||||
if(temp_loops>100){
|
||||
Serial.println("");
|
||||
Serial.print(" pitch Angle = "); Serial.println(angle_pitch_output);
|
||||
@ -240,45 +128,6 @@ void loop() {
|
||||
}
|
||||
}
|
||||
|
||||
void calibrateGyro() {
|
||||
for (int cal_int = 0; cal_int < 1000 ; cal_int ++){ //Read the raw acc and gyro data from the MPU-6050 for 1000 times
|
||||
sensors_event_t a, g, temp;
|
||||
mpu.getEvent(&a, &g, &temp);
|
||||
|
||||
gyro_x = g.gyro.x;
|
||||
gyro_y = g.gyro.y;
|
||||
gyro_z = g.gyro.z;
|
||||
|
||||
gyro_x_cal += g.gyro.x ; //Add the gyro x offset to the gyro_x_cal variable
|
||||
gyro_y_cal += g.gyro.y ; //Add the gyro y offset to the gyro_y_cal variable
|
||||
gyro_z_cal += g.gyro.z ; //Add the gyro z offset to the gyro_z_cal variable
|
||||
|
||||
delay(3); //Delay 3us to have 250Hz for-loop
|
||||
}
|
||||
|
||||
// divide by 1000 to get avarage offset
|
||||
gyro_x_cal /= 1000;
|
||||
gyro_y_cal /= 1000;
|
||||
gyro_z_cal /= 1000;
|
||||
|
||||
loop_timer = micros(); //Reset the loop timer
|
||||
|
||||
if (half_revolutionsA >= 20) {
|
||||
rpmA = 30*1000/(millis() - timeoldA)*half_revolutionsA;
|
||||
timeoldA = millis();
|
||||
half_revolutionsA = 0;
|
||||
Serial.println(rpmA);
|
||||
Serial.print(" pitch Angle = "); Serial.println(angle_pitch_output);
|
||||
}
|
||||
|
||||
if (half_revolutionsB >= 20) {
|
||||
rpmB = 30*1000/(millis() - timeoldB)*half_revolutionsB;
|
||||
timeoldB = millis();
|
||||
half_revolutionsB = 0;
|
||||
Serial.println(rpmB);
|
||||
Serial.print(" pitch Angle = "); Serial.println(angle_pitch_output);
|
||||
}
|
||||
}
|
||||
void magnet_detectA()//This function is called whenever a magnet/interrupt is detected by the arduino
|
||||
{
|
||||
half_revolutionsA++;
|
||||
|
||||
20
EENX15_LQR/LQR.ino
Normal file
20
EENX15_LQR/LQR.ino
Normal file
@ -0,0 +1,20 @@
|
||||
/** Control Law*/
|
||||
const float K[statesNumber] = {0.55192, 8.9867, 0.194, 0.39237};
|
||||
/** Correction factor */
|
||||
const float scaleConst = 2.5;
|
||||
|
||||
/**
|
||||
* LQR control law
|
||||
*/
|
||||
void lqr() {
|
||||
speed = 0;
|
||||
|
||||
speed -= motorAngularPosition * K[0];
|
||||
speed -= correctedAngularPosition * K[1];
|
||||
speed -= motorAngularSpeed * K[2];
|
||||
speed -= angularSpeed * K[3];
|
||||
|
||||
speed = speed*scaleConst;
|
||||
|
||||
}
|
||||
|
||||
177
EENX15_LQR/gyro.ino
Normal file
177
EENX15_LQR/gyro.ino
Normal file
@ -0,0 +1,177 @@
|
||||
//gyroscope stuff
|
||||
#include <Adafruit_MPU6050.h>
|
||||
#include <Adafruit_Sensor.h>
|
||||
#include <Wire.h>
|
||||
|
||||
Adafruit_MPU6050 mpu;
|
||||
|
||||
void gyro_setup(){
|
||||
Serial.println("Adafruit MPU6050 test!");
|
||||
|
||||
// Try to initialize!
|
||||
if (!mpu.begin()) {
|
||||
Serial.println("Failed to find MPU6050 chip");
|
||||
while (1) {
|
||||
delay(10);
|
||||
}
|
||||
}
|
||||
Serial.println("MPU6050 Found!");
|
||||
|
||||
mpu.setAccelerometerRange(MPU6050_RANGE_4_G);
|
||||
Serial.print("Accelerometer range set to: ");
|
||||
switch (mpu.getAccelerometerRange()) {
|
||||
case MPU6050_RANGE_2_G:
|
||||
Serial.println("+-2G");
|
||||
break;
|
||||
case MPU6050_RANGE_4_G:
|
||||
Serial.println("+-4G");
|
||||
break;
|
||||
case MPU6050_RANGE_8_G:
|
||||
Serial.println("+-8G");
|
||||
break;
|
||||
case MPU6050_RANGE_16_G:
|
||||
Serial.println("+-16G");
|
||||
break;
|
||||
}
|
||||
mpu.setGyroRange(MPU6050_RANGE_500_DEG);
|
||||
Serial.print("Gyro range set to: ");
|
||||
switch (mpu.getGyroRange()) {
|
||||
case MPU6050_RANGE_250_DEG:
|
||||
Serial.println("+- 250 deg/s");
|
||||
break;
|
||||
case MPU6050_RANGE_500_DEG:
|
||||
Serial.println("+- 500 deg/s");
|
||||
break;
|
||||
case MPU6050_RANGE_1000_DEG:
|
||||
Serial.println("+- 1000 deg/s");
|
||||
break;
|
||||
case MPU6050_RANGE_2000_DEG:
|
||||
Serial.println("+- 2000 deg/s");
|
||||
break;
|
||||
}
|
||||
|
||||
mpu.setFilterBandwidth(MPU6050_BAND_184_HZ);
|
||||
Serial.print("Filter bandwidth set to: ");
|
||||
switch (mpu.getFilterBandwidth()) {
|
||||
case MPU6050_BAND_260_HZ:
|
||||
Serial.println("260 Hz");
|
||||
break;
|
||||
case MPU6050_BAND_184_HZ:
|
||||
Serial.println("184 Hz");
|
||||
break;
|
||||
case MPU6050_BAND_94_HZ:
|
||||
Serial.println("94 Hz");
|
||||
break;
|
||||
case MPU6050_BAND_44_HZ:
|
||||
Serial.println("44 Hz");
|
||||
break;
|
||||
case MPU6050_BAND_21_HZ:
|
||||
Serial.println("21 Hz");
|
||||
break;
|
||||
case MPU6050_BAND_10_HZ:
|
||||
Serial.println("10 Hz");
|
||||
break;
|
||||
case MPU6050_BAND_5_HZ:
|
||||
Serial.println("5 Hz");
|
||||
break;
|
||||
}
|
||||
|
||||
Serial.println("");
|
||||
delay(100);
|
||||
|
||||
calibrateGyro();
|
||||
}
|
||||
|
||||
void gyro_loop(){
|
||||
|
||||
/* Get new sensor events with the readings */
|
||||
sensors_event_t a, g, temp;
|
||||
mpu.getEvent(&a, &g, &temp);
|
||||
|
||||
|
||||
//Subtract the offset values from the raw gyro values
|
||||
gyro_x = g.gyro.x;
|
||||
gyro_y = g.gyro.y;
|
||||
gyro_z = g.gyro.z;
|
||||
|
||||
acc_x = a.acceleration.x;
|
||||
acc_y = a.acceleration.y;
|
||||
acc_z = a.acceleration.z;
|
||||
|
||||
gyro_x -= gyro_x_cal;
|
||||
gyro_y -= gyro_y_cal;
|
||||
gyro_z -= gyro_z_cal;
|
||||
previousTime = currentTime; // Previous time is stored before the actual time read
|
||||
currentTime = millis(); // Current time actual time read
|
||||
elapsedTime = (currentTime - previousTime) / 1000; // Divide by 1000 to get seconds
|
||||
|
||||
//Gyro angle calculations
|
||||
angle_pitch += gyro_x * elapsedTime * 180/PI; //Calculate the traveled pitch angle and add this to the angle_pitch variable, rad/s ---> degrees
|
||||
angle_roll += gyro_y * elapsedTime * 180/PI ; //Calculate the traveled roll angle and add this to the angle_roll variable
|
||||
//0.000001066 = 0.0000611 * (3.142(PI) / 180degr) sin function is in radians
|
||||
angle_pitch += angle_roll * sin(gyro_z * 0.000001066); //If the IMU has yawed transfer the roll angle to the pitch angel
|
||||
angle_roll -= angle_pitch * sin(gyro_z * 0.000001066); //If the IMU has yawed transfer the pitch angle to the roll angel
|
||||
|
||||
//Accelerometer angle calculations
|
||||
acc_total_vector = sqrt((acc_x*acc_x)+(acc_y*acc_y)+(acc_z*acc_z)); //Calculate the total accelerometer vector
|
||||
//57.296 = 1 / (3.142 / 180) The Arduino asin function is in radians
|
||||
angle_pitch_acc = asin((float)acc_y/acc_total_vector)* 57.296; //Calculate the pitch angle
|
||||
angle_roll_acc = asin((float)acc_x/acc_total_vector)* -57.296; //Calculate the roll angle
|
||||
|
||||
angle_pitch_acc -= 0.0; //Accelerometer calibration value for pitch
|
||||
angle_roll_acc -= 0.0; //Accelerometer calibration value for roll
|
||||
|
||||
if(set_gyro_angles){ //If the IMU is already started
|
||||
angle_pitch = angle_pitch * 0.96 + angle_pitch_acc * 0.04; //Correct the drift of the gyro pitch angle with the accelerometer pitch angle
|
||||
angle_roll = angle_roll * 0.96 + angle_roll_acc * 0.04; //Correct the drift of the gyro roll angle with the accelerometer roll angle
|
||||
}
|
||||
else{ //At first start
|
||||
angle_pitch = angle_pitch_acc; //Set the gyro pitch angle equal to the accelerometer pitch angle
|
||||
angle_roll = angle_roll_acc; //Set the gyro roll angle equal to the accelerometer roll angle
|
||||
set_gyro_angles = true; //IMU started flag
|
||||
}
|
||||
|
||||
// complementary filter
|
||||
angle_pitch_output = angle_pitch_output * 0.8 + angle_pitch * 0.2; //Take 90% of the output pitch value and add 10% of the raw pitch value
|
||||
angle_roll_output = angle_roll_output * 0.8 + angle_roll * 0.2; //Take 90% of the output roll value and add 10% of the raw roll value
|
||||
}
|
||||
|
||||
void calibrateGyro() {
|
||||
for (int cal_int = 0; cal_int < 1000 ; cal_int ++){ //Read the raw acc and gyro data from the MPU-6050 for 1000 times
|
||||
sensors_event_t a, g, temp;
|
||||
mpu.getEvent(&a, &g, &temp);
|
||||
|
||||
gyro_x = g.gyro.x;
|
||||
gyro_y = g.gyro.y;
|
||||
gyro_z = g.gyro.z;
|
||||
|
||||
gyro_x_cal += g.gyro.x ; //Add the gyro x offset to the gyro_x_cal variable
|
||||
gyro_y_cal += g.gyro.y ; //Add the gyro y offset to the gyro_y_cal variable
|
||||
gyro_z_cal += g.gyro.z ; //Add the gyro z offset to the gyro_z_cal variable
|
||||
|
||||
delay(3); //Delay 3us to have 250Hz for-loop
|
||||
}
|
||||
|
||||
// divide by 1000 to get avarage offset
|
||||
gyro_x_cal /= 1000;
|
||||
gyro_y_cal /= 1000;
|
||||
gyro_z_cal /= 1000;
|
||||
|
||||
loop_timer = micros(); //Reset the loop timer
|
||||
|
||||
if (half_revolutionsA >= 20) {
|
||||
rpmA = 30*1000/(millis() - timeoldA)*half_revolutionsA;
|
||||
timeoldA = millis();
|
||||
half_revolutionsA = 0;
|
||||
Serial.println(rpmA);
|
||||
Serial.print(" pitch Angle = "); Serial.println(angle_pitch_output);
|
||||
}
|
||||
|
||||
if (half_revolutionsB >= 20) {
|
||||
rpmB = 30*1000/(millis() - timeoldB)*half_revolutionsB;
|
||||
timeoldB = millis();
|
||||
half_revolutionsB = 0;
|
||||
Serial.println(rpmB);
|
||||
Serial.print(" pitch Angle = "); Serial.println(angle_pitch_output);
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user