-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathCasseroleThrottle.ino
84 lines (68 loc) · 1.85 KB
/
CasseroleThrottle.ino
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
#include "encoder.h"
#include "display.h"
#include "led.h"
#include "pwmOutput.h"
#define MIN_CMD_STEP 0.05/4.0
#define DEBUG
boolean isEnabled = false;
boolean btnPrev = false;
double curMotorCmd = 0.0;
long prevEncoderCount = 0;
void setup()
{
initEncoder();
initLED();
initPWMOutput();
Serial.begin(115200);
}
void loop()
{
// Read inputs from encoder
updateEncoder();
// Read button and update enabled/disabled state
boolean btnCur = getEncoderButton();
if(btnCur == true && btnPrev == false){
isEnabled = !isEnabled;
if(isEnabled){
setPWMOutputActive();
} else {
setPWMOutputInactive();
}
}
// Read the new encoder position and convert to a change in current speed command
long curEncCount = getEncoderPosition();
double motorSpdDelta = (curEncCount - prevEncoderCount)*MIN_CMD_STEP;
// Calculate current motor command, including range limits.
if(isEnabled){
curMotorCmd += motorSpdDelta;
if(curMotorCmd > 1.0){
curMotorCmd = 1.0;
} else if (curMotorCmd < -1.0){
curMotorCmd = -1.0;
}
} else {
curMotorCmd = 0.0;
}
setPWMVal(curMotorCmd);
// Calculate LED state
if(isEnabled){
setLEDActiveOutput(curMotorCmd);
} else {
setLEDIdle();
}
// Update outputs
updatePWM();
updateLED();
// Save state for next loop
btnPrev = btnCur;
prevEncoderCount = curEncCount;
//debug
#ifdef DEBUG
Serial.println("==============================");
Serial.println(curEncCount);
Serial.println(btnCur);
Serial.println(isEnabled);
Serial.println(curMotorCmd);
Serial.println("==============================\n\n");
#endif
}