Repository navigation
Expand file tree
/
Copy pathmain.cpp
More file actions
131 lines (113 loc) · 4.31 KB
/
Copy pathmain.cpp
File metadata and controls
131 lines (113 loc) · 4.31 KB
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
/*----------------------------------------------------------------------------*/
/* */
/* Module: main.cpp */
/* Author: C:\Users\0edlo */
/* Created: Sun Jan 05 2020 */
/* Description: V5 project */
/* */
/*----------------------------------------------------------------------------*/
// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name] [Type] [Port(s)]
// VerticalMotorLeft motor 1
// Controller1 controller
// VerticalMotorRight motor 2
// HorizontalMotorBot motor 3
// HorizontalMotorTop motor 4
// leftIntakeMotor motor 5
// rightIntakeMotor motor 6
// leftRampLever motor 7
// rightRampLever motor 8
// SexYMotor motor 21
// ---- END VEXCODE CONFIGURED DEVICES ----
#include "vex.h"
using namespace vex;
void configureMotors() {
//Configuring Driving motors
VerticalMotorLeft.spin(forward);
VerticalMotorRight.spin(forward);
HorizontalMotorBot.spin(forward);
HorizontalMotorTop.spin(forward);
//Configuring Intake Motors
leftIntakeMotor.spin(forward);
rightIntakeMotor.spin(forward);
//Configuring Ramp Levers
leftRampLever.spin(forward);
rightRampLever.spin(forward);
}
void setDriveMotorVelocities(double verticalVelocity, double horizontalVelocity) {
VerticalMotorLeft.setVelocity(verticalVelocity,rpm);
VerticalMotorRight.setVelocity(verticalVelocity,rpm);
HorizontalMotorBot.setVelocity(horizontalVelocity,rpm);
HorizontalMotorTop.setVelocity(horizontalVelocity,rpm);
}
int main() {
// Initializing Robot Configuration. DO NOT REMOVE!
vexcodeInit();
//Configures all of Robot's Motors
configureMotors();
bool leftIntakeOn = false;
bool rightIntakeOn = false;
bool leftIntakeDirection = true;
bool rightIntakeDirection = false;
double verticalVelocities = 0;
double horizontalVelocities = 0;
while(true) {
//Sets drive motor velocities base on right joystick
verticalVelocities = Controller1.Axis2.position(percent);
horizontalVelocities = Controller1.Axis1.position(percent);
setDriveMotorVelocities(verticalVelocities, horizontalVelocities);
//Turns ON and OFF right intake
if(Controller1.ButtonR1.pressing()) {
rightIntakeOn = !rightIntakeOn;
if(rightIntakeOn) {
rightIntakeMotor.setVelocity(100,rpm);
} else {
rightIntakeMotor.setVelocity(0,rpm);
}
}
//Turns ON and OFF left intake
if(Controller1.ButtonL1.pressing()) {
leftIntakeOn = !leftIntakeOn;
if(leftIntakeOn) {
leftIntakeMotor.setVelocity(100,rpm);
} else {
leftIntakeMotor.setVelocity(0,rpm);
}
}
//Reverses right intake flow
if(Controller1.ButtonR2.pressing()) {
rightIntakeDirection = !rightIntakeDirection;
if(rightIntakeDirection) {
rightIntakeMotor.spin(forward);
} else {
rightIntakeMotor.spin(reverse);
}
}
//Reverses left intake flow
if(Controller1.ButtonL2.pressing()) {
leftIntakeDirection = !leftIntakeDirection;
if(leftIntakeDirection) {
leftIntakeMotor.spin(forward);
} else {
leftIntakeMotor.spin(reverse);
}
}
//Raises left ramp updwards
if(Controller1.ButtonUp.pressing()) {
//TO DO: Code that implements PID control to raise left ramp upwards
}
//Raises left ramp downwards
if(Controller1.ButtonDown.pressing()) {
//TO DO: Code to raise ramp down and stop when hits bumper switch
}
//Raises right ramp updwards
if(Controller1.ButtonX.pressing()) {
//TO DO: Code that implements PID control to raise right ramp upwards
}
//Raises right ramp downwards
if(Controller1.ButtonB.pressing()) {
//TO DO: Code to raise ramp down and stop when hits bumper switch
}
}
}