Merge branch 'lqr-testing' into nytt-test-bös

EENX15_LQR/EENX15_LQR.ino

@@ -1,3 +1,4 @@
+//EENX15_LQR.ino
 #include <Wire.h>
 
 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;
@@ -29,7 +30,7 @@ float force;
 int PWM;
 
 
-//gyro stuff
+//gyro
 float AccX, AccY, AccZ;
 float GyroX, GyroY, GyroZ;
 float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
@@ -50,7 +51,7 @@ float angle_pitch_output, angle_roll_output;
 long loop_timer;
 int temp;
 
-//motor stuff
+//motor
 #define encoderA1 2
 #define encoderB1 3
 
@@ -84,7 +85,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();
   
@@ -94,7 +95,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);
@@ -112,14 +112,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();
   }
 }
@@ -150,25 +149,9 @@ 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;

EENX15_LQR/LQR.ino

@@ -1,40 +1,17 @@
-//LQR-stuff
+//LQR.ino
-//#include "Arduino_skal.h"
-
-// | ///////////////////////////////////
-// | //Row 24-52 in Arduino_skal_data.cpp
-// v ///////////////////////////////////
-
-  // Expression: A
-  //  Referenced by: '<Root>/Gain4'
 
 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: '<Root>/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: '<Root>/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: '<Root>/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: '<Root>/Sum2'
   for (int i = 0; i < 4; i++) {
-    // Sum: '<Root>/Sum4' incorporates:
-    //   Gain: '<Root>/Gain2'
-    //   Gain: '<Root>/Gain3'
-    //   Gain: '<Root>/Gain4'
-    //   Integrator: '<Root>/Integrator1'
-    //   Sum: '<Root>/Sum3'
   
   
     Sum3[i] = ((matrix_L[i + 4] * tmp[1] + matrix_L[i]
@@ -122,28 +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: '<Root>/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: '<Root>/Integrator1'
   for (int i = 0; i < 4; i++) {
     Integrator1_CSTATE[i] = Sum4[i] * fastTimer/1000.0;
     Serial.print("Integrator: "); Serial.println(Integrator1_CSTATE[i]);

EENX15_LQR/gyro.ino

@@ -1,4 +1,4 @@
-//gyroscope stuff
+//gyro.ino
 #include <Adafruit_MPU6050.h>
 #include <Adafruit_Sensor.h>
 
@@ -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()) {

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,25 +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 angle_speed_rs) {
-  /*
-  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)