clean more

EENX15_LQR/EENX15_LQR.ino

@@ -1,3 +1,4 @@
+//EENX15_LQR.ino
 #include <Wire.h>
 
 int lastCorrectionTime = 0;
@@ -6,7 +7,7 @@ int lastPrintTime = 0;
 static int fastTimer = 80; //ms
 static int slowTimer = 800; //ms
 
-//lqr stuff
+//lqr
 const uint8_t statesNumber = 4;
 /** Low pass filter angular Position*/
 float angularPositionLP = 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(MotorSpeedB, speed);
-    analogWrite(MotorSpeedA, speed); //First experiment wheel
+    analogWrite(MotorSpeedA, speed);
   }
   else{
     speed = 0;

EENX15_LQR/LQR.ino

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

EENX15_LQR/gyro.ino

@@ -1,4 +1,4 @@
-//gyroscope stuff
+//gyro.ino
 #include <Adafruit_MPU6050.h>
 #include <Adafruit_Sensor.h>