-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathmain
129 lines (117 loc) · 4.48 KB
/
main
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
#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, port9, elevator, tmotorServoContinuousRotation, openLoop)
//*!!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(){
motor[frontRightMotor] = 127;
motor[backRightMotor] = 127;
motor[frontLeftMotor] = 127;
motor[backLeftMotor] = 127;
}
void backward(){
motor[frontRightMotor] = -127;
motor[backRightMotor] = -127;
motor[frontLeftMotor] = -127;
motor[backLeftMotor] = -127;
}
void left(){
motor[frontRightMotor] = -127;
motor[backRightMotor] = 127;
motor[frontLeftMotor] = -127;
motor[backLeftMotor] = 127;
}
void right(){
motor[frontRightMotor] = 127;
motor[backRightMotor] = -127;
motor[frontLeftMotor] = 127;
motor[backLeftMotor] = -127;
}
void turnRight(){
motor[frontRightMotor] = 127;
motor[backRightMotor] = 127;
motor[frontLeftMotor] = -127;
motor[backLeftMotor] = -127;
}
void turnLeft(){
motor[frontRightMotor] = -127;
motor[backRightMotor] = -127;
motor[frontLeftMotor] = 127;
motor[backLeftMotor] = 127;
}
void elevate(){
ClearTimer(T1);
while(time1[T1] < 1500){
motor[elevator] = 120;
}
}
void elevateDown(){
ClearTimer(T2);
while(time1[T2] < 1500){
motor[elevator] = -120;
}
}
void stopMotors() {
motor[frontRightMotor] = 0;
motor[backRightMotor] = 0;
motor[frontLeftMotor] = 0;
motor[backLeftMotor] = 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();
}
if(vexRT[Btn5D] == 1){
elevateDown();
}
if(vexRT[Btn6U] == 1){
motor[elevator] = 0;
}
if(vexRT[Btn6D] == 1){
stopMotors();
}
}
}