-
Notifications
You must be signed in to change notification settings - Fork 0
/
Medic.cpp
114 lines (92 loc) · 2.01 KB
/
Medic.cpp
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
#include "MedicDrive.h"
#include "MedicOperatorInterface.h"
int step;
class Medic: public IterativeRobot
{
MedicDrive *drive;
Compressor *comp599;
MedicOperatorInterface *oi;
// Solenoid *rpmSensor;
// DigitalInput *rpmIRSensor;
Timer *timer;
public:
Medic()
{
oi = new MedicOperatorInterface();
drive = new MedicDrive();
comp599 = new Compressor(1, 1, 1, 1);//TODO: check values
// rpmSensor = new Solenoid(PNEUMATICS_24V_SLOT, 5);
// rpmIRSensor = new DigitalInput(1, SHOOTER_WHEEL_IR_CHANNEL);
oi->dashboard->init();
comp599->Start();
timer = new Timer();
}
void RobotInit()
{
}
void DisabledInit()
{
drive->leftEncoder->Start();
drive->rightEncoder->Start();
}
void AutonomousInit()
{
step = 0;
drive->leftEncoder->Reset();
drive->rightEncoder->Reset();
}
void TeleopInit()
{
drive->setLinVelocity(0);
drive->setTurnSpeed(0, false);
drive->drive();
}
void TestInit()
{
}
void DisabledPeriodic()
{
step = 0;
drive->leftEncoder->Reset();
drive->rightEncoder->Reset();
smartDashboardPrint();
}
void AutonomousPeriodic()
{
smartDashboardPrint();
}
void TeleopPeriodic()
{
comp599->Start();
while(IsOperatorControl())
{
teleDrive();
}
}
void TestPeriodic()
{
}
void teleDrive()
{
drive->setLinVelocity(-oi->getDriveJoystick()->GetY(Joystick::kRightHand));
drive->setTurnSpeed(oi->getDriveJoystick()->GetX(Joystick::kRightHand), oi->getDriveJoystickButton(1));
drive->drive();
drive->shift(oi->getDriveJoystickButton(8), oi->getDriveJoystickButton(9));
if(oi->getDriveJoystickButton(6))
{
comp599->Start();
}
else if(oi->getDriveJoystickButton(7))
{
comp599->Stop();
}
}
void smartDashboardPrint()
{
oi->dashboard->PutNumber("Linear Velocity", drive->getLinVelocity());
oi->dashboard->PutNumber("Left Encoder", drive->leftEncoder->Get());
oi->dashboard->PutNumber("Right Encoder", drive->rightEncoder->Get());
oi->dashboard->PutNumber("Step", step);
}
};
START_ROBOT_CLASS(Medic);