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

EENX15_LQR/EENX15_LQR.ino

@@ -1,12 +1,13 @@
+//EENX15_LQR.ino
 #include <Wire.h>
 
 int lastCorrectionTime = 0;
 int lastPrintTime = 0;
 
-static int fastTimer = 80; //ms
-static int slowTimer = 800; //ms
+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;
@@ -22,11 +23,14 @@ float motorAngularPosition = 0;
 float motorAngularSpeed = 0;
 
 /** PWM signal applied to the motor's driver 255 is 100% */
-int32_t speed;
+int speed;
+float Va;
 int safe_angle;
+float force;
+int PWM;
 
 
-//gyro stuff
+//gyro
 float AccX, AccY, AccZ;
 float GyroX, GyroY, GyroZ;
 float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
@@ -47,7 +51,7 @@ float angle_pitch_output, angle_roll_output;
 long loop_timer;
 int temp;
 
-//motor stuff
+//motor
 #define encoderA1 2
 #define encoderB1 3
 
@@ -81,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();
   
@@ -91,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);
@@ -100,6 +103,7 @@ void setup() {
   pinMode(MotorPinB, OUTPUT);
   pinMode(MotorSpeedB, OUTPUT);
   pinMode(MotorBrakeB, OUTPUT);
+
 }
  
 void loop() {
@@ -108,12 +112,12 @@ 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 ever 800ms (1.25hz)
+  if (m - lastPrintTime >= slowTimer) { //run this code every [slowTimer]ms
     lastPrintTime = m;
     printInfo();
   }
@@ -139,6 +143,7 @@ void printInfo(){
   Serial.print("pitch Angle measured = "); Serial.println(angle_pitch);
   Serial.print("Position: "); Serial.println(countA);
   Serial.print("Position (m): "); Serial.println(countA/174.76); //r*2pi
+  Serial.print("speed (m/s): "); Serial.println(rps * 0.05); //r*rads
   Serial.print("Full Rotations: "); Serial.println(countA/56.0); //ca. 56 tick per rotation
   Serial.print("Rads rotated: "); Serial.println(countA/8.91); //ca. 56 tick per rotation, 6.26 rads per rotation
   Serial.print("RPM: "); Serial.println(rpm); //ca. 56 tick per rotation
@@ -147,34 +152,37 @@ void printInfo(){
 
 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;
-    speed = inputToControlSystem(position_m, angle_r);
-    speed *= 22;
-    if(speed<0){
+    float angle_speed_rs = rps;
+    //speed = lqr_fullstate(position_m, speed_ms, angle_r, angle_speed_rs);/// 0.019608; // (0.20*255)
+    force = lqr_fullstate(0, 0, angle_r, 0);/// 0.019608; // (0.20*255)
+    //speed = -22 * inputToControlSystem(0, 1);
+    if(force<0){
       digitalWrite(MotorPinB, CW);
       digitalWrite(MotorPinA, CCW);
-      speed -= 30;
     }
-    else if(speed>0){
+    else if(force>0){
       digitalWrite(MotorPinB, CCW);
       digitalWrite(MotorPinA, CW);
-      speed += 30;
     }
     else {
-      speed = 0;
+      force = 0;
     }
-    speed = abs(speed);
-    speed = constrain(speed, 0, 250);
-    analogWrite(MotorSpeedB, speed); //Wheel close to connections
-    analogWrite(MotorSpeedA, speed); //First experiment wheel
-  }
-  else{
-    speed = 0;
-    analogWrite(MotorSpeedB, speed);
-    analogWrite(MotorSpeedA, speed);
+    if(force!=0){
+		Va = calc_speed(force, angle_speed_rs);
+	}
+	else {
+		Va = 0;
+	}
+    Va = abs(Va);
+    PWM = 255*Va/12;
+    PWM = constrain(PWM, 0, 255);
+    analogWrite(MotorSpeedB, PWM); //Wheel close to connections
+    analogWrite(MotorSpeedA, PWM); //First experiment wheel
   } 
+  Serial.print("PWM to motors: "); Serial.println(PWM);
 }
 int directionA(){
   if(digitalRead(encoderA2) ==  HIGH){                             

EENX15_LQR/LQR.ino

@@ -1,125 +1,84 @@
-//LQR-stuff
-#include "Arduino_skal.h"
+//LQR.ino
 
-// | ///////////////////////////////////
-// | //Row 24-52 in Arduino_skal_data.cpp
-// v ///////////////////////////////////
-const double Arduino_skalModelClass::ConstP rtConstP = {
-  // Expression: [100;200]
-  //  Referenced by: '<Root>/vartejag'
+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 };
 
-  { 100.0, 200.0 },
+const double matrix_C [8] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0 };
 
-  // Expression: A
-  //  Referenced by: '<Root>/Gain4'
-
-  { 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 },
-
-  // Expression: C
-  //  Referenced by: '<Root>/Gain6'
-
-  { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0 },
-
-  // Expression: L
-  //  Referenced by: '<Root>/Gain2'
-
-  { 116.63033952875418, 3387.8673967111704, -1.4473912197449676,
+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 },
+    117.16185100039935, 3490.0480780568214 };
 
-  // Expression: B
-  //  Referenced by: '<Root>/Gain3'
+const double matrix_B [4] = { 0.0, 2.078094708544223, 0.0, 5.2810302415000852 };
 
-  { 0.0, 2.078094708544223, 0.0, 5.2810302415000852 }
-};
 
-// | ///////////////////////////////////
-// | //Row 261-264 in Arduino_skal.cpp
-// v ///////////////////////////////////
+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};
 
-rtX.Integrator1_CSTATE[0] = 0.0;
-rtX.Integrator1_CSTATE[1] = 0.0;
-rtX.Integrator1_CSTATE[2] = 0.0;
-rtX.Integrator1_CSTATE[3] = 0.0;
+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 ///////////////////////////////////
-
-real_T tmp[2];
-int rtb_Saturation;
-
-// | ///////////////////////////////////
-// | //Row 140-143 in Arduino_skal.cpp
-// v ///////////////////////////////////
-
-// Denna funktion bör anropas när styrka + riktning till motorer ska bestämmas.
-int saturatedSignalToMotors(){
-  rtb_Saturation = ((-31.622776601683942 * rtX.Integrator1_CSTATE[0] +
-             -21.286439360075747 * rtX.Integrator1_CSTATE[1]) +
-             80.789376267003959 * rtX.Integrator1_CSTATE[2]) +
-             13.42463576551093 * rtX.Integrator1_CSTATE[3];
+double tmp[2];
+double rtb_Saturation = 0.0;
+double saturatedSignalToMotors(){
+  rtb_Saturation = ((matrix_K[0] * Integrator1_CSTATE[0] +
+             matrix_K[1] * Integrator1_CSTATE[1]) +
+             matrix_K[2] * Integrator1_CSTATE[2]) +
+             matrix_K[3] * Integrator1_CSTATE[3];
   
   if (0.0 - rtb_Saturation > 11.5) {
-    rtb_Saturation = 11.5;
+    rtb_Saturation = 3.0;
   } else if (0.0 - rtb_Saturation < -11.5) {
-    rtb_Saturation = -11.5;
+    rtb_Saturation = -3.0;
   } else {
     rtb_Saturation = 0.0 - rtb_Saturation;
   }
+  Serial.print("Saturation  = "); Serial.println(rtb_Saturation);
   return rtb_Saturation;
 }
-// | ///////////////////////////////////
-// | //Row 165-188 in Arduino_skal.cpp
-// v ///////////////////////////////////
-int inputToControlSystem(float position_m, float angle_r){
+double inputToControlSystem(float position_m, float angle_r){
   float posAndAng[] = {position_m, angle_r};
-  for (i = 0; i < 2; i++) {
-    tmp[i] = rtConstP.posAndAng[i] - (((rtConstP.Gain6_Gain[i + 2] *
-      rtX.Integrator1_CSTATE[1] + rtConstP.Gain6_Gain[i] *
-      rtX.Integrator1_CSTATE[0]) + rtConstP.Gain6_Gain[i + 4] *
-      rtX.Integrator1_CSTATE[2]) + rtConstP.Gain6_Gain[i + 6] *
-      rtX.Integrator1_CSTATE[3]);
+  for (int i = 0; i < 2; i++) {
+    tmp[i] = posAndAng[i] - (((matrix_C[i + 2] *
+      Integrator1_CSTATE[1] + matrix_C[i] *
+      Integrator1_CSTATE[0]) + matrix_C[i + 4] *
+      Integrator1_CSTATE[2]) + matrix_C[i + 6] *
+      Integrator1_CSTATE[3]);
   }
+  for (int i = 0; i < 4; i++) {
   
-  // End of Sum: '<Root>/Sum2'
-  for (i = 0; i < 4; i++) {
-    // Sum: '<Root>/Sum4' incorporates:
-    //   Gain: '<Root>/Gain2'
-    //   Gain: '<Root>/Gain3'
-    //   Gain: '<Root>/Gain4'
-    //   Integrator: '<Root>/Integrator1'
-    //   Sum: '<Root>/Sum3'
   
-    rtDW.Sum4[i] = ((rtConstP.Gain2_Gain[i + 4] * tmp[1] + rtConstP.Gain2_Gain[i]
-            * tmp[0]) + rtConstP.Gain3_Gain[i] * rtb_Saturation) +
-            (rtConstP.Gain4_Gain[i + 12] * rtX.Integrator1_CSTATE[3] +
-            (rtConstP.Gain4_Gain[i + 8] * rtX.Integrator1_CSTATE[2] +
-            (rtConstP.Gain4_Gain[i + 4] * rtX.Integrator1_CSTATE[1] +
-            rtConstP.Gain4_Gain[i] * rtX.Integrator1_CSTATE[0])));
+    Sum3[i] = ((matrix_L[i + 4] * tmp[1] + matrix_L[i]
+            * tmp[0]) + matrix_B[i] * rtb_Saturation);
+      
+    Sum4[i] = Sum3[i] +
+            (matrix_A[i + 12] * Integrator1_CSTATE[3] +
+            (matrix_A[i + 8] * Integrator1_CSTATE[2] +
+            (matrix_A[i + 4] * Integrator1_CSTATE[1] +
+            matrix_A[i] * Integrator1_CSTATE[0])));
   }
-  /*
-  Integrator1_CSTATE[0] = rtDW.Sum4[0];
-  Integrator1_CSTATE[1] = rtDW.Sum4[1];
-  Integrator1_CSTATE[2] = rtDW.Sum4[2];
-  Integrator1_CSTATE[3] = rtDW.Sum4[3];
-  */
+  Serial.print("Sum3 0 = "); Serial.println(Sum3[0]);
+  Serial.print("Sum3 1 = "); Serial.println(Sum3[1]);
+  Serial.print("Sum3 2 = "); Serial.println(Sum3[2]);
+  Serial.print("Sum3 3 = "); Serial.println(Sum3[3]);
+  
+  Serial.print("Sum4 0 = "); Serial.println(Sum4[0]);
+  Serial.print("Sum4 1 = "); Serial.println(Sum4[1]);
+  Serial.print("Sum4 2 = "); Serial.println(Sum4[2]);
+  Serial.print("Sum4 3 = "); Serial.println(Sum4[3]);
+  
+  Arduino_skal_derivatives();
   return saturatedSignalToMotors();
 }
-
-// | ///////////////////////////////////
-// | //Row 215-225 in Arduino_skal.cpp
-// v ///////////////////////////////////
-
-void Arduino_skalModelClass::Arduino_skal_derivatives()
+void Arduino_skal_derivatives()
 {
-  Arduino_skalModelClass::XDot *_rtXdot;
-  _rtXdot = ((XDot *) (&rtM)->derivs);
-
-  // Derivatives for Integrator: '<Root>/Integrator1'
-  _rtXdot->Integrator1_CSTATE[0] = rtDW.Sum4[0];
-  _rtXdot->Integrator1_CSTATE[1] = rtDW.Sum4[1];
-  _rtXdot->Integrator1_CSTATE[2] = rtDW.Sum4[2];
-  _rtXdot->Integrator1_CSTATE[3] = rtDW.Sum4[3];
+  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,7 @@ 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()) {
   case MPU6050_RANGE_2_G:
@@ -32,7 +32,7 @@ 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()) {
   case MPU6050_RANGE_250_DEG:

EENX15_LQR/lqr_fullstate.ino

@@ -0,0 +1,34 @@
+//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.7071,   -1.7751,   34.5368,    4.8793};
+  
+  float result;
+  result = matrix_K[0] * position_m +
+           matrix_K[1] * speed_ms +
+           matrix_K[2] * angle_r +
+           matrix_K[3] * angle_speed_rs;
+  Serial.print("K calculation (force): "); Serial.println(result); 
+  return result;
+}
+float calc_speed(float input, float angle_speed_rs) {
+  /*
+  float scale = 1.5;
+  input = abs(input)*0.30796; // scale down to rad/s (78,53/255)
+  Serial.print("input: "); Serial.println(input); 
+  float result = 3145.84/(pow((90.75 - input),1.00715)); // break out x from response graph
+  result *= scale;
+  Serial.print("calcspeed: "); Serial.println(result); 
+  return result;
+  */
+  float I = (1/3)*1.74;
+  float km = 0.91*0.01;
+  float ke = 8.68*0.001*2*PI/60;
+  float Ir = I;
+  float Omega= angle_speed_rs;
+  
+  float result = (km*ke/(Ir*5-km))*Omega;
+  Serial.print("RESULT");
+  Serial.print(result);
+  return result;
+  
+}

EENX15_LQR/rtwtypes.h

@@ -1,106 +0,0 @@
-//
-// Academic License - for use in teaching, academic research, and meeting
-// course requirements at degree granting institutions only.  Not for
-// government, commercial, or other organizational use.
-//
-// File: rtwtypes.h
-//
-// Code generated for Simulink model 'Arduino_skal'.
-//
-// Model version                  : 1.1
-// Simulink Coder version         : 9.5 (R2021a) 14-Nov-2020
-// C/C++ source code generated on : Thu Apr 15 22:06:00 2021
-//
-// Target selection: ert.tlc
-// Embedded hardware selection: AMD->x86-64 (Windows64)
-// Code generation objectives:
-//    1. Execution efficiency
-//    2. RAM efficiency
-// Validation result: Not run
-//
-
-#ifndef RTWTYPES_H
-#define RTWTYPES_H
-
-// Logical type definitions
-#if (!defined(__cplusplus))
-#ifndef false
-#define false                          (0U)
-#endif
-
-#ifndef true
-#define true                           (1U)
-#endif
-#endif
-
-//=======================================================================*
-//  Target hardware information
-//    Device type: AMD->x86-64 (Windows64)
-//    Number of bits:     char:   8    short:   16    int:  32
-//                        long:  32    long long:  64
-//                        native word size:  64
-//    Byte ordering: LittleEndian
-//    Signed integer division rounds to: Zero
-//    Shift right on a signed integer as arithmetic shift: on
-// =======================================================================
-
-//=======================================================================*
-//  Fixed width word size data types:                                     *
-//    int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     *
-//    uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   *
-//    real32_T, real64_T           - 32 and 64 bit floating point numbers *
-// =======================================================================
-typedef signed char int8_T;
-typedef unsigned char uint8_T;
-typedef short int16_T;
-typedef unsigned short uint16_T;
-typedef int int32_T;
-typedef unsigned int uint32_T;
-typedef long long int64_T;
-typedef unsigned long long uint64_T;
-typedef float real32_T;
-typedef double real64_T;
-
-//===========================================================================*
-//  Generic type definitions: boolean_T, char_T, byte_T, int_T, uint_T,       *
-//                            real_T, time_T, ulong_T, ulonglong_T.           *
-// ===========================================================================
-typedef double real_T;
-typedef double time_T;
-typedef unsigned char boolean_T;
-typedef int int_T;
-typedef unsigned int uint_T;
-typedef unsigned long ulong_T;
-typedef unsigned long long ulonglong_T;
-typedef char char_T;
-typedef unsigned char uchar_T;
-typedef char_T byte_T;
-
-//=======================================================================*
-//  Min and Max:                                                          *
-//    int8_T, int16_T, int32_T     - signed 8, 16, or 32 bit integers     *
-//    uint8_T, uint16_T, uint32_T  - unsigned 8, 16, or 32 bit integers   *
-// =======================================================================
-#define MAX_int8_T                     ((int8_T)(127))
-#define MIN_int8_T                     ((int8_T)(-128))
-#define MAX_uint8_T                    ((uint8_T)(255U))
-#define MAX_int16_T                    ((int16_T)(32767))
-#define MIN_int16_T                    ((int16_T)(-32768))
-#define MAX_uint16_T                   ((uint16_T)(65535U))
-#define MAX_int32_T                    ((int32_T)(2147483647))
-#define MIN_int32_T                    ((int32_T)(-2147483647-1))
-#define MAX_uint32_T                   ((uint32_T)(0xFFFFFFFFU))
-#define MAX_int64_T                    ((int64_T)(9223372036854775807LL))
-#define MIN_int64_T                    ((int64_T)(-9223372036854775807LL-1LL))
-#define MAX_uint64_T                   ((uint64_T)(0xFFFFFFFFFFFFFFFFULL))
-
-// Block D-Work pointer type
-typedef void * pointer_T;
-
-#endif                                 // RTWTYPES_H
-
-//
-// File trailer for generated code.
-//
-// [EOF]
-//