Stronghold/Arm.cpp at master · Techbrick/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
#include "Arm.h"

Arm::Arm(uint32_t beltMoverID) :
beltMover(beltMoverID)
{
	beltMover.SetControlMode(CANTalon::ControlMode::kPercentVbus);
}

void Arm::Enable()
{
	/* if (!ArmMover.IsEnabled())
		ArmMover.Enable();
	 */

	if (!beltMover.IsEnabled())
		beltMover.Enable();
}

void Arm::Disable()
{
	/* if (ArmMover.IsEnabled())
		ArmMover.Disable();
	*/

	if (beltMover.IsEnabled())
		beltMover.Disable();
}
void Arm::ManualMoveBelt(float speed) {
	beltMover.Set(speed);
}
void Arm::MoveBelt (float speed)
{
	if(beltMover.IsFwdLimitSwitchClosed() && speed > 0) /*upper limit switch hit && speed > 0*/
	{
	beltMover.Set(0);
	}
	else if(beltMover.IsRevLimitSwitchClosed() && speed < 0)/*lower limit switch hit && speed < 0*/
	{
	beltMover.Set(0);
	}
	else
	{
		beltMover.Set(speed);
	}
}

void Arm::PositionBelt (float position)
{

}

/* void Arm::SetAngle (float angle)
{

}

void Arm::OpenSallyPort ()
{

}

void Arm::OpenDrawbridge ()
{

} */

void Arm::OpenPortcullis ()
{

}

void Arm::MoveSeesaw ()
{

}