Compare commits

..

19 Commits

Author SHA1 Message Date
Thefeli73
3d92a9f01c Merge branch 'lqr-testing' into nytt-test-bös 2021-05-13 11:54:57 +02:00
Thefeli73
baa64b6240 clean garbage 2021-05-13 11:38:40 +02:00
Thefeli73
b4cc590cc0 småfixes, kanske inte längre blir 0? 2021-05-12 19:18:04 +02:00
Thefeli73
487837cffa kodning som gjordes hos bragde 2021-05-12 19:01:39 +02:00
Thefeli73
b57bd6ca27 fixes, idk 2021-04-24 11:15:41 +02:00
Thefeli73
5dad9a6f6d fixa kommentar, ändra i funktion som inte används för att testa 2021-04-21 15:28:44 +02:00
Thefeli73
a8d4620cac IMPORTANT fixa linjariserings funktion 2021-04-21 15:27:25 +02:00
Thefeli73
1ab859ec5a testa ny k matrix 2021-04-21 15:26:24 +02:00
Thefeli73
9463979545 few changes, call fullstate lqr 2021-04-20 13:30:48 +02:00
Thefeli73
a118b606dd add k matrix as matrix, try some stuff, add time var 2021-04-20 13:28:01 +02:00
Thefeli73
c2061717e6 try new K matrix, also scale output 2021-04-20 13:27:26 +02:00
Thefeli73
841050f558 take in fullstate x K matrix and compensate for logarithmic response 2021-04-20 13:27:05 +02:00
Thefeli73
a6fb0d5e77 refactor gain to matrix names, break out sum3 out of sum4 2021-04-19 17:21:58 +02:00
Thefeli73
e45732e5d1 testa bös 2021-04-19 17:14:25 +02:00
Thefeli73
1bf3f51783 gyroscope känslighet 2021-04-19 17:14:06 +02:00
Thefeli73
bc7a9a6ec4 small fix 2021-04-17 15:23:11 +02:00
Thefeli73
5f559bb895 try to fix final function 2021-04-17 14:05:54 +02:00
Thefeli73
664b8b0109 no more need for types and .h files 2021-04-17 13:23:26 +02:00
Thefeli73
cf17affee4 able to compile. change variable names, stop using some constructs, change types etc 2021-04-17 13:22:12 +02:00
6 changed files with 133 additions and 238 deletions

View File

