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>
int lastCorrectionTime = 0;
@ -26,7 +27,7 @@ int32_t speed;
int safe_angle;
//gyro stuff
//gyro
float AccX, AccY, AccZ;
float GyroX, GyroY, GyroZ;
float accAngleX, accAngleY, gyroAngleX, gyroAngleY, gyroAngleZ;
@ -47,7 +48,7 @@ float angle_pitch_output, angle_roll_output;
long loop_timer;
int temp;
//motor stuff
//motor
#define encoderA1 2
#define encoderB1 3
@ -81,7 +82,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 +92,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);
@ -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("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("A: "); Serial.println(countA);
//Serial.print("B: "); Serial.println(countB);
}
void setSpeed(){
@ -166,8 +163,8 @@ void setSpeed(){
}
speed = abs(speed);
speed = constrain(speed, 0, 249);
analogWrite(MotorSpeedB, speed); //Wheel close to connections
analogWrite(MotorSpeedA, speed); //First experiment wheel
analogWrite(MotorSpeedB, speed);
analogWrite(MotorSpeedA, speed);
}
else{
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;
}