Power-Up/Gripper.cpp at master · Techbrick/Power-Up · 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
#include "Gripper.h"

Gripper::Gripper(int l, int r, int h1, int h2, int d1, int d2):
	lt(l),
	rt(r),
	holder(h1,h2),
	dropper(d1,d2)
{
	lt.SetInverted(Constants::gLeftInverted);
	rt.SetInverted(Constants::gRightInverted);

	lt.ClearStickyFaults(0);
	rt.ClearStickyFaults(0);
	lt.SetNeutralMode(NeutralMode::Brake);
	rt.SetNeutralMode(NeutralMode::Brake);
	lt.SetInverted(Constants::leftGripperInverted);
	rt.SetInverted(Constants::rightGripperInverted);
}

void Gripper::setMotors(float power)
{
	lt.Set(ControlMode::PercentOutput, power);
	rt.Set(ControlMode::PercentOutput, power);
	if (fabs(power) < .05)
	{
		lt.Set(ControlMode::PercentOutput, 0.0);
		rt.Set(ControlMode::PercentOutput, 0.0);
	}
}

void Gripper::setHolder(bool open)
{
	holder.set(open);
}

void Gripper::setDropper(bool open)
{
	dropper.set(open);
}

bool Gripper::getHolder()
{
	return holder.get();
}

bool Gripper::getDropper()
{
	return dropper.get();
}