Diff: main.cpp

Revision:

1:4afc0dd95be9

Parent:

0:3ee52ce4df19

Child:

2:098bb1322f0e

--- a/main.cpp	Thu Jun 30 20:41:12 2016 +0000
+++ b/main.cpp	Thu Jun 30 22:33:59 2016 +0000
@@ -44,6 +44,10 @@
 /// Timer to transmit timing information.
 Timer timer;
 
+/* The acceleration commanded by control_thread and used as input for motorControl_thread() */
+float setAccel;
+Mutex setAccelMutex;
+
 /* Threads */
 /** @brief Bluetooth Transmission:
  * This thread reads the quadrature encoder and
@@ -51,7 +55,7 @@
  */
 void bluetooth_thread(void const *argument) {
     // Timer to control how long to wait.
-    Timer waitTimer;
+    FixedRefresh refresh;
     // 64-byte buffer is used to store the data. May be expanded as needed.
     char buffer[128];
     // The accelerometer readings.
@@ -60,19 +64,14 @@
     float zAccel;
     
 
-    // Start the timer:
-    waitTimer.start();
     while(true) {
-        waitTimer.reset();
         accelerometer.readAccelerometer(&xAccel, &yAccel, &zAccel);
         // Place the CSV information in the buffer:
-        sprintf(buffer, "%f,%f,%f,%f,%f,%f\n", timer.read(), xAccel, yAccel, zAccel, leftMotor.read(), rightMotor.read());
+        sprintf(buffer, "%f,%f,%f,%f,%f,%f\n\r", timer.read(), xAccel, yAccel, zAccel, leftMotor.read(), rightMotor.read());
         // Transmit the information.
         bluetooth.print(buffer);
-        // Wait the thread for BTTransmitPeriodMS - thread continues after this time.
-        int32_t waitTime = BTTransmitPeriodMS - waitTimer.read_ms();
-        if (waitTime < 0.) waitTime = 0;
-        Thread::wait(waitTime);
+        // Refresh at a specified interval.
+        refresh.refresh_us(BTTransmitPeriodMS * 1000);
     }
 
 }
@@ -83,15 +82,12 @@
  */
 
 void control_thread(void const *argument) {
-        // FixedRefresh object to wait at fixed time
+    // FixedRefresh object to wait at fixed time
     FixedRefresh fixedRefresh;
     // The accelerometer readings.
     float xAccel;
     float yAccel;
     float zAccel;
-    // The quadrature encoder readings.
-    float leftWheel;
-    float rightWheel;
 
     /* Feedback constants: */
     // Accelerometer feedback at n = 0;
@@ -100,9 +96,15 @@
     const float a1 = 0;
     // Accelerometer feedback at n = -2;
     const float a2 = 0;
-    // Output feedback
-    const float o1 = 0;
     
+    /* State Variables: */
+    // System's angle at n = 0
+    float angle = 0;
+    // System's angle at n = -1
+    float angle_n1 = 0;
+    // System's angle at n = -2
+    float angle_n2 = 0;
+
     /* Filtering Constants: */
     // The block size is the number of samples processed by each tick of the digital filter.
     // Since we work sample-by-sample in real time, the block size is 1.
@@ -161,32 +163,18 @@
     +0.0001224923f, +0.0001132540f, +0.0001047243f, +0.0000968224f, +0.0000894684f, 
     +0.0000825832f
     };
+
     /* Filter Object */
     // The filter object used to perform the filtering.
     FIR_f32<NUM_TAPS, BLOCK_SIZE> fir(firCoeffs32);
     // The filtered z Acceleration
     float32_t zAccelFiltered;
     
