Merge branch 'lqr-testing' into nytt-test-bös
This commit is contained in:
Thefeli73
commit
3d92a9f01c
@ -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;
|
||||
|
||||
@ -1,40 +1,17 @@
|
||||
//LQR-stuff
|
||||
//#include "Arduino_skal.h"
|
||||
|
||||
// | ///////////////////////////////////
|
||||
// | //Row 24-52 in Arduino_skal_data.cpp
|
||||
// v ///////////////////////////////////
|
||||
|
||||
// Expression: A
|
||||
// Referenced by: '<Root>/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: '<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]);
|
||||
|
||||
@ -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()) {
|
||||
|
||||
@ -1,39 +1,16 @@
|
||||
//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};
|
||||
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;
|
||||
/*
|
||||
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)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user