-
Notifications
You must be signed in to change notification settings - Fork 0
/
pidShooterControl.c
272 lines (225 loc) · 7.74 KB
/
pidShooterControl.c
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
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
#include "EvenMoreRealShooterPowerTable.c"
#include "StopLightControl.c"
#include "StableCheck.c"
#define ADJUST_LOCKOUT_CYCLES 35
#define PID_INTEGRAL_LIMIT 3000
int shooter_target_speed = 0;
bool speedChange = false;
bool backOut = false;
// backOut is for returning a ball from the shooter back into the intake.
bool toggleShooterWheels = false;
bool firstRecoveryLoop = false;
task shooter_power_control(){
// PID CONSTANTS
float pid_Kp = 0.045;
float pid_Ki = 0.006;
float pid_Kd = 0;
// create variables
// left variables
int last_left_clicks = 0;
float last_Error = 0;
float pid_Integral = 0;
SensorValue[leftShooter] = 0;
// right variables
int last_right_clicks = 0;
SensorValue[rightShooter] = 0;
// Stabilty Checking variables, for auto shooting
int loop_counter = 0;
int green_counter = 0;
// Print Loop Counter
int print_counter = 0;
// loop
while(true){
// First, are we transitioning between forward and backwards, or vise versa?
if (toggleShooterWheels == true){
motor[leftTopShooter] = 0;
motor[rightTopShooter] = 0;
delay(2000);
toggleShooterWheels = false;
}
// Backout mode is simple
if (backOut == true){
motor[leftTopShooter] = -100;
motor[rightTopShooter] = -100;
}
else if (shooter_target_speed == 0){
motor[leftTopShooter] = 0;
motor[rightTopShooter] = 0;
}
// Else we're going forward
else
{
// If the speed was changed significantly, reset the Integral
if (speedChange){
pid_Integral = 0;
speedChange = false;
}
// get speed for both sides
// Variables for power control
// int current_right_clicks = SensorValue[rightShooter];
// Sensor's backwords
int current_left_clicks = SensorValue[rightShooter]/3;
// current raw speed
// int elapsed_right_clicks = current_right_clicks - last_right_clicks;
int elapsed_left_clicks = current_left_clicks - last_left_clicks;
// int elapsed_time = current_read_time - last_read_time;
int elapsed_clicks = elapsed_left_clicks * 1;
// filtered speed
int current_speed = elapsed_clicks;
// Stoplight color controls
int color = NONE;
int right_color = NONE;
// Save Last Numbers for next loop
last_left_clicks = current_left_clicks;
// We may be in RECOVERY MODE
if (ShooterMode == RECOVERY){
// Reseting the integral when we're close to the target speed should
// reduce the overshoot error on the integral, and get to the target speed quicker
// Set motors to 100 power until we're at 90% of target speed, on one of the flywheels
motor[leftTopShooter] = 127;
motor[rightTopShooter] = 127;
if(firstRecoveryLoop)
firstRecoveryLoop = false;
else{
// Set motors to 100 power until we're at 90% of target speed, on one of the flywheels
// at that point, we're in NORMAL MODE
if (current_speed > (shooter_target_speed - 4)){
ShooterMode = NORMAL;
writeDebugStreamLine("Exiting Recovery Mode - Reseting Integral - s.t.s = %d", shooter_target_speed);
writeDebugStreamLine("Left - current: %d", current_speed);
writeDebugStreamLine("Right - current: %d", current_speed);
//pid_Integral = 0;
}
}
//////////////////////////////////////////////////////////////
// Debug info
/////////////////////////////////////////////////////////////
print_counter = print_counter + 1;
if (print_counter == 10){
writeDebugStreamLine("Recovery Mode Speed: %d", current_speed);
print_counter = 0;
}
}
// If we are not in RECOVERY MODE, then use PID
else {
/////////////////////////////////////////////////////////////////
// DO PID
/////////////////////////////////////////////////////////////////
// We have the actual speed, and the current speed.
// Calculate ERROR = Current_Speed = Actual;
float pid_error = shooter_target_speed - current_speed;
// CALCULATE INTEGRAL - if Ki is not 0
if( pid_Ki != 0 )
{
// If the sign of the error changes, then reset the integral
//if (sgn(pid_error_left) != (sgn(pid_Integral_left)
// pid_Integral_left = pid_error_left;
// If we are inside controlable window then integrate the error
//else
if( abs(pid_error) < PID_INTEGRAL_LIMIT )
pid_Integral = pid_Integral + pid_error;
else
pid_Integral = 0;
}
else
pid_Integral = 0;
// CALCULATE DERIVATIVE
float pid_Derivative = pid_error - last_Error;
float pid_correction =
(pid_error * pid_Kp) +
(pid_Integral * pid_Ki) +
(pid_Derivative * pid_Kd);
// SET SPEED FOR Flywheel
int motor_power = getShooterPower(shooter_target_speed) + pid_correction;
if (motor_power > 127)
motor_power = 127;
if (motor_power < 0)
motor_power = 0;
motor[leftTopShooter] = motor_power;
motor[rightTopShooter] = motor_power;
// SAVE Left Variables
last_Error = pid_error;
///////////////////////////////////////////
// DO Lights
///////////////////////////////////////////
color = RED;
if (abs(pid_error) < (shooter_target_speed * .1))
color = YELLOW;
if (abs(pid_error) < (shooter_target_speed * .03))
color = GREEN;
// Set Stoplight
if(ShooterMode == READY_TO_SHOOT){
setStopLight(GREEN);
}
else if(color == NONE){
setStopLight(NONE);
}
else if(color == GREEN){
setStopLight(GREEN);
green_counter = green_counter + 1;
}
else if(color == RED){
setStopLight(RED);
}
else {
setStopLight(YELLOW);
}
//////////////////////////////////////////////////////////////
// Debug info
/////////////////////////////////////////////////////////////
print_counter = print_counter + 1;
if (print_counter == 10){
writeDebugStreamLine("Speed: %d, Offset Power: %d", current_speed, pid_correction);
print_counter = 0;
}
//////////////////////////////////////////////////////////////
// Determine Shooter Mode; for autoshooting
/////////////////////////////////////////////////////////////
// First, detect if we just shot a ball
// If we were ready to shoot, and the speed is way off ( <85% target speed on both sides
// Then we think we just fired a ball, this will reset the mode to "RECOVERY"
if ((ShooterMode == READY_TO_SHOOT)
&& (current_speed < (.9 * shooter_target_speed))){
ballFireDetected();
firstRecoveryLoop = true;
loop_counter = 0;
green_counter = 0;
// reset integrals after firing:
//pid_Integral = 0;
}
// Else we are not ready to shoot. Increment to loop counters, and green counts appropriately
// If there's enough Greens out of the last X times through the loop; then set the ShooterMode
// to READY_TO_SHOOT. This is stay, until Recovery mode.
else {
loop_counter = loop_counter + 1;
//Don't let the counts get too high!!!
if (loop_counter > 16){
green_counter = 0;
loop_counter = 0;
}
if (isShooterReady(green_counter,loop_counter)){
ShooterMode = READY_TO_SHOOT;
}
}
} // End Else (NORMAL MODE)
} // End Else (Going Forward)
delay(20);
} // End While loop
}
void set_shooter_targets(int speed){
// If we are changing to a higher speed (> 100 more speed)
// Then jump into recovery mode
if ((speed - 100) > shooter_target_speed){
ShooterMode = RECOVERY;
firstRecoveryLoop = true;
}
shooter_target_speed = speed;
speedChange = true;
writeDebugStreamLine("Shooter Target Speed: %d", shooter_target_speed);
}
void adjust_shooter_targets(int adjust_speed){
// For now, adjust value a bit, and print a debug statement
shooter_target_speed = shooter_target_speed + adjust_speed;
//speedChange = true;
writeDebugStreamLine("Shooter Target Speed: %d", shooter_target_speed);
}