Weimen Li / MAE433_Library_Tester

Program to test hardware for Princeton's MAE 433

Dependencies:   MAE433_Library

main.cpp@1:2ff0b2e16716, 2016-06-26 (annotated)

Committer:
Electrotiger
Date:
Sun Jun 26 19:35:52 2016 +0000
Revision:
1:2ff0b2e16716
Parent:
0:ba8d8b12f8cc 
Some Commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Electrotiger 0:ba8d8b12f8cc 1 /**
Electrotiger 0:ba8d8b12f8cc 2 * @file main.cpp
Electrotiger 0:ba8d8b12f8cc 3 * @date June 24th, 2016
Electrotiger 0:ba8d8b12f8cc 4 * @author Weimen Li
Electrotiger 0:ba8d8b12f8cc 5 * @mainpage MAE433_Library_Tester
Electrotiger 0:ba8d8b12f8cc 6 * This testing program tests all hardware that should be installed on your robotic vehicle to
Electrotiger 0:ba8d8b12f8cc 7 * ensure that the hardware was assembled correctly. It runs the following programs:
Electrotiger 0:ba8d8b12f8cc 8 */
Electrotiger 0:ba8d8b12f8cc 9
Electrotiger 0:ba8d8b12f8cc 10 #include "mbed.h"
Electrotiger 0:ba8d8b12f8cc 11 #include "rtos.h"
Electrotiger 0:ba8d8b12f8cc 12 #include "FXOS8700CQ.hpp"
Electrotiger 0:ba8d8b12f8cc 13 #include "HBridge.hpp"
Electrotiger 0:ba8d8b12f8cc 14 #include "HC06Bluetooth.hpp"
Electrotiger 0:ba8d8b12f8cc 15 #include "QuadEnc.hpp"
Electrotiger 0:ba8d8b12f8cc 16 #include "dsp.h"
Electrotiger 0:ba8d8b12f8cc 17
Electrotiger 0:ba8d8b12f8cc 18 /* Constants */
Electrotiger 0:ba8d8b12f8cc 19 /// The CPR of the encoder.
Electrotiger 0:ba8d8b12f8cc 20 const float encoderCPR = 100.98f * 12.0f;
Electrotiger 0:ba8d8b12f8cc 21 /// The transmission period of the bluetooth module.
Electrotiger 0:ba8d8b12f8cc 22 const int32_t SerialTransmitPeriodMS = 20;
Electrotiger 0:ba8d8b12f8cc 23 /// The sampling period of the control system.
Electrotiger 0:ba8d8b12f8cc 24 const int32_t controlSampleRateMS = 1;
Electrotiger 0:ba8d8b12f8cc 25 /// The diameter of the wheels in inches
Electrotiger 0:ba8d8b12f8cc 26 const float wheelDiameter = 1.9f;
Electrotiger 0:ba8d8b12f8cc 27
Electrotiger 0:ba8d8b12f8cc 28 /* Forward declaration of functions for the Bluetooth module */
Electrotiger 0:ba8d8b12f8cc 29 // Forward declaration of lineCallback:
Electrotiger 0:ba8d8b12f8cc 30 void lineCallback(const char* receivedString);
Electrotiger 0:ba8d8b12f8cc 31 // Forward declaration of charCallback:
Electrotiger 0:ba8d8b12f8cc 32 void charCallback(char receivedChar);
Electrotiger 0:ba8d8b12f8cc 33
Electrotiger 0:ba8d8b12f8cc 34 /* Objects that encapsulate the hardware: */
Electrotiger 0:ba8d8b12f8cc 35 FXOS8700CQ accelerometer;
Electrotiger 0:ba8d8b12f8cc 36 HBridge leftMotor(D9, D7);
Electrotiger 0:ba8d8b12f8cc 37 HBridge rightMotor(D10, D8);
Electrotiger 0:ba8d8b12f8cc 38 HC06Bluetooth bluetooth(PTD3, PTD2, lineCallback, charCallback);
Electrotiger 0:ba8d8b12f8cc 39 QuadEnc leftQuadEnc(PTB0, PTB1, encoderCPR);
Electrotiger 0:ba8d8b12f8cc 40 QuadEnc rightQuadEnc(PTC2, PTC1, encoderCPR);
Electrotiger 0:ba8d8b12f8cc 41 RawSerial pc(USBTX, USBRX);
Electrotiger 0:ba8d8b12f8cc 42
Electrotiger 0:ba8d8b12f8cc 43 /* Set up Bluetooth module to echo received characters. */
Electrotiger 0:ba8d8b12f8cc 44 /* Callback function when a line has been received from Bluetooth.
Electrotiger 0:ba8d8b12f8cc 45 * This function prints "Line received.\n". */
Electrotiger 0:ba8d8b12f8cc 46 void lineCallback(const char* receivedString) {
Electrotiger 0:ba8d8b12f8cc 47 bluetooth.print("Line received.\n\r");
Electrotiger 0:ba8d8b12f8cc 48 }
Electrotiger 0:ba8d8b12f8cc 49
Electrotiger 0:ba8d8b12f8cc 50 /* Callback function when a char has been received from Bluetooth.
Electrotiger 0:ba8d8b12f8cc 51 * This function echos any character received.
Electrotiger 0:ba8d8b12f8cc 52 */
Electrotiger 0:ba8d8b12f8cc 53 void charCallback(char receivedChar) {
Electrotiger 0:ba8d8b12f8cc 54 bluetooth.print(receivedChar);
Electrotiger 0:ba8d8b12f8cc 55 }
Electrotiger 0:ba8d8b12f8cc 56
Electrotiger 0:ba8d8b12f8cc 57
Electrotiger 0:ba8d8b12f8cc 58 /* Threads */
Electrotiger 0:ba8d8b12f8cc 59
Electrotiger 0:ba8d8b12f8cc 60 Thread *PCSerialThreadObj;
Electrotiger 0:ba8d8b12f8cc 61 void PCSerialThread(const void* arguments) {
Electrotiger 0:ba8d8b12f8cc 62 float xAccel;
Electrotiger 0:ba8d8b12f8cc 63 float yAccel;
Electrotiger 0:ba8d8b12f8cc 64 float zAccel;
Electrotiger 0:ba8d8b12f8cc 65 pc.baud(115200);
Electrotiger 0:ba8d8b12f8cc 66 Timer t;
Electrotiger 0:ba8d8b12f8cc 67 t.start();
Electrotiger 0:ba8d8b12f8cc 68 while(true) {
Electrotiger 0:ba8d8b12f8cc 69 float leftEncoderReading = leftQuadEnc.getRevs();
Electrotiger 0:ba8d8b12f8cc 70 float rightEncoderReading = rightQuadEnc.getRevs();
Electrotiger 0:ba8d8b12f8cc 71 float leftMotorOutput = leftMotor.read();
Electrotiger 0:ba8d8b12f8cc 72 float rightMotorOutput = rightMotor.read();
Electrotiger 0:ba8d8b12f8cc 73 accelerometer.readAccelerometer(&xAccel, &yAccel, &zAccel);
Electrotiger 0:ba8d8b12f8cc 74 pc.printf("Time: %f;"
Electrotiger 0:ba8d8b12f8cc 75 "Left Encoder: %f, Right Encoder: %f; "
Electrotiger 0:ba8d8b12f8cc 76 "leftMotor: %f, rightMotor: %f; "
Electrotiger 0:ba8d8b12f8cc 77 "xAccel: %f, yAccel %f, zAccel: %f;\n\r",
Electrotiger 0:ba8d8b12f8cc 78 t.read(),
Electrotiger 0:ba8d8b12f8cc 79 leftEncoderReading, rightEncoderReading,
Electrotiger 0:ba8d8b12f8cc 80 leftMotorOutput, rightMotorOutput,
Electrotiger 0:ba8d8b12f8cc 81 xAccel, yAccel, zAccel
Electrotiger 0:ba8d8b12f8cc 82 );
Electrotiger 0:ba8d8b12f8cc 83 Thread::wait(SerialTransmitPeriodMS);
Electrotiger 0:ba8d8b12f8cc 84 }
Electrotiger 0:ba8d8b12f8cc 85 }
Electrotiger 0:ba8d8b12f8cc 86
Electrotiger 0:ba8d8b12f8cc 87 Thread *MotorControlThreadObj;
Electrotiger 0:ba8d8b12f8cc 88 void MotorControlThread(const void* arguments) {
Electrotiger 0:ba8d8b12f8cc 89 Timer t;
Electrotiger 0:ba8d8b12f8cc 90 t.start();
Electrotiger 0:ba8d8b12f8cc 91 while(true) {
Electrotiger 1:2ff0b2e16716 92 float output = 1 * arm_sin_f32(2*PI*t.read() / 8);
Electrotiger 0:ba8d8b12f8cc 93 leftMotor.write(output);
Electrotiger 0:ba8d8b12f8cc 94 rightMotor.write(output);
Electrotiger 0:ba8d8b12f8cc 95 Thread::wait(controlSampleRateMS);
Electrotiger 0:ba8d8b12f8cc 96 }
Electrotiger 0:ba8d8b12f8cc 97 }
Electrotiger 0:ba8d8b12f8cc 98
Electrotiger 0:ba8d8b12f8cc 99 int main() {
Electrotiger 0:ba8d8b12f8cc 100 PCSerialThreadObj = new Thread(PCSerialThread);
Electrotiger 0:ba8d8b12f8cc 101 MotorControlThreadObj = new Thread(MotorControlThread);
Electrotiger 0:ba8d8b12f8cc 102 Thread::wait(osWaitForever);
Electrotiger 0:ba8d8b12f8cc 103 }
Electrotiger 0:ba8d8b12f8cc 104