BlueRobot2014/collector.cpp at master · team3238/BlueRobot2014 · GitHub

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
#include <WPILib.h>
#include "collector.h"

collector::collector(UINT8 liftingTalonPort, UINT8 rollerTalonPort, UINT8 ballSensorPort, UINT8 upperLimitSensorPort, UINT8 lowerLimitSensorPort){
	LiftingTalon = new Talon(liftingTalonPort);
	RollerTalon = new Talon(rollerTalonPort);
	BallSensor = new AnalogChannel(ballSensorPort);
	UpperLimitSensor = new DigitalInput(upperLimitSensorPort);
	LowerLimitSensor = new DigitalInput(lowerLimitSensorPort);
	timer = new Timer();
	collectorState = lowering;
}

void collector::ReInit(){
	collectorState = lowering;
}

void collector::Run(){
	enabled = true;
}

int collector::GetBallSensorValue(){
	return BallSensor->GetValue();
}

void collector::Disable(){
	enabled = false;
}

void collector::Idle(){
	if(enabled){
		timer->Start();
		switch(collectorState){

			case lowering:
				if(LowerLimitSensor->Get()){
					LiftingTalon->Set(0.7);
					RollerTalon->Set(0.0);
				}
				else{
					collectorState = waiting;
				}
			break;

			case waiting:
				if(BallSensor->GetValue() < 450){
					LiftingTalon->Set(0.0);
					RollerTalon->Set(1.0);
				}
				else{
					timer->Reset();
					collectorState = waitforball;
				}
			break;

			case waitforball:
				if((timer->Get() * 1000.0) < 500.0){
					LiftingTalon->Set(0.0);
					RollerTalon->Set(1.0);
				}
				else{
					collectorState = raising;
				}
			break;

			case raising:
				if(UpperLimitSensor->Get()){
					LiftingTalon->Set(-1.0);
					RollerTalon->Set(1.0);
				}
				else{
					collectorState = lowering;
				}
			break;

		}
	}
	else{
		LiftingTalon->Set(0.0);
		RollerTalon->Set(0.0);
		timer->Stop();
	}
}