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?
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 |
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 | } |