This repository has been archived by the owner on Feb 21, 2019. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 0
/
RobotCode.c
203 lines (178 loc) · 5.46 KB
/
RobotCode.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
#pragma config(Sensor, dgtl10, redLED, sensorLEDtoVCC)
#pragma config(Sensor, dgtl11, yellowLED, sensorLEDtoVCC)
#pragma config(Sensor, dgtl12, greenLED, sensorLEDtoVCC)
#pragma config(Motor, port2, liftMotor1, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port3, liftMotor2, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port4, rightMotor, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port5, leftMotor, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port6, clawMotor, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port7, swivelMotor, tmotorVex393_MC29, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#pragma platform(VEX2)
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)
#include "Vex_Competition_Includes.c"
/*-----------------------------------------------------------------------------------
Robot code for GHS VEX Team G
2018-19 Competiton Season
Team members are Lucas Ritzdorf, Riker Foster, and Lauryn Vornbrock.
This is the full, (hopefully) competition-ready version of the RobotC code.
-----------------------------------------------------------------------------------*/
/*
Remote mappings:
Left stick Y-value ------- left drive ------- Ch3
Right stick Y-value ------ right drive ------ Ch2
Right shoulder buttons --- lift up, down ---- Btn6U, Btn6D
Right buttons U, D ------- claw up, down ---- Btn8U, Btn8D
Right buttons L, R ------- claw open, close - Btn8L, Btn8R
Top left shoulder button - direction switch - Btn5U
*/
// Variable Setup
// Standard sleep value (in milliseconds)
// Can be adjusted for responsiveness as needed
const int sleepValue = 20;
// Current direction-switching state
// false is normal, true is reversed
bool reversed = false;
// Autonomous Definitions
const float turnTime = 1000;
float clawTime = 500;
int clawDir = -1;
float liftTime = 3000;
int liftDir = 1;
// Pre-autonomous setup
void pre_auton() {
turnLEDOn(redLED);
bStopTasksBetweenModes = true;
// Pre-driving setup
// Set servo positions, etc.
turnLEDOff(redLED);
return;
}
// Task to open or close the claw concurrently
task claw()
{
setMotor(clawMotor, clawDir*127);
sleep(clawTime);
stopMotor(clawMotor);
}
// Task to raise or lower the lift concurrently
task lift()
{
setMultipleMotors(liftDir*127, liftMotor1, liftMotor2);
sleep(liftTime);
stopMultipleMotors(liftMotor1, liftMotor2);
}
// Just for points:
task autonomous {
setMultipleMotors(127, liftMotor1, liftMotor2);
sleep(1500);
stopMultipleMotors(liftMotor1,liftMotor2);
setMultipleMotors(100, leftMotor, rightMotor);
sleep(2500);
stopAllMotors();
}
// Autonomous mode
/*task autonomous() {
turnLEDOn(yellowLED);
// Begin autonomous code
waitUntil(vexRT[Btn8R] == 1);
turnLEDOn(greenLED);
// Turn left
setMotor(rightMotor, 127);
sleep(1700);
// Drive forwards
setMultipleMotors(127, leftMotor, rightMotor);
sleep(1750);
// Turn slightly right and drive forwards
setMotor(leftMotor, 64);
stopMotor(rightMotor);
sleep(500);
setMotor(rightMotor, 64);
sleep(1000);
// Back up and turn (and open claw)
clawDir = -1; startTask(claw); clearTimer(T1);
setMotor(leftMotor, -127);
setMotor(rightMotor, -32);
sleep(1500);
// Back up
setMultipleMotors(leftMotor, rightMotor, -127);
sleep(1500);
// Turn right
motor[leftMotor] = 127;
motor[rightMotor] = -127;
sleep(turnTime+500);
// Drive forwards (after the claw has opened)
while(time1[T1] < clawTime) { sleep(0.1); }
setMultipleMotors(64, leftMotor, rightMotor);
sleep(1500);
// Flip the cap
setMultipleMotors(32, leftMotor, rightMotor);
setMotor(liftMotor, 127);
sleep(1500);
stopAllMotors();
sleep(250);
// Back up and lower lift
liftDir = -1; liftTime = 3; startTask(lift); clearTimer(T2);
setMultipleMotors(-127, leftMotor, rightMotor);
sleep(1750);
// Turn right
setMotor(leftMotor, 127);
setMotor(rightMotor, -127);
sleep(turnTime);
// Drive forward
setMultipleMotors(127, leftMotor, rightMotor);
sleep(1000);
// Turn left
setMotor(leftMotor, -127);
setMotor(rightMotor, 127);
sleep(turnTime);
// Drive forward
setMultipleMotors(127, leftMotor, rightMotor);
sleep(2500);
// Lift the cap
setMotor(liftMotor, 64);
setMultipleMotors(32, leftMotor, rightMotor);
sleep(1000);
// Turn and set the cap down
setMotor(leftMotor, -32);
setMotor(rightMotor, 16);
sleep(2000);
// Done
stopAllMotors();
// End autonomous code
turnLEDOff(yellowLED);
}
*/
// Driver-control code
task usercontrol() {
turnLEDOn(greenLED);
while (true) {
// Drive motor control
if (reversed) {
motor[leftMotor] = -vexRT[Ch2];
motor[rightMotor] = -vexRT[Ch3];
}
else {
motor[leftMotor] = vexRT[Ch3];
motor[rightMotor] = vexRT[Ch2];
}
// Drive motor reversal
if (vexRT[Btn5U] == 1) { reversed = !reversed; sleep(20); }
// Lift control
if (vexRT[Btn6U] == 1) { setMultipleMotors(127, liftMotor1, liftMotor2); }
else if (vexRT[Btn6D] == 1) { setMultipleMotors(-100, liftMotor1, liftMotor2); }
else { stopMultipleMotors(liftMotor1, liftMotor2); }
// Claw swivel control
if (vexRT[Btn8U] == 1) { motor[swivelMotor] = 127; }
else if (vexRT[Btn8D] == 1) { motor[swivelMotor] = -100; }
else { stopMotor(swivelMotor); }
// Claw grip control
if (vexRT[Btn8L] == 1) { motor[clawMotor] = 127; }
else if (vexRT[Btn8R] == 1) { motor[clawMotor] = -127; }
else { stopMotor(clawMotor); }
// Pause
sleep(sleepValue);
}
}