-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathSUDO-Robot Code
137 lines (107 loc) · 2.6 KB
/
SUDO-Robot Code
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
#include "WPILib.h"
#include "string"
class FYRERobot: public IterativeRobot
{
RobotDrive *m_robotDrive; // RobotDrive object using PWM 1-4 for drive motors
Joystick *m_driveStick; // Joystick object on USB port 1 (mecanum drive)
Talon *m_robotLift;
Joystick *m_liftStick;
Encoder *m_liftEncoder;
DigitalInput *m_topLimitSwitch;
DigitalInput *m_bottomLimitSwitch;
float rightXboxY;
bool rightBumper;
bool m_topLimit;
bool m_bottomLimit;
public:
FYRERobot(void)
{
// Create a RobotDrive object using PWMS 1, 2, 3, and 4
m_robotDrive = new RobotDrive(0, 1);
// Define joystick being used at USB port #1 on the Drivers Station
m_driveStick = new Joystick(0);
// Twist is on Axis 3 for the Extreme 3D Pro
m_driveStick->SetAxisChannel(Joystick::kTwistAxis, 3);
// Create a RobotDrive object using PWMS 5
m_robotLift = new Talon(2);
m_liftEncoder = new Encoder(0, 1);
// Define joystick being used at USB port #2 on the Drivers Station
m_liftStick = new Joystick(1);
// Define two swtiches
m_topLimitSwitch = new DigitalInput(4);
m_bottomLimitSwitch = new DigitalInput(5);
}
private:
LiveWindow *lw;
void RobotInit()
{
lw = LiveWindow::GetInstance();
}
void AutonomousInit()
{
//liftEncoder->Reset();
// lift box
m_robotLift->Set(.5);
Wait(1000);
// while(m_liftEncoder<=1000) {
// m_robotLift->Set(.5)
//Wait(20)}
m_robotLift->Set(0);
//drive forward
m_robotDrive->Drive(.5, 0);
Wait(8000);
m_robotDrive->Drive(0,0);
// lower box
m_robotLift->Set(-.5);
Wait(1000);
// while(m_liftEncoder>=500) {
// m_robotLift->Set(.5)
// Wait(20)}
m_robotLift->Set(0);
// drive backward
m_robotDrive->Drive(-.5,0);
Wait(1000);
m_robotDrive->Drive(0,0);
}
void AutonomousPeriodic()
{
}
void TeleopInit()
{
m_liftEncoder->Reset();
}
void TeleopPeriodic()
{
// Update Variables
rightXboxY = m_liftStick->GetRawAxis(5);
rightBumper = m_liftStick->GetRawButton(6);
m_topLimit = m_topLimitSwitch->Get();
m_bottomLimit = m_bottomLimitSwitch->Get();
m_robotDrive->ArcadeDrive(m_driveStick->GetY(), (m_driveStick->GetX())*-1);
if(m_bottomLimit == 1 && rightXboxY<=0){
m_robotLift->Set(0);
}
else if(m_topLimit == 1 && rightXboxY >=0){
m_robotLift->Set(0);
}
else{
if (rightXboxY >= 0.2 || rightXboxY <= -0.2){
m_robotLift->Set(rightXboxY);
}
else
{
m_robotLift->Set(0);
}
}
if (rightBumper == true){
m_robotLift->Set(rightXboxY);
}
//printf("%F",rightXboxY);
printf("%F\n", m_liftEncoder->GetDistance());
}
void TestPeriodic()
{
lw->Run();
}
};
START_ROBOT_CLASS(FYRERobot);