-
Notifications
You must be signed in to change notification settings - Fork 0
/
ShooterPowerControlV5.c
80 lines (63 loc) · 1.62 KB
/
ShooterPowerControlV5.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
#include "Filter.c"
#include "RealRobotPowerTable.c"
#include "StopLightControl.c"
#include "StableCheck.c"
#define ADJUST_LOCKOUT_CYCLES 40
int shooter_target_speed = 0;
int power_offset = 0;
bool backOut = false;
// backOut is for returning a ball from the shooter back into the intake.
bool toggleShooterWheels = false;
// Global Variables for shooter control
// These are set by the joystick methods
task shooter_power_control()
{
int RangePower = 0;
while(true)
{
if (backOut == true){
motor[leftTopShooter] = -100;
motor[rightTopShooter] = -100;
}
else{
// Set the range_power by Shooter Target Speed
// If the Shooter Target Speed is 0, Stop
if (shooter_target_speed == 0){
RangePower = 0;
}
// Else if STS is 910 (long range) then set RangePower to power high
else if (shooter_target_speed == 910){
RangePower = 68;
}
// Else if STS is 780, set RangePower to mid
else if (shooter_target_speed == 780){
RangePower = 38;
}
// Else if STS is 620, set to short
else if (shooter_target_speed == 620){
RangePower = 32;
}
// Else something is wrong, set to 0
else {
RangePower = 0;
}
// Control motor power
motor[leftTopShooter] = RangePower + power_offset;
motor[rightTopShooter] = RangePower + power_offset;
// End of power control work.
delay(20);
}
}
}
void set_shooter_targets(int speed){
// For now, set value and print a debug statement
shooter_target_speed = speed;
}
void adjust_shooter_targets(int adjustment){
if (adjustment == 10){
power_offset = power_offset + 1;
}
else if (adjustment == -10){
power_offset = power_offset - 1;
}
}