My final year project
Dependencies: BSP_DISCO_F746NG F746_GUI LCD_DISCO_F746NG SDFileSystem TS_DISCO_F746NG ResistiveTouchController Map CYS8218Controller MedianFilter
main.cpp@50:0e0cf6b350f7, 2016-10-18 (annotated)
- Committer:
- Kerneels Bezuidenhout
- Date:
- Tue Oct 18 02:11:07 2016 +0200
- Revision:
- 50:0e0cf6b350f7
- Parent:
- 49:a907d4f447a9
- Child:
- 54:75dc5c3b7cfc
Testing I
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.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 |
1:43fb2d03c36f | 166 | } |
Kerneels Bezuidenhout |
1:43fb2d03c36f | 167 | activeScreen->Draw(); |
Generic | 6:f717ce705c5d | 168 | currentScreen = nextScreen; |
Kerneels Bezuidenhout |
1:43fb2d03c36f | 169 | } |
Kerneels Bezuidenhout |
1:43fb2d03c36f | 170 | |
Kerneels Bezuidenhout |
1:43fb2d03c36f | 171 | activeScreen->Process(); |
Kerneels Bezuidenhout |
1:43fb2d03c36f | 172 | |
Generic | 17:3b6d66253c97 | 173 | Thread::wait(10); |
Kerneels Bezuidenhout |
1:43fb2d03c36f | 174 | } |
Kerneels Bezuidenhout |
1:43fb2d03c36f | 175 | } |
Kerneels Bezuidenhout |
1:43fb2d03c36f | 176 | |
Kerneels Bezuidenhout |
1:43fb2d03c36f | 177 | void Init() |
Kerneels Bezuidenhout |
1:43fb2d03c36f | 178 | { |
kerneels.bez@gmail.com | 47:c04b0d51e1e3 | 179 | // TODO Read values from sd card |
Generic | 39:7bb23ad4bf87 | 180 | ImageFromSD splash("/sd/Splash.txt"); |
Generic | 9:355a59fd962b | 181 | pc.baud(57600); |
Kerneels Bezuidenhout |
46:9f66021615f9 | 182 | xMotor.SetZero(28.0); |
Kerneels Bezuidenhout |
46:9f66021615f9 | 183 | yMotor.SetZero(25.0); |
Generic | 2:5fded6444350 | 184 | nextScreen = Screen::MAIN_MENU_SCREEN; |
Kerneels Bezuidenhout |
38:f1c571d22919 | 185 | |
Kerneels Bezuidenhout |
38:f1c571d22919 | 186 | Thread::wait(5000); |
Kerneels Bezuidenhout |
38:f1c571d22919 | 187 | |
Kerneels Bezuidenhout |
1:43fb2d03c36f | 188 | guiThread.start(GUIThread); |
Kerneels Bezuidenhout |
1:43fb2d03c36f | 189 | } |
Kerneels Bezuidenhout |
7:1a6e18b818a5 | 190 | |
Kerneels Bezuidenhout |
7:1a6e18b818a5 | 191 | void ZeroPlate() |
Kerneels Bezuidenhout |
7:1a6e18b818a5 | 192 | { |
Kerneels Bezuidenhout |
12:650ae2f4c7df | 193 | |
Kerneels Bezuidenhout |
46:9f66021615f9 | 194 | xMotor.Set(zeroAlpha); |
kerneels.bez@gmail.com | 47:c04b0d51e1e3 | 195 | yMotor.Set(-zeroBeta); |
Kerneels Bezuidenhout |
12:650ae2f4c7df | 196 | |
Kerneels Bezuidenhout |
7:1a6e18b818a5 | 197 | if( saved) |
Kerneels Bezuidenhout |
7:1a6e18b818a5 | 198 | { |
kerneels.bez@gmail.com | 47:c04b0d51e1e3 | 199 | // TODO Save zero position on sd card |
Kerneels Bezuidenhout |
46:9f66021615f9 | 200 | xMotor.SetZero(zeroAlpha); |
kerneels.bez@gmail.com | 47:c04b0d51e1e3 | 201 | yMotor.SetZero(-zeroBeta); |
Kerneels Bezuidenhout |
46:9f66021615f9 | 202 | zeroAlpha = 0; |
Kerneels Bezuidenhout |
46:9f66021615f9 | 203 | zeroBeta = 0; |
Generic | 9:355a59fd962b | 204 | nextScreen = Screen::MAIN_SETTINGS_SCREEN; |
Kerneels Bezuidenhout |
7:1a6e18b818a5 | 205 | saved = false; |
Kerneels Bezuidenhout |
7:1a6e18b818a5 | 206 | } |
Kerneels Bezuidenhout |
12:650ae2f4c7df | 207 | |
Generic | 9:355a59fd962b | 208 | Thread::wait(20); |
Kerneels Bezuidenhout |
7:1a6e18b818a5 | 209 | } |
Kerneels Bezuidenhout |
12:650ae2f4c7df | 210 | |
Kerneels Bezuidenhout |
12:650ae2f4c7df | 211 | void Manual() |
Kerneels Bezuidenhout |
12:650ae2f4c7df | 212 | { |
Generic | 49:a907d4f447a9 | 213 | xMotor.Set(-alpha); |
Generic | 49:a907d4f447a9 | 214 | yMotor.Set(beta); |
kerneels.bez@gmail.com | 47:c04b0d51e1e3 | 215 | Thread::wait(10); |
Kerneels Bezuidenhout |
12:650ae2f4c7df | 216 | } |
Kerneels Bezuidenhout |
13:21dc070a47a9 | 217 | |
Kerneels Bezuidenhout |
13:21dc070a47a9 | 218 | void Automatic() |
Kerneels Bezuidenhout |
13:21dc070a47a9 | 219 | { |
Kerneels Bezuidenhout |
13:21dc070a47a9 | 220 | //TODO Add automatic control mode logic |
Generic | 42:29ce1ff3b220 | 221 | ballOnPlate = plate.TouchDetected(); |
Kerneels Bezuidenhout |
41:204e2788fb46 | 222 | |
kerneels.bez@gmail.com | 47:c04b0d51e1e3 | 223 | if( ballOnPlate && started ) |
Kerneels Bezuidenhout |
41:204e2788fb46 | 224 | { |
kerneels.bez@gmail.com | 47:c04b0d51e1e3 | 225 | cycleTimer.start(); |
Kerneels Bezuidenhout |
41:204e2788fb46 | 226 | plate.Measure(); |
Kerneels Bezuidenhout |
50:0e0cf6b350f7 | 227 | |
Generic | 49:a907d4f447a9 | 228 | //x = xMapper.Calculate(floor(plate.X())); |
Generic | 49:a907d4f447a9 | 229 | //y = yMapper.Calculate(floor(plate.Y())); |
Kerneels Bezuidenhout |
50:0e0cf6b350f7 | 230 | |
Generic | 49:a907d4f447a9 | 231 | x = xMapper.Calculate(floor(xFilter.Process(plate.X()))); |
Generic | 49:a907d4f447a9 | 232 | y = yMapper.Calculate(floor(yFilter.Process(plate.Y()))); |
kerneels.bez@gmail.com | 47:c04b0d51e1e3 | 233 | |
kerneels.bez@gmail.com | 47:c04b0d51e1e3 | 234 | xError = xSP - x; |
kerneels.bez@gmail.com | 47:c04b0d51e1e3 | 235 | yError = ySP - y; |
kerneels.bez@gmail.com | 47:c04b0d51e1e3 | 236 | |
Kerneels Bezuidenhout |
50:0e0cf6b350f7 | 237 | xSummation += xError; |
Kerneels Bezuidenhout |
50:0e0cf6b350f7 | 238 | ySummation += yError; |
Kerneels Bezuidenhout |
50:0e0cf6b350f7 | 239 | |
Kerneels Bezuidenhout |
50:0e0cf6b350f7 | 240 | |
Kerneels Bezuidenhout |
50:0e0cf6b350f7 | 241 | |
Kerneels Bezuidenhout |
50:0e0cf6b350f7 | 242 | alpha = P*xError + D*(xError-xPrevError) + I*xSummation; |
Kerneels Bezuidenhout |
50:0e0cf6b350f7 | 243 | beta = P*yError + D*(yError-yPrevError) + I*ySummation; |
kerneels.bez@gmail.com | 47:c04b0d51e1e3 | 244 | |
kerneels.bez@gmail.com | 47:c04b0d51e1e3 | 245 | xPrevError = xError; |
kerneels.bez@gmail.com | 47:c04b0d51e1e3 | 246 | yPrevError = yError; |
kerneels.bez@gmail.com | 47:c04b0d51e1e3 | 247 | |
kerneels.bez@gmail.com | 47:c04b0d51e1e3 | 248 | alpha = ( alpha > 10 ? 10 : (alpha < -10 ? -10 : alpha)); |
kerneels.bez@gmail.com | 47:c04b0d51e1e3 | 249 | beta = ( beta > 10 ? 10 : (beta < -10 ? -10 : beta) ); |
kerneels.bez@gmail.com | 47:c04b0d51e1e3 | 250 | |
Generic | 49:a907d4f447a9 | 251 | xMotor.Set(alpha); |
Generic | 49:a907d4f447a9 | 252 | yMotor.Set(-beta); |
kerneels.bez@gmail.com | 47:c04b0d51e1e3 | 253 | |
kerneels.bez@gmail.com | 47:c04b0d51e1e3 | 254 | cycleTimer.stop(); |
kerneels.bez@gmail.com | 47:c04b0d51e1e3 | 255 | measuredTs = cycleTimer.read_ms(); |
kerneels.bez@gmail.com | 47:c04b0d51e1e3 | 256 | cycleTimer.reset(); |
kerneels.bez@gmail.com | 47:c04b0d51e1e3 | 257 | } |
kerneels.bez@gmail.com | 47:c04b0d51e1e3 | 258 | else |
kerneels.bez@gmail.com | 47:c04b0d51e1e3 | 259 | { |
kerneels.bez@gmail.com | 47:c04b0d51e1e3 | 260 | xMotor.Off(); |
kerneels.bez@gmail.com | 47:c04b0d51e1e3 | 261 | yMotor.Off(); |
Kerneels Bezuidenhout |
41:204e2788fb46 | 262 | } |
Kerneels Bezuidenhout |
41:204e2788fb46 | 263 | |
Kerneels Bezuidenhout |
13:21dc070a47a9 | 264 | } |
Kerneels Bezuidenhout |
34:0b308a399f72 | 265 | |
Kerneels Bezuidenhout |
34:0b308a399f72 | 266 | void Calibrate() |
Kerneels Bezuidenhout |
34:0b308a399f72 | 267 | { |
Generic | 49:a907d4f447a9 | 268 | ballOnPlate = plate.TouchDetected(); |
Kerneels Bezuidenhout |
35:6baa558d3b86 | 269 | |
Kerneels Bezuidenhout |
36:9ca4125e526e | 270 | if( calStarted ) |
Kerneels Bezuidenhout |
36:9ca4125e526e | 271 | { |
Kerneels Bezuidenhout |
36:9ca4125e526e | 272 | if( calPoint < 3 ) |
Kerneels Bezuidenhout |
36:9ca4125e526e | 273 | { |
Kerneels Bezuidenhout |
36:9ca4125e526e | 274 | if( ballOnPlate ) |
Kerneels Bezuidenhout |
36:9ca4125e526e | 275 | { |
Kerneels Bezuidenhout |
36:9ca4125e526e | 276 | calTimer.start(); |
Kerneels Bezuidenhout |
36:9ca4125e526e | 277 | calHeldTime = calTimer.read(); |
Kerneels Bezuidenhout |
36:9ca4125e526e | 278 | |
Kerneels Bezuidenhout |
37:93c7563f2078 | 279 | if( calHeldTime >= 5 ) |
Kerneels Bezuidenhout |
36:9ca4125e526e | 280 | { |
Generic | 49:a907d4f447a9 | 281 | while( plate.TouchDetected() ); |
Kerneels Bezuidenhout |
36:9ca4125e526e | 282 | |
kerneels.bez@gmail.com | 47:c04b0d51e1e3 | 283 | //TODO Test calibration function |
Kerneels Bezuidenhout |
36:9ca4125e526e | 284 | calPoint++; |
Kerneels Bezuidenhout |
36:9ca4125e526e | 285 | calTimer.stop(); |
Kerneels Bezuidenhout |
36:9ca4125e526e | 286 | calTimer.reset(); |
Kerneels Bezuidenhout |
36:9ca4125e526e | 287 | } |
Kerneels Bezuidenhout |
36:9ca4125e526e | 288 | } |
Kerneels Bezuidenhout |
35:6baa558d3b86 | 289 | else |
Kerneels Bezuidenhout |
35:6baa558d3b86 | 290 | { |
Kerneels Bezuidenhout |
37:93c7563f2078 | 291 | calTimer.stop(); |
Kerneels Bezuidenhout |
35:6baa558d3b86 | 292 | calTimer.reset(); |
Kerneels Bezuidenhout |
35:6baa558d3b86 | 293 | } |
Kerneels Bezuidenhout |
35:6baa558d3b86 | 294 | } |
Kerneels Bezuidenhout |
37:93c7563f2078 | 295 | else |
Kerneels Bezuidenhout |
37:93c7563f2078 | 296 | { |
Kerneels Bezuidenhout |
37:93c7563f2078 | 297 | calFinished = true; |
Kerneels Bezuidenhout |
37:93c7563f2078 | 298 | calPoint = 0; |
Kerneels Bezuidenhout |
37:93c7563f2078 | 299 | calStarted = false; |
Kerneels Bezuidenhout |
37:93c7563f2078 | 300 | } |
Kerneels Bezuidenhout |
35:6baa558d3b86 | 301 | } |
Kerneels Bezuidenhout |
34:0b308a399f72 | 302 | } |