Cornelius Bezuidenhout / Mbed OS Heiko

My final year project

Dependencies:   BSP_DISCO_F746NG F746_GUI LCD_DISCO_F746NG SDFileSystem TS_DISCO_F746NG ResistiveTouchController Map CYS8218Controller MedianFilter

main.cpp@46:9f66021615f9, 2016-10-09 (annotated)

Committer:
Kerneels Bezuidenhout
Date:
Sun Oct 09 00:58:52 2016 +0200
Revision:
46:9f66021615f9
Parent:
45:e5b175383624
Child:
47:c04b0d51e1e3
Testing motors

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Kerneels Bezuidenhout 1:43fb2d03c36f 1 #include "mbed.h"
Kerneels Bezuidenhout 1:43fb2d03c36f 2 #include "Screens.hpp"
Kerneels Bezuidenhout 41:204e2788fb46 3 #include "ResistiveTouchController.hpp"
Kerneels Bezuidenhout 46:9f66021615f9 4 #include "CYS8218Controller.hpp"
Kerneels Bezuidenhout 44:560ed042317c 5 #include "Map.hpp"
Kerneels Bezuidenhout 46:9f66021615f9 6 #include "MedianFilter.hpp"
Kerneels Bezuidenhout 1:43fb2d03c36f 7
Kerneels Bezuidenhout 41:204e2788fb46 8 //--- Function Prototypes
Kerneels Bezuidenhout 1:43fb2d03c36f 9 void GUIThread();
Kerneels Bezuidenhout 1:43fb2d03c36f 10 void Init();
Kerneels Bezuidenhout 1:43fb2d03c36f 11
Kerneels Bezuidenhout 7:1a6e18b818a5 12 void ZeroPlate();
Kerneels Bezuidenhout 12:650ae2f4c7df 13 void Manual();
Kerneels Bezuidenhout 13:21dc070a47a9 14 void Automatic();
Kerneels Bezuidenhout 34:0b308a399f72 15 void Calibrate();
Kerneels Bezuidenhout 41:204e2788fb46 16 //-----------------------
Kerneels Bezuidenhout 7:1a6e18b818a5 17
Kerneels Bezuidenhout 41:204e2788fb46 18 //-- GUI Thread Variables and Objects
Kerneels Bezuidenhout 1:43fb2d03c36f 19 int currentScreen = 0;
Kerneels Bezuidenhout 1:43fb2d03c36f 20 int nextScreen = 0;
Kerneels Bezuidenhout 1:43fb2d03c36f 21 Thread guiThread;
Kerneels Bezuidenhout 1:43fb2d03c36f 22 Screen *activeScreen = NULL;
Kerneels Bezuidenhout 41:204e2788fb46 23 //----------------------------------
Kerneels Bezuidenhout 1:43fb2d03c36f 24
Kerneels Bezuidenhout 41:204e2788fb46 25 //-- X Axis variables
Kerneels Bezuidenhout 12:650ae2f4c7df 26 float zeroAlpha = 0;
Kerneels Bezuidenhout 12:650ae2f4c7df 27 float alpha = 0;
Generic 17:3b6d66253c97 28 float xSP = -20;
Kerneels Bezuidenhout 13:21dc070a47a9 29 float x = 0;
Kerneels Bezuidenhout 46:9f66021615f9 30 MedianFilter xFilter(7);
Kerneels Bezuidenhout 7:1a6e18b818a5 31
Kerneels Bezuidenhout 41:204e2788fb46 32 //-- Y Axis Variables
Kerneels Bezuidenhout 12:650ae2f4c7df 33 float zeroBeta = 0;
Kerneels Bezuidenhout 12:650ae2f4c7df 34 float beta = 0;
Generic 17:3b6d66253c97 35 float ySP = 50;
Kerneels Bezuidenhout 13:21dc070a47a9 36 float y = 0;
Kerneels Bezuidenhout 46:9f66021615f9 37 MedianFilter yFilter(7);
Kerneels Bezuidenhout 13:21dc070a47a9 38
Kerneels Bezuidenhout 41:204e2788fb46 39
Kerneels Bezuidenhout 41:204e2788fb46 40 //-- Controller Variables
Kerneels Bezuidenhout 22:dd9ff475b34c 41 float P = 0;
Kerneels Bezuidenhout 22:dd9ff475b34c 42 float I = 0;
Kerneels Bezuidenhout 22:dd9ff475b34c 43 float D = 0;
Kerneels Bezuidenhout 30:e8eb64ad8526 44 float Ts = 4;
Kerneels Bezuidenhout 41:204e2788fb46 45 //-----------------------
Kerneels Bezuidenhout 41:204e2788fb46 46
Kerneels Bezuidenhout 41:204e2788fb46 47
Kerneels Bezuidenhout 41:204e2788fb46 48 //-- Sensors and Actuator objects
Kerneels Bezuidenhout 41:204e2788fb46 49 ResistiveTouchController plate(A0,A1,A3,A4,A5);
Kerneels Bezuidenhout 44:560ed042317c 50 Map plateMapper(0,1024, -170,170);
Kerneels Bezuidenhout 46:9f66021615f9 51 CYS8218Controller xMotor(D9);
Kerneels Bezuidenhout 46:9f66021615f9 52 CYS8218Controller yMotor(D8);
Kerneels Bezuidenhout 46:9f66021615f9 53
Kerneels Bezuidenhout 22:dd9ff475b34c 54
Kerneels Bezuidenhout 32:d490f8ca6d2a 55 float tCal[3][2];
Kerneels Bezuidenhout 35:6baa558d3b86 56 int calPoint = 0;
Kerneels Bezuidenhout 35:6baa558d3b86 57 bool calStarted = false;
Kerneels Bezuidenhout 35:6baa558d3b86 58 Timer calTimer;
Kerneels Bezuidenhout 38:f1c571d22919 59 float calHeldTime;
Kerneels Bezuidenhout 37:93c7563f2078 60 bool calFinished = false;
Kerneels Bezuidenhout 32:d490f8ca6d2a 61
Generic 23:bab197887d5e 62 int measuredTs = 0;
Kerneels Bezuidenhout 13:21dc070a47a9 63
Generic 17:3b6d66253c97 64 bool started = true;
Kerneels Bezuidenhout 13:21dc070a47a9 65
Generic 17:3b6d66253c97 66 int ballOnPlate = 1;
Kerneels Bezuidenhout 7:1a6e18b818a5 67
Kerneels Bezuidenhout 12:650ae2f4c7df 68 bool saved = false;
Kerneels Bezuidenhout 7:1a6e18b818a5 69
Generic 17:3b6d66253c97 70 // TODO Delete these variables
Generic 17:3b6d66253c97 71 float t = 0;
Generic 9:355a59fd962b 72 Serial pc(USBTX,USBRX);
Kerneels Bezuidenhout 19:ef78ac1a98f8 73 DigitalIn test(D3);
Generic 9:355a59fd962b 74
Kerneels Bezuidenhout 1:43fb2d03c36f 75 int main()
Kerneels Bezuidenhout 1:43fb2d03c36f 76 {
Kerneels Bezuidenhout 1:43fb2d03c36f 77 Init();
Kerneels Bezuidenhout 1:43fb2d03c36f 78
Kerneels Bezuidenhout 1:43fb2d03c36f 79 while(1)
Kerneels Bezuidenhout 1:43fb2d03c36f 80 {
Kerneels Bezuidenhout 7:1a6e18b818a5 81 switch (currentScreen)
Kerneels Bezuidenhout 7:1a6e18b818a5 82 {
Kerneels Bezuidenhout 7:1a6e18b818a5 83 case Screen::ZERO_PLATE_SCREEN :
Kerneels Bezuidenhout 7:1a6e18b818a5 84 ZeroPlate();
Kerneels Bezuidenhout 7:1a6e18b818a5 85 break;
Kerneels Bezuidenhout 12:650ae2f4c7df 86 case Screen::MANUAL_CONTROL_SCREEN :
Kerneels Bezuidenhout 12:650ae2f4c7df 87 Manual();
Kerneels Bezuidenhout 12:650ae2f4c7df 88 break;
Kerneels Bezuidenhout 13:21dc070a47a9 89 case Screen::AUTOMATIC_CONTROL_SCREEN :
Kerneels Bezuidenhout 13:21dc070a47a9 90 Automatic();
Kerneels Bezuidenhout 13:21dc070a47a9 91 break;
Kerneels Bezuidenhout 34:0b308a399f72 92 case Screen::CALIBRATE_SCREEN :
Kerneels Bezuidenhout 34:0b308a399f72 93 Calibrate();
Kerneels Bezuidenhout 34:0b308a399f72 94 break;
Kerneels Bezuidenhout 7:1a6e18b818a5 95 }
Kerneels Bezuidenhout 1:43fb2d03c36f 96 }
Kerneels Bezuidenhout 1:43fb2d03c36f 97 }
Kerneels Bezuidenhout 1:43fb2d03c36f 98
Kerneels Bezuidenhout 1:43fb2d03c36f 99 void GUIThread()
Kerneels Bezuidenhout 1:43fb2d03c36f 100 {
Kerneels Bezuidenhout 1:43fb2d03c36f 101 while(1)
Kerneels Bezuidenhout 1:43fb2d03c36f 102 {
Kerneels Bezuidenhout 1:43fb2d03c36f 103 if( nextScreen != currentScreen )
Kerneels Bezuidenhout 1:43fb2d03c36f 104 {
Generic 39:7bb23ad4bf87 105 Thread::wait(100);
Kerneels Bezuidenhout 41:204e2788fb46 106
Kerneels Bezuidenhout 1:43fb2d03c36f 107 if( activeScreen != NULL)
Kerneels Bezuidenhout 1:43fb2d03c36f 108 delete activeScreen;
Kerneels Bezuidenhout 1:43fb2d03c36f 109
Kerneels Bezuidenhout 1:43fb2d03c36f 110 switch (nextScreen)
Kerneels Bezuidenhout 1:43fb2d03c36f 111 {
Kerneels Bezuidenhout 7:1a6e18b818a5 112 case Screen::MAIN_MENU_SCREEN :
Kerneels Bezuidenhout 1:43fb2d03c36f 113 activeScreen = new MainMenuScreen(&nextScreen);
Kerneels Bezuidenhout 1:43fb2d03c36f 114 break;
Kerneels Bezuidenhout 7:1a6e18b818a5 115 case Screen::MAIN_SETTINGS_SCREEN :
Kerneels Bezuidenhout 5:383f1108187d 116 activeScreen = new MainSettingsScreen(&nextScreen);
Kerneels Bezuidenhout 5:383f1108187d 117 break;
Kerneels Bezuidenhout 7:1a6e18b818a5 118 case Screen::ZERO_PLATE_SCREEN :
Kerneels Bezuidenhout 7:1a6e18b818a5 119 activeScreen = new ZeroPlateScreen(&nextScreen, &zeroAlpha, &zeroBeta, &saved);
Kerneels Bezuidenhout 7:1a6e18b818a5 120 break;
Generic 15:bc63b55a426d 121 case Screen::MANUAL_CONTROL_SCREEN :
Kerneels Bezuidenhout 12:650ae2f4c7df 122 activeScreen = new ManualControlScreen(&nextScreen, &alpha, &beta);
Kerneels Bezuidenhout 12:650ae2f4c7df 123 break;
Kerneels Bezuidenhout 13:21dc070a47a9 124 case Screen::AUTOMATIC_CONTROL_SCREEN :
Kerneels Bezuidenhout 22:dd9ff475b34c 125 activeScreen = new AutomaticControlScreen(&nextScreen, &x, &y, &measuredTs, &started, &ballOnPlate, &xSP, &ySP);
Kerneels Bezuidenhout 13:21dc070a47a9 126 break;
Kerneels Bezuidenhout 18:1b9579ac9f41 127 case Screen::AUTOMATIC_MORE_SCREEN :
Kerneels Bezuidenhout 18:1b9579ac9f41 128 activeScreen = new AutomaticMoreScreen(&nextScreen);
Kerneels Bezuidenhout 18:1b9579ac9f41 129 break;
Kerneels Bezuidenhout 20:70c5b1e499f0 130 case Screen::MANUAL_SETPOINT_SCREEN :
Generic 23:bab197887d5e 131 activeScreen = new ManualSetpointScreen(&nextScreen, &xSP, &ySP, &ballOnPlate, &started);
Kerneels Bezuidenhout 20:70c5b1e499f0 132 break;
Generic 23:bab197887d5e 133 case Screen::AUTOMATIC_SETTINGS_SCREEN :
Kerneels Bezuidenhout 22:dd9ff475b34c 134 activeScreen = new AutomaticSettingsScreen(&nextScreen, &P, &I, &D, &Ts);
Kerneels Bezuidenhout 26:f57db8d6d79e 135 break;
Kerneels Bezuidenhout 25:bfff3d661f92 136 case Screen::CHANGE_P_SCREEN :
Kerneels Bezuidenhout 25:bfff3d661f92 137 activeScreen = new ChangeValScreen(&nextScreen, currentScreen, &P, "Change P");
Kerneels Bezuidenhout 25:bfff3d661f92 138 break;
Kerneels Bezuidenhout 26:f57db8d6d79e 139 case Screen::CHANGE_I_SCREEN :
Kerneels Bezuidenhout 26:f57db8d6d79e 140 activeScreen = new ChangeValScreen(&nextScreen, currentScreen, &I, "Change I");
Kerneels Bezuidenhout 26:f57db8d6d79e 141 break;
Kerneels Bezuidenhout 26:f57db8d6d79e 142 case Screen::CHANGE_D_SCREEN :
Kerneels Bezuidenhout 26:f57db8d6d79e 143 activeScreen = new ChangeValScreen(&nextScreen, currentScreen, &D, "Change D");
Kerneels Bezuidenhout 26:f57db8d6d79e 144 break;
Kerneels Bezuidenhout 26:f57db8d6d79e 145 case Screen::CHANGE_TS_SCREEN :
Kerneels Bezuidenhout 31:5ece593c49a7 146 activeScreen = new ChangeValWholeScreen(&nextScreen, currentScreen, &Ts, "Change Ts", false, 4);
Kerneels Bezuidenhout 26:f57db8d6d79e 147 break;
Kerneels Bezuidenhout 32:d490f8ca6d2a 148 case Screen::CALIBRATE_SCREEN :
Kerneels Bezuidenhout 38:f1c571d22919 149 activeScreen = new CalibrateScreen(&nextScreen, &ballOnPlate, &calStarted, &calFinished, &calPoint, &calHeldTime);
Kerneels Bezuidenhout 32:d490f8ca6d2a 150 break;
Kerneels Bezuidenhout 1:43fb2d03c36f 151 }
Kerneels Bezuidenhout 1:43fb2d03c36f 152 activeScreen->Draw();
Generic 6:f717ce705c5d 153 currentScreen = nextScreen;
Kerneels Bezuidenhout 1:43fb2d03c36f 154 }
Kerneels Bezuidenhout 1:43fb2d03c36f 155
Kerneels Bezuidenhout 1:43fb2d03c36f 156 activeScreen->Process();
Kerneels Bezuidenhout 1:43fb2d03c36f 157
Generic 17:3b6d66253c97 158 Thread::wait(10);
Kerneels Bezuidenhout 1:43fb2d03c36f 159 }
Kerneels Bezuidenhout 1:43fb2d03c36f 160 }
Kerneels Bezuidenhout 1:43fb2d03c36f 161
Kerneels Bezuidenhout 1:43fb2d03c36f 162 void Init()
Kerneels Bezuidenhout 1:43fb2d03c36f 163 {
Generic 39:7bb23ad4bf87 164 ImageFromSD splash("/sd/Splash.txt");
Generic 9:355a59fd962b 165 pc.baud(57600);
Kerneels Bezuidenhout 46:9f66021615f9 166 xMotor.SetZero(28.0);
Kerneels Bezuidenhout 46:9f66021615f9 167 yMotor.SetZero(25.0);
Generic 2:5fded6444350 168 nextScreen = Screen::MAIN_MENU_SCREEN;
Kerneels Bezuidenhout 38:f1c571d22919 169
Kerneels Bezuidenhout 38:f1c571d22919 170 Thread::wait(5000);
Kerneels Bezuidenhout 38:f1c571d22919 171
Kerneels Bezuidenhout 1:43fb2d03c36f 172 guiThread.start(GUIThread);
Kerneels Bezuidenhout 1:43fb2d03c36f 173 }
Kerneels Bezuidenhout 7:1a6e18b818a5 174
Kerneels Bezuidenhout 7:1a6e18b818a5 175 void ZeroPlate()
Kerneels Bezuidenhout 7:1a6e18b818a5 176 {
Kerneels Bezuidenhout 12:650ae2f4c7df 177
Kerneels Bezuidenhout 46:9f66021615f9 178 xMotor.Set(zeroAlpha);
Kerneels Bezuidenhout 46:9f66021615f9 179 yMotor.Set(zeroBeta);
Kerneels Bezuidenhout 12:650ae2f4c7df 180
Kerneels Bezuidenhout 7:1a6e18b818a5 181 if( saved)
Kerneels Bezuidenhout 7:1a6e18b818a5 182 {
Kerneels Bezuidenhout 12:650ae2f4c7df 183 // TODO Save zero position on servos
Kerneels Bezuidenhout 46:9f66021615f9 184 xMotor.SetZero(zeroAlpha);
Kerneels Bezuidenhout 46:9f66021615f9 185 yMotor.SetZero(zeroBeta);
Kerneels Bezuidenhout 46:9f66021615f9 186 zeroAlpha = 0;
Kerneels Bezuidenhout 46:9f66021615f9 187 zeroBeta = 0;
Generic 9:355a59fd962b 188 nextScreen = Screen::MAIN_SETTINGS_SCREEN;
Kerneels Bezuidenhout 7:1a6e18b818a5 189 saved = false;
Kerneels Bezuidenhout 7:1a6e18b818a5 190 }
Kerneels Bezuidenhout 12:650ae2f4c7df 191
Generic 9:355a59fd962b 192 Thread::wait(20);
Kerneels Bezuidenhout 7:1a6e18b818a5 193 }
Kerneels Bezuidenhout 12:650ae2f4c7df 194
Kerneels Bezuidenhout 12:650ae2f4c7df 195 void Manual()
Kerneels Bezuidenhout 12:650ae2f4c7df 196 {
Kerneels Bezuidenhout 12:650ae2f4c7df 197 // TODO Add manual control logic
Kerneels Bezuidenhout 12:650ae2f4c7df 198 }
Kerneels Bezuidenhout 13:21dc070a47a9 199
Kerneels Bezuidenhout 13:21dc070a47a9 200 void Automatic()
Kerneels Bezuidenhout 13:21dc070a47a9 201 {
Kerneels Bezuidenhout 13:21dc070a47a9 202 //TODO Add automatic control mode logic
Generic 42:29ce1ff3b220 203 ballOnPlate = plate.TouchDetected();
Kerneels Bezuidenhout 41:204e2788fb46 204
Kerneels Bezuidenhout 41:204e2788fb46 205 if( ballOnPlate )
Kerneels Bezuidenhout 41:204e2788fb46 206 {
Kerneels Bezuidenhout 41:204e2788fb46 207 plate.Measure();
Generic 45:e5b175383624 208 x = plateMapper.Calculate(floor(plate.X()));
Generic 45:e5b175383624 209 y = plateMapper.Calculate(floor(plate.Y()));
Kerneels Bezuidenhout 41:204e2788fb46 210 }
Kerneels Bezuidenhout 41:204e2788fb46 211
Kerneels Bezuidenhout 13:21dc070a47a9 212 }
Kerneels Bezuidenhout 34:0b308a399f72 213
Kerneels Bezuidenhout 34:0b308a399f72 214 void Calibrate()
Kerneels Bezuidenhout 34:0b308a399f72 215 {
Kerneels Bezuidenhout 34:0b308a399f72 216 ballOnPlate = test.read();
Kerneels Bezuidenhout 35:6baa558d3b86 217
Kerneels Bezuidenhout 36:9ca4125e526e 218 if( calStarted )
Kerneels Bezuidenhout 36:9ca4125e526e 219 {
Kerneels Bezuidenhout 36:9ca4125e526e 220 if( calPoint < 3 )
Kerneels Bezuidenhout 36:9ca4125e526e 221 {
Kerneels Bezuidenhout 36:9ca4125e526e 222 if( ballOnPlate )
Kerneels Bezuidenhout 36:9ca4125e526e 223 {
Kerneels Bezuidenhout 36:9ca4125e526e 224 calTimer.start();
Kerneels Bezuidenhout 36:9ca4125e526e 225 calHeldTime = calTimer.read();
Kerneels Bezuidenhout 36:9ca4125e526e 226
Kerneels Bezuidenhout 37:93c7563f2078 227 if( calHeldTime >= 5 )
Kerneels Bezuidenhout 36:9ca4125e526e 228 {
Kerneels Bezuidenhout 36:9ca4125e526e 229 while( test.read() );
Kerneels Bezuidenhout 36:9ca4125e526e 230
Kerneels Bezuidenhout 36:9ca4125e526e 231 calPoint++;
Kerneels Bezuidenhout 36:9ca4125e526e 232 calTimer.stop();
Kerneels Bezuidenhout 36:9ca4125e526e 233 calTimer.reset();
Kerneels Bezuidenhout 36:9ca4125e526e 234 }
Kerneels Bezuidenhout 36:9ca4125e526e 235 }
Kerneels Bezuidenhout 35:6baa558d3b86 236 else
Kerneels Bezuidenhout 35:6baa558d3b86 237 {
Kerneels Bezuidenhout 37:93c7563f2078 238 calTimer.stop();
Kerneels Bezuidenhout 35:6baa558d3b86 239 calTimer.reset();
Kerneels Bezuidenhout 35:6baa558d3b86 240 }
Kerneels Bezuidenhout 35:6baa558d3b86 241 }
Kerneels Bezuidenhout 37:93c7563f2078 242 else
Kerneels Bezuidenhout 37:93c7563f2078 243 {
Kerneels Bezuidenhout 37:93c7563f2078 244 calFinished = true;
Kerneels Bezuidenhout 37:93c7563f2078 245 calPoint = 0;
Kerneels Bezuidenhout 37:93c7563f2078 246 calStarted = false;
Kerneels Bezuidenhout 37:93c7563f2078 247 }
Kerneels Bezuidenhout 35:6baa558d3b86 248 }
Kerneels Bezuidenhout 34:0b308a399f72 249 }