2013Robot/Climber.cpp at master · team3238/2013Robot · 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
#include "Climber.h"
#include "Settings.h"

Climber::Climber(UINT8 leftLiftIn, UINT8 rightLiftIn, UINT32 leftEncIn, UINT32 rightEncIn, UINT32 deployerLeftIn, UINT32 deployerRightIn) :

	leftLiftPort(leftLiftIn), rightLiftPort(rightLiftIn),
    deployerLeftPort(deployerLeftIn), deployerRightPort(deployerRightIn),
    leftEncPort(leftEncIn), rightEncPort(rightEncIn),
    leftLift(leftLiftPort), rightLift(rightLiftPort),
    deployerLeft(deployerLeftPort), deployerRight(deployerRightPort),
    leftEnc(leftEncPort), rightEnc(rightEncPort)
    {
    // Set initial values for tunables
    syncP = 0.05;
    initialized = false;
}

bool Climber::Init() {
    leftEnc.Start();
    rightEnc.Start();
    leftEnc.Reset();
    rightEnc.Reset();
    leftLift.SetSafetyEnabled(false);
    rightLift.SetSafetyEnabled(false);
    initialized = true;
    return initialized;
}

void Climber::Disable() {
    leftLift .Set(0);
    rightLift.Set(0);
}

void Climber::ManualClimb(double left, double right) {
    leftLift.Set(left);
    rightLift.Set(right);
}

void Climber::ManualClimb(double spd) {
    INT32 lEncD = leftEnc.Get(), rEncD = rightEnc.Get();
    double adj = syncP * (lEncD - rEncD);
    SmartDashboard::PutNumber("ClimbLEnc", lEncD);
    SmartDashboard::PutNumber("ClimbREnc", rEncD);
    SmartDashboard::PutNumber("ClimbAdj", adj);
    SmartDashboard::PutNumber("Given spd", spd);
    SmartDashboard::PutNumber("climb_syncP", syncP);
    if (spd < -0.1) {
         leftLift.Set(spd);
        rightLift.Set(spd);
    }
    else if (spd > 0.1) {
         leftLift.Set(spd + adj);
        rightLift.Set(spd - adj);
    }
    else {
         leftLift.Set(0);
        rightLift.Set(0);
    }
}

void Climber::Deploy(bool deploy) {
    deployerLeft.Set(deploy);
    deployerRight.Set(deploy);
}

void Climber::RaiseHooks(bool move){
	if(move){
		if (leftLift.GetReverseLimitOK()){
			leftLift.Set(-1);
		}
		else {
			leftLift.Set(0);
		}
		if (rightLift.GetReverseLimitOK()){
			rightLift.Set(-1);
		}
		else {
			rightLift.Set(0);
			move = false;
		}
	}
	else{}
}

void Climber::Idle() {
//    INT32 lEncD = leftEnc.Get(), rEncD = rightEnc.Get();
//    SmartDashboard::PutNumber("Climb_LeftEnc", lEncD);
//    SmartDashboard::PutNumber("Climb_RightEnc", rEncD);

    syncP = Settings.getDouble("Climb_syncP", syncP, false);
    if (!leftLift.GetReverseLimitOK() && !rightLift.GetReverseLimitOK()) {
    	leftEnc.Reset();
    	rightEnc.Reset();
    }
}