@ -1,12 +1,13 @@
//EENX15_LQR.ino
#include <Wire.h> #include <Wire.h>
int lastCorrectionTime = 0; int lastCorrectionTime = 0;
int lastPrintTime = 0; int lastPrintTime = 0;
static int fastTimer = 80; //ms static int fastTimer = 10; //ms
static int slowTimer = 800; //ms static int slowTimer = 1000; //ms
//lqr stuff //lqr
const uint8_t statesNumber = 4; const uint8_t statesNumber = 4;
/** Low pass filter angular Position*/ /** Low pass filter angular Position*/
float angularPositionLP = 0; float angularPositionLP = 0;
@ -22,11 +23,14 @@ float motorAngularPosition = 0;
float motorAngularSpeed = 0; float motorAngularSpeed = 0;
/** PWM signal applied to the motor's driver 255 is 100% */ /** PWM signal applied to the motor's driver 255 is 100% */
int32_t speed; int speed;
float Va;
int safe_angle; int safe_angle;
float force;
int PWM;
//gyro stuff //gyro
float AccX, AccY, AccZ; float AccX, AccY, AccZ;
float GyroX, GyroY, GyroZ; float GyroX, GyroY, GyroZ;
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ; float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
@ -47,7 +51,7 @@ float angle_pitch_output, angle_roll_output;
long loop_timer; long loop_timer;
int temp; int temp;
//motor stuff //motor
#define encoderA1 2 #define encoderA1 2
#define encoderB1 3 #define encoderB1 3
@ -81,7 +85,7 @@ void setup() {
Wire.begin(); Wire.begin();
Serial.begin(115200); Serial.begin(115200);
while (!Serial) while (!Serial)
delay(10); // will pause Zero, Leonardo, etc until serial console opens delay(10); // will pause until serial console opens
gyro_setup(); gyro_setup();
@ -91,7 +95,6 @@ void setup() {
pinMode(encoderA2, INPUT_PULLUP); pinMode(encoderA2, INPUT_PULLUP);
pinMode(encoderB2, INPUT_PULLUP); pinMode(encoderB2, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(encoderA1), pulseA, RISING); attachInterrupt(digitalPinToInterrupt(encoderA1), pulseA, RISING);
//attachInterrupt(digitalPinToInterrupt(encoderB1), pulseB, RISING);
pinMode(MotorPinA, OUTPUT); pinMode(MotorPinA, OUTPUT);
pinMode(MotorSpeedA, OUTPUT); pinMode(MotorSpeedA, OUTPUT);
@ -100,6 +103,7 @@ void setup() {
pinMode(MotorPinB, OUTPUT); pinMode(MotorPinB, OUTPUT);
pinMode(MotorSpeedB, OUTPUT); pinMode(MotorSpeedB, OUTPUT);
pinMode(MotorBrakeB, OUTPUT); pinMode(MotorBrakeB, OUTPUT);
} }
void loop() { void loop() {
@ -108,12 +112,12 @@ void loop() {
int m = millis(); 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; lastCorrectionTime = m;
getSpeed(); getSpeed();
setSpeed(); setSpeed();
} }
if (m - lastPrintTime >= slowTimer) { //run this code ever 800ms (1.25hz) if (m - lastPrintTime >= slowTimer) { //run this code every [slowTimer]ms
lastPrintTime = m; lastPrintTime = m;
printInfo(); printInfo();
} }
@ -139,6 +143,7 @@ void printInfo(){
Serial.print("pitch Angle measured = "); Serial.println(angle_pitch); Serial.print("pitch Angle measured = "); Serial.println(angle_pitch);
Serial.print("Position: "); Serial.println(countA); Serial.print("Position: "); Serial.println(countA);
Serial.print("Position (m): "); Serial.println(countA/174.76); //r*2pi 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("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("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 Serial.print("RPM: "); Serial.println(rpm); //ca. 56 tick per rotation
@ -147,34 +152,37 @@ void printInfo(){
void setSpeed(){ void setSpeed(){
if(abs(safe_angle)<50 ){ if(abs(safe_angle)<50 ){
//speed = 8*safe_angle;
float position_m = countA/174.76; float position_m = countA/174.76;
float speed_ms = rps * 0.05;
float angle_r = angle_pitch_output * 0.318; float angle_r = angle_pitch_output * 0.318;
speed = inputToControlSystem(position_m, angle_r); float angle_speed_rs = rps;
speed *= 22; //speed = lqr_fullstate(position_m, speed_ms, angle_r, angle_speed_rs);/// 0.019608; // (0.20*255)
if(speed<0){ 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(MotorPinB, CW);
digitalWrite(MotorPinA, CCW); digitalWrite(MotorPinA, CCW);
speed -= 30;
} }
else if(speed>0){ else if(force>0){
digitalWrite(MotorPinB, CCW); digitalWrite(MotorPinB, CCW);
digitalWrite(MotorPinA, CW); digitalWrite(MotorPinA, CW);
speed += 30;
} }
else { else {
speed = 0; force = 0;
} }
speed = abs(speed); if(force!=0){
speed = constrain(speed, 0, 250); Va = calc_speed(force, angle_speed_rs);
analogWrite(MotorSpeedB, speed); //Wheel close to connections
analogWrite(MotorSpeedA, speed); //First experiment wheel
} }
else { else {
speed = 0; Va = 0;
analogWrite(MotorSpeedB, speed);
analogWrite(MotorSpeedA, speed);
} }
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(){ int directionA(){
if(digitalRead(encoderA2) == HIGH){ if(digitalRead(encoderA2) == HIGH){

View File

@ -1,125 +1,84 @@
//LQR-stuff //LQR.ino
#include "Arduino_skal.h"
// | /////////////////////////////////// const double matrix_A [16] = { 0.0, 0.0, 0.0, 0.0,
// | //Row 24-52 in Arduino_skal_data.cpp 1.0, -0.20780947085442231, 0.0, -0.52810302415000854,
// v /////////////////////////////////// 0.0, 13.239785742831822, 0.0, 58.601480177829842,
const double Arduino_skalModelClass::ConstP rtConstP = { 0.0, 0.0, 1.0, 0.0 };
// Expression: [100;200]
// Referenced by: '<Root>/vartejag'
{ 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 const double matrix_L [8] = { 56.7847, 799.5294, -1.4914, -57.4160,
// Referenced by: '<Root>/Gain4' -1.0363, -16.1071, 57.0075, 870.8172 };
const double matrix_L_old [8] = { 116.63033952875418, 3387.8673967111704, -1.4473912197449676,
{ 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,
-115.34372132703447, -1.0534041975488044, -48.223441605702455, -115.34372132703447, -1.0534041975488044, -48.223441605702455,
117.16185100039935, 3490.0480780568214 }, 117.16185100039935, 3490.0480780568214 };
// Expression: B const double matrix_B [4] = { 0.0, 2.078094708544223, 0.0, 5.2810302415000852 };
// Referenced by: '<Root>/Gain3'
{ 0.0, 2.078094708544223, 0.0, 5.2810302415000852 }
};
// | /////////////////////////////////// const double matrix_K_old [4] = {-31.622776601683942, -21.286439360075747, 80.789376267003959, 13.42463576551093};
// | //Row 261-264 in Arduino_skal.cpp const double matrix_K [4] = {-0.0316, -0.3938, 22.9455, 3.0629};
// v ///////////////////////////////////
rtX.Integrator1_CSTATE[0] = 0.0; double Integrator1_CSTATE [4] = {0.0, 0.0, 0.0, 0.0};
rtX.Integrator1_CSTATE[1] = 0.0; double Sum3[4];
rtX.Integrator1_CSTATE[2] = 0.0; double Sum4[4];
rtX.Integrator1_CSTATE[3] = 0.0;
// | /////////////////////////////////// double tmp[2];
// | //Row 123-124 in Arduino_skal.cpp double rtb_Saturation = 0.0;
// v /////////////////////////////////// double saturatedSignalToMotors(){
rtb_Saturation = ((matrix_K[0] * Integrator1_CSTATE[0] +
real_T tmp[2]; matrix_K[1] * Integrator1_CSTATE[1]) +
int rtb_Saturation; matrix_K[2] * Integrator1_CSTATE[2]) +
matrix_K[3] * Integrator1_CSTATE[3];
// | ///////////////////////////////////
// | //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];
if (0.0 - rtb_Saturation > 11.5) { if (0.0 - rtb_Saturation > 11.5) {
rtb_Saturation = 11.5; rtb_Saturation = 3.0;
} else if (0.0 - rtb_Saturation < -11.5) { } else if (0.0 - rtb_Saturation < -11.5) {
rtb_Saturation = -11.5; rtb_Saturation = -3.0;
} else { } else {
rtb_Saturation = 0.0 - rtb_Saturation; rtb_Saturation = 0.0 - rtb_Saturation;
} }
Serial.print("Saturation = "); Serial.println(rtb_Saturation);
return rtb_Saturation; return rtb_Saturation;
} }
// | /////////////////////////////////// double inputToControlSystem(float position_m, float angle_r){
// | //Row 165-188 in Arduino_skal.cpp
// v ///////////////////////////////////
int inputToControlSystem(float position_m, float angle_r){
float posAndAng[] = {position_m, angle_r}; float posAndAng[] = {position_m, angle_r};
for (i = 0; i < 2; i++) { for (int i = 0; i < 2; i++) {
tmp[i] = rtConstP.posAndAng[i] - (((rtConstP.Gain6_Gain[i + 2] * tmp[i] = posAndAng[i] - (((matrix_C[i + 2] *
rtX.Integrator1_CSTATE[1] + rtConstP.Gain6_Gain[i] * Integrator1_CSTATE[1] + matrix_C[i] *
rtX.Integrator1_CSTATE[0]) + rtConstP.Gain6_Gain[i + 4] * Integrator1_CSTATE[0]) + matrix_C[i + 4] *
rtX.Integrator1_CSTATE[2]) + rtConstP.Gain6_Gain[i + 6] * Integrator1_CSTATE[2]) + matrix_C[i + 6] *
rtX.Integrator1_CSTATE[3]); 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] Sum3[i] = ((matrix_L[i + 4] * tmp[1] + matrix_L[i]
* tmp[0]) + rtConstP.Gain3_Gain[i] * rtb_Saturation) + * tmp[0]) + matrix_B[i] * rtb_Saturation);
(rtConstP.Gain4_Gain[i + 12] * rtX.Integrator1_CSTATE[3] +
(rtConstP.Gain4_Gain[i + 8] * rtX.Integrator1_CSTATE[2] + Sum4[i] = Sum3[i] +
(rtConstP.Gain4_Gain[i + 4] * rtX.Integrator1_CSTATE[1] + (matrix_A[i + 12] * Integrator1_CSTATE[3] +
rtConstP.Gain4_Gain[i] * rtX.Integrator1_CSTATE[0]))); (matrix_A[i + 8] * Integrator1_CSTATE[2] +
(matrix_A[i + 4] * Integrator1_CSTATE[1] +
matrix_A[i] * Integrator1_CSTATE[0])));
} }
/* Serial.print("Sum3 0 = "); Serial.println(Sum3[0]);
Integrator1_CSTATE[0] = rtDW.Sum4[0]; Serial.print("Sum3 1 = "); Serial.println(Sum3[1]);
Integrator1_CSTATE[1] = rtDW.Sum4[1]; Serial.print("Sum3 2 = "); Serial.println(Sum3[2]);
Integrator1_CSTATE[2] = rtDW.Sum4[2]; Serial.print("Sum3 3 = "); Serial.println(Sum3[3]);
Integrator1_CSTATE[3] = rtDW.Sum4[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(); return saturatedSignalToMotors();
} }
void Arduino_skal_derivatives()
// | ///////////////////////////////////
// | //Row 215-225 in Arduino_skal.cpp
// v ///////////////////////////////////
void Arduino_skalModelClass::Arduino_skal_derivatives()
{ {
Arduino_skalModelClass::XDot *_rtXdot; for (int i = 0; i < 4; i++) {
_rtXdot = ((XDot *) (&rtM)->derivs); Integrator1_CSTATE[i] = Sum4[i] * fastTimer/1000.0;
Serial.print("Integrator: "); Serial.println(Integrator1_CSTATE[i]);
// 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];
} }

View File

@ -1,4 +1,4 @@
//gyroscope stuff //gyro.ino
#include <Adafruit_MPU6050.h> #include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h> #include <Adafruit_Sensor.h>
@ -16,7 +16,7 @@ void gyro_setup(){
} }
Serial.println("MPU6050 Found!"); Serial.println("MPU6050 Found!");
mpu.setAccelerometerRange(MPU6050_RANGE_4_G); mpu.setAccelerometerRange(MPU6050_RANGE_16_G);
Serial.print("Accelerometer range set to: "); Serial.print("Accelerometer range set to: ");
switch (mpu.getAccelerometerRange()) { switch (mpu.getAccelerometerRange()) {
case MPU6050_RANGE_2_G: case MPU6050_RANGE_2_G:
@ -32,7 +32,7 @@ void gyro_setup(){
Serial.println("+-16G"); Serial.println("+-16G");
break; break;
} }
mpu.setGyroRange(MPU6050_RANGE_500_DEG); mpu.setGyroRange(MPU6050_RANGE_2000_DEG);
Serial.print("Gyro range set to: "); Serial.print("Gyro range set to: ");
switch (mpu.getGyroRange()) { switch (mpu.getGyroRange()) {
case MPU6050_RANGE_250_DEG: case MPU6050_RANGE_250_DEG:

View File

@ -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;
}

View File

@ -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]
//