shared-code/DriveTrain.cpp at main · Burnside-Robotics/shared-code · 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
//Value to Tune the acceleration speed of the robot
const int MOTOR_ACCEL_LIMIT = 2;

float s_lastL = 0;
float s_lastR = 0;

//Example Function for Controlling Motors, function should be called within the drive control loop, with lSpeed and rSpeed being set to the Joystick Axis
void SetMotors(int lSpeed, int rSpeed) {
    //Cap lSpeed and rSpeed to Accel limit
    if ((lSpeed - s_lastL) > MOTOR_ACCEL_LIMIT) {
      lSpeed = s_lastL + MOTOR_ACCEL_LIMIT;
    }
    if ((lSpeed - s_lastL) < -MOTOR_ACCEL_LIMIT) {
      lSpeed = s_lastL - MOTOR_ACCEL_LIMIT;
    }
    if ((rSpeed - s_lastR) > MOTOR_ACCEL_LIMIT) {
      rSpeed = s_lastR + MOTOR_ACCEL_LIMIT;
    }
    if ((rSpeed - s_lastR) < -MOTOR_ACCEL_LIMIT) {
      rSpeed = s_lastR - MOTOR_ACCEL_LIMIT;
    }

    s_lastL = lSpeed;
    s_lastR = rSpeed;

    //Set Speeds to Motors
    if (lSpeed == 0) {
      lDrive.stop(brakeType::brake);
    } else {
      lDrive.spin(directionType::fwd, lSpeed, velocityUnits::pct);
    }
    if (rSpeed == 0) {
      rDrive.stop(brakeType::brake);
    } else {
      rDrive.spin(directionType::fwd, rSpeed, velocityUnits::pct);
}