My final year project
Dependencies: BSP_DISCO_F746NG F746_GUI LCD_DISCO_F746NG SDFileSystem TS_DISCO_F746NG ResistiveTouchController Map CYS8218Controller MedianFilter
main.cpp
- Committer:
- Kerneels Bezuidenhout
- Date:
- 2016-10-19
- Revision:
- 54:75dc5c3b7cfc
- Parent:
- 50:0e0cf6b350f7
- Child:
- 55:d1a00f344351
File content as of revision 54:75dc5c3b7cfc:
#include "mbed.h" #include "Screens.hpp" #include "ResistiveTouchController.hpp" #include "CYS8218Controller.hpp" #include "Map.hpp" #include "MedianFilter.hpp" //--- Function Prototypes void GUIThread(); void Init(); void ZeroPlate(); void Manual(); void Automatic(); void Calibrate(); //----------------------- //-- GUI Thread Variables and Objects int currentScreen = 0; int nextScreen = 0; Thread guiThread; Screen *activeScreen = NULL; //---------------------------------- //-- X Axis variables float zeroAlpha = 0; float alpha = 0; float xSP = 0; float x = 0; float xError = 0; float xPrevError = 0; float xSummation = 0; MedianFilter xFilter(7); //-- Y Axis Variables float zeroBeta = 0; float beta = 0; float ySP = 0; float y = 0; float yError = 0; float yPrevError = 0; float ySummation = 0; MedianFilter yFilter(7); //-- Controller Variables float P = 0; float I = 0; float D = 0; float Ts = 0.004; Timer cycleTimer; //----------------------- //-- Sensors and Actuator objects ResistiveTouchController plate(A0,A1,A3,A4,A5); Map xMapper(0,1024, -170,170); Map yMapper(0,1024, -136,136); CYS8218Controller xMotor(D8); CYS8218Controller yMotor(D9); float tCal[3][2]; int calPoint = 0; bool calStarted = false; Timer calTimer; float calHeldTime; bool calFinished = false; int measuredTs = 0; bool started = false; int ballOnPlate = 0; bool saved = false; // TODO Delete these variables Serial pc(USBTX,USBRX); int main() { Init(); while(1) { switch (currentScreen) { case Screen::ZERO_PLATE_SCREEN : ZeroPlate(); break; case Screen::MANUAL_CONTROL_SCREEN : Manual(); break; case Screen::AUTOMATIC_CONTROL_SCREEN : Automatic(); break; case Screen::CALIBRATE_SCREEN : Calibrate(); break; case Screen::MANUAL_SETPOINT_SCREEN : Automatic(); break; default : xMotor.Off(); yMotor.Off(); break; } } } void GUIThread() { while(1) { if( nextScreen != currentScreen ) { started = false; Thread::wait(100); if( activeScreen != NULL) delete activeScreen; switch (nextScreen) { case Screen::MAIN_MENU_SCREEN : activeScreen = new MainMenuScreen(&nextScreen); break; case Screen::MAIN_SETTINGS_SCREEN : activeScreen = new MainSettingsScreen(&nextScreen); break; case Screen::ZERO_PLATE_SCREEN : activeScreen = new ZeroPlateScreen(&nextScreen, &zeroAlpha, &zeroBeta, &saved); break; case Screen::MANUAL_CONTROL_SCREEN : activeScreen = new ManualControlScreen(&nextScreen, &alpha, &beta); break; case Screen::AUTOMATIC_CONTROL_SCREEN : activeScreen = new AutomaticControlScreen(&nextScreen, &x, &y, &measuredTs, &started, &ballOnPlate, &xSP, &ySP); break; case Screen::AUTOMATIC_MORE_SCREEN : activeScreen = new AutomaticMoreScreen(&nextScreen); break; case Screen::MANUAL_SETPOINT_SCREEN : activeScreen = new ManualSetpointScreen(&nextScreen, &xSP, &ySP, &ballOnPlate, &started); break; case Screen::AUTOMATIC_SETTINGS_SCREEN : activeScreen = new AutomaticSettingsScreen(&nextScreen, &P, &I, &D, &Ts); break; case Screen::CHANGE_P_SCREEN : activeScreen = new ChangeValScreen(&nextScreen, currentScreen, &P, "Change P"); break; case Screen::CHANGE_I_SCREEN : activeScreen = new ChangeValScreen(&nextScreen, currentScreen, &I, "Change I"); break; case Screen::CHANGE_D_SCREEN : activeScreen = new ChangeValScreen(&nextScreen, currentScreen, &D, "Change D"); break; case Screen::CHANGE_TS_SCREEN : activeScreen = new ChangeValWholeScreen(&nextScreen, currentScreen, &Ts, "Change Ts", false, 4); break; case Screen::CALIBRATE_SCREEN : activeScreen = new CalibrateScreen(&nextScreen, &ballOnPlate, &calStarted, &calFinished, &calPoint, &calHeldTime); break; case Screen::SQUARE_SCREEN : activeScreen = new SquareScreen(&nextScreen,*xSP, &ySP, &ballOnPlate, &started ); break; } activeScreen->Draw(); currentScreen = nextScreen; } activeScreen->Process(); Thread::wait(10); } } void Init() { // TODO Read values from sd card ImageFromSD splash("/sd/Splash.txt"); pc.baud(57600); xMotor.SetZero(28.0); yMotor.SetZero(25.0); nextScreen = Screen::MAIN_MENU_SCREEN; Thread::wait(5000); guiThread.start(GUIThread); } void ZeroPlate() { xMotor.Set(zeroAlpha); yMotor.Set(-zeroBeta); if( saved) { // TODO Save zero position on sd card xMotor.SetZero(zeroAlpha); yMotor.SetZero(-zeroBeta); zeroAlpha = 0; zeroBeta = 0; nextScreen = Screen::MAIN_SETTINGS_SCREEN; saved = false; } Thread::wait(20); } void Manual() { xMotor.Set(-alpha); yMotor.Set(beta); Thread::wait(10); } void Automatic() { //TODO Add automatic control mode logic ballOnPlate = plate.TouchDetected(); if( ballOnPlate && started ) { cycleTimer.start(); plate.Measure(); //x = xMapper.Calculate(floor(plate.X())); //y = yMapper.Calculate(floor(plate.Y())); x = xMapper.Calculate(floor(xFilter.Process(plate.X()))); y = yMapper.Calculate(floor(yFilter.Process(plate.Y()))); xError = xSP - x; yError = ySP - y; xSummation += xError; ySummation += yError; alpha = P*xError + D*(xError-xPrevError) + I*xSummation; beta = P*yError + D*(yError-yPrevError) + I*ySummation; xPrevError = xError; yPrevError = yError; alpha = ( alpha > 10 ? 10 : (alpha < -10 ? -10 : alpha)); beta = ( beta > 10 ? 10 : (beta < -10 ? -10 : beta) ); xMotor.Set(alpha); yMotor.Set(-beta); cycleTimer.stop(); measuredTs = cycleTimer.read_ms(); cycleTimer.reset(); } else { xMotor.Off(); yMotor.Off(); } } void Calibrate() { ballOnPlate = plate.TouchDetected(); if( calStarted ) { if( calPoint < 3 ) { if( ballOnPlate ) { calTimer.start(); calHeldTime = calTimer.read(); if( calHeldTime >= 5 ) { while( plate.TouchDetected() ); //TODO Test calibration function calPoint++; calTimer.stop(); calTimer.reset(); } } else { calTimer.stop(); calTimer.reset(); } } else { calFinished = true; calPoint = 0; calStarted = false; } } }