Power-Up/SuperLifter.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
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
/*
 * SuperLifter.cpp
 *
 *  Created on: Jan 13, 2018
 *      Author: Joseph Mattson
 */

#include "SuperLifter.h"
#include <iostream>

SuperLifter::SuperLifter(int liftChannel, int helpChannel):
	lift(liftChannel),
	help(helpChannel),
	p(0.0001),
	d(0.00001),
	penc(0),
	tgt(-1),
	zero(0),
	brakePow(0)
{
	lift.ClearStickyFaults(0);
	help.ClearStickyFaults(0);

	lift.SetNeutralMode(ctre::phoenix::motorcontrol::NeutralMode::Brake);
	help.SetNeutralMode(NeutralMode::Brake);

	lift.SetInverted(Constants::liftInverted);
	help.SetInverted(Constants::helpInverted);

//	lift.ConfigSelectedFeedbackSensor(motorcontrol::FeedbackDevice::QuadEncoder, 0, 0);
//
//	lift.SetSensorPhase(Constants::liftEncoderInverted);
//
//	lift.SetSelectedSensorPosition(0,0,0);

	int absolutePosition = lift.GetSelectedSensorPosition(0) & 0xFFF; /* mask out the bottom12 bits, we don't care about the wrap arounds */
	lift.SetSelectedSensorPosition(absolutePosition, 0,0);
	lift.ConfigSelectedFeedbackSensor(motorcontrol::FeedbackDevice::CTRE_MagEncoder_Absolute, 0,0);
	lift.SetSensorPhase(Constants::liftEncoderInverted);
	lift.Config_kF(0, 0.0, 0);
	lift.Config_kP(0, 0.5, 0);
	lift.Config_kI(0, 0.0, 0);
	lift.Config_kD(0, 0.0, 0);

	help.Follow(lift);

	lift.ConfigPeakCurrentLimit(30,0);
	help.ConfigPeakCurrentLimit(30,0);

	lift.ConfigPeakOutputReverse(-0.4,0);
	help.ConfigPeakOutputReverse(-0.4,0);

//	lift.ConfigForwardSoftLimitThreshold(16000, 10);
//	lift.ConfigReverseSoftLimitThreshold(-100, 10);
	lift.ConfigForwardSoftLimitEnable(false, 0);
	lift.ConfigReverseSoftLimitEnable(false, 0);
	lift.OverrideLimitSwitchesEnable(false);
}

void SuperLifter::Lift(float pow)
{
	//if (lift.GetOutputCurrent() < Constants::lifterMaxCurrent)
//	if(true)
//	{
		lift.Set(ControlMode::PercentOutput, pow + brakePow);
//		help.Set(ControlMode::PercentOutput, pow);
//		std::cout << lift.GetSelectedSensorPosition(0) << "\n";
		this->Reset();
//	}
//	else
//	{
//		this->Brake();
//	}
}

void SuperLifter::Brake()
{
	if (tgt == -1)
		tgt = lift.GetSelectedSensorPosition(0);
//	float kp = (tgt - lift.GetSelectedSensorPosition(0));
//	float kd = -1*(penc - lift.GetSelectedSensorPosition(0))/0.005;
//	float pow = kp * p + kd * d;
//	penc = lift.GetSelectedSensorPosition(0);
//	Lift(pow);
//
//	std::cout << penc << "         " << kp << "      " << kd << "         " << pow << "\n";
	lift.Set(ControlMode::Position, tgt);
//	std::cout << lift.GetSelectedSensorPosition(0) << "\n";

	brakePow = (lift.GetMotorOutputPercent() + help.GetMotorOutputPercent())/2;

}

void SuperLifter::Reset()
{
	tgt = -1;
}

void SuperLifter::Position()
{
	lift.Set(ControlMode::Position, penc + zero);
	this->Reset();
}

void SuperLifter::SetPosition(float revs)
{
	penc = revs * 4096;
}

int SuperLifter::GetEncoder()
{
	return lift.GetSelectedSensorPosition(0) - zero;
}

void SuperLifter::Zero()
{
	zero = lift.GetSelectedSensorPosition(0);
}

float SuperLifter::GetCurrent()
{
	return float(lift.GetOutputCurrent() + help.GetOutputCurrent())/2;
}