-
Notifications
You must be signed in to change notification settings - Fork 0
/
main.cpp
208 lines (180 loc) · 5.05 KB
/
main.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
/* PROJECT: MPRO Laboraufgabe 4
*
* DEVELOPERS: Jannik Kreucher
* Nick Diendorf
* Benedikt Wendling
*/
#include "mbed.h"
#include <cstdint>
/*** PIN DEFINITIONS ***/
// Nucleo On-Board
#define LED_NUCLEO PA_5
#define SW_NUCLEO PC_13
// Velleman STEM Shield I/O
#define LED_RED1 PB_0
#define LED_ORANGE1 PC_1
#define LED_GREEN1 PA_10
#define LED_RED2 PB_3
#define LED_ORANGE2 PB_5
#define LED_GREEN2 PB_4
#define SW_1 PA_9
#define SW_2 PC_7
#define SW_3 PB_6
#define SW_4 PA_7
#define SW_5 PA_6
#define PIN_SERVO PB_10
#define PRESS_SHORT 50 // in multiples of task period (here 10ms)
#define SERVO_PERIOD_MAX 2100 //in us
#define SERVO_PERIOD_MIN 1100 //in us
// Input / Output
PwmOut pinServo(PIN_SERVO);
DigitalOut pinLedOrange1(LED_ORANGE1);
DigitalOut pinLedOrange2(LED_ORANGE2);
DigitalOut pinLedRed2(LED_RED2);
DigitalOut pinLedGreen2(LED_GREEN2);
DigitalIn pinSW1(SW_1);
DigitalIn pinSW2(SW_2);
DigitalIn pinSW3(SW_3);
DigitalIn pinSW4(SW_4);
DigitalIn pinSW5(SW_5);
// timer interrupt
Ticker sysTick;
// task struct
struct stTask {
void (*handler)();
uint16_t period;
uint16_t counter;
};
// global variables
int32_t servoPeriod = 0;
int32_t servoPeriodSave[2];
volatile uint8_t standby = 0;
uint16_t counterSW3 = 0, counterSW4 = 0;
void vButtonsPosition() {
// check if in program mode
if(pinSW1 == 1) {
// check switch 2
if(pinSW2 == 1) {
servoPeriod += 10;
pinLedOrange2 = 1;
pinLedGreen2 = 0;
} else {
pinLedOrange2 = 0;
}
// check switch 5
if(pinSW5 == 1) {
servoPeriod -= 10;
pinLedOrange1 = 1;
pinLedGreen2 = 0;
} else {
pinLedOrange1 = 0;
}
}
}
void vButtonsMemory() {
// check switch 3
if(pinSW3 == 1) {
// button pressed
if(counterSW3 < PRESS_SHORT) {
counterSW3++;
} else if(counterSW3 == PRESS_SHORT) {
// long press
// set counter to one count above short press so this does not get executed one more time
counterSW3++;
// check if in program mode
if(pinSW1 == 1) {
// save servo position
servoPeriodSave[0] = servoPeriod;
pinLedRed2 = 1;
}
}
} else {
// button not pressed (released)
if((counterSW3 < PRESS_SHORT) && (counterSW3 != 0)) {
// short press detected
// recall saved position
servoPeriod = servoPeriodSave[0];
pinLedGreen2 = 1;
}
// check if button has been pressed
if(counterSW3 != 0) {
pinLedRed2 = 0;
}
// reset switch counter
counterSW3 = 0;
}
// check switch 4
if(pinSW4 == 1) {
// button pressed
if(counterSW4 < PRESS_SHORT) {
counterSW4++;
} else if(counterSW4 == PRESS_SHORT) {
// long press
// set counter to one above short press so this does not get executed one more time
counterSW4++;
// check if in program mode
if(pinSW1 == 1) {
// save servo position
servoPeriodSave[1] = servoPeriod;
pinLedRed2 = 1;
}
}
} else {
// button not pressed (released)
if((counterSW4 < PRESS_SHORT) && (counterSW4 != 0)) {
// short press detected
// recall saved position
servoPeriod = servoPeriodSave[1];
pinLedGreen2 = 1;
}
// check if button has been pressed
if(counterSW4 != 0) {
pinLedRed2 = 0;
}
// reset switch counter
counterSW4 = 0;
}
// clamp servo position
if(servoPeriod > SERVO_PERIOD_MAX) servoPeriod = SERVO_PERIOD_MAX;
if(servoPeriod < SERVO_PERIOD_MIN) servoPeriod = SERVO_PERIOD_MIN;
// set servo period
pinServo.pulsewidth_us(servoPeriod);
}
// task list
struct stTask schedulerTasks[] = {
{vButtonsMemory, 10, 0},
{vButtonsPosition, 15, 0}
};
void vScheduler() {
// check for each task if it needs to be called
for(uint8_t taskid=0; taskid < (sizeof(schedulerTasks)/sizeof(stTask)); taskid++) {
if(schedulerTasks[taskid].counter == 0) {
// time has come
// reset counter
schedulerTasks[taskid].counter = schedulerTasks[taskid].period;
// call task handler
schedulerTasks[taskid].handler();
} else {
// decrement task counter
schedulerTasks[taskid].counter--;
}
}
}
void vTick() {
standby = 0;
}
int main() {
// configure servo frequency to 50Hz
pinServo.period_ms(20);
// time slice interrupt
sysTick.attach(&vTick, 1ms);
while(1) {
// execute scheduler every 1ms
// this function checks if a task needs to be executed
vScheduler();
// wait for time slice
standby = 1;
while(standby);
}
return 0;
}