RedRobot2014/catapult.cpp at master · team3238/RedRobot2014 · 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
#include <WPILib.h>
#include "catapult.h"

catapult::catapult(UINT8 talonOnePort, UINT8 talonTwoPort, UINT8 encoderPortA, UINT8 encoderPortB, UINT8 limitSwitchPort){
	MotorOneTalon = new Talon(talonOnePort);
	MotorTwoTalon = new Talon(talonTwoPort);
	CatapultEncoder = new Encoder(encoderPortA, encoderPortB, true);
	LoweringTimer = new Timer();
	LoweringTimer->Start();
	CatapultEncoder->Start();
	firingState = waiting;
}

void catapult::ReInit(){
	firingState = waiting;
}

void catapult::Fire(){
	firingState = firing;
}

void catapult::AutonomousLower(){
	firingState = autonomousLowering;
}

void catapult::SetStoppingPoint(int clicks){
	stoppingClicks = clicks;
}

void catapult::SetMotorPower(float power){
	motorPower = power;
}

int catapult::GetEncoderCount(){
	return CatapultEncoder->Get();
}

int catapult::GetState(){
	return firingState;
}

void catapult::ResetEncoder(){
	CatapultEncoder->Reset();
}

void catapult::ResetLoweringTimer(){
	LoweringTimer->Reset();
}

void catapult::Idle(){
	switch(firingState){

		case waiting:
			MotorOneTalon->Set(0.0);
			MotorTwoTalon->Set(0.0);
		break;


		case firing:
			if(CatapultEncoder->Get() < stoppingClicks){
				MotorOneTalon->Set(motorPower);
				MotorTwoTalon->Set(motorPower);
			}
			else{
				MotorOneTalon->Set(0.0);
				MotorTwoTalon->Set(0.0);
				LoweringTimer->Reset();
				firingState = lowering;
			}
		break;

		case lowering:
			if((LoweringTimer->Get() * 1000.0) < 1250.0){
				MotorOneTalon->Set(-0.07);
				MotorTwoTalon->Set(-0.07);
			}
			else{
				firingState = zeroing;
			}
		break;

		case zeroing:
			CatapultEncoder->Reset();
			firingState = waiting;
		break;

		case autonomousLowering:
			if((LoweringTimer->Get() * 1000.0) < 500.0){
				MotorOneTalon->Set(-0.3);
				MotorTwoTalon->Set(-0.3);
			}
			else{
				firingState = zeroing;
			}
		break;
	}
}