-
Notifications
You must be signed in to change notification settings - Fork 1
/
TopServo.cpp
116 lines (98 loc) · 3.26 KB
/
TopServo.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
/**************************************************************************/
/**
Top Servo
@file TopServo.cpp
@author Charlie Chen <Charlie@CharmySoft.com>
@section Circuit
- Top Servo
+ Digital pin 13
*/
/**************************************************************************/
#define PIN_TOP_SERVO 13 /**< The pin attaching to the top servo */
/** Enable the timeout for the top servo. If enabled, the top servo will
move backwards when the color sensor's measuring cycle is longer than
the defined TIMEOUT value. This is used to solve jamming issues as
the Skittles get stuck in the top servo. */
#define F_TOP_SERVO_TIMEOUT_ENABLED true
#define C_TOP_SERVO_TIMEOUT 6000 /**< Timeout value in milliseconds */
/** How many times to retry if the color cannot be determined by the color
sensor. If set to 0, then this will be disabled. */
#define C_RETRY_UNKNOWN_COLOR 1
#include "TopServo.h"
#include "ColorSensor.h"
#include "LCD.h"
TopServo::TopServo()
{
#if F_TOP_SERVO_TIMEOUT_ENABLED
bJammed = false;
#endif
bRemeasuring = false;
bRemDir = false;
iRetry = 0;
}
void TopServo::setup()
{
this->attach(PIN_TOP_SERVO);
}
void TopServo::update()
{
boolean dirction = false; // Set direction to forwards
// Check if the color is being re-measured
if (bRemeasuring) dirction = bRemDir;
#if F_TOP_SERVO_TIMEOUT_ENABLED
// If jammed, reverse direction
if (bJammed) dirction = !dirction;
#endif
// Set the speed of the top servo
// with 0 being full-speed in one direction, 180 being full speed in the other, and a value near 90 being no movement.
this->write(dirction ? 180 : 0);
#if F_TOP_SERVO_TIMEOUT_ENABLED
static unsigned long lastStuckTime = 0;
boolean isTimeout = millis() - max(colorSensor.getLastSkittleTime(), lastStuckTime) > C_TOP_SERVO_TIMEOUT;
// If the top servo gets stuck
if (!bJammed && isTimeout
&& millis() - lastStuckTime > 1000) {
Serial.println(millis() - colorSensor.getLastSkittleTime());
Serial.println("Top servo is stuck; Direction reversed.");
lcd.setTopText("Stuck Reverse");
// Reserve the top servo's direction
bJammed = true;
lastStuckTime = millis();
}
if (bJammed && millis() - lastStuckTime > 1000) {
// Set direction back to normal after 1s
bJammed = false;
lastStuckTime = millis();
}
#endif
}
void TopServo::remeasureColor()
{
lcd.setBottomText("Color unknown");
if (iRetry < C_RETRY_UNKNOWN_COLOR) {
iRetry++;
bRemeasuring = true;
Serial.println("Color cannot be determined. Start going back and re-measuring it.");
} else {
Serial.println("Color cannot be determined.");
}
if (bRemeasuring) {
bRemDir = !bRemDir; // Reverse the remeasuring direction
Serial.print("The top servo is now going ");
Serial.println(bRemDir ? "backwards" : "forwards");
if (iRetry >= C_RETRY_UNKNOWN_COLOR) {
// If tried too many times and still can't get the color
// Just f*ck it cuz we ain't gonna get color however we try
Serial.println("Tried re-measuring but still couldn't get the color.");
stopRemeasuring();
}
}
}
void TopServo::stopRemeasuring() {
bRemeasuring = false;
bRemDir = false;
iRetry = 0;
}
boolean TopServo::isRemeasuring() {
return bRemeasuring;
}