Weimen Li / RobotBalancerv2

Self-Balancing Robot Package for MAE 433.

Dependencies:   MAE433_Library

main.cpp@0:3ee52ce4df19, 2016-06-30 (annotated)

Committer:
Electrotiger
Date:
Thu Jun 30 20:41:12 2016 +0000
Revision:
0:3ee52ce4df19
Child:
1:4afc0dd95be9
First RobotBalancer had its version control merged with MotorMirror due to a bug. This version should now have its own versioning.

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 0:3ee52ce4df19 27 /// The diameter of the wheels in inches
Electrotiger 0:3ee52ce4df19 28 const float wheelDiameter = 1.9f;
Electrotiger 0:3ee52ce4df19 29
Electrotiger 0:3ee52ce4df19 30 /* Declaring objects */
Electrotiger 0:3ee52ce4df19 31 /// The quad encoder of the left motor.
Electrotiger 0:3ee52ce4df19 32 QuadEnc leftQuadEnc(PTB0, PTB1, encoderCPR);
Electrotiger 0:3ee52ce4df19 33 /// The quad encoder of the right motor.
Electrotiger 0:3ee52ce4df19 34 QuadEnc rightQuadEnc(PTC2, PTC1, encoderCPR);
Electrotiger 0:3ee52ce4df19 35 /// The H-Bridge driving the left motor.
Electrotiger 0:3ee52ce4df19 36 HBridge leftMotor(D9, D7);
Electrotiger 0:3ee52ce4df19 37 /// The H-Bridge driving the right motor.
Electrotiger 0:3ee52ce4df19 38 HBridge rightMotor(D10, D8);
Electrotiger 0:3ee52ce4df19 39 /// The accelerometer on the board
Electrotiger 0:3ee52ce4df19 40 FXOS8700CQ accelerometer(PTB3, PTB2, PTD0, PTD1);
Electrotiger 0:3ee52ce4df19 41
Electrotiger 0:3ee52ce4df19 42 /// The Bluetooth link used to transmit data.
Electrotiger 0:3ee52ce4df19 43 HC06Bluetooth bluetooth(PTD3, PTD2);
Electrotiger 0:3ee52ce4df19 44 /// Timer to transmit timing information.
Electrotiger 0:3ee52ce4df19 45 Timer timer;
Electrotiger 0:3ee52ce4df19 46
Electrotiger 0:3ee52ce4df19 47 /* Threads */
Electrotiger 0:3ee52ce4df19 48 /** @brief Bluetooth Transmission:
Electrotiger 0:3ee52ce4df19 49 * This thread reads the quadrature encoder and
Electrotiger 0:3ee52ce4df19 50 * output data and transmits it as comma-separated values over Bluetooth.
Electrotiger 0:3ee52ce4df19 51 */
Electrotiger 0:3ee52ce4df19 52 void bluetooth_thread(void const *argument) {
Electrotiger 0:3ee52ce4df19 53 // Timer to control how long to wait.
Electrotiger 0:3ee52ce4df19 54 Timer waitTimer;
Electrotiger 0:3ee52ce4df19 55 // 64-byte buffer is used to store the data. May be expanded as needed.
Electrotiger 0:3ee52ce4df19 56 char buffer[128];
Electrotiger 0:3ee52ce4df19 57 // The accelerometer readings.
Electrotiger 0:3ee52ce4df19 58 float xAccel;
Electrotiger 0:3ee52ce4df19 59 float yAccel;
Electrotiger 0:3ee52ce4df19 60 float zAccel;
Electrotiger 0:3ee52ce4df19 61
Electrotiger 0:3ee52ce4df19 62
Electrotiger 0:3ee52ce4df19 63 // Start the timer:
Electrotiger 0:3ee52ce4df19 64 waitTimer.start();
Electrotiger 0:3ee52ce4df19 65 while(true) {
Electrotiger 0:3ee52ce4df19 66 waitTimer.reset();
Electrotiger 0:3ee52ce4df19 67 accelerometer.readAccelerometer(&xAccel, &yAccel, &zAccel);
Electrotiger 0:3ee52ce4df19 68 // Place the CSV information in the buffer:
Electrotiger 0:3ee52ce4df19 69 sprintf(buffer, "%f,%f,%f,%f,%f,%f\n", timer.read(), xAccel, yAccel, zAccel, leftMotor.read(), rightMotor.read());
Electrotiger 0:3ee52ce4df19 70 // Transmit the information.
Electrotiger 0:3ee52ce4df19 71 bluetooth.print(buffer);
Electrotiger 0:3ee52ce4df19 72 // Wait the thread for BTTransmitPeriodMS - thread continues after this time.
Electrotiger 0:3ee52ce4df19 73 int32_t waitTime = BTTransmitPeriodMS - waitTimer.read_ms();
Electrotiger 0:3ee52ce4df19 74 if (waitTime < 0.) waitTime = 0;
Electrotiger 0:3ee52ce4df19 75 Thread::wait(waitTime);
Electrotiger 0:3ee52ce4df19 76 }
Electrotiger 0:3ee52ce4df19 77
Electrotiger 0:3ee52ce4df19 78 }
Electrotiger 0:3ee52ce4df19 79
Electrotiger 0:3ee52ce4df19 80 /** @brief Control Thread:
Electrotiger 0:3ee52ce4df19 81 * This thread reads the current quadrature counts and actuates
Electrotiger 0:3ee52ce4df19 82 * the motor through the H-Bridge.
Electrotiger 0:3ee52ce4df19 83 */
Electrotiger 0:3ee52ce4df19 84
Electrotiger 0:3ee52ce4df19 85 void control_thread(void const *argument) {
Electrotiger 0:3ee52ce4df19 86 // FixedRefresh object to wait at fixed time
Electrotiger 0:3ee52ce4df19 87 FixedRefresh fixedRefresh;
Electrotiger 0:3ee52ce4df19 88 // The accelerometer readings.
Electrotiger 0:3ee52ce4df19 89 float xAccel;
Electrotiger 0:3ee52ce4df19 90 float yAccel;
Electrotiger 0:3ee52ce4df19 91 float zAccel;
Electrotiger 0:3ee52ce4df19 92 // The quadrature encoder readings.
Electrotiger 0:3ee52ce4df19 93 float leftWheel;
Electrotiger 0:3ee52ce4df19 94 float rightWheel;
Electrotiger 0:3ee52ce4df19 95
Electrotiger 0:3ee52ce4df19 96 /* Feedback constants: */
Electrotiger 0:3ee52ce4df19 97 // Accelerometer feedback at n = 0;
Electrotiger 0:3ee52ce4df19 98 const float a0 = 0.15;
Electrotiger 0:3ee52ce4df19 99 // Accelerometer feedback at n = -1;
Electrotiger 0:3ee52ce4df19 100 const float a1 = 0;
Electrotiger 0:3ee52ce4df19 101 // Accelerometer feedback at n = -2;
Electrotiger 0:3ee52ce4df19 102 const float a2 = 0;
Electrotiger 0:3ee52ce4df19 103 // Output feedback
Electrotiger 0:3ee52ce4df19 104 const float o1 = 0;
Electrotiger 0:3ee52ce4df19 105
Electrotiger 0:3ee52ce4df19 106 /* Filtering Constants: */
Electrotiger 0:3ee52ce4df19 107 // The block size is the number of samples processed by each tick of the digital filter.
Electrotiger 0:3ee52ce4df19 108 // Since we work sample-by-sample in real time, the block size is 1.
Electrotiger 0:3ee52ce4df19 109 const uint32_t BLOCK_SIZE = 1;
Electrotiger 0:3ee52ce4df19 110
Electrotiger 0:3ee52ce4df19 111 /* FIR Coefficients buffer generated using fir1() MATLAB function. */
Electrotiger 0:3ee52ce4df19 112 const uint32_t NUM_TAPS = 241;
Electrotiger 0:3ee52ce4df19 113 const float32_t firCoeffs32[NUM_TAPS] = {
Electrotiger 0:3ee52ce4df19 114 +0.0000825832f, +0.0000894684f, +0.0000968224f, +0.0001047243f, +0.0001132540f,
Electrotiger 0:3ee52ce4df19 115 +0.0001224923f, +0.0001325206f, +0.0001434207f, +0.0001552749f, +0.0001681654f,
Electrotiger 0:3ee52ce4df19 116 +0.0001821744f, +0.0001973838f, +0.0002138753f, +0.0002317296f, +0.0002510270f,
Electrotiger 0:3ee52ce4df19 117 +0.0002718465f, +0.0002942661f, +0.0003183624f, +0.0003442106f, +0.0003718838f,
Electrotiger 0:3ee52ce4df19 118 +0.0004014538f, +0.0004329897f, +0.0004665588f, +0.0005022258f, +0.0005400529f,
Electrotiger 0:3ee52ce4df19 119 +0.0005800994f, +0.0006224218f, +0.0006670735f, +0.0007141047f, +0.0007635622f,
Electrotiger 0:3ee52ce4df19 120 +0.0008154892f, +0.0008699254f, +0.0009269065f, +0.0009864647f, +0.0010486275f,
Electrotiger 0:3ee52ce4df19 121 +0.0011134190f, +0.0011808585f, +0.0012509612f, +0.0013237379f, +0.0013991947f,
Electrotiger 0:3ee52ce4df19 122 +0.0014773333f, +0.0015581504f, +0.0016416385f, +0.0017277849f, +0.0018165723f,
Electrotiger 0:3ee52ce4df19 123 +0.0019079784f, +0.0020019764f, +0.0020985340f, +0.0021976146f, +0.0022991768f,
Electrotiger 0:3ee52ce4df19 124 +0.0024031731f, +0.0025095530f, +0.0026182598f, +0.0027292324f, +0.0028424053f,
Electrotiger 0:3ee52ce4df19 125 +0.0029577080f, +0.0030750656f, +0.0031943982f, +0.0033156220f, +0.0034386488f,
Electrotiger 0:3ee52ce4df19 126 +0.0035633859f, +0.0036897366f, +0.0038176002f, +0.0039468720f, +0.0040774439f,
Electrotiger 0:3ee52ce4df19 127 +0.0042092041f, +0.0043420363f, +0.0044758227f, +0.0046104412f, +0.0047457665f,
Electrotiger 0:3ee52ce4df19 128 +0.0048816721f, +0.0050180266f, +0.0051546986f, +0.0052915523f, +0.0054284511f,
Electrotiger 0:3ee52ce4df19 129 +0.0055652573f, +0.0057018292f, +0.0058380268f, +0.0059737056f, +0.0061087236f,
Electrotiger 0:3ee52ce4df19 130 +0.0062429351f, +0.0063761952f, +0.0065083597f, +0.0066392822f, +0.0067688185f,
Electrotiger 0:3ee52ce4df19 131 +0.0068968232f, +0.0070231529f, +0.0071476642f, +0.0072702146f, +0.0073906644f,
Electrotiger 0:3ee52ce4df19 132 +0.0075088735f, +0.0076247053f, +0.0077380240f, +0.0078486968f, +0.0079565924f,
Electrotiger 0:3ee52ce4df19 133 +0.0080615841f, +0.0081635462f, +0.0082623558f, +0.0083578955f, +0.0084500499f,
Electrotiger 0:3ee52ce4df19 134 +0.0085387062f, +0.0086237583f, +0.0087051028f, +0.0087826382f, +0.0088562714f,
Electrotiger 0:3ee52ce4df19 135 +0.0089259110f, +0.0089914715f, +0.0090528717f, +0.0091100354f, +0.0091628926f,
Electrotiger 0:3ee52ce4df19 136 +0.0092113772f, +0.0092554288f, +0.0092949932f, +0.0093300194f, +0.0093604643f,
Electrotiger 0:3ee52ce4df19 137 +0.0093862908f, +0.0094074663f, +0.0094239628f, +0.0094357608f, +0.0094428463f,
Electrotiger 0:3ee52ce4df19 138 +0.0094452091f, +0.0094428463f, +0.0094357608f, +0.0094239628f, +0.0094074663f,
Electrotiger 0:3ee52ce4df19 139 +0.0093862908f, +0.0093604643f, +0.0093300194f, +0.0092949932f, +0.0092554288f,
Electrotiger 0:3ee52ce4df19 140 +0.0092113772f, +0.0091628926f, +0.0091100354f, +0.0090528717f, +0.0089914715f,
Electrotiger 0:3ee52ce4df19 141 +0.0089259110f, +0.0088562714f, +0.0087826382f, +0.0087051028f, +0.0086237583f,
Electrotiger 0:3ee52ce4df19 142 +0.0085387062f, +0.0084500499f, +0.0083578955f, +0.0082623558f, +0.0081635462f,
Electrotiger 0:3ee52ce4df19 143 +0.0080615841f, +0.0079565924f, +0.0078486968f, +0.0077380240f, +0.0076247053f,
Electrotiger 0:3ee52ce4df19 144 +0.0075088735f, +0.0073906644f, +0.0072702146f, +0.0071476642f, +0.0070231529f,
Electrotiger 0:3ee52ce4df19 145 +0.0068968232f, +0.0067688185f, +0.0066392822f, +0.0065083597f, +0.0063761952f,
Electrotiger 0:3ee52ce4df19 146 +0.0062429351f, +0.0061087236f, +0.0059737056f, +0.0058380268f, +0.0057018292f,
Electrotiger 0:3ee52ce4df19 147 +0.0055652573f, +0.0054284511f, +0.0052915523f, +0.0051546986f, +0.0050180266f,
Electrotiger 0:3ee52ce4df19 148 +0.0048816721f, +0.0047457665f, +0.0046104412f, +0.0044758227f, +0.0043420363f,
Electrotiger 0:3ee52ce4df19 149 +0.0042092041f, +0.0040774439f, +0.0039468720f, +0.0038176002f, +0.0036897366f,
Electrotiger 0:3ee52ce4df19 150 +0.0035633859f, +0.0034386488f, +0.0033156220f, +0.0031943982f, +0.0030750656f,
Electrotiger 0:3ee52ce4df19 151 +0.0029577080f, +0.0028424053f, +0.0027292324f, +0.0026182598f, +0.0025095530f,
Electrotiger 0:3ee52ce4df19 152 +0.0024031731f, +0.0022991768f, +0.0021976146f, +0.0020985340f, +0.0020019764f,
Electrotiger 0:3ee52ce4df19 153 +0.0019079784f, +0.0018165723f, +0.0017277849f, +0.0016416385f, +0.0015581504f,
Electrotiger 0:3ee52ce4df19 154 +0.0014773333f, +0.0013991947f, +0.0013237379f, +0.0012509612f, +0.0011808585f,
Electrotiger 0:3ee52ce4df19 155 +0.0011134190f, +0.0010486275f, +0.0009864647f, +0.0009269065f, +0.0008699254f,
Electrotiger 0:3ee52ce4df19 156 +0.0008154892f, +0.0007635622f, +0.0007141047f, +0.0006670735f, +0.0006224218f,
Electrotiger 0:3ee52ce4df19 157 +0.0005800994f, +0.0005400529f, +0.0005022258f, +0.0004665588f, +0.0004329897f,
Electrotiger 0:3ee52ce4df19 158 +0.0004014538f, +0.0003718838f, +0.0003442106f, +0.0003183624f, +0.0002942661f,
Electrotiger 0:3ee52ce4df19 159 +0.0002718465f, +0.0002510270f, +0.0002317296f, +0.0002138753f, +0.0001973838f,
Electrotiger 0:3ee52ce4df19 160 +0.0001821744f, +0.0001681654f, +0.0001552749f, +0.0001434207f, +0.0001325206f,
Electrotiger 0:3ee52ce4df19 161 +0.0001224923f, +0.0001132540f, +0.0001047243f, +0.0000968224f, +0.0000894684f,
Electrotiger 0:3ee52ce4df19 162 +0.0000825832f
Electrotiger 0:3ee52ce4df19 163 };
Electrotiger 0:3ee52ce4df19 164 /* Filter Object */
Electrotiger 0:3ee52ce4df19 165 // The filter object used to perform the filtering.
Electrotiger 0:3ee52ce4df19 166 FIR_f32<NUM_TAPS, BLOCK_SIZE> fir(firCoeffs32);
Electrotiger 0:3ee52ce4df19 167 // The filtered z Acceleration
Electrotiger 0:3ee52ce4df19 168 float32_t zAccelFiltered;
Electrotiger 0:3ee52ce4df19 169
Electrotiger 0:3ee52ce4df19 170
Electrotiger 0:3ee52ce4df19 171 // State variables
Electrotiger 0:3ee52ce4df19 172 // System's angle at n = 0
Electrotiger 0:3ee52ce4df19 173 float angle = 0;
Electrotiger 0:3ee52ce4df19 174 // System's angle at n = -1
Electrotiger 0:3ee52ce4df19 175 float angle_n1 = 0;
Electrotiger 0:3ee52ce4df19 176 // System's angle at n = -2
Electrotiger 0:3ee52ce4df19 177 float angle_n2 = 0;
Electrotiger 0:3ee52ce4df19 178 // Previous system output
Electrotiger 0:3ee52ce4df19 179 float output_n1 = 0;
Electrotiger 0:3ee52ce4df19 180 // Position of the system.
Electrotiger 0:3ee52ce4df19 181 float position = 0;
Electrotiger 0:3ee52ce4df19 182 while (true) {
Electrotiger 0:3ee52ce4df19 183 // Read the state from the accelerometer:
Electrotiger 0:3ee52ce4df19 184 accelerometer.readAccelerometer(&xAccel, &yAccel, &zAccel);
Electrotiger 0:3ee52ce4df19 185 // Filter the accelerometer state:
Electrotiger 0:3ee52ce4df19 186 fir.process(&zAccel, &zAccelFiltered);
Electrotiger 0:3ee52ce4df19 187 // Read the state from the quadrature encoder:
Electrotiger 0:3ee52ce4df19 188 leftWheel = leftQuadEnc.getRevs();
Electrotiger 0:3ee52ce4df19 189 rightWheel = rightQuadEnc.getRevs();
Electrotiger 0:3ee52ce4df19 190
Electrotiger 0:3ee52ce4df19 191 // Acquire the system's angle:
Electrotiger 0:3ee52ce4df19 192 // angle = atan2f(yAccel, zAccel);
Electrotiger 0:3ee52ce4df19 193 arm_sqrt_f32(2*zAccelFiltered, &angle);
Electrotiger 0:3ee52ce4df19 194 if(yAccel < 0) {
Electrotiger 0:3ee52ce4df19 195 angle = -angle;
Electrotiger 0:3ee52ce4df19 196 }
Electrotiger 0:3ee52ce4df19 197 // Acquire the system's position:
Electrotiger 0:3ee52ce4df19 198 position = (leftWheel + rightWheel) / 2.;
Electrotiger 0:3ee52ce4df19 199
Electrotiger 0:3ee52ce4df19 200 // Control Law to regulate system:
Electrotiger 0:3ee52ce4df19 201 float output = a0 * angle + a1 * angle_n1 + a2 * angle_n2 + o1 * output_n1;
Electrotiger 0:3ee52ce4df19 202 // Write the output to the motor.
Electrotiger 0:3ee52ce4df19 203 leftMotor.write(output);
Electrotiger 0:3ee52ce4df19 204 rightMotor.write(output);
Electrotiger 0:3ee52ce4df19 205
Electrotiger 0:3ee52ce4df19 206 // Update the state variables
Electrotiger 0:3ee52ce4df19 207 angle_n1 = angle;
Electrotiger 0:3ee52ce4df19 208 angle_n2 = angle_n1;
Electrotiger 0:3ee52ce4df19 209 output_n1 = output;
Electrotiger 0:3ee52ce4df19 210
Electrotiger 0:3ee52ce4df19 211 fixedRefresh.refresh_us(controlSampleRateUS);
Electrotiger 0:3ee52ce4df19 212 }
Electrotiger 0:3ee52ce4df19 213 }
Electrotiger 0:3ee52ce4df19 214
Electrotiger 0:3ee52ce4df19 215 /** @brief Main thread:
Electrotiger 0:3ee52ce4df19 216 * This thread performs any additional initialization necessary for the declared objects.
Electrotiger 0:3ee52ce4df19 217 * Then, it starts the threads.
Electrotiger 0:3ee52ce4df19 218 */
Electrotiger 0:3ee52ce4df19 219 int main() {
Electrotiger 0:3ee52ce4df19 220 /* Initialize objects */
Electrotiger 0:3ee52ce4df19 221 // Set the offset value for the accelerometer.
Electrotiger 0:3ee52ce4df19 222 accelerometer.setOffset(0.012, -0.032, 0.032);
Electrotiger 0:3ee52ce4df19 223 // Start the global timer.
Electrotiger 0:3ee52ce4df19 224 timer.start();
Electrotiger 0:3ee52ce4df19 225
Electrotiger 0:3ee52ce4df19 226 /* Initialize threads. */
Electrotiger 0:3ee52ce4df19 227 // Thread priority is set as normal.
Electrotiger 0:3ee52ce4df19 228 Thread thread(bluetooth_thread, NULL, osPriorityNormal);
Electrotiger 0:3ee52ce4df19 229 // Thread priority is set as above normal: If the control_thread and bluetooth_thread
Electrotiger 0:3ee52ce4df19 230 // ever want to happen at the same time, control_thread wins.
Electrotiger 0:3ee52ce4df19 231 Thread thread2(control_thread, NULL, osPriorityAboveNormal);
Electrotiger 0:3ee52ce4df19 232 while (true) {
Electrotiger 0:3ee52ce4df19 233 // Main thread does nothing else, so we tell it to wait forever.
Electrotiger 0:3ee52ce4df19 234 Thread::wait(osWaitForever);
Electrotiger 0:3ee52ce4df19 235 }
Electrotiger 0:3ee52ce4df19 236 }