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@55:d1a00f344351, 2016-10-19 (annotated)

Committer:
Generic
Date:
Wed Oct 19 18:08:23 2016 +0000
Revision:
55:d1a00f344351
Parent:
54:75dc5c3b7cfc
Child:
56:c9389039ecc9
Fixed

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