-
Notifications
You must be signed in to change notification settings - Fork 0
/
competitionV2.0
190 lines (173 loc) · 6.29 KB
/
competitionV2.0
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
#pragma config(Motor, port1, tilter, tmotorVex393_HBridge, openLoop)
#pragma config(Motor, port2, frontRightMotor, tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor, port3, backRightMotor, tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor, port4, frontLeftMotor, tmotorServoContinuousRotation, openLoop)
#pragma config(Motor, port5, backLeftMotor, tmotorServoContinuousRotation, openLoop)
#pragma config(Motor, port6, elevator, tmotorServoContinuousRotation, openLoop)
#pragma config(Motor, port7, elevator2, tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor, port8, elevatorR, tmotorServoContinuousRotation, openLoop, reversed)
#pragma config(Motor, port9, elevatorL, tmotorServoContinuousRotation, openLoop)
#pragma config(Motor, port10, claw, tmotorVex393_HBridge, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
/*----------------------------------------------------------------------------------------------------*\
|* - Dual Joystick Control with 4 Motors - *|
|* ROBOTC on VEX 2.0 Cortex *|
|* *|
|* This program uses both the Left and the Right joysticks to run the robot using "tank control". *|
|* *|
|* ROBOT CONFIGURATION *|
|* NOTES: *|
|* 1) Reversing both right-side motors (ports 2 and 3) in the "Motors and Sensors Setup" is *|
|* needed with the "VEX Tumbler" model, but may not be needed for all robot configurations. *|
|* 2) Ch1 is the X axis and Ch2 is the Y axis for the RIGHT joystick. *|
|* 3) Ch3 is the Y axis and Ch4 is the X axis for the LEFT joystick. *|
|* *|
|* MOTORS & SENSORS: *|
|* [I/O Port] [Name] [Type] [Description] *|
|* Motor Port 2 frontRightMotor VEX 3-wire module Right side motor *|
|* Motor Port 3 backRightMotor VEX 3-wire module Right side motor *|
|* Motor Port 4 frontLefttMotor VEX 3-wire module Left side motor *|
|* Motor Port 5 backLeftMotor VEX 3-wire module Left side motor *|
\*----------------------------------------------------------------------------------------------------*/
//+++++++++++++++++++++++++++++++++++++++++++++| MAIN |+++++++++++++++++++++++++++++++++++++++++++++++
void forward(){//moves robot forward
motor[frontRightMotor] = -70;
motor[backRightMotor] = -70;
motor[frontLeftMotor] = -70;
motor[backLeftMotor] = -70;
}
void backward(){//moves robot backwards
motor[frontRightMotor] = 70;
motor[backRightMotor] = 70;
motor[frontLeftMotor] = 70;
motor[backLeftMotor] = 70;
}
/* void left(){
motor[frontRightMotor] = 70;
motor[backRightMotor] = 70;
motor[frontLeftMotor] = -70;
motor[backLeftMotor] = -70;
}
void right(){
motor[frontRightMotor] = -70;
motor[backRightMotor] = -70;
motor[frontLeftMotor] = 70;
motor[backLeftMotor] = 70;
}*/
void turnRight(){//rotates the robot right
motor[frontRightMotor] = 70;
motor[backRightMotor] = 70;
motor[frontLeftMotor] = -70;
motor[backLeftMotor] = -70;
}
void turnLeft(){//rotates the robot left
motor[frontRightMotor] = -70;
motor[backRightMotor] = -70;
motor[frontLeftMotor] = 70;
motor[backLeftMotor] = 70;
}
void elevate(){//raises the forklift up to is maximum point (takes max 1.5s)
clearTimer(T1);
while(time1[T1] < 1500){
motor[elevator] = 127;
motor[elevator2] = 127;
motor[elevatorR] = 127;
motor[elevatorL] = 127;
}
motor[elevator] = 0;
motor[elevator2] = 0;
motor[elevatorR] = 0;
motor[elevatorL] = 0;
}
void elevateDown(){//lowers the forklift to its neutral position (takes max 1.4s)
clearTimer(T2);
while(time1[T2] < 1400){
motor[elevator] = -127;
motor[elevator2] = -127;
motor[elevatorR] = -127;
motor[elevatorL] = -127;
}
motor[elevator] = 0;
motor[elevator2] = 0;
motor[elevatorR] = 0;
motor[elevatorL] = 0;
}
void tiltUp(){//tilts the forklift up
clearTimer(T3);
while(time1[T3] < 500){
motor[tilter] = 120;
}
motor[tilter] = 0;
}
void tiltDown(){//tilts the forklift down to pick up stuff
clearTimer(T3);
while(time1[T3] < 500){
motor[tilter] = -120;
}
motor[tilter] = 0;
}
void grab(){//snaps the clamps together to 'grab' the thing
clearTimer(T4);
while(time1[T4] < 500){
motor[tilter] = 120;
}
motor[tilter] = 0;
}
void release(){//release the clamp
clearTimer(T4);
while(time1[T4] < 500){
motor[tilter] = -120;
}
motor[tilter] = 0;
}
void stopMotors() {//stops all motors
motor[frontRightMotor] = 0;
motor[backRightMotor] = 0;
motor[frontLeftMotor] = 0;
motor[backLeftMotor] = 0;
motor[claw] = 0;
motor[tilter] = 0;
motor[elevator] = 0;
motor[elevator2] = 0;
motor[elevatorR] = 0;
motor[elevatorL] = 0;
}
task main()
{
while(1 == 1)
{
if(vexRT[Btn7U] == 1){
forward();
}
/*if(vexRT[Btn7L] == 1){
left();
}
if(vexRT[Btn7R] == 1){
right();
}*/
if(vexRT[Btn7D] == 1){
backward();
}
if(vexRT[Btn8L] == 1){
turnLeft();
}
if(vexRT[Btn8R] == 1){
turnRight();
}
if(vexRT[Btn5U] == 1){
elevate();
tilt();
}
if(vexRT[Btn5D] == 1){
elevateDown();
tiltDown();
}
if(vexRT[Btn7D] == 1){
grab();
}
if(vexRT[Btn7U] == 1){
release();
}
stopMotors();
}
}