forked from FRC1296/RHSRobot2015
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Conveyor.cpp
161 lines (132 loc) · 3.85 KB
/
Conveyor.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
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
/*
* The Conveyor component class handles the Pallet Jack's tote intake and output.
*/
#include "WPILib.h"
//Robot
#include "ComponentBase.h"
#include "RobotParams.h"
//Local
#include "Conveyor.h"
Conveyor::Conveyor() :
ComponentBase(CONVEYOR_TASKNAME, CONVEYOR_QUEUE,
CONVEYOR_PRIORITY) {
conveyorMotor = new CANTalon(CAN_PALLET_JACK_CONVEYOR);
wpi_assert(conveyorMotor);
conveyorMotor->SetControlMode(CANSpeedController::kPercentVbus);
conveyorMotor->SetVoltageRampRate(120.0);
conveyorMotor->ConfigLimitMode(CANSpeedController::kLimitMode_SwitchInputsOnly);
intakeLeftMotor = new CANTalon(CAN_PALLET_JACK_INTAKE_VERTICAL_LEFT);
wpi_assert(intakeLeftMotor);
intakeLeftMotor->SetControlMode(CANSpeedController::kPercentVbus);
intakeRightMotor = new CANTalon(CAN_PALLET_JACK_INTAKE_VERTICAL_RIGHT);
wpi_assert(intakeRightMotor);
intakeRightMotor->SetControlMode(CANSpeedController::kPercentVbus);
pTask = new Task(CONVEYOR_TASKNAME, (FUNCPTR) &Conveyor::StartTask,
CONVEYOR_PRIORITY, CONVEYOR_STACKSIZE);
wpi_assert(pTask);
pTask->Start((int)this);
}
Conveyor::~Conveyor() {
delete(pTask);
}
void Conveyor::OnStateChange() {
switch (localMessage.command) {
case COMMAND_ROBOT_STATE_AUTONOMOUS:
conveyorMotor->Set(0.0);
intakeLeftMotor->Set(0.0);
intakeRightMotor->Set(0.0);
break;
case COMMAND_ROBOT_STATE_TEST:
conveyorMotor->Set(0.0);
intakeLeftMotor->Set(0.0);
intakeRightMotor->Set(0.0);
break;
case COMMAND_ROBOT_STATE_TELEOPERATED:
conveyorMotor->Set(0.0);
intakeLeftMotor->Set(0.0);
intakeRightMotor->Set(0.0);
break;
case COMMAND_ROBOT_STATE_DISABLED:
conveyorMotor->Set(0.0);
intakeLeftMotor->Set(0.0);
intakeRightMotor->Set(0.0);
break;
case COMMAND_ROBOT_STATE_UNKNOWN:
conveyorMotor->Set(0.0);
intakeLeftMotor->Set(0.0);
intakeRightMotor->Set(0.0);
break;
default:
conveyorMotor->Set(0.0);
intakeLeftMotor->Set(0.0);
intakeRightMotor->Set(0.0);
break;
}
}
void Conveyor::Run() {
switch (localMessage.command) //Reads the message command
{
case COMMAND_CONVEYOR_RUN_FWD:
conveyorMotor->ConfigLimitMode(CANSpeedController::kLimitMode_SrxDisableSwitchInputs);
conveyorMotor->Set(1.0);
break;
case COMMAND_CONVEYOR_RUN_BCK:
conveyorMotor->ConfigLimitMode(CANSpeedController::kLimitMode_SwitchInputsOnly);
conveyorMotor->Set(-1.0);
break;
case COMMAND_CONVEYOR_STOP:
conveyorMotor->ConfigLimitMode(CANSpeedController::kLimitMode_SwitchInputsOnly);
conveyorMotor->Set(0.0);
break;
case COMMAND_CONVEYOR_INTAKELEFT_IN:
intakeLeftMotor->Set(1.0);
break;
case COMMAND_CONVEYOR_INTAKELEFT_OUT:
intakeLeftMotor->Set(-1.0);
break;
case COMMAND_CONVEYOR_INTAKELEFT_STOP:
intakeLeftMotor->Set(0.0);
break;
case COMMAND_CONVEYOR_INTAKERIGHT_IN:
intakeRightMotor->Set(-1.0);
break;
case COMMAND_CONVEYOR_INTAKERIGHT_OUT:
intakeRightMotor->Set(1.0);
break;
case COMMAND_CONVEYOR_INTAKERIGHT_STOP:
intakeRightMotor->Set(0.0);
break;
case COMMAND_CONVEYOR_INTAKEBOTH_IN:
intakeLeftMotor->Set(1.0);
intakeRightMotor->Set(-1.0);
break;
case COMMAND_CONVEYOR_INTAKEBOTH_OUT:
intakeLeftMotor->Set(-0.0);
intakeRightMotor->Set(1.0);
break;
case COMMAND_CONVEYOR_INTAKEBOTH_STOP:
intakeLeftMotor->Set(0.0);
intakeRightMotor->Set(0.0);
break;
case COMMAND_CONVEYOR_RUNALL_FWD:
conveyorMotor->ConfigLimitMode(CANSpeedController::kLimitMode_SrxDisableSwitchInputs);
conveyorMotor->Set(-1.0);
intakeLeftMotor->Set(.65);
intakeRightMotor->Set(-.65);
break;
case COMMAND_CONVEYOR_RUNALL_BCK:
conveyorMotor->ConfigLimitMode(CANSpeedController::kLimitMode_SwitchInputsOnly);
conveyorMotor->Set(1.0);
intakeLeftMotor->Set(-.65);
intakeRightMotor->Set(.65);
break;
case COMMAND_CONVEYOR_RUNALL_STOP:
conveyorMotor->Set(0.0);
intakeLeftMotor->Set(0.0);
intakeRightMotor->Set(0.0);
break;
case COMMAND_SYSTEM_MSGTIMEOUT:
default:
break;
}
}