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