File content as of revision 2:098bb1322f0e:

/**
 * @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 sampling period of the motor control subsystem
const uint32_t motorControlSampleRateUS = 333;
/// 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;

/* The acceleration commanded by control_thread and used as input for motorControl_thread() */
float setAccel;
Mutex setAccelMutex;

/* Threads */
/** @brief Bluetooth Transmission:
 * 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;
    // 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;
    

    while(true) {
        accelerometer.readAccelerometer(&xAccel, &yAccel, &zAccel);
        // Place the CSV information in the buffer:
        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.
        refresh.refresh_us(BTTransmitPeriodMS * 1000);
    }

}

/** @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;

    /* 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;
    
    /* 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;

    /* 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 = 121;
    const float32_t firCoeffs32[NUM_TAPS] = { 
    -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 */
    // The filter object used to perform the filtering.
    FIR_f32<NUM_TAPS, BLOCK_SIZE> fir(firCoeffs32);
    // The filtered z Acceleration
    float32_t zAccelFiltered;
    
    while (true) {
        // Read the state from the accelerometer:
        accelerometer.readAccelerometer(&xAccel, &yAccel, &zAccel);
        fir.process(&zAccel, &zAccelFiltered);


        // Acquire the system's angle:
        // angle = atan2f(yAccel, zAccel);
        arm_sqrt_f32(2*zAccelFiltered, &angle);
        if(yAccel < 0) {
            angle = -angle;
        }

        // 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;
        setAccelMutex.unlock();

        // Update the state variables
        angle_n1 = angle;
        angle_n2 = angle_n1;

        // Thread::wait(100);
        fixedRefresh.refresh_us(controlSampleRateUS);
    }
}


/**
 * @brief This thread controls the motors to have a specified acceleration, with the value
 * coming from control_thread.
 */
void motorControl_thread(void const* argument) {
    // Control variable constants
    const float kConst = 0.5;
    const float iConst = 4 * motorControlSampleRateUS / 1000000.;

    // Left Wheel State Variables
    float leftPosition = 0;
    float prevLeftPosition = 0;
    float leftPositionDeriv = 0;
    float prevLeftPositionDeriv = 0;
    float leftPositionDDeriv = 0;
    float leftErrorInt = 0;

    // Right Wheel State Variables
    float rightPosition = 0;
    float prevRightPosition = 0;
    float rightPositionDeriv = 0;
    float prevRightPositionDeriv = 0;
    float rightPositionDDeriv = 0;
    float rightErrorInt = 0;

    FixedRefresh refresh;

    while(true) {
        // Calculate new values for the state variables. Note that the steps are performed in reverse.
        leftPositionDDeriv = leftPositionDeriv - prevLeftPositionDeriv;
        rightPositionDDeriv = rightPositionDeriv - prevRightPositionDeriv;
        leftPositionDeriv = leftPosition - prevLeftPosition;
        rightPositionDeriv = rightPosition - prevRightPosition;
        leftPosition = leftQuadEnc.getRevs();
        rightPosition = rightQuadEnc.getRevs();

        // Get the commanded acceleration:
        setAccelMutex.lock();
        float localSetAccel = setAccel;
        setAccelMutex.unlock();

        // Calculate the error:

        float leftError = localSetAccel - leftPositionDDeriv;
        float rightError = localSetAccel - rightPositionDDeriv;

        // Calculate the output:
        float leftOutput = kConst * leftError + iConst * leftErrorInt;
        float rightOutput = kConst * leftError + iConst * rightErrorInt;
        leftErrorInt += leftError;
        rightErrorInt += rightError;
        // Write the output:
        leftMotor.write(leftOutput);
        rightMotor.write(rightOutput);
        // Update the previous state variable values:
        prevLeftPosition = leftPosition;
        prevRightPosition = rightPosition;
        prevLeftPositionDeriv = leftPositionDeriv;
        prevRightPositionDeriv = rightPositionDeriv;
        refresh.refresh_us(motorControlSampleRateUS);
    }
}

/** @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);
    Thread thread3(motorControl_thread, NULL, osPriorityAboveNormal);
    while (true) {
        // Main thread does nothing else, so we tell it to wait forever.
        Thread::wait(osWaitForever);
    }
}