File content as of revision 1:2ff0b2e16716:

/**
 * @file main.cpp
 * @date June 24th, 2016
 * @author Weimen Li
 * @mainpage MAE433_Library_Tester
 * This testing program tests all hardware that should be installed on your robotic vehicle to
 * ensure that the hardware was assembled correctly. It runs the following programs:
 */

#include "mbed.h"
#include "rtos.h"
#include "FXOS8700CQ.hpp"
#include "HBridge.hpp"
#include "HC06Bluetooth.hpp"
#include "QuadEnc.hpp"
#include "dsp.h"

/* Constants */
/// The CPR of the encoder.
const float encoderCPR = 100.98f * 12.0f;
/// The transmission period of the bluetooth module.
const int32_t SerialTransmitPeriodMS = 20;
/// The sampling period of the control system.
const int32_t controlSampleRateMS = 1;
/// The diameter of the wheels in inches
const float wheelDiameter = 1.9f;

/* Forward declaration of functions for the Bluetooth module */
// Forward declaration of lineCallback:
void lineCallback(const char* receivedString);
// Forward declaration of charCallback:
void charCallback(char receivedChar);

/* Objects that encapsulate the hardware: */
FXOS8700CQ accelerometer;
HBridge leftMotor(D9, D7);
HBridge rightMotor(D10, D8);
HC06Bluetooth bluetooth(PTD3, PTD2, lineCallback, charCallback);
QuadEnc leftQuadEnc(PTB0, PTB1, encoderCPR);
QuadEnc rightQuadEnc(PTC2, PTC1, encoderCPR);
RawSerial pc(USBTX, USBRX);

/* Set up Bluetooth module to echo received characters. */
/* Callback function when a line has been received from Bluetooth.
 * This function prints "Line received.\n". */
void lineCallback(const char* receivedString) {
    bluetooth.print("Line received.\n\r");
}

/* Callback function when a char has been received from Bluetooth.
 * This function echos any character received.
 */
void charCallback(char receivedChar) {
    bluetooth.print(receivedChar);
}


/* Threads */

Thread *PCSerialThreadObj;
void PCSerialThread(const void* arguments) {
    float xAccel;
    float yAccel;
    float zAccel;
    pc.baud(115200);
    Timer t;
    t.start();
    while(true) {
        float leftEncoderReading = leftQuadEnc.getRevs();
        float rightEncoderReading = rightQuadEnc.getRevs();
        float leftMotorOutput = leftMotor.read();
        float rightMotorOutput = rightMotor.read();
        accelerometer.readAccelerometer(&xAccel, &yAccel, &zAccel);
        pc.printf("Time: %f;"
                "Left Encoder: %f, Right Encoder: %f; "
                "leftMotor: %f, rightMotor: %f; "
                "xAccel: %f, yAccel %f, zAccel: %f;\n\r",
                t.read(),
                leftEncoderReading, rightEncoderReading,
                leftMotorOutput, rightMotorOutput,
                xAccel, yAccel, zAccel
                );
        Thread::wait(SerialTransmitPeriodMS);
    }
}

Thread *MotorControlThreadObj;
void MotorControlThread(const void* arguments) {
    Timer t;
    t.start();
    while(true) {
        float output = 1 * arm_sin_f32(2*PI*t.read() / 8);
        leftMotor.write(output);
        rightMotor.write(output);
        Thread::wait(controlSampleRateMS);
    }
}

int main() {
    PCSerialThreadObj = new Thread(PCSerialThread);
    MotorControlThreadObj = new Thread(MotorControlThread);
    Thread::wait(osWaitForever);
}