-
Notifications
You must be signed in to change notification settings - Fork 0
/
RangeTesting.c
115 lines (106 loc) · 3.07 KB
/
RangeTesting.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
#pragma config(Sensor, dgtl1, greenLED, sensorLEDtoVCC)
#pragma config(Sensor, dgtl4, LeftEncoder, sensorQuadEncoder)
#pragma config(Sensor, dgtl6, RightEncoder, sensorQuadEncoder)
#pragma config(Sensor, dgtl11, , sensorQuadEncoder)
#pragma config(Motor, port8, Shooter8, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port9, Shooter9, tmotorVex393_MC29, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#define JOYSTICK_AVAIL true
int shooter_target_speed = 0;
task Shooting()
{
int print_value = 0;
int last_clicks_left = 0;
int last_clicks_right = 0;
while(true)
{
motor[Shooter8] = shooter_target_speed;
motor[Shooter9] = shooter_target_speed;
if (shooter_target_speed > 80)
{
SensorValue[dgtl1] = 1;
}
else
{
SensorValue[dgtl1] = 0;
}
//get current
int current_right_clicks = SensorValue[RightEncoder];
int current_left_clicks = SensorValue[LeftEncoder];
//calculate elapsed
int elapsed_right_clicks = current_right_clicks - last_clicks_right;
int elapsed_left_clicks = current_left_clicks - last_clicks_left;
//print
print_value = (print_value + 1);
if(print_value == 20){
writeDebugStreamLine("elapsed_right_clicks: %d", elapsed_right_clicks);
writeDebugStreamLine("elapese_left_clicks: %d", elapsed_left_clicks);
//save current to last
writeDebugStreamLine("shooter_target_speed: %d", shooter_target_speed);
last_clicks_left = current_left_clicks;
last_clicks_right = current_right_clicks;
delay(100);
print_value = 0;
}
}
}
void set_motor_target_speed(int speed){
// For now, set value and print a debug statement
shooter_target_speed = speed;
writeDebugStreamLine("Shooter Target Speed: %d", shooter_target_speed);
}
void adjust_target_speed(int adjustment){
// For now, adjust value a bit, and print a debug statement
shooter_target_speed = shooter_target_speed + adjustment;
writeDebugStreamLine("Shooter Target Speed: %d", shooter_target_speed);
}
task Controls()
{
while(true){
if (!JOYSTICK_AVAIL){
set_motor_target_speed(65);
delay(10000);
}
// Check each button and do actions
else if(vexRT[Btn8U] == 1){
// Set motor target speed (Far), Channel 8, Button U
set_motor_target_speed(80);
delay(500);
}
else if(vexRT[Btn8R] == 1){
// Set motor target speed (Medium), Channel 8, Button R
set_motor_target_speed(60);
delay(500);
}
else if(vexRT[Btn8D] == 1){
// Set motor target speed (Short), Channel 8, Button D
set_motor_target_speed(40);
delay(500);
}
else if(vexRT[Btn6U] == 1){
// Adjust motor target speed (5 up), Channel 6, Button U
adjust_target_speed(1);
delay(250);
}
else if(vexRT[Btn6D] == 1){
// Adjust motor target speed (5 down), Channel 6, Button D
adjust_target_speed(-1);
delay(250);
}
else if(vexRT[Btn8L] == 1){
// Stop all motor speed (0), Channel 8, Button L
set_motor_target_speed(0);
delay(500);
}
delay(10);
}
}
task main()
{
startTask(Shooting);
startTask(Controls);
while(true)
{
delay(1000);
}
}