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@62:58e846621435, 2016-10-26 (annotated)

Committer:
Generic
Date:
Wed Oct 26 10:40:54 2016 +0000
Revision:
62:58e846621435
Parent:
61:d16e5ad857ba 
Firmware

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.bez@gmail.com 47:c04b0d51e1e3 16
Kerneels Bezuidenhout 41:204e2788fb46 17 //-----------------------
Kerneels Bezuidenhout 7:1a6e18b818a5 18
Kerneels Bezuidenhout 41:204e2788fb46 19 //-- GUI Thread Variables and Objects
Kerneels Bezuidenhout 1:43fb2d03c36f 20 int currentScreen = 0;
Kerneels Bezuidenhout 1:43fb2d03c36f 21 int nextScreen = 0;
Kerneels Bezuidenhout 1:43fb2d03c36f 22 Thread guiThread;
Kerneels Bezuidenhout 1:43fb2d03c36f 23 Screen *activeScreen = NULL;
Kerneels Bezuidenhout 41:204e2788fb46 24 //----------------------------------
Kerneels Bezuidenhout 1:43fb2d03c36f 25
Kerneels Bezuidenhout 41:204e2788fb46 26 //-- X Axis variables
Kerneels Bezuidenhout 12:650ae2f4c7df 27 float zeroAlpha = 0;
Kerneels Bezuidenhout 12:650ae2f4c7df 28 float alpha = 0;
Generic 49:a907d4f447a9 29 float xSP = 0;
Kerneels Bezuidenhout 13:21dc070a47a9 30 float x = 0;
kerneels.bez@gmail.com 47:c04b0d51e1e3 31 float xError = 0;
kerneels.bez@gmail.com 47:c04b0d51e1e3 32 float xPrevError = 0;
Kerneels Bezuidenhout 50:0e0cf6b350f7 33 float xSummation = 0;
Kerneels Bezuidenhout 46:9f66021615f9 34 MedianFilter xFilter(7);
Kerneels Bezuidenhout 7:1a6e18b818a5 35
Kerneels Bezuidenhout 41:204e2788fb46 36 //-- Y Axis Variables
Kerneels Bezuidenhout 12:650ae2f4c7df 37 float zeroBeta = 0;
Kerneels Bezuidenhout 12:650ae2f4c7df 38 float beta = 0;
Generic 49:a907d4f447a9 39 float ySP = 0;
Kerneels Bezuidenhout 13:21dc070a47a9 40 float y = 0;
kerneels.bez@gmail.com 47:c04b0d51e1e3 41 float yError = 0;
kerneels.bez@gmail.com 47:c04b0d51e1e3 42 float yPrevError = 0;
Kerneels Bezuidenhout 50:0e0cf6b350f7 43 float ySummation = 0;
Kerneels Bezuidenhout 46:9f66021615f9 44 MedianFilter yFilter(7);
Kerneels Bezuidenhout 13:21dc070a47a9 45
Kerneels Bezuidenhout 41:204e2788fb46 46
Kerneels Bezuidenhout 41:204e2788fb46 47 //-- Controller Variables
Kerneels Bezuidenhout 61:d16e5ad857ba 48 float P = 0.1;
Kerneels Bezuidenhout 22:dd9ff475b34c 49 float I = 0;
Generic 62:58e846621435 50 float D = 9;
Generic 62:58e846621435 51 float Ts = 4;
kerneels.bez@gmail.com 47:c04b0d51e1e3 52 Timer cycleTimer;
Kerneels Bezuidenhout 41:204e2788fb46 53 //-----------------------
Kerneels Bezuidenhout 41:204e2788fb46 54
Generic 62:58e846621435 55 float t[3][2];
Generic 62:58e846621435 56 float d[3][2];
Generic 62:58e846621435 57
Kerneels Bezuidenhout 41:204e2788fb46 58
Kerneels Bezuidenhout 41:204e2788fb46 59 //-- Sensors and Actuator objects
Kerneels Bezuidenhout 41:204e2788fb46 60 ResistiveTouchController plate(A0,A1,A3,A4,A5);
kerneels.bez@gmail.com 47:c04b0d51e1e3 61 Map xMapper(0,1024, -170,170);
kerneels.bez@gmail.com 47:c04b0d51e1e3 62 Map yMapper(0,1024, -136,136);
Generic 49:a907d4f447a9 63 CYS8218Controller xMotor(D8);
Generic 49:a907d4f447a9 64 CYS8218Controller yMotor(D9);
Kerneels Bezuidenhout 46:9f66021615f9 65
Kerneels Bezuidenhout 22:dd9ff475b34c 66
Kerneels Bezuidenhout 32:d490f8ca6d2a 67 float tCal[3][2];
Kerneels Bezuidenhout 35:6baa558d3b86 68 int calPoint = 0;
Kerneels Bezuidenhout 35:6baa558d3b86 69 bool calStarted = false;
Kerneels Bezuidenhout 35:6baa558d3b86 70 Timer calTimer;
Kerneels Bezuidenhout 38:f1c571d22919 71 float calHeldTime;
Kerneels Bezuidenhout 37:93c7563f2078 72 bool calFinished = false;
Kerneels Bezuidenhout 32:d490f8ca6d2a 73
Generic 23:bab197887d5e 74 int measuredTs = 0;
Kerneels Bezuidenhout 13:21dc070a47a9 75
Generic 49:a907d4f447a9 76 bool started = false;
Kerneels Bezuidenhout 13:21dc070a47a9 77
Generic 49:a907d4f447a9 78 int ballOnPlate = 0;
Kerneels Bezuidenhout 7:1a6e18b818a5 79
Kerneels Bezuidenhout 12:650ae2f4c7df 80 bool saved = false;
Kerneels Bezuidenhout 7:1a6e18b818a5 81
Generic 9:355a59fd962b 82
Kerneels Bezuidenhout 1:43fb2d03c36f 83 int main()
Kerneels Bezuidenhout 1:43fb2d03c36f 84 {
Kerneels Bezuidenhout 1:43fb2d03c36f 85 Init();
Kerneels Bezuidenhout 1:43fb2d03c36f 86
Kerneels Bezuidenhout 1:43fb2d03c36f 87 while(1)
Kerneels Bezuidenhout 1:43fb2d03c36f 88 {
Kerneels Bezuidenhout 7:1a6e18b818a5 89 switch (currentScreen)
Kerneels Bezuidenhout 7:1a6e18b818a5 90 {
Kerneels Bezuidenhout 7:1a6e18b818a5 91 case Screen::ZERO_PLATE_SCREEN :
Kerneels Bezuidenhout 7:1a6e18b818a5 92 ZeroPlate();
Kerneels Bezuidenhout 7:1a6e18b818a5 93 break;
Kerneels Bezuidenhout 12:650ae2f4c7df 94 case Screen::MANUAL_CONTROL_SCREEN :
Kerneels Bezuidenhout 12:650ae2f4c7df 95 Manual();
Kerneels Bezuidenhout 12:650ae2f4c7df 96 break;
Kerneels Bezuidenhout 13:21dc070a47a9 97 case Screen::AUTOMATIC_CONTROL_SCREEN :
Kerneels Bezuidenhout 13:21dc070a47a9 98 Automatic();
Kerneels Bezuidenhout 13:21dc070a47a9 99 break;
Kerneels Bezuidenhout 34:0b308a399f72 100 case Screen::CALIBRATE_SCREEN :
Kerneels Bezuidenhout 34:0b308a399f72 101 Calibrate();
Kerneels Bezuidenhout 34:0b308a399f72 102 break;
kerneels.bez@gmail.com 47:c04b0d51e1e3 103 case Screen::MANUAL_SETPOINT_SCREEN :
kerneels.bez@gmail.com 47:c04b0d51e1e3 104 Automatic();
kerneels.bez@gmail.com 47:c04b0d51e1e3 105 break;
Kerneels Bezuidenhout 56:c9389039ecc9 106 case Screen::SQUARE_SCREEN :
Kerneels Bezuidenhout 56:c9389039ecc9 107 Automatic();
Kerneels Bezuidenhout 56:c9389039ecc9 108 break;
kerneels.bez@gmail.com 47:c04b0d51e1e3 109 default :
Kerneels Bezuidenhout 61:d16e5ad857ba 110 xMotor.Set(0);
Kerneels Bezuidenhout 61:d16e5ad857ba 111 yMotor.Set(0);
Kerneels Bezuidenhout 61:d16e5ad857ba 112 Thread::wait(20);
kerneels.bez@gmail.com 47:c04b0d51e1e3 113 break;
Kerneels Bezuidenhout 7:1a6e18b818a5 114 }
Kerneels Bezuidenhout 1:43fb2d03c36f 115 }
Kerneels Bezuidenhout 1:43fb2d03c36f 116 }
Kerneels Bezuidenhout 1:43fb2d03c36f 117
Kerneels Bezuidenhout 1:43fb2d03c36f 118 void GUIThread()
Kerneels Bezuidenhout 1:43fb2d03c36f 119 {
Kerneels Bezuidenhout 1:43fb2d03c36f 120 while(1)
Kerneels Bezuidenhout 1:43fb2d03c36f 121 {
Kerneels Bezuidenhout 1:43fb2d03c36f 122 if( nextScreen != currentScreen )
Kerneels Bezuidenhout 1:43fb2d03c36f 123 {
Kerneels Bezuidenhout 50:0e0cf6b350f7 124 started = false;
Generic 62:58e846621435 125 xSummation = 0;
Generic 62:58e846621435 126 ySummation = 0;
Generic 39:7bb23ad4bf87 127 Thread::wait(100);
Kerneels Bezuidenhout 41:204e2788fb46 128
Kerneels Bezuidenhout 1:43fb2d03c36f 129 if( activeScreen != NULL)
Kerneels Bezuidenhout 1:43fb2d03c36f 130 delete activeScreen;
Kerneels Bezuidenhout 1:43fb2d03c36f 131
Kerneels Bezuidenhout 1:43fb2d03c36f 132 switch (nextScreen)
Kerneels Bezuidenhout 1:43fb2d03c36f 133 {
Kerneels Bezuidenhout 7:1a6e18b818a5 134 case Screen::MAIN_MENU_SCREEN :
Kerneels Bezuidenhout 1:43fb2d03c36f 135 activeScreen = new MainMenuScreen(&nextScreen);
Kerneels Bezuidenhout 1:43fb2d03c36f 136 break;
Kerneels Bezuidenhout 7:1a6e18b818a5 137 case Screen::MAIN_SETTINGS_SCREEN :
Kerneels Bezuidenhout 5:383f1108187d 138 activeScreen = new MainSettingsScreen(&nextScreen);
Kerneels Bezuidenhout 5:383f1108187d 139 break;
Kerneels Bezuidenhout 7:1a6e18b818a5 140 case Screen::ZERO_PLATE_SCREEN :
Kerneels Bezuidenhout 7:1a6e18b818a5 141 activeScreen = new ZeroPlateScreen(&nextScreen, &zeroAlpha, &zeroBeta, &saved);
Kerneels Bezuidenhout 7:1a6e18b818a5 142 break;
Generic 15:bc63b55a426d 143 case Screen::MANUAL_CONTROL_SCREEN :
Kerneels Bezuidenhout 12:650ae2f4c7df 144 activeScreen = new ManualControlScreen(&nextScreen, &alpha, &beta);
Kerneels Bezuidenhout 12:650ae2f4c7df 145 break;
Kerneels Bezuidenhout 13:21dc070a47a9 146 case Screen::AUTOMATIC_CONTROL_SCREEN :
Kerneels Bezuidenhout 22:dd9ff475b34c 147 activeScreen = new AutomaticControlScreen(&nextScreen, &x, &y, &measuredTs, &started, &ballOnPlate, &xSP, &ySP);
Kerneels Bezuidenhout 13:21dc070a47a9 148 break;
Kerneels Bezuidenhout 18:1b9579ac9f41 149 case Screen::AUTOMATIC_MORE_SCREEN :
Kerneels Bezuidenhout 18:1b9579ac9f41 150 activeScreen = new AutomaticMoreScreen(&nextScreen);
Kerneels Bezuidenhout 18:1b9579ac9f41 151 break;
Kerneels Bezuidenhout 20:70c5b1e499f0 152 case Screen::MANUAL_SETPOINT_SCREEN :
Generic 23:bab197887d5e 153 activeScreen = new ManualSetpointScreen(&nextScreen, &xSP, &ySP, &ballOnPlate, &started);
Kerneels Bezuidenhout 20:70c5b1e499f0 154 break;
Generic 23:bab197887d5e 155 case Screen::AUTOMATIC_SETTINGS_SCREEN :
Kerneels Bezuidenhout 22:dd9ff475b34c 156 activeScreen = new AutomaticSettingsScreen(&nextScreen, &P, &I, &D, &Ts);
Kerneels Bezuidenhout 26:f57db8d6d79e 157 break;
Kerneels Bezuidenhout 25:bfff3d661f92 158 case Screen::CHANGE_P_SCREEN :
Kerneels Bezuidenhout 25:bfff3d661f92 159 activeScreen = new ChangeValScreen(&nextScreen, currentScreen, &P, "Change P");
Kerneels Bezuidenhout 25:bfff3d661f92 160 break;
Kerneels Bezuidenhout 26:f57db8d6d79e 161 case Screen::CHANGE_I_SCREEN :
Kerneels Bezuidenhout 26:f57db8d6d79e 162 activeScreen = new ChangeValScreen(&nextScreen, currentScreen, &I, "Change I");
Kerneels Bezuidenhout 26:f57db8d6d79e 163 break;
Kerneels Bezuidenhout 26:f57db8d6d79e 164 case Screen::CHANGE_D_SCREEN :
Kerneels Bezuidenhout 26:f57db8d6d79e 165 activeScreen = new ChangeValScreen(&nextScreen, currentScreen, &D, "Change D");
Kerneels Bezuidenhout 26:f57db8d6d79e 166 break;
Kerneels Bezuidenhout 26:f57db8d6d79e 167 case Screen::CHANGE_TS_SCREEN :
Kerneels Bezuidenhout 31:5ece593c49a7 168 activeScreen = new ChangeValWholeScreen(&nextScreen, currentScreen, &Ts, "Change Ts", false, 4);
Kerneels Bezuidenhout 26:f57db8d6d79e 169 break;
Kerneels Bezuidenhout 32:d490f8ca6d2a 170 case Screen::CALIBRATE_SCREEN :
Kerneels Bezuidenhout 38:f1c571d22919 171 activeScreen = new CalibrateScreen(&nextScreen, &ballOnPlate, &calStarted, &calFinished, &calPoint, &calHeldTime);
Kerneels Bezuidenhout 32:d490f8ca6d2a 172 break;
Kerneels Bezuidenhout 54:75dc5c3b7cfc 173 case Screen::SQUARE_SCREEN :
Generic 60:b6352a55d850 174 activeScreen = new SquareScreen(&nextScreen,&xSP, &ySP, &ballOnPlate, &started, &xError, &yError );
Kerneels Bezuidenhout 54:75dc5c3b7cfc 175 break;
Kerneels Bezuidenhout 1:43fb2d03c36f 176 }
Kerneels Bezuidenhout 1:43fb2d03c36f 177 activeScreen->Draw();
Generic 6:f717ce705c5d 178 currentScreen = nextScreen;
Kerneels Bezuidenhout 1:43fb2d03c36f 179 }
Kerneels Bezuidenhout 1:43fb2d03c36f 180
Kerneels Bezuidenhout 1:43fb2d03c36f 181 activeScreen->Process();
Kerneels Bezuidenhout 1:43fb2d03c36f 182
Generic 17:3b6d66253c97 183 Thread::wait(10);
Kerneels Bezuidenhout 1:43fb2d03c36f 184 }
Kerneels Bezuidenhout 1:43fb2d03c36f 185 }
Kerneels Bezuidenhout 1:43fb2d03c36f 186
Kerneels Bezuidenhout 1:43fb2d03c36f 187 void Init()
Kerneels Bezuidenhout 1:43fb2d03c36f 188 {
Generic 62:58e846621435 189
Generic 62:58e846621435 190 d[0][0] = 921.6;
Generic 62:58e846621435 191 d[1][0] = 512;
Generic 62:58e846621435 192 d[2][0] = 102.4;
Generic 62:58e846621435 193
Generic 62:58e846621435 194 d[0][1] = 512;
Generic 62:58e846621435 195 d[1][1] = 921.6;
Generic 62:58e846621435 196 d[2][1] = 102.4;
Generic 62:58e846621435 197
Generic 39:7bb23ad4bf87 198 ImageFromSD splash("/sd/Splash.txt");
Kerneels Bezuidenhout 46:9f66021615f9 199 xMotor.SetZero(28.0);
Kerneels Bezuidenhout 46:9f66021615f9 200 yMotor.SetZero(25.0);
Generic 2:5fded6444350 201 nextScreen = Screen::MAIN_MENU_SCREEN;
Kerneels Bezuidenhout 38:f1c571d22919 202
Kerneels Bezuidenhout 38:f1c571d22919 203 Thread::wait(5000);
Kerneels Bezuidenhout 38:f1c571d22919 204
Kerneels Bezuidenhout 1:43fb2d03c36f 205 guiThread.start(GUIThread);
Kerneels Bezuidenhout 1:43fb2d03c36f 206 }
Kerneels Bezuidenhout 7:1a6e18b818a5 207
Kerneels Bezuidenhout 7:1a6e18b818a5 208 void ZeroPlate()
Kerneels Bezuidenhout 7:1a6e18b818a5 209 {
Kerneels Bezuidenhout 12:650ae2f4c7df 210
Kerneels Bezuidenhout 46:9f66021615f9 211 xMotor.Set(zeroAlpha);
kerneels.bez@gmail.com 47:c04b0d51e1e3 212 yMotor.Set(-zeroBeta);
Kerneels Bezuidenhout 12:650ae2f4c7df 213
Kerneels Bezuidenhout 7:1a6e18b818a5 214 if( saved)
Kerneels Bezuidenhout 7:1a6e18b818a5 215 {
Kerneels Bezuidenhout 46:9f66021615f9 216 xMotor.SetZero(zeroAlpha);
kerneels.bez@gmail.com 47:c04b0d51e1e3 217 yMotor.SetZero(-zeroBeta);
Kerneels Bezuidenhout 46:9f66021615f9 218 zeroAlpha = 0;
Kerneels Bezuidenhout 46:9f66021615f9 219 zeroBeta = 0;
Generic 9:355a59fd962b 220 nextScreen = Screen::MAIN_SETTINGS_SCREEN;
Kerneels Bezuidenhout 7:1a6e18b818a5 221 saved = false;
Kerneels Bezuidenhout 7:1a6e18b818a5 222 }
Kerneels Bezuidenhout 12:650ae2f4c7df 223
Generic 9:355a59fd962b 224 Thread::wait(20);
Kerneels Bezuidenhout 7:1a6e18b818a5 225 }
Kerneels Bezuidenhout 12:650ae2f4c7df 226
Kerneels Bezuidenhout 12:650ae2f4c7df 227 void Manual()
Kerneels Bezuidenhout 12:650ae2f4c7df 228 {
Generic 49:a907d4f447a9 229 xMotor.Set(-alpha);
Generic 49:a907d4f447a9 230 yMotor.Set(beta);
kerneels.bez@gmail.com 47:c04b0d51e1e3 231 Thread::wait(10);
Kerneels Bezuidenhout 12:650ae2f4c7df 232 }
Kerneels Bezuidenhout 13:21dc070a47a9 233
Kerneels Bezuidenhout 13:21dc070a47a9 234 void Automatic()
Kerneels Bezuidenhout 13:21dc070a47a9 235 {
Kerneels Bezuidenhout 13:21dc070a47a9 236 //TODO Add automatic control mode logic
Generic 42:29ce1ff3b220 237 ballOnPlate = plate.TouchDetected();
Kerneels Bezuidenhout 41:204e2788fb46 238
kerneels.bez@gmail.com 47:c04b0d51e1e3 239 if( ballOnPlate && started )
Kerneels Bezuidenhout 41:204e2788fb46 240 {
kerneels.bez@gmail.com 47:c04b0d51e1e3 241 cycleTimer.start();
Kerneels Bezuidenhout 41:204e2788fb46 242 plate.Measure();
Kerneels Bezuidenhout 50:0e0cf6b350f7 243
Generic 49:a907d4f447a9 244 //x = xMapper.Calculate(floor(plate.X()));
Generic 49:a907d4f447a9 245 //y = yMapper.Calculate(floor(plate.Y()));
Kerneels Bezuidenhout 50:0e0cf6b350f7 246
Generic 49:a907d4f447a9 247 x = xMapper.Calculate(floor(xFilter.Process(plate.X())));
Generic 49:a907d4f447a9 248 y = yMapper.Calculate(floor(yFilter.Process(plate.Y())));
kerneels.bez@gmail.com 47:c04b0d51e1e3 249
kerneels.bez@gmail.com 47:c04b0d51e1e3 250 xError = xSP - x;
kerneels.bez@gmail.com 47:c04b0d51e1e3 251 yError = ySP - y;
kerneels.bez@gmail.com 47:c04b0d51e1e3 252
Kerneels Bezuidenhout 50:0e0cf6b350f7 253 xSummation += xError;
Kerneels Bezuidenhout 50:0e0cf6b350f7 254 ySummation += yError;
Kerneels Bezuidenhout 50:0e0cf6b350f7 255
Kerneels Bezuidenhout 50:0e0cf6b350f7 256
Kerneels Bezuidenhout 50:0e0cf6b350f7 257
Kerneels Bezuidenhout 50:0e0cf6b350f7 258 alpha = P*xError + D*(xError-xPrevError) + I*xSummation;
Kerneels Bezuidenhout 50:0e0cf6b350f7 259 beta = P*yError + D*(yError-yPrevError) + I*ySummation;
kerneels.bez@gmail.com 47:c04b0d51e1e3 260
kerneels.bez@gmail.com 47:c04b0d51e1e3 261 xPrevError = xError;
kerneels.bez@gmail.com 47:c04b0d51e1e3 262 yPrevError = yError;
kerneels.bez@gmail.com 47:c04b0d51e1e3 263
kerneels.bez@gmail.com 47:c04b0d51e1e3 264 alpha = ( alpha > 10 ? 10 : (alpha < -10 ? -10 : alpha));
kerneels.bez@gmail.com 47:c04b0d51e1e3 265 beta = ( beta > 10 ? 10 : (beta < -10 ? -10 : beta) );
kerneels.bez@gmail.com 47:c04b0d51e1e3 266
Generic 49:a907d4f447a9 267 xMotor.Set(alpha);
Generic 62:58e846621435 268 yMotor.Set(beta);
kerneels.bez@gmail.com 47:c04b0d51e1e3 269
Generic 62:58e846621435 270 float waitTime = Ts/1000.0;
Kerneels Bezuidenhout 61:d16e5ad857ba 271
Generic 62:58e846621435 272 while( cycleTimer.read() < waitTime);
Kerneels Bezuidenhout 61:d16e5ad857ba 273
kerneels.bez@gmail.com 47:c04b0d51e1e3 274 cycleTimer.stop();
kerneels.bez@gmail.com 47:c04b0d51e1e3 275 measuredTs = cycleTimer.read_ms();
kerneels.bez@gmail.com 47:c04b0d51e1e3 276 cycleTimer.reset();
kerneels.bez@gmail.com 47:c04b0d51e1e3 277 }
kerneels.bez@gmail.com 47:c04b0d51e1e3 278 else
kerneels.bez@gmail.com 47:c04b0d51e1e3 279 {
kerneels.bez@gmail.com 47:c04b0d51e1e3 280 xMotor.Off();
kerneels.bez@gmail.com 47:c04b0d51e1e3 281 yMotor.Off();
Kerneels Bezuidenhout 41:204e2788fb46 282 }
Kerneels Bezuidenhout 41:204e2788fb46 283
Kerneels Bezuidenhout 13:21dc070a47a9 284 }
Kerneels Bezuidenhout 34:0b308a399f72 285
Kerneels Bezuidenhout 34:0b308a399f72 286 void Calibrate()
Kerneels Bezuidenhout 34:0b308a399f72 287 {
Generic 49:a907d4f447a9 288 ballOnPlate = plate.TouchDetected();
Kerneels Bezuidenhout 35:6baa558d3b86 289
Kerneels Bezuidenhout 36:9ca4125e526e 290 if( calStarted )
Kerneels Bezuidenhout 36:9ca4125e526e 291 {
Kerneels Bezuidenhout 36:9ca4125e526e 292 if( calPoint < 3 )
Kerneels Bezuidenhout 36:9ca4125e526e 293 {
Kerneels Bezuidenhout 36:9ca4125e526e 294 if( ballOnPlate )
Kerneels Bezuidenhout 36:9ca4125e526e 295 {
Kerneels Bezuidenhout 36:9ca4125e526e 296 calTimer.start();
Kerneels Bezuidenhout 36:9ca4125e526e 297 calHeldTime = calTimer.read();
Generic 62:58e846621435 298 plate.Measure();
Generic 62:58e846621435 299
Generic 62:58e846621435 300 t[calPoint][0] = plate.RawX();
Generic 62:58e846621435 301 t[calPoint][1] = plate.RawY();
Kerneels Bezuidenhout 36:9ca4125e526e 302
Kerneels Bezuidenhout 37:93c7563f2078 303 if( calHeldTime >= 5 )
Kerneels Bezuidenhout 36:9ca4125e526e 304 {
Generic 49:a907d4f447a9 305 while( plate.TouchDetected() );
Generic 62:58e846621435 306
Kerneels Bezuidenhout 36:9ca4125e526e 307 calPoint++;
Kerneels Bezuidenhout 36:9ca4125e526e 308 calTimer.stop();
Kerneels Bezuidenhout 36:9ca4125e526e 309 calTimer.reset();
Kerneels Bezuidenhout 36:9ca4125e526e 310 }
Kerneels Bezuidenhout 36:9ca4125e526e 311 }
Kerneels Bezuidenhout 35:6baa558d3b86 312 else
Kerneels Bezuidenhout 35:6baa558d3b86 313 {
Kerneels Bezuidenhout 37:93c7563f2078 314 calTimer.stop();
Kerneels Bezuidenhout 35:6baa558d3b86 315 calTimer.reset();
Kerneels Bezuidenhout 35:6baa558d3b86 316 }
Kerneels Bezuidenhout 35:6baa558d3b86 317 }
Kerneels Bezuidenhout 37:93c7563f2078 318 else
Kerneels Bezuidenhout 37:93c7563f2078 319 {
Generic 62:58e846621435 320 plate.Calibrate(t,d);
Kerneels Bezuidenhout 37:93c7563f2078 321 calFinished = true;
Kerneels Bezuidenhout 37:93c7563f2078 322 calPoint = 0;
Kerneels Bezuidenhout 37:93c7563f2078 323 calStarted = false;
Kerneels Bezuidenhout 37:93c7563f2078 324 }
Kerneels Bezuidenhout 35:6baa558d3b86 325 }
Kerneels Bezuidenhout 34:0b308a399f72 326 }