2013Robot/Collector.cpp at master · team3238/2013Robot · 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
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
#include "Collector.h"
#include "Portnums.h"

Collector::Collector(Swag* theSwagIn, UINT32 BotFloorOpenSwitch, UINT32  BotFloorCloseSwitch, UINT32  bucketThingy) {
    theSwag = theSwagIn;

	BottomFloorOpenSwitch = new DigitalInput(BotFloorOpenSwitch);
	BottomFloorCloseSwitch = new DigitalInput(BotFloorCloseSwitch);
	bucketStatusSwitch = new DigitalInput(bucketThingy);
	FloorDrive = new Relay(FloorMotorRelay);
	ServoLockRight = new Servo(IrisServoRightPort);
	ServoLockLeft = new Servo(IrisServoLeftPort);
	AllPurposeTimer = new Timer();
	AllPurposeTimer->Start();
}

void Collector::Init(){
	CS = ST::READY;
	lockingServos = closingFloor = openingFloor = false;
}

bool Collector::doneDropping(){
	return CS == ST::READY || CS == ST::FLOOR_CLOSE || CS == ST::SERVOS_UNLOCK;
}

void Collector::manualMode(bool mode){
	if(mode) manual = true;
	else manual = false;
}

void Collector::manualFloorControl(int direction){
	if (manual) {
		if (direction > 0)	openFloor();
		else if (direction < 0) closeFloor();
		else stopFloor();
	}
}

string Collector::getState(){
	switch(CS){
	case ST::READY:			return "READY";	break;
	case ST::SERVOS_LOCK:	return "SERVOS_LOCK";	break;
	case ST::FLOOR_OPEN:	return "FLOOR_OPEN";	break;
	case ST::FLOOR_WAIT:	return "FLOOR_WAIT";	break;
	case ST::FLOOR_CLOSE:	return "FLOOR_CLOSE";	break;
	case ST::SERVOS_UNLOCK:	return "SERVOS_UNLOCK"; break;
	default:				return "wat";			break;
	}
}

void Collector::dropDisc(){
	if (CS == ST::READY) {
		CS = ST::SERVOS_LOCK;
		AllPurposeTimer->Reset();
        theSwag->DropFrisbee();
	}
}

static const float servoUnlockTime = 0.2;
static const float servoLockTime = 0.2;
static const float floorWaitTime = 0.1;

void Collector::Idle(){
	switch (CS) {
	case ST::READY:
		break;
	case ST::SERVOS_LOCK:
		lockServos();
		stopFloor();
		if (AllPurposeTimer->HasPeriodPassed(servoLockTime)) {
			CS = ST::FLOOR_OPEN;
		}
		break;
	case ST::FLOOR_OPEN:
		openFloor();
		if (isFloorOpen()) {
			CS = ST::FLOOR_WAIT;
			AllPurposeTimer->Reset();
		}
		break;
	case ST::FLOOR_WAIT:
		stopFloor();
		if (AllPurposeTimer->HasPeriodPassed(floorWaitTime)) {
			CS = ST::FLOOR_CLOSE;
		}
		break;
	case ST::FLOOR_CLOSE:
		closeFloor();
		if (isFloorClosed()) {
			CS = ST::SERVOS_UNLOCK;
			AllPurposeTimer->Reset();
		}
		break;
	case ST::SERVOS_UNLOCK:
		unlockServos();
		stopFloor();
		if (AllPurposeTimer->HasPeriodPassed(servoUnlockTime)) {
			CS = ST::READY;
		}
		break;
	}

	if (lockingServos) {
		ServoLockRight->Set(1);
		ServoLockLeft->Set(0);
	}
	else {
		ServoLockRight->Set(0);
		ServoLockLeft->Set(1);
	}
	if (closingFloor) {
		if (isFloorClosed()) {
			FloorDrive->Set(Relay::kOff);
			closingFloor = false;
		}
		else FloorDrive->Set(Relay::kReverse);
	}
	else if (openingFloor) {
		if (BottomFloorOpenSwitch->Get()) {
			openingFloor = false;
			FloorDrive->Set(Relay::kOff);
		}
		else FloorDrive->Set(Relay::kForward);
	}
	else {
		FloorDrive->Set(Relay::kOff);
	}
}

void Collector::Disable(){

}