-
-    // State variables
-    // System's angle at n = 0
-    float angle = 0;
-    // System's angle at n = -1
-    float angle_n1 = 0;
-    // System's angle at n = -2
-    float angle_n2 = 0;
-    // Previous system output
-    float output_n1 = 0;
-    // Position of the system.
-    float position = 0;
     while (true) {
         // Read the state from the accelerometer:
         accelerometer.readAccelerometer(&xAccel, &yAccel, &zAccel);
         // Filter the accelerometer state:
         fir.process(&zAccel, &zAccelFiltered);
-        // Read the state from the quadrature encoder:
-        leftWheel = leftQuadEnc.getRevs();
-        rightWheel = rightQuadEnc.getRevs();
 
         // Acquire the system's angle:
         // angle = atan2f(yAccel, zAccel);
@@ -194,21 +182,84 @@
         if(yAccel < 0) {
             angle = -angle;
         }
-        // Acquire the system's position:
-        position = (leftWheel + rightWheel) / 2.;
 
         // Control Law to regulate system:
-        float output = a0 * angle + a1 * angle_n1 + a2 * angle_n2 + o1 * output_n1;
-        // Write the output to the motor.
-        leftMotor.write(output);
-        rightMotor.write(output);
+        float output = a0 * angle + a1 * angle_n1 + a2 * angle_n2;
+        // Set the output for motorControl_thread to handle.
+        setAccelMutex.lock();
+        setAccel = output;
+        setAccelMutex.unlock();
 
         // Update the state variables
         angle_n1 = angle;
         angle_n2 = angle_n1;
-        output_n1 = output;
+
+        // Thread::wait(100);
+        fixedRefresh.refresh_us(controlSampleRateUS);
+    }
+}
+
+
+/**
+ * @brief This thread controls the motors to have a specified acceleration, with the value
+ * coming from control_thread.
+ */
+void motorControl_thread(void const* argument) {
+    // Control variable constants
+    const float kConst = 1;
+    const float iConst = 1;
+
+    // Left Wheel State Variables
+    float leftPosition = 0;
+    float prevLeftPosition = 0;
+    float leftPositionDeriv = 0;
+    float prevLeftPositionDeriv = 0;
+    float leftPositionDDeriv = 0;
+    float leftErrorInt = 0;
+
+    // Right Wheel State Variables
+    float rightPosition = 0;
+    float prevRightPosition = 0;
+    float rightPositionDeriv = 0;
+    float prevRightPositionDeriv = 0;
+    float rightPositionDDeriv = 0;
+    float rightErrorInt = 0;
+
+    FixedRefresh refresh;
 
-        fixedRefresh.refresh_us(controlSampleRateUS);
+    while(true) {
+        // Calculate new values for the state variables. Note that the steps are performed in reverse.
+        leftPositionDDeriv = leftPositionDeriv - prevLeftPositionDeriv;
+        rightPositionDDeriv = rightPositionDeriv - prevRightPositionDeriv;
+        leftPositionDeriv = leftPosition - prevLeftPosition;
+        rightPositionDeriv = rightPosition - prevRightPosition;
+        leftPosition = leftQuadEnc.getRevs();
+        rightPosition = rightQuadEnc.getRevs();
+
+        // Get the commanded acceleration:
+        setAccelMutex.lock();
+        float localSetAccel = setAccel;
+        setAccelMutex.unlock();
+
+        // Calculate the error:
+
+        float leftError = localSetAccel - leftPositionDDeriv;
+        float rightError = localSetAccel - rightPositionDDeriv;
+
+        // Calculate the output:
+        float leftOutput = kConst * leftError + iConst * leftErrorInt;
+        float rightOutput = kConst * leftError + iConst * rightErrorInt;
+        leftErrorInt += leftError;
+        rightErrorInt += rightError;
+        // Write the output:
+        leftMotor.write(leftOutput);
+        rightMotor.write(rightOutput);
+        // Update the previous state variable values:
+        prevLeftPosition = leftPosition;
+        prevRightPosition = rightPosition;
+        prevLeftPositionDeriv = leftPositionDeriv;
+        prevRightPositionDeriv = rightPositionDeriv;
+        refresh.refresh_us(1250);
     }
 }
 
@@ -228,7 +279,8 @@
     Thread thread(bluetooth_thread, NULL, osPriorityNormal);
     // Thread priority is set as above normal: If the control_thread and bluetooth_thread
     // ever want to happen at the same time, control_thread wins.
-    Thread thread2(control_thread, NULL, osPriorityAboveNormal);
+    //Thread thread2(control_thread, NULL, osPriorityAboveNormal);
+    Thread thread3(motorControl_thread, NULL, osPriorityAboveNormal);
     while (true) {
         // Main thread does nothing else, so we tell it to wait forever.
         Thread::wait(osWaitForever);