Weimen Li / RobotBalancerv2

Self-Balancing Robot Package for MAE 433.

Dependencies:   MAE433_Library

main.cpp@2:098bb1322f0e, 2016-06-30 (annotated)

Committer:
Electrotiger
Date:
Thu Jun 30 23:53:28 2016 +0000
Revision:
2:098bb1322f0e
Parent:
1:4afc0dd95be9 
Functional - Turns out that it didn't work because there were too many taps in the FIR filter, not because of the FixedRefresh class.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Electrotiger 0:3ee52ce4df19 1 /**
Electrotiger 0:3ee52ce4df19 2 * @file main.cpp
Electrotiger 0:3ee52ce4df19 3 * @date June 9th, 2015
Electrotiger 0:3ee52ce4df19 4 * @author Weimen Li
Electrotiger 0:3ee52ce4df19 5 * @mainpage RobotBalancer
Electrotiger 0:3ee52ce4df19 6 * This program is a demo lab for MAE 433. It controls a robot to balance it.
Electrotiger 0:3ee52ce4df19 7 * @see main.cpp
Electrotiger 0:3ee52ce4df19 8 */
Electrotiger 0:3ee52ce4df19 9
Electrotiger 0:3ee52ce4df19 10 /* Inclusions */
Electrotiger 0:3ee52ce4df19 11 #include "mbed.h"
Electrotiger 0:3ee52ce4df19 12 #include "rtos.h"
Electrotiger 0:3ee52ce4df19 13 #include "HBridge.hpp"
Electrotiger 0:3ee52ce4df19 14 #include "QuadEnc.hpp"
Electrotiger 0:3ee52ce4df19 15 #include "HC06Bluetooth.hpp"
Electrotiger 0:3ee52ce4df19 16 #include "FXOS8700CQ.hpp"
Electrotiger 0:3ee52ce4df19 17 #include "dsp.h"
Electrotiger 0:3ee52ce4df19 18 #include "FixedRefresh.hpp"
Electrotiger 0:3ee52ce4df19 19
Electrotiger 0:3ee52ce4df19 20 /* Constants */
Electrotiger 0:3ee52ce4df19 21 /// The CPR of the encoder.
Electrotiger 0:3ee52ce4df19 22 const float encoderCPR = 100.98f * 12.0f;
Electrotiger 0:3ee52ce4df19 23 /// The transmission period of the bluetooth module.
Electrotiger 0:3ee52ce4df19 24 const uint32_t BTTransmitPeriodMS = 1;
Electrotiger 0:3ee52ce4df19 25 /// The sampling period of the control system.
Electrotiger 0:3ee52ce4df19 26 const uint32_t controlSampleRateUS = 1250;
Electrotiger 2:098bb1322f0e 27 /// The sampling period of the motor control subsystem
Electrotiger 2:098bb1322f0e 28 const uint32_t motorControlSampleRateUS = 333;
Electrotiger 0:3ee52ce4df19 29 /// The diameter of the wheels in inches
Electrotiger 0:3ee52ce4df19 30 const float wheelDiameter = 1.9f;
Electrotiger 0:3ee52ce4df19 31
Electrotiger 0:3ee52ce4df19 32 /* Declaring objects */
Electrotiger 0:3ee52ce4df19 33 /// The quad encoder of the left motor.
Electrotiger 0:3ee52ce4df19 34 QuadEnc leftQuadEnc(PTB0, PTB1, encoderCPR);
Electrotiger 0:3ee52ce4df19 35 /// The quad encoder of the right motor.
Electrotiger 0:3ee52ce4df19 36 QuadEnc rightQuadEnc(PTC2, PTC1, encoderCPR);
Electrotiger 0:3ee52ce4df19 37 /// The H-Bridge driving the left motor.
Electrotiger 0:3ee52ce4df19 38 HBridge leftMotor(D9, D7);
Electrotiger 0:3ee52ce4df19 39 /// The H-Bridge driving the right motor.
Electrotiger 0:3ee52ce4df19 40 HBridge rightMotor(D10, D8);
Electrotiger 0:3ee52ce4df19 41 /// The accelerometer on the board
Electrotiger 0:3ee52ce4df19 42 FXOS8700CQ accelerometer(PTB3, PTB2, PTD0, PTD1);
Electrotiger 0:3ee52ce4df19 43
Electrotiger 0:3ee52ce4df19 44 /// The Bluetooth link used to transmit data.
Electrotiger 0:3ee52ce4df19 45 HC06Bluetooth bluetooth(PTD3, PTD2);
Electrotiger 0:3ee52ce4df19 46 /// Timer to transmit timing information.
Electrotiger 0:3ee52ce4df19 47 Timer timer;
Electrotiger 0:3ee52ce4df19 48
Electrotiger 1:4afc0dd95be9 49 /* The acceleration commanded by control_thread and used as input for motorControl_thread() */
Electrotiger 1:4afc0dd95be9 50 float setAccel;
Electrotiger 1:4afc0dd95be9 51 Mutex setAccelMutex;
Electrotiger 1:4afc0dd95be9 52
Electrotiger 0:3ee52ce4df19 53 /* Threads */
Electrotiger 0:3ee52ce4df19 54 /** @brief Bluetooth Transmission:
Electrotiger 0:3ee52ce4df19 55 * This thread reads the quadrature encoder and
Electrotiger 0:3ee52ce4df19 56 * output data and transmits it as comma-separated values over Bluetooth.
Electrotiger 0:3ee52ce4df19 57 */
Electrotiger 2:098bb1322f0e 58 float angle;
Electrotiger 0:3ee52ce4df19 59 void bluetooth_thread(void const *argument) {
Electrotiger 0:3ee52ce4df19 60 // Timer to control how long to wait.
Electrotiger 1:4afc0dd95be9 61 FixedRefresh refresh;
Electrotiger 0:3ee52ce4df19 62 // 64-byte buffer is used to store the data. May be expanded as needed.
Electrotiger 0:3ee52ce4df19 63 char buffer[128];
Electrotiger 0:3ee52ce4df19 64 // The accelerometer readings.
Electrotiger 0:3ee52ce4df19 65 float xAccel;
Electrotiger 0:3ee52ce4df19 66 float yAccel;
Electrotiger 0:3ee52ce4df19 67 float zAccel;
Electrotiger 0:3ee52ce4df19 68
Electrotiger 0:3ee52ce4df19 69
Electrotiger 0:3ee52ce4df19 70 while(true) {
Electrotiger 0:3ee52ce4df19 71 accelerometer.readAccelerometer(&xAccel, &yAccel, &zAccel);
Electrotiger 0:3ee52ce4df19 72 // Place the CSV information in the buffer:
Electrotiger 2:098bb1322f0e 73 sprintf(buffer, "%f,%f,%f,%f,%f,%f,%f,%f\n\r", timer.read(), xAccel, yAccel, zAccel, angle, setAccel, leftMotor.read(), rightMotor.read());
Electrotiger 0:3ee52ce4df19 74 // Transmit the information.
Electrotiger 0:3ee52ce4df19 75 bluetooth.print(buffer);
Electrotiger 1:4afc0dd95be9 76 // Refresh at a specified interval.
Electrotiger 1:4afc0dd95be9 77 refresh.refresh_us(BTTransmitPeriodMS * 1000);
Electrotiger 0:3ee52ce4df19 78 }
Electrotiger 0:3ee52ce4df19 79
Electrotiger 0:3ee52ce4df19 80 }
Electrotiger 0:3ee52ce4df19 81
Electrotiger 0:3ee52ce4df19 82 /** @brief Control Thread:
Electrotiger 0:3ee52ce4df19 83 * This thread reads the current quadrature counts and actuates
Electrotiger 0:3ee52ce4df19 84 * the motor through the H-Bridge.
Electrotiger 0:3ee52ce4df19 85 */
Electrotiger 0:3ee52ce4df19 86
Electrotiger 0:3ee52ce4df19 87 void control_thread(void const *argument) {
Electrotiger 1:4afc0dd95be9 88 // FixedRefresh object to wait at fixed time
Electrotiger 0:3ee52ce4df19 89 FixedRefresh fixedRefresh;
Electrotiger 0:3ee52ce4df19 90 // The accelerometer readings.
Electrotiger 0:3ee52ce4df19 91 float xAccel;
Electrotiger 0:3ee52ce4df19 92 float yAccel;
Electrotiger 0:3ee52ce4df19 93 float zAccel;
Electrotiger 0:3ee52ce4df19 94
Electrotiger 0:3ee52ce4df19 95 /* Feedback constants: */
Electrotiger 0:3ee52ce4df19 96 // Accelerometer feedback at n = 0;
Electrotiger 0:3ee52ce4df19 97 const float a0 = 0.15;
Electrotiger 0:3ee52ce4df19 98 // Accelerometer feedback at n = -1;
Electrotiger 0:3ee52ce4df19 99 const float a1 = 0;
Electrotiger 0:3ee52ce4df19 100 // Accelerometer feedback at n = -2;
Electrotiger 0:3ee52ce4df19 101 const float a2 = 0;
Electrotiger 0:3ee52ce4df19 102
Electrotiger 1:4afc0dd95be9 103 /* State Variables: */
Electrotiger 1:4afc0dd95be9 104 // System's angle at n = 0
Electrotiger 2:098bb1322f0e 105 // float angle = 0;
Electrotiger 1:4afc0dd95be9 106 // System's angle at n = -1
Electrotiger 1:4afc0dd95be9 107 float angle_n1 = 0;
Electrotiger 1:4afc0dd95be9 108 // System's angle at n = -2
Electrotiger 1:4afc0dd95be9 109 float angle_n2 = 0;
Electrotiger 1:4afc0dd95be9 110
Electrotiger 0:3ee52ce4df19 111 /* Filtering Constants: */
Electrotiger 0:3ee52ce4df19 112 // The block size is the number of samples processed by each tick of the digital filter.
Electrotiger 0:3ee52ce4df19 113 // Since we work sample-by-sample in real time, the block size is 1.
Electrotiger 0:3ee52ce4df19 114 const uint32_t BLOCK_SIZE = 1;
Electrotiger 0:3ee52ce4df19 115
Electrotiger 0:3ee52ce4df19 116 /* FIR Coefficients buffer generated using fir1() MATLAB function. */
Electrotiger 2:098bb1322f0e 117 const uint32_t NUM_TAPS = 121;
Electrotiger 0:3ee52ce4df19 118 const float32_t firCoeffs32[NUM_TAPS] = {
Electrotiger 2:098bb1322f0e 119 -0.0000000000f, -0.0002560113f, -0.0004312516f, -0.0004555333f, -0.0003012438f,
Electrotiger 2:098bb1322f0e 120 +0.0000000000f, +0.0003556327f, +0.0006322583f, +0.0006981380f, +0.0004780164f,
Electrotiger 2:098bb1322f0e 121 -0.0000000000f, -0.0005895668f, -0.0010599506f, -0.0011770516f, -0.0008069672f,
Electrotiger 2:098bb1322f0e 122 +0.0000000000f, +0.0009886023f, +0.0017658793f, +0.0019458113f, +0.0013225924f,
Electrotiger 2:098bb1322f0e 123 -0.0000000000f, -0.0015907108f, -0.0028149413f, -0.0030732930f, -0.0020702914f,
Electrotiger 2:098bb1322f0e 124 +0.0000000000f, +0.0024483239f, +0.0042990157f, +0.0046595251f, +0.0031177229f,
Electrotiger 2:098bb1322f0e 125 -0.0000000000f, -0.0036437681f, -0.0063662040f, -0.0068700365f, -0.0045798332f,
Electrotiger 2:098bb1322f0e 126 +0.0000000000f, +0.0053245081f, +0.0092889853f, +0.0100177610f, +0.0066800457f,
Electrotiger 2:098bb1322f0e 127 -0.0000000000f, -0.0077943369f, -0.0136463577f, -0.0147897471f, -0.0099262614f,
Electrotiger 2:098bb1322f0e 128 +0.0000000000f, +0.0117998710f, +0.0209259298f, +0.0230400730f, +0.0157663561f,
Electrotiger 2:098bb1322f0e 129 -0.0000000000f, -0.0197710935f, -0.0363819152f, -0.0419784039f, -0.0305190869f,
Electrotiger 2:098bb1322f0e 130 +0.0000000000f, +0.0463622585f, +0.1004643664f, +0.1511729509f, +0.1872140169f,
Electrotiger 2:098bb1322f0e 131 +0.2002504170f, +0.1872140169f, +0.1511729509f, +0.1004643664f, +0.0463622585f,
Electrotiger 2:098bb1322f0e 132 +0.0000000000f, -0.0305190869f, -0.0419784039f, -0.0363819152f, -0.0197710935f,
Electrotiger 2:098bb1322f0e 133 -0.0000000000f, +0.0157663561f, +0.0230400730f, +0.0209259298f, +0.0117998710f,
Electrotiger 2:098bb1322f0e 134 +0.0000000000f, -0.0099262614f, -0.0147897471f, -0.0136463577f, -0.0077943369f,
Electrotiger 2:098bb1322f0e 135 -0.0000000000f, +0.0066800457f, +0.0100177610f, +0.0092889853f, +0.0053245081f,
Electrotiger 2:098bb1322f0e 136 +0.0000000000f, -0.0045798332f, -0.0068700365f, -0.0063662040f, -0.0036437681f,
Electrotiger 2:098bb1322f0e 137 -0.0000000000f, +0.0031177229f, +0.0046595251f, +0.0042990157f, +0.0024483239f,
Electrotiger 2:098bb1322f0e 138 +0.0000000000f, -0.0020702914f, -0.0030732930f, -0.0028149413f, -0.0015907108f,
Electrotiger 2:098bb1322f0e 139 -0.0000000000f, +0.0013225924f, +0.0019458113f, +0.0017658793f, +0.0009886023f,
Electrotiger 2:098bb1322f0e 140 +0.0000000000f, -0.0008069672f, -0.0011770516f, -0.0010599506f, -0.0005895668f,
Electrotiger 2:098bb1322f0e 141 -0.0000000000f, +0.0004780164f, +0.0006981380f, +0.0006322583f, +0.0003556327f,
Electrotiger 2:098bb1322f0e 142 +0.0000000000f, -0.0003012438f, -0.0004555333f, -0.0004312516f, -0.0002560113f,
Electrotiger 2:098bb1322f0e 143 -0.0000000000f
Electrotiger 0:3ee52ce4df19 144 };
Electrotiger 1:4afc0dd95be9 145
Electrotiger 0:3ee52ce4df19 146 /* Filter Object */
Electrotiger 0:3ee52ce4df19 147 // The filter object used to perform the filtering.
Electrotiger 0:3ee52ce4df19 148 FIR_f32<NUM_TAPS, BLOCK_SIZE> fir(firCoeffs32);
Electrotiger 0:3ee52ce4df19 149 // The filtered z Acceleration
Electrotiger 0:3ee52ce4df19 150 float32_t zAccelFiltered;
Electrotiger 0:3ee52ce4df19 151
Electrotiger 0:3ee52ce4df19 152 while (true) {
Electrotiger 0:3ee52ce4df19 153 // Read the state from the accelerometer:
Electrotiger 0:3ee52ce4df19 154 accelerometer.readAccelerometer(&xAccel, &yAccel, &zAccel);
Electrotiger 0:3ee52ce4df19 155 fir.process(&zAccel, &zAccelFiltered);
Electrotiger 0:3ee52ce4df19 156
Electrotiger 2:098bb1322f0e 157
Electrotiger 0:3ee52ce4df19 158 // Acquire the system's angle:
Electrotiger 0:3ee52ce4df19 159 // angle = atan2f(yAccel, zAccel);
Electrotiger 0:3ee52ce4df19 160 arm_sqrt_f32(2*zAccelFiltered, &angle);
Electrotiger 0:3ee52ce4df19 161 if(yAccel < 0) {
Electrotiger 0:3ee52ce4df19 162 angle = -angle;
Electrotiger 0:3ee52ce4df19 163 }
Electrotiger 0:3ee52ce4df19 164
Electrotiger 0:3ee52ce4df19 165 // Control Law to regulate system:
Electrotiger 1:4afc0dd95be9 166 float output = a0 * angle + a1 * angle_n1 + a2 * angle_n2;
Electrotiger 2:098bb1322f0e 167 // Filter the accelerometer state:
Electrotiger 2:098bb1322f0e 168
Electrotiger 1:4afc0dd95be9 169 // Set the output for motorControl_thread to handle.
Electrotiger 1:4afc0dd95be9 170 setAccelMutex.lock();
Electrotiger 1:4afc0dd95be9 171 setAccel = output;
Electrotiger 1:4afc0dd95be9 172 setAccelMutex.unlock();
Electrotiger 0:3ee52ce4df19 173
Electrotiger 0:3ee52ce4df19 174 // Update the state variables
Electrotiger 0:3ee52ce4df19 175 angle_n1 = angle;
Electrotiger 0:3ee52ce4df19 176 angle_n2 = angle_n1;
Electrotiger 1:4afc0dd95be9 177
Electrotiger 1:4afc0dd95be9 178 // Thread::wait(100);
Electrotiger 1:4afc0dd95be9 179 fixedRefresh.refresh_us(controlSampleRateUS);
Electrotiger 1:4afc0dd95be9 180 }
Electrotiger 1:4afc0dd95be9 181 }
Electrotiger 1:4afc0dd95be9 182
Electrotiger 1:4afc0dd95be9 183
Electrotiger 1:4afc0dd95be9 184 /**
Electrotiger 1:4afc0dd95be9 185 * @brief This thread controls the motors to have a specified acceleration, with the value
Electrotiger 1:4afc0dd95be9 186 * coming from control_thread.
Electrotiger 1:4afc0dd95be9 187 */
Electrotiger 1:4afc0dd95be9 188 void motorControl_thread(void const* argument) {
Electrotiger 1:4afc0dd95be9 189 // Control variable constants
Electrotiger 2:098bb1322f0e 190 const float kConst = 0.5;
Electrotiger 2:098bb1322f0e 191 const float iConst = 4 * motorControlSampleRateUS / 1000000.;
Electrotiger 1:4afc0dd95be9 192
Electrotiger 1:4afc0dd95be9 193 // Left Wheel State Variables
Electrotiger 1:4afc0dd95be9 194 float leftPosition = 0;
Electrotiger 1:4afc0dd95be9 195 float prevLeftPosition = 0;
Electrotiger 1:4afc0dd95be9 196 float leftPositionDeriv = 0;
Electrotiger 1:4afc0dd95be9 197 float prevLeftPositionDeriv = 0;
Electrotiger 1:4afc0dd95be9 198 float leftPositionDDeriv = 0;
Electrotiger 1:4afc0dd95be9 199 float leftErrorInt = 0;
Electrotiger 1:4afc0dd95be9 200
Electrotiger 1:4afc0dd95be9 201 // Right Wheel State Variables
Electrotiger 1:4afc0dd95be9 202 float rightPosition = 0;
Electrotiger 1:4afc0dd95be9 203 float prevRightPosition = 0;
Electrotiger 1:4afc0dd95be9 204 float rightPositionDeriv = 0;
Electrotiger 1:4afc0dd95be9 205 float prevRightPositionDeriv = 0;
Electrotiger 1:4afc0dd95be9 206 float rightPositionDDeriv = 0;
Electrotiger 1:4afc0dd95be9 207 float rightErrorInt = 0;
Electrotiger 1:4afc0dd95be9 208
Electrotiger 1:4afc0dd95be9 209 FixedRefresh refresh;
Electrotiger 0:3ee52ce4df19 210
Electrotiger 1:4afc0dd95be9 211 while(true) {
Electrotiger 1:4afc0dd95be9 212 // Calculate new values for the state variables. Note that the steps are performed in reverse.
Electrotiger 1:4afc0dd95be9 213 leftPositionDDeriv = leftPositionDeriv - prevLeftPositionDeriv;
Electrotiger 1:4afc0dd95be9 214 rightPositionDDeriv = rightPositionDeriv - prevRightPositionDeriv;
Electrotiger 1:4afc0dd95be9 215 leftPositionDeriv = leftPosition - prevLeftPosition;
Electrotiger 1:4afc0dd95be9 216 rightPositionDeriv = rightPosition - prevRightPosition;
Electrotiger 1:4afc0dd95be9 217 leftPosition = leftQuadEnc.getRevs();
Electrotiger 1:4afc0dd95be9 218 rightPosition = rightQuadEnc.getRevs();
Electrotiger 1:4afc0dd95be9 219
Electrotiger 1:4afc0dd95be9 220 // Get the commanded acceleration:
Electrotiger 1:4afc0dd95be9 221 setAccelMutex.lock();
Electrotiger 1:4afc0dd95be9 222 float localSetAccel = setAccel;
Electrotiger 1:4afc0dd95be9 223 setAccelMutex.unlock();
Electrotiger 1:4afc0dd95be9 224
Electrotiger 1:4afc0dd95be9 225 // Calculate the error:
Electrotiger 1:4afc0dd95be9 226
Electrotiger 1:4afc0dd95be9 227 float leftError = localSetAccel - leftPositionDDeriv;
Electrotiger 1:4afc0dd95be9 228 float rightError = localSetAccel - rightPositionDDeriv;
Electrotiger 1:4afc0dd95be9 229
Electrotiger 1:4afc0dd95be9 230 // Calculate the output:
Electrotiger 1:4afc0dd95be9 231 float leftOutput = kConst * leftError + iConst * leftErrorInt;
Electrotiger 1:4afc0dd95be9 232 float rightOutput = kConst * leftError + iConst * rightErrorInt;
Electrotiger 1:4afc0dd95be9 233 leftErrorInt += leftError;
Electrotiger 1:4afc0dd95be9 234 rightErrorInt += rightError;
Electrotiger 1:4afc0dd95be9 235 // Write the output:
Electrotiger 1:4afc0dd95be9 236 leftMotor.write(leftOutput);
Electrotiger 1:4afc0dd95be9 237 rightMotor.write(rightOutput);
Electrotiger 1:4afc0dd95be9 238 // Update the previous state variable values:
Electrotiger 1:4afc0dd95be9 239 prevLeftPosition = leftPosition;
Electrotiger 1:4afc0dd95be9 240 prevRightPosition = rightPosition;
Electrotiger 1:4afc0dd95be9 241 prevLeftPositionDeriv = leftPositionDeriv;
Electrotiger 1:4afc0dd95be9 242 prevRightPositionDeriv = rightPositionDeriv;
Electrotiger 2:098bb1322f0e 243 refresh.refresh_us(motorControlSampleRateUS);
Electrotiger 0:3ee52ce4df19 244 }
Electrotiger 0:3ee52ce4df19 245 }
Electrotiger 0:3ee52ce4df19 246
Electrotiger 0:3ee52ce4df19 247 /** @brief Main thread:
Electrotiger 0:3ee52ce4df19 248 * This thread performs any additional initialization necessary for the declared objects.
Electrotiger 0:3ee52ce4df19 249 * Then, it starts the threads.
Electrotiger 0:3ee52ce4df19 250 */
Electrotiger 0:3ee52ce4df19 251 int main() {
Electrotiger 0:3ee52ce4df19 252 /* Initialize objects */
Electrotiger 0:3ee52ce4df19 253 // Set the offset value for the accelerometer.
Electrotiger 0:3ee52ce4df19 254 accelerometer.setOffset(0.012, -0.032, 0.032);
Electrotiger 0:3ee52ce4df19 255 // Start the global timer.
Electrotiger 0:3ee52ce4df19 256 timer.start();
Electrotiger 0:3ee52ce4df19 257
Electrotiger 0:3ee52ce4df19 258 /* Initialize threads. */
Electrotiger 0:3ee52ce4df19 259 // Thread priority is set as normal.
Electrotiger 0:3ee52ce4df19 260 Thread thread(bluetooth_thread, NULL, osPriorityNormal);
Electrotiger 0:3ee52ce4df19 261 // Thread priority is set as above normal: If the control_thread and bluetooth_thread
Electrotiger 0:3ee52ce4df19 262 // ever want to happen at the same time, control_thread wins.
Electrotiger 2:098bb1322f0e 263 Thread thread2(control_thread, NULL, osPriorityAboveNormal);
Electrotiger 1:4afc0dd95be9 264 Thread thread3(motorControl_thread, NULL, osPriorityAboveNormal);
Electrotiger 0:3ee52ce4df19 265 while (true) {
Electrotiger 0:3ee52ce4df19 266 // Main thread does nothing else, so we tell it to wait forever.
Electrotiger 0:3ee52ce4df19 267 Thread::wait(osWaitForever);
Electrotiger 0:3ee52ce4df19 268 }
Electrotiger 0:3ee52ce4df19 269 }