Self-Balancing Robot Package for MAE 433.
main.cpp@1:4afc0dd95be9, 2016-06-30 (annotated)
- Committer:
- Electrotiger
- Date:
- Thu Jun 30 22:33:59 2016 +0000
- Revision:
- 1:4afc0dd95be9
- Parent:
- 0:3ee52ce4df19
- Child:
- 2:098bb1322f0e
Used another thread to control the acceleration of the motors. Still Buggy.
Who changed what in which revision?
User | Revision | Line number | New 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 | 1:4afc0dd95be9 | 47 | /* The acceleration commanded by control_thread and used as input for motorControl_thread() */ |
Electrotiger | 1:4afc0dd95be9 | 48 | float setAccel; |
Electrotiger | 1:4afc0dd95be9 | 49 | Mutex setAccelMutex; |
Electrotiger | 1:4afc0dd95be9 | 50 | |
Electrotiger | 0:3ee52ce4df19 | 51 | /* Threads */ |
Electrotiger | 0:3ee52ce4df19 | 52 | /** @brief Bluetooth Transmission: |
Electrotiger | 0:3ee52ce4df19 | 53 | * This thread reads the quadrature encoder and |
Electrotiger | 0:3ee52ce4df19 | 54 | * output data and transmits it as comma-separated values over Bluetooth. |
Electrotiger | 0:3ee52ce4df19 | 55 | */ |
Electrotiger | 0:3ee52ce4df19 | 56 | void bluetooth_thread(void const *argument) { |
Electrotiger | 0:3ee52ce4df19 | 57 | // Timer to control how long to wait. |
Electrotiger | 1:4afc0dd95be9 | 58 | FixedRefresh refresh; |
Electrotiger | 0:3ee52ce4df19 | 59 | // 64-byte buffer is used to store the data. May be expanded as needed. |
Electrotiger | 0:3ee52ce4df19 | 60 | char buffer[128]; |
Electrotiger | 0:3ee52ce4df19 | 61 | // The accelerometer readings. |
Electrotiger | 0:3ee52ce4df19 | 62 | float xAccel; |
Electrotiger | 0:3ee52ce4df19 | 63 | float yAccel; |
Electrotiger | 0:3ee52ce4df19 | 64 | float zAccel; |
Electrotiger | 0:3ee52ce4df19 | 65 | |
Electrotiger | 0:3ee52ce4df19 | 66 | |
Electrotiger | 0:3ee52ce4df19 | 67 | while(true) { |
Electrotiger | 0:3ee52ce4df19 | 68 | accelerometer.readAccelerometer(&xAccel, &yAccel, &zAccel); |
Electrotiger | 0:3ee52ce4df19 | 69 | // Place the CSV information in the buffer: |
Electrotiger | 1:4afc0dd95be9 | 70 | sprintf(buffer, "%f,%f,%f,%f,%f,%f\n\r", timer.read(), xAccel, yAccel, zAccel, leftMotor.read(), rightMotor.read()); |
Electrotiger | 0:3ee52ce4df19 | 71 | // Transmit the information. |
Electrotiger | 0:3ee52ce4df19 | 72 | bluetooth.print(buffer); |
Electrotiger | 1:4afc0dd95be9 | 73 | // Refresh at a specified interval. |
Electrotiger | 1:4afc0dd95be9 | 74 | refresh.refresh_us(BTTransmitPeriodMS * 1000); |
Electrotiger | 0:3ee52ce4df19 | 75 | } |
Electrotiger | 0:3ee52ce4df19 | 76 | |
Electrotiger | 0:3ee52ce4df19 | 77 | } |
Electrotiger | 0:3ee52ce4df19 | 78 | |
Electrotiger | 0:3ee52ce4df19 | 79 | /** @brief Control Thread: |
Electrotiger | 0:3ee52ce4df19 | 80 | * This thread reads the current quadrature counts and actuates |
Electrotiger | 0:3ee52ce4df19 | 81 | * the motor through the H-Bridge. |
Electrotiger | 0:3ee52ce4df19 | 82 | */ |
Electrotiger | 0:3ee52ce4df19 | 83 | |
Electrotiger | 0:3ee52ce4df19 | 84 | void control_thread(void const *argument) { |
Electrotiger | 1:4afc0dd95be9 | 85 | // FixedRefresh object to wait at fixed time |
Electrotiger | 0:3ee52ce4df19 | 86 | FixedRefresh fixedRefresh; |
Electrotiger | 0:3ee52ce4df19 | 87 | // The accelerometer readings. |
Electrotiger | 0:3ee52ce4df19 | 88 | float xAccel; |
Electrotiger | 0:3ee52ce4df19 | 89 | float yAccel; |
Electrotiger | 0:3ee52ce4df19 | 90 | float zAccel; |
Electrotiger | 0:3ee52ce4df19 | 91 | |
Electrotiger | 0:3ee52ce4df19 | 92 | /* Feedback constants: */ |
Electrotiger | 0:3ee52ce4df19 | 93 | // Accelerometer feedback at n = 0; |
Electrotiger | 0:3ee52ce4df19 | 94 | const float a0 = 0.15; |
Electrotiger | 0:3ee52ce4df19 | 95 | // Accelerometer feedback at n = -1; |
Electrotiger | 0:3ee52ce4df19 | 96 | const float a1 = 0; |
Electrotiger | 0:3ee52ce4df19 | 97 | // Accelerometer feedback at n = -2; |
Electrotiger | 0:3ee52ce4df19 | 98 | const float a2 = 0; |
Electrotiger | 0:3ee52ce4df19 | 99 | |
Electrotiger | 1:4afc0dd95be9 | 100 | /* State Variables: */ |
Electrotiger | 1:4afc0dd95be9 | 101 | // System's angle at n = 0 |
Electrotiger | 1:4afc0dd95be9 | 102 | float angle = 0; |
Electrotiger | 1:4afc0dd95be9 | 103 | // System's angle at n = -1 |
Electrotiger | 1:4afc0dd95be9 | 104 | float angle_n1 = 0; |
Electrotiger | 1:4afc0dd95be9 | 105 | // System's angle at n = -2 |
Electrotiger | 1:4afc0dd95be9 | 106 | float angle_n2 = 0; |
Electrotiger | 1:4afc0dd95be9 | 107 | |
Electrotiger | 0:3ee52ce4df19 | 108 | /* Filtering Constants: */ |
Electrotiger | 0:3ee52ce4df19 | 109 | // The block size is the number of samples processed by each tick of the digital filter. |
Electrotiger | 0:3ee52ce4df19 | 110 | // Since we work sample-by-sample in real time, the block size is 1. |
Electrotiger | 0:3ee52ce4df19 | 111 | const uint32_t BLOCK_SIZE = 1; |
Electrotiger | 0:3ee52ce4df19 | 112 | |
Electrotiger | 0:3ee52ce4df19 | 113 | /* FIR Coefficients buffer generated using fir1() MATLAB function. */ |
Electrotiger | 0:3ee52ce4df19 | 114 | const uint32_t NUM_TAPS = 241; |
Electrotiger | 0:3ee52ce4df19 | 115 | const float32_t firCoeffs32[NUM_TAPS] = { |
Electrotiger | 0:3ee52ce4df19 | 116 | +0.0000825832f, +0.0000894684f, +0.0000968224f, +0.0001047243f, +0.0001132540f, |
Electrotiger | 0:3ee52ce4df19 | 117 | +0.0001224923f, +0.0001325206f, +0.0001434207f, +0.0001552749f, +0.0001681654f, |
Electrotiger | 0:3ee52ce4df19 | 118 | +0.0001821744f, +0.0001973838f, +0.0002138753f, +0.0002317296f, +0.0002510270f, |
Electrotiger | 0:3ee52ce4df19 | 119 | +0.0002718465f, +0.0002942661f, +0.0003183624f, +0.0003442106f, +0.0003718838f, |
Electrotiger | 0:3ee52ce4df19 | 120 | +0.0004014538f, +0.0004329897f, +0.0004665588f, +0.0005022258f, +0.0005400529f, |
Electrotiger | 0:3ee52ce4df19 | 121 | +0.0005800994f, +0.0006224218f, +0.0006670735f, +0.0007141047f, +0.0007635622f, |
Electrotiger | 0:3ee52ce4df19 | 122 | +0.0008154892f, +0.0008699254f, +0.0009269065f, +0.0009864647f, +0.0010486275f, |
Electrotiger | 0:3ee52ce4df19 | 123 | +0.0011134190f, +0.0011808585f, +0.0012509612f, +0.0013237379f, +0.0013991947f, |
Electrotiger | 0:3ee52ce4df19 | 124 | +0.0014773333f, +0.0015581504f, +0.0016416385f, +0.0017277849f, +0.0018165723f, |
Electrotiger | 0:3ee52ce4df19 | 125 | +0.0019079784f, +0.0020019764f, +0.0020985340f, +0.0021976146f, +0.0022991768f, |
Electrotiger | 0:3ee52ce4df19 | 126 | +0.0024031731f, +0.0025095530f, +0.0026182598f, +0.0027292324f, +0.0028424053f, |
Electrotiger | 0:3ee52ce4df19 | 127 | +0.0029577080f, +0.0030750656f, +0.0031943982f, +0.0033156220f, +0.0034386488f, |
Electrotiger | 0:3ee52ce4df19 | 128 | +0.0035633859f, +0.0036897366f, +0.0038176002f, +0.0039468720f, +0.0040774439f, |
Electrotiger | 0:3ee52ce4df19 | 129 | +0.0042092041f, +0.0043420363f, +0.0044758227f, +0.0046104412f, +0.0047457665f, |
Electrotiger | 0:3ee52ce4df19 | 130 | +0.0048816721f, +0.0050180266f, +0.0051546986f, +0.0052915523f, +0.0054284511f, |
Electrotiger | 0:3ee52ce4df19 | 131 | +0.0055652573f, +0.0057018292f, +0.0058380268f, +0.0059737056f, +0.0061087236f, |
Electrotiger | 0:3ee52ce4df19 | 132 | +0.0062429351f, +0.0063761952f, +0.0065083597f, +0.0066392822f, +0.0067688185f, |
Electrotiger | 0:3ee52ce4df19 | 133 | +0.0068968232f, +0.0070231529f, +0.0071476642f, +0.0072702146f, +0.0073906644f, |
Electrotiger | 0:3ee52ce4df19 | 134 | +0.0075088735f, +0.0076247053f, +0.0077380240f, +0.0078486968f, +0.0079565924f, |
Electrotiger | 0:3ee52ce4df19 | 135 | +0.0080615841f, +0.0081635462f, +0.0082623558f, +0.0083578955f, +0.0084500499f, |
Electrotiger | 0:3ee52ce4df19 | 136 | +0.0085387062f, +0.0086237583f, +0.0087051028f, +0.0087826382f, +0.0088562714f, |
Electrotiger | 0:3ee52ce4df19 | 137 | +0.0089259110f, +0.0089914715f, +0.0090528717f, +0.0091100354f, +0.0091628926f, |
Electrotiger | 0:3ee52ce4df19 | 138 | +0.0092113772f, +0.0092554288f, +0.0092949932f, +0.0093300194f, +0.0093604643f, |
Electrotiger | 0:3ee52ce4df19 | 139 | +0.0093862908f, +0.0094074663f, +0.0094239628f, +0.0094357608f, +0.0094428463f, |
Electrotiger | 0:3ee52ce4df19 | 140 | +0.0094452091f, +0.0094428463f, +0.0094357608f, +0.0094239628f, +0.0094074663f, |
Electrotiger | 0:3ee52ce4df19 | 141 | +0.0093862908f, +0.0093604643f, +0.0093300194f, +0.0092949932f, +0.0092554288f, |
Electrotiger | 0:3ee52ce4df19 | 142 | +0.0092113772f, +0.0091628926f, +0.0091100354f, +0.0090528717f, +0.0089914715f, |
Electrotiger | 0:3ee52ce4df19 | 143 | +0.0089259110f, +0.0088562714f, +0.0087826382f, +0.0087051028f, +0.0086237583f, |
Electrotiger | 0:3ee52ce4df19 | 144 | +0.0085387062f, +0.0084500499f, +0.0083578955f, +0.0082623558f, +0.0081635462f, |
Electrotiger | 0:3ee52ce4df19 | 145 | +0.0080615841f, +0.0079565924f, +0.0078486968f, +0.0077380240f, +0.0076247053f, |
Electrotiger | 0:3ee52ce4df19 | 146 | +0.0075088735f, +0.0073906644f, +0.0072702146f, +0.0071476642f, +0.0070231529f, |
Electrotiger | 0:3ee52ce4df19 | 147 | +0.0068968232f, +0.0067688185f, +0.0066392822f, +0.0065083597f, +0.0063761952f, |
Electrotiger | 0:3ee52ce4df19 | 148 | +0.0062429351f, +0.0061087236f, +0.0059737056f, +0.0058380268f, +0.0057018292f, |
Electrotiger | 0:3ee52ce4df19 | 149 | +0.0055652573f, +0.0054284511f, +0.0052915523f, +0.0051546986f, +0.0050180266f, |
Electrotiger | 0:3ee52ce4df19 | 150 | +0.0048816721f, +0.0047457665f, +0.0046104412f, +0.0044758227f, +0.0043420363f, |
Electrotiger | 0:3ee52ce4df19 | 151 | +0.0042092041f, +0.0040774439f, +0.0039468720f, +0.0038176002f, +0.0036897366f, |
Electrotiger | 0:3ee52ce4df19 | 152 | +0.0035633859f, +0.0034386488f, +0.0033156220f, +0.0031943982f, +0.0030750656f, |
Electrotiger | 0:3ee52ce4df19 | 153 | +0.0029577080f, +0.0028424053f, +0.0027292324f, +0.0026182598f, +0.0025095530f, |
Electrotiger | 0:3ee52ce4df19 | 154 | +0.0024031731f, +0.0022991768f, +0.0021976146f, +0.0020985340f, +0.0020019764f, |
Electrotiger | 0:3ee52ce4df19 | 155 | +0.0019079784f, +0.0018165723f, +0.0017277849f, +0.0016416385f, +0.0015581504f, |
Electrotiger | 0:3ee52ce4df19 | 156 | +0.0014773333f, +0.0013991947f, +0.0013237379f, +0.0012509612f, +0.0011808585f, |
Electrotiger | 0:3ee52ce4df19 | 157 | +0.0011134190f, +0.0010486275f, +0.0009864647f, +0.0009269065f, +0.0008699254f, |
Electrotiger | 0:3ee52ce4df19 | 158 | +0.0008154892f, +0.0007635622f, +0.0007141047f, +0.0006670735f, +0.0006224218f, |
Electrotiger | 0:3ee52ce4df19 | 159 | +0.0005800994f, +0.0005400529f, +0.0005022258f, +0.0004665588f, +0.0004329897f, |
Electrotiger | 0:3ee52ce4df19 | 160 | +0.0004014538f, +0.0003718838f, +0.0003442106f, +0.0003183624f, +0.0002942661f, |
Electrotiger | 0:3ee52ce4df19 | 161 | +0.0002718465f, +0.0002510270f, +0.0002317296f, +0.0002138753f, +0.0001973838f, |
Electrotiger | 0:3ee52ce4df19 | 162 | +0.0001821744f, +0.0001681654f, +0.0001552749f, +0.0001434207f, +0.0001325206f, |
Electrotiger | 0:3ee52ce4df19 | 163 | +0.0001224923f, +0.0001132540f, +0.0001047243f, +0.0000968224f, +0.0000894684f, |
Electrotiger | 0:3ee52ce4df19 | 164 | +0.0000825832f |
Electrotiger | 0:3ee52ce4df19 | 165 | }; |
Electrotiger | 1:4afc0dd95be9 | 166 | |
Electrotiger | 0:3ee52ce4df19 | 167 | /* Filter Object */ |
Electrotiger | 0:3ee52ce4df19 | 168 | // The filter object used to perform the filtering. |
Electrotiger | 0:3ee52ce4df19 | 169 | FIR_f32<NUM_TAPS, BLOCK_SIZE> fir(firCoeffs32); |
Electrotiger | 0:3ee52ce4df19 | 170 | // The filtered z Acceleration |
Electrotiger | 0:3ee52ce4df19 | 171 | float32_t zAccelFiltered; |
Electrotiger | 0:3ee52ce4df19 | 172 | |
Electrotiger | 0:3ee52ce4df19 | 173 | while (true) { |
Electrotiger | 0:3ee52ce4df19 | 174 | // Read the state from the accelerometer: |
Electrotiger | 0:3ee52ce4df19 | 175 | accelerometer.readAccelerometer(&xAccel, &yAccel, &zAccel); |
Electrotiger | 0:3ee52ce4df19 | 176 | // Filter the accelerometer state: |
Electrotiger | 0:3ee52ce4df19 | 177 | fir.process(&zAccel, &zAccelFiltered); |
Electrotiger | 0:3ee52ce4df19 | 178 | |
Electrotiger | 0:3ee52ce4df19 | 179 | // Acquire the system's angle: |
Electrotiger | 0:3ee52ce4df19 | 180 | // angle = atan2f(yAccel, zAccel); |
Electrotiger | 0:3ee52ce4df19 | 181 | arm_sqrt_f32(2*zAccelFiltered, &angle); |
Electrotiger | 0:3ee52ce4df19 | 182 | if(yAccel < 0) { |
Electrotiger | 0:3ee52ce4df19 | 183 | angle = -angle; |
Electrotiger | 0:3ee52ce4df19 | 184 | } |
Electrotiger | 0:3ee52ce4df19 | 185 | |
Electrotiger | 0:3ee52ce4df19 | 186 | // Control Law to regulate system: |
Electrotiger | 1:4afc0dd95be9 | 187 | float output = a0 * angle + a1 * angle_n1 + a2 * angle_n2; |
Electrotiger | 1:4afc0dd95be9 | 188 | // Set the output for motorControl_thread to handle. |
Electrotiger | 1:4afc0dd95be9 | 189 | setAccelMutex.lock(); |
Electrotiger | 1:4afc0dd95be9 | 190 | setAccel = output; |
Electrotiger | 1:4afc0dd95be9 | 191 | setAccelMutex.unlock(); |
Electrotiger | 0:3ee52ce4df19 | 192 | |
Electrotiger | 0:3ee52ce4df19 | 193 | // Update the state variables |
Electrotiger | 0:3ee52ce4df19 | 194 | angle_n1 = angle; |
Electrotiger | 0:3ee52ce4df19 | 195 | angle_n2 = angle_n1; |
Electrotiger | 1:4afc0dd95be9 | 196 | |
Electrotiger | 1:4afc0dd95be9 | 197 | // Thread::wait(100); |
Electrotiger | 1:4afc0dd95be9 | 198 | fixedRefresh.refresh_us(controlSampleRateUS); |
Electrotiger | 1:4afc0dd95be9 | 199 | } |
Electrotiger | 1:4afc0dd95be9 | 200 | } |
Electrotiger | 1:4afc0dd95be9 | 201 | |
Electrotiger | 1:4afc0dd95be9 | 202 | |
Electrotiger | 1:4afc0dd95be9 | 203 | /** |
Electrotiger | 1:4afc0dd95be9 | 204 | * @brief This thread controls the motors to have a specified acceleration, with the value |
Electrotiger | 1:4afc0dd95be9 | 205 | * coming from control_thread. |
Electrotiger | 1:4afc0dd95be9 | 206 | */ |
Electrotiger | 1:4afc0dd95be9 | 207 | void motorControl_thread(void const* argument) { |
Electrotiger | 1:4afc0dd95be9 | 208 | // Control variable constants |
Electrotiger | 1:4afc0dd95be9 | 209 | const float kConst = 1; |
Electrotiger | 1:4afc0dd95be9 | 210 | const float iConst = 1; |
Electrotiger | 1:4afc0dd95be9 | 211 | |
Electrotiger | 1:4afc0dd95be9 | 212 | // Left Wheel State Variables |
Electrotiger | 1:4afc0dd95be9 | 213 | float leftPosition = 0; |
Electrotiger | 1:4afc0dd95be9 | 214 | float prevLeftPosition = 0; |
Electrotiger | 1:4afc0dd95be9 | 215 | float leftPositionDeriv = 0; |
Electrotiger | 1:4afc0dd95be9 | 216 | float prevLeftPositionDeriv = 0; |
Electrotiger | 1:4afc0dd95be9 | 217 | float leftPositionDDeriv = 0; |
Electrotiger | 1:4afc0dd95be9 | 218 | float leftErrorInt = 0; |
Electrotiger | 1:4afc0dd95be9 | 219 | |
Electrotiger | 1:4afc0dd95be9 | 220 | // Right Wheel State Variables |
Electrotiger | 1:4afc0dd95be9 | 221 | float rightPosition = 0; |
Electrotiger | 1:4afc0dd95be9 | 222 | float prevRightPosition = 0; |
Electrotiger | 1:4afc0dd95be9 | 223 | float rightPositionDeriv = 0; |
Electrotiger | 1:4afc0dd95be9 | 224 | float prevRightPositionDeriv = 0; |
Electrotiger | 1:4afc0dd95be9 | 225 | float rightPositionDDeriv = 0; |
Electrotiger | 1:4afc0dd95be9 | 226 | float rightErrorInt = 0; |
Electrotiger | 1:4afc0dd95be9 | 227 | |
Electrotiger | 1:4afc0dd95be9 | 228 | FixedRefresh refresh; |
Electrotiger | 0:3ee52ce4df19 | 229 | |
Electrotiger | 1:4afc0dd95be9 | 230 | while(true) { |
Electrotiger | 1:4afc0dd95be9 | 231 | // Calculate new values for the state variables. Note that the steps are performed in reverse. |
Electrotiger | 1:4afc0dd95be9 | 232 | leftPositionDDeriv = leftPositionDeriv - prevLeftPositionDeriv; |
Electrotiger | 1:4afc0dd95be9 | 233 | rightPositionDDeriv = rightPositionDeriv - prevRightPositionDeriv; |
Electrotiger | 1:4afc0dd95be9 | 234 | leftPositionDeriv = leftPosition - prevLeftPosition; |
Electrotiger | 1:4afc0dd95be9 | 235 | rightPositionDeriv = rightPosition - prevRightPosition; |
Electrotiger | 1:4afc0dd95be9 | 236 | leftPosition = leftQuadEnc.getRevs(); |
Electrotiger | 1:4afc0dd95be9 | 237 | rightPosition = rightQuadEnc.getRevs(); |
Electrotiger | 1:4afc0dd95be9 | 238 | |
Electrotiger | 1:4afc0dd95be9 | 239 | // Get the commanded acceleration: |
Electrotiger | 1:4afc0dd95be9 | 240 | setAccelMutex.lock(); |
Electrotiger | 1:4afc0dd95be9 | 241 | float localSetAccel = setAccel; |
Electrotiger | 1:4afc0dd95be9 | 242 | setAccelMutex.unlock(); |
Electrotiger | 1:4afc0dd95be9 | 243 | |
Electrotiger | 1:4afc0dd95be9 | 244 | // Calculate the error: |
Electrotiger | 1:4afc0dd95be9 | 245 | |
Electrotiger | 1:4afc0dd95be9 | 246 | float leftError = localSetAccel - leftPositionDDeriv; |
Electrotiger | 1:4afc0dd95be9 | 247 | float rightError = localSetAccel - rightPositionDDeriv; |
Electrotiger | 1:4afc0dd95be9 | 248 | |
Electrotiger | 1:4afc0dd95be9 | 249 | // Calculate the output: |
Electrotiger | 1:4afc0dd95be9 | 250 | float leftOutput = kConst * leftError + iConst * leftErrorInt; |
Electrotiger | 1:4afc0dd95be9 | 251 | float rightOutput = kConst * leftError + iConst * rightErrorInt; |
Electrotiger | 1:4afc0dd95be9 | 252 | leftErrorInt += leftError; |
Electrotiger | 1:4afc0dd95be9 | 253 | rightErrorInt += rightError; |
Electrotiger | 1:4afc0dd95be9 | 254 | // Write the output: |
Electrotiger | 1:4afc0dd95be9 | 255 | leftMotor.write(leftOutput); |
Electrotiger | 1:4afc0dd95be9 | 256 | rightMotor.write(rightOutput); |
Electrotiger | 1:4afc0dd95be9 | 257 | // Update the previous state variable values: |
Electrotiger | 1:4afc0dd95be9 | 258 | prevLeftPosition = leftPosition; |
Electrotiger | 1:4afc0dd95be9 | 259 | prevRightPosition = rightPosition; |
Electrotiger | 1:4afc0dd95be9 | 260 | prevLeftPositionDeriv = leftPositionDeriv; |
Electrotiger | 1:4afc0dd95be9 | 261 | prevRightPositionDeriv = rightPositionDeriv; |
Electrotiger | 1:4afc0dd95be9 | 262 | refresh.refresh_us(1250); |
Electrotiger | 0:3ee52ce4df19 | 263 | } |
Electrotiger | 0:3ee52ce4df19 | 264 | } |
Electrotiger | 0:3ee52ce4df19 | 265 | |
Electrotiger | 0:3ee52ce4df19 | 266 | /** @brief Main thread: |
Electrotiger | 0:3ee52ce4df19 | 267 | * This thread performs any additional initialization necessary for the declared objects. |
Electrotiger | 0:3ee52ce4df19 | 268 | * Then, it starts the threads. |
Electrotiger | 0:3ee52ce4df19 | 269 | */ |
Electrotiger | 0:3ee52ce4df19 | 270 | int main() { |
Electrotiger | 0:3ee52ce4df19 | 271 | /* Initialize objects */ |
Electrotiger | 0:3ee52ce4df19 | 272 | // Set the offset value for the accelerometer. |
Electrotiger | 0:3ee52ce4df19 | 273 | accelerometer.setOffset(0.012, -0.032, 0.032); |
Electrotiger | 0:3ee52ce4df19 | 274 | // Start the global timer. |
Electrotiger | 0:3ee52ce4df19 | 275 | timer.start(); |
Electrotiger | 0:3ee52ce4df19 | 276 | |
Electrotiger | 0:3ee52ce4df19 | 277 | /* Initialize threads. */ |
Electrotiger | 0:3ee52ce4df19 | 278 | // Thread priority is set as normal. |
Electrotiger | 0:3ee52ce4df19 | 279 | Thread thread(bluetooth_thread, NULL, osPriorityNormal); |
Electrotiger | 0:3ee52ce4df19 | 280 | // Thread priority is set as above normal: If the control_thread and bluetooth_thread |
Electrotiger | 0:3ee52ce4df19 | 281 | // ever want to happen at the same time, control_thread wins. |
Electrotiger | 1:4afc0dd95be9 | 282 | //Thread thread2(control_thread, NULL, osPriorityAboveNormal); |
Electrotiger | 1:4afc0dd95be9 | 283 | Thread thread3(motorControl_thread, NULL, osPriorityAboveNormal); |
Electrotiger | 0:3ee52ce4df19 | 284 | while (true) { |
Electrotiger | 0:3ee52ce4df19 | 285 | // Main thread does nothing else, so we tell it to wait forever. |
Electrotiger | 0:3ee52ce4df19 | 286 | Thread::wait(osWaitForever); |
Electrotiger | 0:3ee52ce4df19 | 287 | } |
Electrotiger | 0:3ee52ce4df19 | 288 | } |