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

ShooterSubsystem::ShooterSubsystem(int rotatorChannel, int shooterChannel, int agitatorChannel) :
  rotator(rotatorChannel),
  shooter(shooterChannel),
  noah(agitatorChannel),
  accel(I2C::Port::kOnboard)
{
  //rotator.SetTalonControlMode(CANTalon::TalonControlMode::kThrottleMode); //enum might be wrong - check in eclipse
  //shooter.SetFeedbackDevice(CANTalon::FeedbackDevice::CtreMagEncoder_Relative);
  //shooter.SetTalonControlMode(CANTalon::TalonControlMode::kSpeedMode);
  //shooter.SetTalonControlMode(CANTalon::TalonControlMode::kThrottleMode);
  //shooter.SetF(0.1);
//  shooter.SetP(0.1);
//  shooter.SetI(0.1);
//  shooter.SetD(0.1);
//  shooter.EnableControl();
	//rotator.SetTalonControlMode(CANTalon::TalonControlMode::kThrottleMode);
	//shooter.SetTalonControlMode(CANTalon::TalonControlMode::kThrottleMode);
	shooter.ClearStickyFaults();
	rotator.ClearStickyFaults();
}

void ShooterSubsystem::enable() { //enable cantalons
  rotator.Enable();
  shooter.Enable();
}

void ShooterSubsystem::disable() { //disable cantalons
  rotator.Disable();
  shooter.Disable();
}

void ShooterSubsystem::agitate(float speed) { //move agitator
	noah.Set(speed); //set agitator speed
}

void ShooterSubsystem::move(float moveValue) { //rotate the shooter at a speed (kPercentVbus)
  rotator.Set(moveValue); //move shooter up and down
}

float ShooterSubsystem::getAngle() {
	return Roll();
}

bool ShooterSubsystem::setAngle(float angle) { //return true when completed
  float currentAngle = getAngle();
  if (fabs(currentAngle - angle) > 1) { //if it's close enough
    move(.25); //slowly angle
    return false; //not done yet
  } else {
    move(0.0); //stop moving - you're there
    return true; //done
  }
}

void ShooterSubsystem::setSpeed(float speed) { //set speed to shoot the balls at
	//shooter.Set(800);
	//shooter.Set(speed * Constants::shooterMaxSpeed);
	shooter.Set(speed);
}

void ShooterSubsystem::shoot(float speed) { //shoot the balls at a certain speed
	agitate(-1 * -1.0);
	setSpeed(speed);
}

void ShooterSubsystem::stop() { //completely stop the shooter from moving
	agitate(0.0);
	setSpeed(0.0);
	move(0.0);
}

float ShooterSubsystem::getEncoder() {
	return shooter.GetEncPosition();
}

float ShooterSubsystem::Roll() {
	return -(atan2(accel.GetX(),sqrt(accel.GetY()*accel.GetY()+accel.GetZ()*accel.GetZ()))  * 180.0) / PI;
}

float ShooterSubsystem::Pitch(){
	return (atan2(accel.GetY(),sqrt(accel.GetX()*accel.GetX()+accel.GetZ()*accel.GetZ()))  * 180.0) / PI;
}

bool ShooterSubsystem::rotatorForwardLimitSwitch() {
	return rotator.IsFwdLimitSwitchClosed();
}

bool ShooterSubsystem::rotatorReverseLimitSwitch() {
	return rotator.IsRevLimitSwitchClosed();
}

bool ShooterSubsystem::rotatorForwardLimitSwitchOK() {
	return rotator.GetForwardLimitOK();
}

bool ShooterSubsystem::rotatorReverseLimitSwitchOK() {
	return rotator.GetReverseLimitOK();
}