Rambot2012/VictorGroup.cpp at master · MMRambotics/Rambot2012 · 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
#include "VictorGroup.h"

VictorGroup::VictorGroup(Victor *leftVictor, Victor *rightVictor, Type type) {
    leftMotor = leftVictor;
    rightMotor = rightVictor;

    defaultSpeed = 0.0;

    mode = type;
}

void VictorGroup::PIDWrite(float output) {
    float rightSpeed = 0.0;
    float leftSpeed = 0.0;

    if (mode == STRAIGHT) {
        rightSpeed = defaultSpeed - output;
        leftSpeed = -(defaultSpeed + output);
    } else if (mode == TURN) {
        rightSpeed = -output;
        leftSpeed = -output;
    }

    leftMotor->PIDWrite(leftSpeed);
    rightMotor->PIDWrite(rightSpeed);

    SmartDashboard::GetInstance()->Log(leftSpeed, "Left Motors");
    SmartDashboard::GetInstance()->Log(rightSpeed, "Right Motors");
}

void VictorGroup::SetSpeed(float speed) {
    defaultSpeed = speed;
}

void VictorGroup::SetModeStraight() {
    mode = STRAIGHT;
}

void VictorGroup::SetModeTurn() {
    mode = TURN;
}