Diff: main.cpp

Revision:

2:098bb1322f0e

Parent:

1:4afc0dd95be9

diff -r 4afc0dd95be9 -r 098bb1322f0e main.cpp
--- a/main.cpp	Thu Jun 30 22:33:59 2016 +0000
+++ b/main.cpp	Thu Jun 30 23:53:28 2016 +0000
@@ -24,6 +24,8 @@
 const uint32_t BTTransmitPeriodMS = 1;
 /// The sampling period of the control system.
 const uint32_t controlSampleRateUS = 1250;
+/// The sampling period of the motor control subsystem
+const uint32_t motorControlSampleRateUS = 333;
 /// The diameter of the wheels in inches
 const float wheelDiameter = 1.9f;
 
@@ -53,6 +55,7 @@
  * This thread reads the quadrature encoder and
  * output data and transmits it as comma-separated values over Bluetooth.
  */
+ float angle;
 void bluetooth_thread(void const *argument) {
     // Timer to control how long to wait.
     FixedRefresh refresh;
@@ -67,7 +70,7 @@
     while(true) {
         accelerometer.readAccelerometer(&xAccel, &yAccel, &zAccel);
         // Place the CSV information in the buffer:
-        sprintf(buffer, "%f,%f,%f,%f,%f,%f\n\r", timer.read(), xAccel, yAccel, zAccel, leftMotor.read(), rightMotor.read());
+        sprintf(buffer, "%f,%f,%f,%f,%f,%f,%f,%f\n\r", timer.read(), xAccel, yAccel, zAccel, angle, setAccel, leftMotor.read(), rightMotor.read());
         // Transmit the information.
         bluetooth.print(buffer);
         // Refresh at a specified interval.
@@ -99,7 +102,7 @@
     
     /* State Variables: */
     // System's angle at n = 0
-    float angle = 0;
+    // float angle = 0;
     // System's angle at n = -1
     float angle_n1 = 0;
     // System's angle at n = -2
@@ -111,57 +114,33 @@
     const uint32_t BLOCK_SIZE = 1;
  
     /* FIR Coefficients buffer generated using fir1() MATLAB function. */
-    const uint32_t NUM_TAPS = 241;
+    const uint32_t NUM_TAPS = 121;
     const float32_t firCoeffs32[NUM_TAPS] = { 
-    +0.0000825832f, +0.0000894684f, +0.0000968224f, +0.0001047243f, +0.0001132540f, 
-    +0.0001224923f, +0.0001325206f, +0.0001434207f, +0.0001552749f, +0.0001681654f, 
-    +0.0001821744f, +0.0001973838f, +0.0002138753f, +0.0002317296f, +0.0002510270f, 
-    +0.0002718465f, +0.0002942661f, +0.0003183624f, +0.0003442106f, +0.0003718838f, 
-    +0.0004014538f, +0.0004329897f, +0.0004665588f, +0.0005022258f, +0.0005400529f, 
-    +0.0005800994f, +0.0006224218f, +0.0006670735f, +0.0007141047f, +0.0007635622f, 
-    +0.0008154892f, +0.0008699254f, +0.0009269065f, +0.0009864647f, +0.0010486275f, 
-    +0.0011134190f, +0.0011808585f, +0.0012509612f, +0.0013237379f, +0.0013991947f, 
-    +0.0014773333f, +0.0015581504f, +0.0016416385f, +0.0017277849f, +0.0018165723f, 
-    +0.0019079784f, +0.0020019764f, +0.0020985340f, +0.0021976146f, +0.0022991768f, 
-    +0.0024031731f, +0.0025095530f, +0.0026182598f, +0.0027292324f, +0.0028424053f, 
-    +0.0029577080f, +0.0030750656f, +0.0031943982f, +0.0033156220f, +0.0034386488f, 
-    +0.0035633859f, +0.0036897366f, +0.0038176002f, +0.0039468720f, +0.0040774439f, 
-    +0.0042092041f, +0.0043420363f, +0.0044758227f, +0.0046104412f, +0.0047457665f, 
-    +0.0048816721f, +0.0050180266f, +0.0051546986f, +0.0052915523f, +0.0054284511f, 
-    +0.0055652573f, +0.0057018292f, +0.0058380268f, +0.0059737056f, +0.0061087236f, 
-    +0.0062429351f, +0.0063761952f, +0.0065083597f, +0.0066392822f, +0.0067688185f, 
-    +0.0068968232f, +0.0070231529f, +0.0071476642f, +0.0072702146f, +0.0073906644f, 
-    +0.0075088735f, +0.0076247053f, +0.0077380240f, +0.0078486968f, +0.0079565924f, 
-    +0.0080615841f, +0.0081635462f, +0.0082623558f, +0.0083578955f, +0.0084500499f, 
-    +0.0085387062f, +0.0086237583f, +0.0087051028f, +0.0087826382f, +0.0088562714f, 
-    +0.0089259110f, +0.0089914715f, +0.0090528717f, +0.0091100354f, +0.0091628926f, 
-    +0.0092113772f, +0.0092554288f, +0.0092949932f, +0.0093300194f, +0.0093604643f, 
-    +0.0093862908f, +0.0094074663f, +0.0094239628f, +0.0094357608f, +0.0094428463f, 
-    +0.0094452091f, +0.0094428463f, +0.0094357608f, +0.0094239628f, +0.0094074663f, 
-    +0.0093862908f, +0.0093604643f, +0.0093300194f, +0.0092949932f, +0.0092554288f, 
-    +0.0092113772f, +0.0091628926f, +0.0091100354f, +0.0090528717f, +0.0089914715f, 
-    +0.0089259110f, +0.0088562714f, +0.0087826382f, +0.0087051028f, +0.0086237583f, 
-    +0.0085387062f, +0.0084500499f, +0.0083578955f, +0.0082623558f, +0.0081635462f, 
-    +0.0080615841f, +0.0079565924f, +0.0078486968f, +0.0077380240f, +0.0076247053f, 
-    +0.0075088735f, +0.0073906644f, +0.0072702146f, +0.0071476642f, +0.0070231529f, 
-    +0.0068968232f, +0.0067688185f, +0.0066392822f, +0.0065083597f, +0.0063761952f, 
-    +0.0062429351f, +0.0061087236f, +0.0059737056f, +0.0058380268f, +0.0057018292f, 
-    +0.0055652573f, +0.0054284511f, +0.0052915523f, +0.0051546986f, +0.0050180266f, 
-    +0.0048816721f, +0.0047457665f, +0.0046104412f, +0.0044758227f, +0.0043420363f, 
-    +0.0042092041f, +0.0040774439f, +0.0039468720f, +0.0038176002f, +0.0036897366f, 
-    +0.0035633859f, +0.0034386488f, +0.0033156220f, +0.0031943982f, +0.0030750656f, 
-    +0.0029577080f, +0.0028424053f, +0.0027292324f, +0.0026182598f, +0.0025095530f, 
-    +0.0024031731f, +0.0022991768f, +0.0021976146f, +0.0020985340f, +0.0020019764f, 
-    +0.0019079784f, +0.0018165723f, +0.0017277849f, +0.0016416385f, +0.0015581504f, 
-    +0.0014773333f, +0.0013991947f, +0.0013237379f, +0.0012509612f, +0.0011808585f, 
-    +0.0011134190f, +0.0010486275f, +0.0009864647f, +0.0009269065f, +0.0008699254f, 
-    +0.0008154892f, +0.0007635622f, +0.0007141047f, +0.0006670735f, +0.0006224218f, 
-    +0.0005800994f, +0.0005400529f, +0.0005022258f, +0.0004665588f, +0.0004329897f, 
-    +0.0004014538f, +0.0003718838f, +0.0003442106f, +0.0003183624f, +0.0002942661f, 
-    +0.0002718465f, +0.0002510270f, +0.0002317296f, +0.0002138753f, +0.0001973838f, 
-    +0.0001821744f, +0.0001681654f, +0.0001552749f, +0.0001434207f, +0.0001325206f, 
-    +0.0001224923f, +0.0001132540f, +0.0001047243f, +0.0000968224f, +0.0000894684f, 
-    +0.0000825832f
+    -0.0000000000f, -0.0002560113f, -0.0004312516f, -0.0004555333f, -0.0003012438f, 
+    +0.0000000000f, +0.0003556327f, +0.0006322583f, +0.0006981380f, +0.0004780164f, 
+    -0.0000000000f, -0.0005895668f, -0.0010599506f, -0.0011770516f, -0.0008069672f, 
+    +0.0000000000f, +0.0009886023f, +0.0017658793f, +0.0019458113f, +0.0013225924f, 
+    -0.0000000000f, -0.0015907108f, -0.0028149413f, -0.0030732930f, -0.0020702914f, 
+    +0.0000000000f, +0.0024483239f, +0.0042990157f, +0.0046595251f, +0.0031177229f, 
+    -0.0000000000f, -0.0036437681f, -0.0063662040f, -0.0068700365f, -0.0045798332f, 
+    +0.0000000000f, +0.0053245081f, +0.0092889853f, +0.0100177610f, +0.0066800457f, 
+    -0.0000000000f, -0.0077943369f, -0.0136463577f, -0.0147897471f, -0.0099262614f, 
+    +0.0000000000f, +0.0117998710f, +0.0209259298f, +0.0230400730f, +0.0157663561f, 
+    -0.0000000000f, -0.0197710935f, -0.0363819152f, -0.0419784039f, -0.0305190869f, 
+    +0.0000000000f, +0.0463622585f, +0.1004643664f, +0.1511729509f, +0.1872140169f, 
+    +0.2002504170f, +0.1872140169f, +0.1511729509f, +0.1004643664f, +0.0463622585f, 
+    +0.0000000000f, -0.0305190869f, -0.0419784039f, -0.0363819152f, -0.0197710935f, 
+    -0.0000000000f, +0.0157663561f, +0.0230400730f, +0.0209259298f, +0.0117998710f, 
+    +0.0000000000f, -0.0099262614f, -0.0147897471f, -0.0136463577f, -0.0077943369f, 
+    -0.0000000000f, +0.0066800457f, +0.0100177610f, +0.0092889853f, +0.0053245081f, 
+    +0.0000000000f, -0.0045798332f, -0.0068700365f, -0.0063662040f, -0.0036437681f, 
+    -0.0000000000f, +0.0031177229f, +0.0046595251f, +0.0042990157f, +0.0024483239f, 
+    +0.0000000000f, -0.0020702914f, -0.0030732930f, -0.0028149413f, -0.0015907108f, 
+    -0.0000000000f, +0.0013225924f, +0.0019458113f, +0.0017658793f, +0.0009886023f, 
+    +0.0000000000f, -0.0008069672f, -0.0011770516f, -0.0010599506f, -0.0005895668f, 
+    -0.0000000000f, +0.0004780164f, +0.0006981380f, +0.0006322583f, +0.0003556327f, 
+    +0.0000000000f, -0.0003012438f, -0.0004555333f, -0.0004312516f, -0.0002560113f, 
+    -0.0000000000f
     };
 
     /* Filter Object */
@@ -173,9 +152,9 @@
     while (true) {
         // Read the state from the accelerometer:
         accelerometer.readAccelerometer(&xAccel, &yAccel, &zAccel);
-        // Filter the accelerometer state:
         fir.process(&zAccel, &zAccelFiltered);
 
+
         // Acquire the system's angle:
         // angle = atan2f(yAccel, zAccel);
         arm_sqrt_f32(2*zAccelFiltered, &angle);
@@ -185,6 +164,8 @@
 
         // Control Law to regulate system:
         float output = a0 * angle + a1 * angle_n1 + a2 * angle_n2;
+                // Filter the accelerometer state:
+
         // Set the output for motorControl_thread to handle.
         setAccelMutex.lock();
         setAccel = output;
@@ -206,8 +187,8 @@
  */
 void motorControl_thread(void const* argument) {
     // Control variable constants
-    const float kConst = 1;
-    const float iConst = 1;
+    const float kConst = 0.5;
+    const float iConst = 4 * motorControlSampleRateUS / 1000000.;
 
     // Left Wheel State Variables
     float leftPosition = 0;
@@ -259,7 +240,7 @@
         prevRightPosition = rightPosition;
         prevLeftPositionDeriv = leftPositionDeriv;
         prevRightPositionDeriv = rightPositionDeriv;
-        refresh.refresh_us(1250);
+        refresh.refresh_us(motorControlSampleRateUS);
     }
 }
 
@@ -279,7 +260,7 @@
     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.