Self-Balancing Robot Package for MAE 433.
main.cpp
- Committer:
- Electrotiger
- Date:
- 2016-06-30
- Revision:
- 0:3ee52ce4df19
- Child:
- 1:4afc0dd95be9
File content as of revision 0:3ee52ce4df19:
/** * @file main.cpp * @date June 9th, 2015 * @author Weimen Li * @mainpage RobotBalancer * This program is a demo lab for MAE 433. It controls a robot to balance it. * @see main.cpp */ /* Inclusions */ #include "mbed.h" #include "rtos.h" #include "HBridge.hpp" #include "QuadEnc.hpp" #include "HC06Bluetooth.hpp" #include "FXOS8700CQ.hpp" #include "dsp.h" #include "FixedRefresh.hpp" /* Constants */ /// The CPR of the encoder. const float encoderCPR = 100.98f * 12.0f; /// The transmission period of the bluetooth module. const uint32_t BTTransmitPeriodMS = 1; /// The sampling period of the control system. const uint32_t controlSampleRateUS = 1250; /// The diameter of the wheels in inches const float wheelDiameter = 1.9f; /* Declaring objects */ /// The quad encoder of the left motor. QuadEnc leftQuadEnc(PTB0, PTB1, encoderCPR); /// The quad encoder of the right motor. QuadEnc rightQuadEnc(PTC2, PTC1, encoderCPR); /// The H-Bridge driving the left motor. HBridge leftMotor(D9, D7); /// The H-Bridge driving the right motor. HBridge rightMotor(D10, D8); /// The accelerometer on the board FXOS8700CQ accelerometer(PTB3, PTB2, PTD0, PTD1); /// The Bluetooth link used to transmit data. HC06Bluetooth bluetooth(PTD3, PTD2); /// Timer to transmit timing information. Timer timer; /* Threads */ /** @brief Bluetooth Transmission: * This thread reads the quadrature encoder and * output data and transmits it as comma-separated values over Bluetooth. */ void bluetooth_thread(void const *argument) { // Timer to control how long to wait. Timer waitTimer; // 64-byte buffer is used to store the data. May be expanded as needed. char buffer[128]; // The accelerometer readings. float xAccel; float yAccel; 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()); // 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); } } /** @brief Control Thread: * This thread reads the current quadrature counts and actuates * the motor through the H-Bridge. */ void control_thread(void const *argument) { // 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; const float a0 = 0.15; // Accelerometer feedback at n = -1; const float a1 = 0; // Accelerometer feedback at n = -2; const float a2 = 0; // Output feedback const float o1 = 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. const uint32_t BLOCK_SIZE = 1; /* FIR Coefficients buffer generated using fir1() MATLAB function. */ const uint32_t NUM_TAPS = 241; 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 }; /* 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); arm_sqrt_f32(2*zAccelFiltered, &angle); 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); // Update the state variables angle_n1 = angle; angle_n2 = angle_n1; output_n1 = output; fixedRefresh.refresh_us(controlSampleRateUS); } } /** @brief Main thread: * This thread performs any additional initialization necessary for the declared objects. * Then, it starts the threads. */ int main() { /* Initialize objects */ // Set the offset value for the accelerometer. accelerometer.setOffset(0.012, -0.032, 0.032); // Start the global timer. timer.start(); /* Initialize threads. */ // Thread priority is set as normal. 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); while (true) { // Main thread does nothing else, so we tell it to wait forever. Thread::wait(osWaitForever); } }