Compare commits
16 Commits
main
...
lqr-testin
Author | SHA1 | Date | |
---|---|---|---|
|
baa64b6240 | ||
|
b57bd6ca27 | ||
|
5dad9a6f6d | ||
|
a8d4620cac | ||
|
1ab859ec5a | ||
|
9463979545 | ||
|
a118b606dd | ||
|
c2061717e6 | ||
|
841050f558 | ||
|
a6fb0d5e77 | ||
|
e45732e5d1 | ||
|
1bf3f51783 | ||
|
bc7a9a6ec4 | ||
|
5f559bb895 | ||
|
664b8b0109 | ||
|
cf17affee4 |
@ -1,228 +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: Arduino_skal.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 RTW_HEADER_Arduino_skal_h_
|
|
||||||
#define RTW_HEADER_Arduino_skal_h_
|
|
||||||
//#include <cstring>
|
|
||||||
#include "rtwtypes.h"
|
|
||||||
//#include "rtw_continuous.h"
|
|
||||||
//#include "rtw_solver.h"
|
|
||||||
|
|
||||||
// Model Code Variants
|
|
||||||
|
|
||||||
// Macros for accessing real-time model data structure
|
|
||||||
#ifndef rtmGetErrorStatus
|
|
||||||
#define rtmGetErrorStatus(rtm) ((rtm)->errorStatus)
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifndef rtmSetErrorStatus
|
|
||||||
#define rtmSetErrorStatus(rtm, val) ((rtm)->errorStatus = (val))
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifndef rtmGetStopRequested
|
|
||||||
#define rtmGetStopRequested(rtm) ((rtm)->Timing.stopRequestedFlag)
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifndef rtmSetStopRequested
|
|
||||||
#define rtmSetStopRequested(rtm, val) ((rtm)->Timing.stopRequestedFlag = (val))
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifndef rtmGetStopRequestedPtr
|
|
||||||
#define rtmGetStopRequestedPtr(rtm) (&((rtm)->Timing.stopRequestedFlag))
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifndef rtmGetT
|
|
||||||
#define rtmGetT(rtm) (rtmGetTPtr((rtm))[0])
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifndef rtmGetTPtr
|
|
||||||
#define rtmGetTPtr(rtm) ((rtm)->Timing.t)
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#ifndef ODE3_INTG
|
|
||||||
#define ODE3_INTG
|
|
||||||
|
|
||||||
// ODE3 Integration Data
|
|
||||||
struct ODE3_IntgData {
|
|
||||||
real_T *y; // output
|
|
||||||
real_T *f[3]; // derivatives
|
|
||||||
};
|
|
||||||
|
|
||||||
#endif
|
|
||||||
|
|
||||||
// Class declaration for model Arduino_skal
|
|
||||||
class Arduino_skalModelClass {
|
|
||||||
// public data and function members
|
|
||||||
public:
|
|
||||||
// Block signals and states (default storage) for system '<Root>'
|
|
||||||
struct DW {
|
|
||||||
real_T Sum4[4]; // '<Root>/Sum4'
|
|
||||||
};
|
|
||||||
|
|
||||||
// Continuous states (default storage)
|
|
||||||
struct X {
|
|
||||||
real_T Integrator1_CSTATE[4]; // '<Root>/Integrator1'
|
|
||||||
};
|
|
||||||
|
|
||||||
// State derivatives (default storage)
|
|
||||||
struct XDot {
|
|
||||||
real_T Integrator1_CSTATE[4]; // '<Root>/Integrator1'
|
|
||||||
};
|
|
||||||
|
|
||||||
// State disabled
|
|
||||||
struct XDis {
|
|
||||||
boolean_T Integrator1_CSTATE[4]; // '<Root>/Integrator1'
|
|
||||||
};
|
|
||||||
|
|
||||||
// Constant parameters (default storage)
|
|
||||||
struct ConstP {
|
|
||||||
// Expression: [100;200]
|
|
||||||
// Referenced by: '<Root>/vartejag'
|
|
||||||
|
|
||||||
real_T vartejag_Value[2];
|
|
||||||
|
|
||||||
// Expression: A
|
|
||||||
// Referenced by: '<Root>/Gain4'
|
|
||||||
|
|
||||||
real_T Gain4_Gain[16];
|
|
||||||
|
|
||||||
// Expression: C
|
|
||||||
// Referenced by: '<Root>/Gain6'
|
|
||||||
|
|
||||||
real_T Gain6_Gain[8];
|
|
||||||
|
|
||||||
// Expression: L
|
|
||||||
// Referenced by: '<Root>/Gain2'
|
|
||||||
|
|
||||||
real_T Gain2_Gain[8];
|
|
||||||
|
|
||||||
// Expression: B
|
|
||||||
// Referenced by: '<Root>/Gain3'
|
|
||||||
|
|
||||||
real_T Gain3_Gain[4];
|
|
||||||
};
|
|
||||||
|
|
||||||
// Real-time Model Data Structure
|
|
||||||
struct RT_MODEL {
|
|
||||||
const char_T *errorStatus;
|
|
||||||
//RTWSolverInfo solverInfo;
|
|
||||||
X *contStates;
|
|
||||||
int_T *periodicContStateIndices;
|
|
||||||
real_T *periodicContStateRanges;
|
|
||||||
real_T *derivs;
|
|
||||||
boolean_T *contStateDisabled;
|
|
||||||
boolean_T zCCacheNeedsReset;
|
|
||||||
boolean_T derivCacheNeedsReset;
|
|
||||||
boolean_T CTOutputIncnstWithState;
|
|
||||||
real_T odeY[4];
|
|
||||||
real_T odeF[3][4];
|
|
||||||
ODE3_IntgData intgData;
|
|
||||||
|
|
||||||
//
|
|
||||||
// Sizes:
|
|
||||||
// The following substructure contains sizes information
|
|
||||||
// for many of the model attributes such as inputs, outputs,
|
|
||||||
// dwork, sample times, etc.
|
|
||||||
|
|
||||||
struct {
|
|
||||||
int_T numContStates;
|
|
||||||
int_T numPeriodicContStates;
|
|
||||||
int_T numSampTimes;
|
|
||||||
} Sizes;
|
|
||||||
|
|
||||||
//
|
|
||||||
// Timing:
|
|
||||||
// The following substructure contains information regarding
|
|
||||||
// the timing information for the model.
|
|
||||||
|
|
||||||
struct {
|
|
||||||
uint32_T clockTick0;
|
|
||||||
time_T stepSize0;
|
|
||||||
uint32_T clockTick1;
|
|
||||||
SimTimeStep simTimeStep;
|
|
||||||
boolean_T stopRequestedFlag;
|
|
||||||
time_T *t;
|
|
||||||
time_T tArray[2];
|
|
||||||
} Timing;
|
|
||||||
};
|
|
||||||
|
|
||||||
// model initialize function
|
|
||||||
void initialize();
|
|
||||||
|
|
||||||
// model step function
|
|
||||||
void step();
|
|
||||||
|
|
||||||
// Constructor
|
|
||||||
Arduino_skalModelClass();
|
|
||||||
|
|
||||||
// Destructor
|
|
||||||
~Arduino_skalModelClass();
|
|
||||||
|
|
||||||
// Real-Time Model get method
|
|
||||||
Arduino_skalModelClass::RT_MODEL * getRTM();
|
|
||||||
|
|
||||||
// private data and function members
|
|
||||||
private:
|
|
||||||
// Block signals and states
|
|
||||||
DW rtDW;
|
|
||||||
X rtX; // Block continuous states
|
|
||||||
|
|
||||||
// Real-Time Model
|
|
||||||
RT_MODEL rtM;
|
|
||||||
|
|
||||||
// Continuous states update member function
|
|
||||||
//void rt_ertODEUpdateContinuousStates(RTWSolverInfo *si );
|
|
||||||
|
|
||||||
// Derivatives member function
|
|
||||||
void Arduino_skal_derivatives();
|
|
||||||
};
|
|
||||||
|
|
||||||
// Constant parameters (default storage)
|
|
||||||
extern const Arduino_skalModelClass::ConstP rtConstP;
|
|
||||||
|
|
||||||
//-
|
|
||||||
// These blocks were eliminated from the model due to optimizations:
|
|
||||||
//
|
|
||||||
// Block '<Root>/Kr' : Eliminated nontunable gain of 1
|
|
||||||
|
|
||||||
|
|
||||||
//-
|
|
||||||
// The generated code includes comments that allow you to trace directly
|
|
||||||
// back to the appropriate location in the model. The basic format
|
|
||||||
// is <system>/block_name, where system is the system number (uniquely
|
|
||||||
// assigned by Simulink) and block_name is the name of the block.
|
|
||||||
//
|
|
||||||
// Use the MATLAB hilite_system command to trace the generated code back
|
|
||||||
// to the model. For example,
|
|
||||||
//
|
|
||||||
// hilite_system('<S3>') - opens system 3
|
|
||||||
// hilite_system('<S3>/Kp') - opens and selects block Kp which resides in S3
|
|
||||||
//
|
|
||||||
// Here is the system hierarchy for this model
|
|
||||||
//
|
|
||||||
// '<Root>' : 'Arduino_skal'
|
|
||||||
|
|
||||||
#endif // RTW_HEADER_Arduino_skal_h_
|
|
||||||
|
|
||||||
//
|
|
||||||
// File trailer for generated code.
|
|
||||||
//
|
|
||||||
// [EOF]
|
|
||||||
//
|
|
@ -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,11 @@ 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;
|
||||||
int safe_angle;
|
int safe_angle;
|
||||||
|
|
||||||
|
|
||||||
//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 +48,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 +82,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 +92,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 +100,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 +109,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 +140,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 +149,34 @@ 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(0, 0, angle_r, 0);/// 0.019608; // (0.20*255)
|
||||||
if(speed<0){
|
if(speed<0){
|
||||||
digitalWrite(MotorPinB, CW);
|
digitalWrite(MotorPinB, CW);
|
||||||
digitalWrite(MotorPinA, CCW);
|
digitalWrite(MotorPinA, CCW);
|
||||||
speed -= 30;
|
|
||||||
}
|
}
|
||||||
else if(speed>0){
|
else if(speed>0){
|
||||||
digitalWrite(MotorPinB, CCW);
|
digitalWrite(MotorPinB, CCW);
|
||||||
digitalWrite(MotorPinA, CW);
|
digitalWrite(MotorPinA, CW);
|
||||||
speed += 30;
|
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
speed = 0;
|
speed = 0;
|
||||||
}
|
}
|
||||||
|
if(speed!=0){ speed = constrain(speed, -255, 255);speed = calc_speed(speed); }
|
||||||
speed = abs(speed);
|
speed = abs(speed);
|
||||||
speed = constrain(speed, 0, 250);
|
speed = constrain(speed, 0, 255);
|
||||||
analogWrite(MotorSpeedB, speed); //Wheel close to connections
|
analogWrite(MotorSpeedB, speed);
|
||||||
analogWrite(MotorSpeedA, speed); //First experiment wheel
|
analogWrite(MotorSpeedA, speed);
|
||||||
}
|
}
|
||||||
else{
|
else{
|
||||||
speed = 0;
|
speed = 0;
|
||||||
analogWrite(MotorSpeedB, speed);
|
analogWrite(MotorSpeedB, speed);
|
||||||
analogWrite(MotorSpeedA, speed);
|
analogWrite(MotorSpeedA, speed);
|
||||||
}
|
}
|
||||||
|
Serial.print("Speed to motors: "); Serial.println(speed);
|
||||||
}
|
}
|
||||||
int directionA(){
|
int directionA(){
|
||||||
if(digitalRead(encoderA2) == HIGH){
|
if(digitalRead(encoderA2) == HIGH){
|
||||||
|
@ -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:
|
Sum3[i] = ((matrix_L[i + 4] * tmp[1] + matrix_L[i]
|
||||||
// Gain: '<Root>/Gain2'
|
* tmp[0]) + matrix_B[i] * rtb_Saturation);
|
||||||
// Gain: '<Root>/Gain3'
|
|
||||||
// Gain: '<Root>/Gain4'
|
Sum4[i] = Sum3[i] +
|
||||||
// Integrator: '<Root>/Integrator1'
|
(matrix_A[i + 12] * Integrator1_CSTATE[3] +
|
||||||
// Sum: '<Root>/Sum3'
|
(matrix_A[i + 8] * Integrator1_CSTATE[2] +
|
||||||
|
(matrix_A[i + 4] * Integrator1_CSTATE[1] +
|
||||||
rtDW.Sum4[i] = ((rtConstP.Gain2_Gain[i + 4] * tmp[1] + rtConstP.Gain2_Gain[i]
|
matrix_A[i] * Integrator1_CSTATE[0])));
|
||||||
* 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])));
|
|
||||||
}
|
}
|
||||||
/*
|
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];
|
|
||||||
}
|
}
|
||||||
|
@ -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:
|
||||||
|
22
EENX15_LQR/lqr_fullstate.ino
Normal file
22
EENX15_LQR/lqr_fullstate.ino
Normal file
@ -0,0 +1,22 @@
|
|||||||
|
//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 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;
|
||||||
|
|
||||||
|
}
|
@ -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]
|
|
||||||
//
|
|
Loading…
Reference in New Issue
Block a user