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