Stronghold/Controls.cpp at master · FRC4903/Stronghold · 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
/*
 * @File:			Controls.cpp
 * @Description:	Constrols class handles output to motors and motor controllers.
 * @Author: 		Rikin Katyal
 */

#include "Controls.h"
#include "Data.h"

Controls::Controls()
	// initialize talons
	: talon0(0),
	  talon1(1),
	  talon2(2),
	  talon3(3),
	  intake_motor(0),
	  holding_motor(1),
	  trebuchet_top_motor(2),
	  trebuchet_bot_motor(3),
	  triangleSolenoid(1, 2)
{
	library = Data::getInstance();

 	update();

 	reset();

}

Controls::~Controls() {
	// Controls destructor
}

void Controls::driveBase() {
	j_x = library->getSensors()->getBaseMovementInputX() / moderator;
	j_y = library->getSensors()->getBaseMovementInputY() / moderator;

	speedL += -j_y + j_x;
	speedR += -j_y - j_x;

	// Sets the talons for the base
	setDriveTalons();

	// reset speeds to 0
	reset();

}

void Controls::intake() {
	intake_value = 1.0;

	if (!library->getSensors()->getIntakeSensor()) {
		hold_value = 1.0;
	}
}

void Controls::trebuchet() {
	top_value = -1.0;
	bottom_value = 1.0;
}

void Controls::liftSpears() {
	spearSolenoid->Set(DoubleSolenoid::kForward);
}

void Controls::lowerSpears() {
	spearSolenoid->Set(DoubleSolenoid::kReverse);
}

void Controls::stopSpears() {
	spearSolenoid->Set(DoubleSolenoid::kOff);
}

void Controls::update() {
	if (library->getSensors()->getDriveControllerAButton()) {
		// boost driving when holding "A" button on controller
		moderator = 1.5;
	}
	else {
		// standard moderator
		moderator = 2.0;
	}

	reset();
}

void Controls::reset() {
	// drive variables
	speedL = 0.0;
	speedR = 0.0;
	j_x = 0.0;
	j_y = 0.0;

	// ball intake/holding variables
	intake_value = 0.0;
	hold_value = 0.0;

	// trebuchet variables
	top_value = 0.0;
	bottom_value = 0.0;
}

void Controls::setTalons() {
	// Sets the talons for the base
	talon0.Set(-speedR);
	talon1.Set(-speedR);
	talon2.Set(speedL);
	talon3.Set(speedL);

	// Set intake and holding talons
	intake_motor.Set(intake_value);
	holding_motor.Set(hold_value);

	// Set trebuchet talons
	trebuchet_top_motor.Set(top_value);
	trebuchet_bottom_motor.Set(bottom_value);
}