From baa64b6240151d0c51e8850f258ce0917670ff30 Mon Sep 17 00:00:00 2001 From: Thefeli73 Date: Thu, 13 May 2021 11:38:40 +0200 Subject: [PATCH] clean garbage --- EENX15_LQR/EENX15_LQR.ino | 38 +++++--------------- EENX15_LQR/LQR.ino | 70 +----------------------------------- EENX15_LQR/gyro.ino | 4 +-- EENX15_LQR/lqr_fullstate.ino | 24 +------------ 4 files changed, 12 insertions(+), 124 deletions(-) diff --git a/EENX15_LQR/EENX15_LQR.ino b/EENX15_LQR/EENX15_LQR.ino index 289a59d..8d8e0d9 100644 --- a/EENX15_LQR/EENX15_LQR.ino +++ b/EENX15_LQR/EENX15_LQR.ino @@ -1,3 +1,4 @@ +//EENX15_LQR.ino #include int lastCorrectionTime = 0; @@ -6,7 +7,7 @@ int lastPrintTime = 0; static int fastTimer = 10; //ms static int slowTimer = 1000; //ms -//lqr stuff +//lqr const uint8_t statesNumber = 4; /** Low pass filter angular Position*/ float angularPositionLP = 0; @@ -26,7 +27,7 @@ int speed; int safe_angle; -//gyro stuff +//gyro float AccX, AccY, AccZ; float GyroX, GyroY, GyroZ; float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ; @@ -47,7 +48,7 @@ float angle_pitch_output, angle_roll_output; long loop_timer; int temp; -//motor stuff +//motor #define encoderA1 2 #define encoderB1 3 @@ -81,7 +82,7 @@ void setup() { Wire.begin(); Serial.begin(115200); while (!Serial) - delay(10); // will pause Zero, Leonardo, etc until serial console opens + delay(10); // will pause until serial console opens gyro_setup(); @@ -91,7 +92,6 @@ void setup() { pinMode(encoderA2, INPUT_PULLUP); pinMode(encoderB2, INPUT_PULLUP); attachInterrupt(digitalPinToInterrupt(encoderA1), pulseA, RISING); - //attachInterrupt(digitalPinToInterrupt(encoderB1), pulseB, RISING); pinMode(MotorPinA, OUTPUT); pinMode(MotorSpeedA, OUTPUT); @@ -109,14 +109,13 @@ void loop() { int m = millis(); - if (m - lastCorrectionTime >= fastTimer) { //run this code ever 80ms (12.5hz) + if (m - lastCorrectionTime >= fastTimer) { //run this code every [fastTimer]ms lastCorrectionTime = m; getSpeed(); setSpeed(); } - if (m - lastPrintTime >= slowTimer) { //run this code every + if (m - lastPrintTime >= slowTimer) { //run this code every [slowTimer]ms lastPrintTime = m; - //set_test_speed(); printInfo(); } } @@ -147,33 +146,14 @@ void printInfo(){ Serial.print("RPM: "); Serial.println(rpm); //ca. 56 tick per rotation Serial.print("Rads per second: "); Serial.println(rps); //ca. 56 tick per rotation, 6.26 rads per rotation } -/* -int temp_counter = 0; -void set_test_speed(){ - digitalWrite(MotorPinB, CCW); - digitalWrite(MotorPinA, CW); - speed = temp_counter; - Serial.print("Speed pre calc: "); Serial.println(speed); //ca. 56 tick per rotation, 6.26 rads per rotation - speed = 3145.84/(pow((90.75 - speed),1.00715)); - speed = constrain(speed, 0, 255); - analogWrite(MotorSpeedB, speed); //Wheel close to connections - analogWrite(MotorSpeedA, speed); //First experiment wheel - Serial.print("Rads per second: "); Serial.println(rps); //ca. 56 tick per rotation, 6.26 rads per rotation - Serial.print("Speed: "); Serial.println(speed); //ca. 56 tick per rotation, 6.26 rads per rotation - temp_counter += 3; -} -*/ void setSpeed(){ if(abs(safe_angle)<50 ){ - //speed = 8*safe_angle; float position_m = countA/174.76; float speed_ms = rps * 0.05; float angle_r = angle_pitch_output * 0.318; float angle_speed_rs = rps; - //speed = lqr_fullstate(position_m, speed_ms, angle_r, angle_speed_rs);/// 0.019608; // (0.20*255) speed = lqr_fullstate(0, 0, angle_r, 0);/// 0.019608; // (0.20*255) - //speed = -22 * inputToControlSystem(0, 1); if(speed<0){ digitalWrite(MotorPinB, CW); digitalWrite(MotorPinA, CCW); @@ -188,8 +168,8 @@ void setSpeed(){ if(speed!=0){ speed = constrain(speed, -255, 255);speed = calc_speed(speed); } speed = abs(speed); speed = constrain(speed, 0, 255); - analogWrite(MotorSpeedB, speed); //Wheel close to connections - analogWrite(MotorSpeedA, speed); //First experiment wheel + analogWrite(MotorSpeedB, speed); + analogWrite(MotorSpeedA, speed); } else{ speed = 0; diff --git a/EENX15_LQR/LQR.ino b/EENX15_LQR/LQR.ino index 897eafb..76faca9 100644 --- a/EENX15_LQR/LQR.ino +++ b/EENX15_LQR/LQR.ino @@ -1,40 +1,17 @@ -//LQR-stuff -//#include "Arduino_skal.h" - -// | /////////////////////////////////// -// | //Row 24-52 in Arduino_skal_data.cpp -// v /////////////////////////////////// - - // Expression: A - // Referenced by: '/Gain4' +//LQR.ino const double matrix_A [16] = { 0.0, 0.0, 0.0, 0.0, 1.0, -0.20780947085442231, 0.0, -0.52810302415000854, 0.0, 13.239785742831822, 0.0, 58.601480177829842, 0.0, 0.0, 1.0, 0.0 }; -//const double matrix_A [16] = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}; - - // Expression: C - // Referenced by: '/Gain6' - const double matrix_C [8] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0 }; -//const double matrix_C [8] = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; - - // Expression: L - // Referenced by: '/Gain2' const double matrix_L [8] = { 56.7847, 799.5294, -1.4914, -57.4160, -1.0363, -16.1071, 57.0075, 870.8172 }; const double matrix_L_old [8] = { 116.63033952875418, 3387.8673967111704, -1.4473912197449676, -115.34372132703447, -1.0534041975488044, -48.223441605702455, 117.16185100039935, 3490.0480780568214 }; -//const double matrix_L [8] = { 116.63033952875418, 338.78673967111704, -1.4473912197449676, -// -115.34372132703447, -1.0534041975488044, -48.223441605702455, -// 117.16185100039935, 34.900480780568214 }; - - // Expression: B - // Referenced by: '/Gain3' const double matrix_B [4] = { 0.0, 2.078094708544223, 0.0, 5.2810302415000852 }; @@ -42,25 +19,12 @@ const double matrix_B [4] = { 0.0, 2.078094708544223, 0.0, 5.2810302415000852 }; const double matrix_K_old [4] = {-31.622776601683942, -21.286439360075747, 80.789376267003959, 13.42463576551093}; const double matrix_K [4] = {-0.0316, -0.3938, 22.9455, 3.0629}; -// | /////////////////////////////////// -// | //Row 261-264 in Arduino_skal.cpp -// v /////////////////////////////////// double Integrator1_CSTATE [4] = {0.0, 0.0, 0.0, 0.0}; double Sum3[4]; double Sum4[4]; -// | /////////////////////////////////// -// | //Row 123-124 in Arduino_skal.cpp -// v /////////////////////////////////// - double tmp[2]; double rtb_Saturation = 0.0; - -// | /////////////////////////////////// -// | //Row 140-143 in Arduino_skal.cpp -// v /////////////////////////////////// - -// Denna funktion bör anropas när styrka + riktning till motorer ska bestämmas. double saturatedSignalToMotors(){ rtb_Saturation = ((matrix_K[0] * Integrator1_CSTATE[0] + matrix_K[1] * Integrator1_CSTATE[1]) + @@ -77,9 +41,6 @@ double saturatedSignalToMotors(){ Serial.print("Saturation = "); Serial.println(rtb_Saturation); return rtb_Saturation; } -// | /////////////////////////////////// -// | //Row 165-188 in Arduino_skal.cpp -// v /////////////////////////////////// double inputToControlSystem(float position_m, float angle_r){ float posAndAng[] = {position_m, angle_r}; for (int i = 0; i < 2; i++) { @@ -89,15 +50,7 @@ double inputToControlSystem(float position_m, float angle_r){ Integrator1_CSTATE[2]) + matrix_C[i + 6] * Integrator1_CSTATE[3]); } - - // End of Sum: '/Sum2' for (int i = 0; i < 4; i++) { - // Sum: '/Sum4' incorporates: - // Gain: '/Gain2' - // Gain: '/Gain3' - // Gain: '/Gain4' - // Integrator: '/Integrator1' - // Sum: '/Sum3' Sum3[i] = ((matrix_L[i + 4] * tmp[1] + matrix_L[i] @@ -122,29 +75,8 @@ double inputToControlSystem(float position_m, float angle_r){ Arduino_skal_derivatives(); return saturatedSignalToMotors(); } - -// | /////////////////////////////////// -// | //Row 215-225 in Arduino_skal.cpp -// v /////////////////////////////////// - void Arduino_skal_derivatives() { - /* - XDot *_rtXdot; - _rtXdot = ((XDot *) (&rtM)->derivs); - // Derivatives for Integrator: '/Integrator1' - _rtXdot->Integrator1_CSTATE[0] = Sum4[0]; - _rtXdot->Integrator1_CSTATE[1] = Sum4[1]; - _rtXdot->Integrator1_CSTATE[2] = Sum4[2]; - _rtXdot->Integrator1_CSTATE[3] = Sum4[3]; - - - Integrator1_CSTATE[0] = Sum4[0] * (fastTimer/1000.0); - Integrator1_CSTATE[1] = Sum4[1] * (fastTimer/1000.0); - Integrator1_CSTATE[2] = Sum4[2] * (fastTimer/1000.0); - Integrator1_CSTATE[3] = Sum4[3] * (fastTimer/1000.0); -*/ - // Derivatives for Integrator: '/Integrator1' for (int i = 0; i < 4; i++) { Integrator1_CSTATE[i] = Sum4[i] * fastTimer/1000.0; Serial.print("Integrator: "); Serial.println(Integrator1_CSTATE[i]); diff --git a/EENX15_LQR/gyro.ino b/EENX15_LQR/gyro.ino index eac9cd5..5911d2a 100644 --- a/EENX15_LQR/gyro.ino +++ b/EENX15_LQR/gyro.ino @@ -1,4 +1,4 @@ -//gyroscope stuff +//gyro.ino #include #include @@ -16,7 +16,6 @@ void gyro_setup(){ } Serial.println("MPU6050 Found!"); - //mpu.setAccelerometerRange(MPU6050_RANGE_4_G); mpu.setAccelerometerRange(MPU6050_RANGE_16_G); Serial.print("Accelerometer range set to: "); switch (mpu.getAccelerometerRange()) { @@ -33,7 +32,6 @@ void gyro_setup(){ Serial.println("+-16G"); break; } - //mpu.setGyroRange(MPU6050_RANGE_500_DEG); mpu.setGyroRange(MPU6050_RANGE_2000_DEG); Serial.print("Gyro range set to: "); switch (mpu.getGyroRange()) { diff --git a/EENX15_LQR/lqr_fullstate.ino b/EENX15_LQR/lqr_fullstate.ino index 34dd634..2b31684 100644 --- a/EENX15_LQR/lqr_fullstate.ino +++ b/EENX15_LQR/lqr_fullstate.ino @@ -1,13 +1,5 @@ +//lqr_fullstate.ino float lqr_fullstate(float position_m, float speed_ms, float angle_r, float angle_speed_rs){ -// const float matrix_K [4] = {-0.0316, -0.3938, 22.9455, 3.0629}; -// const float matrix_K [4] = {-1.0000, -1.8855, 26.6999, 3.8592}; -// const float matrix_K [4] = {-1.0000, -1.9058, 27.0579, 3.8886}; -// const float matrix_K [4] = {-0.3162, -1.0381, 24.6060, 3.4143}; -// const float matrix_K [4] = {-0.1054, -0.6273, 23.5822, 3.1936}; -// const float matrix_K [4] = {-0.1054, -0.5273, 23.5546, 3.1876}; -// const float matrix_K [4] = {-0.1054, -0.5273, 23.5546, 3.1876}; -// const float matrix_K [4] = {-0.7071, -1.5720, 26.0726, 3.7050}; -// const float matrix_K [4] = {-0.7071, -1.5980, 26.6081, 4.3220}; const float matrix_K [4] = {-0.7071, -1.7751, 34.5368, 4.8793}; float result; @@ -15,24 +7,10 @@ float lqr_fullstate(float position_m, float speed_ms, float angle_r, float angle matrix_K[1] * speed_ms + matrix_K[2] * angle_r + matrix_K[3] * angle_speed_rs; - /* - if (result > 0.29) { - result = 0.29; - } else if (result < -0.29) { - result = -0.29; - } else { - result = result; - }*/ Serial.print("K calculation (force): "); Serial.println(result); return result; } float calc_speed(float input) { - /* - float a = -2971; - float b = -0.9929; - float c = 90.75; - float radps = a * pow(speed, b) + c; ////// the response graph - */ float scale = 1.5; input = abs(input)*0.30796; // scale down to rad/s (78,53/255) Serial.print("input: "); Serial.println(input);