schulze/EENX15
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

clear unused garbage

Browse Source
This commit is contained in:
Thefeli73 2021-05-13 11:27:39 +02:00
parent 423af101e2
commit c369a2912b
2 changed files with 6 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
EENX15_LQR/EENX15_LQR.ino
View File

@ -1,3 +1,4 @@
//EENX15_LQR.ino
#include <Wire.h> #include <Wire.h>
int lastCorrectionTime = 0; int lastCorrectionTime = 0;
@ -26,7 +27,7 @@ int32_t 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);
@ -143,9 +143,6 @@ void printInfo(){
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
Serial.print("Rads per second: "); Serial.println(rps); //ca. 56 tick per rotation, 6.26 rads per rotation Serial.print("Rads per second: "); Serial.println(rps); //ca. 56 tick per rotation, 6.26 rads per rotation
//Serial.print("A: "); Serial.println(countA);
//Serial.print("B: "); Serial.println(countB);
} }
void setSpeed(){ void setSpeed(){
@ -166,8 +163,8 @@ void setSpeed(){
} }
speed = abs(speed); speed = abs(speed);
speed = constrain(speed, 0, 249); speed = constrain(speed, 0, 249);
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;

23
EENX15_LQR/LQR.ino
View File

@ -1,23 +0,0 @@
/** Control Law*/
const float K[statesNumber] = {0.55192, 8.9867, 0.194, 0.39237};
//K from our calculation
//const float K[statesNumber] = {-31.6228, -21.2864, 80.7894, 13.4246};
/** Correction factor */
const float scaleConst = 2.5;
/**
* LQR control law
*/
void lqr() {
speed = 0;
speed -= motorAngularPosition * K[0];
speed -= correctedAngularPosition * K[1];
speed -= motorAngularSpeed * K[2];
speed -= angularSpeed * K[3];
speed = speed*scaleConst;
}