4 Motor Drivetrain & Catapult PID Help

Programming Support VEXcode Pro V5 Text Tech Support
, ,
1

First time posting on this forum, but I have read a lot of forum posts about PID. Check below for my problem. Hi, so, I have set my constants and done everything that a Youtube Tutorial from Conrad did. Here’s my code.

/*----------------------------------------------------------------------------*/
/*                                                                            */
/*    Module:       main.cpp                                                  */
/*    Author:       VEX                                                       */
/*    Created:      Thu Sep 26 2019                                           */
/*    Description:  Competition Template                                      */
/*                                                                            */
/*----------------------------------------------------------------------------*/

// ---- START VEXCODE CONFIGURED DEVICES ----
// ---- END VEXCODE CONFIGURED DEVICES ----

#include "vex.h"

using namespace vex;

// A global instance of competition
competition Competition;

// Define

// Catapult PID constants
double kP_catapult = 0.0;
double kI_catapult = 0.0;
double kD_catapult = 0.0;

// Drivetrain PD Constants
double kP = 0.0005;
double kD = 0.0;

double turnkP = 0.0;
double turnkD = 0.0;

int desiredValue = 200;
int desiredTurnValue  = 0;

// Variables for the Catapult PID Control
double error_catapult;
double derivative_catapult;
double previous_error_catapult = 0;
double targetPosition_catapult = 0.0; // Desired catapult angle

// Variables for the drivetrain PID control
int error; // SensorValue - DesiredValue : Position
int prevError = 0; // Position 20 milliseconds ago
int derivative; // error - prevError : Speed

int turnError; // SensorValue - DesiredValue : Position
int turnPrevError = 0; // Position 20 milliseconds ago
int turnDerivative; // error - prevError : Speed

bool resetDriveSensors = false;
bool enableDrivePID = true;

// Catapult PID FUNCTION
void catapultPID() {
    // Calculate the error between the target position and the current position
    error_catapult = targetPosition_catapult - Catapult.position(degrees);
    
    // Calculate the derivative
    derivative_catapult = error_catapult - previous_error_catapult;
    
    // Calculate motor power
    double motorPower_catapult = kP_catapult * error_catapult + kD_catapult * derivative_catapult;
    
    // Set the motor power
    Catapult.spin(forward, motorPower_catapult, voltageUnits::volt);
    
    // Update the previous error
    previous_error_catapult = error_catapult;
    
    // Check if the catapult is within an acceptable range
    if (abs(error_catapult) < 5) { // Assuming 5 is the acceptable range
        vex::task::sleep(500); // Hold for 0.5 seconds
    }
    
    vex::task::sleep(20); // Sleep for 20 milliseconds
}

// Drivetrain PD function
// Drivetrain PID function
int drivePID() {
    while(enableDrivePID) {
        if (resetDriveSensors) {
            resetDriveSensors = false;
            FrontLeft.setPosition(0, degrees);
            FrontRight.setPosition(0, degrees);
            BackLeft.setPosition(0, degrees);
            BackRight.setPosition(0, degrees);
        }

        // Get the position of both sides of the drivetrain
        int leftPosition = (FrontLeft.position(degrees) + BackLeft.position(degrees)) / 2;
        int rightPosition = (FrontRight.position(degrees) + BackRight.position(degrees)) / 2;

        // Lateral Movement PID
        int averagePosition = (leftPosition + rightPosition) / 2;
        error = averagePosition - desiredValue;
        derivative = error - prevError;
        double lateralMotorPower = error * kP + derivative * kD;

        // Turning Movement PID
        int turnDifference = leftPosition - rightPosition;
        turnError = turnDifference - desiredTurnValue;
        turnDerivative = turnError - turnPrevError;
        double turnMotorPower = turnError * turnkP + turnDerivative * turnkD;

        // Apply motor power
        FrontLeft.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
        BackLeft.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
        FrontRight.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
        BackRight.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);


        // Update previous errors for the next loop iteration
        prevError = error;
        turnPrevError = turnError;

        // Sleep for 20 milliseconds
        vex::task::sleep(20);
    }
    return 1;
}

void pre_auton(void) {
  // Initializing Robot Configuration. DO NOT REMOVE!
  vexcodeInit();

  // All activities that occur before the competition starts
  // Example: clearing encoders, setting servo positions, ...
}

// Auton
void autonomous(void) {
  // Step 1: Launch the catapult
    vex::task testerPID(drivePID); // Comment if doesn't work

    targetPosition_catapult = 500.0; // Desired angle
    catapultPID(); // Run the catapult PID function

    desiredValue = 5000; 
    desiredTurnValue = 500;
    resetDriveSensors = true;
    enableDrivePID = true;
    drivePID();

    // FrontLeft.spin(forward, )







    // Step 2: Drive and turn
    // desiredValue = 200; // Move 200 units forward
    // desiredTurnValue = 15; // Turn 15 units
    // resetDriveSensors = true;
    // enableDrivePID = true;
    // drivePID(); // Run the drivetrain PID function

    // Step 3: Activate pneumatics for the wings
    // Wings.set(true);

    // Step 4: Drive forward, pick up balls, then drive backward
    // This step might involve more specific commands or additional PID/target value adjustments

    // Ensure to end tasks or movements appropriately, potentially with additional steps or checks
}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              User Control Task                            */
/*                                                                           */
/*  This task is used to control your robot during the user control phase of */
/*  a VEX Competition.                                                       */
/*                                                                           */
/*  You must modify the code to add your own robot specific commands here.   */
/*---------------------------------------------------------------------------*/

void usercontrol(void) {
  // User control code here, inside the loop
  while (1) {
    bool wasButtonAPressed = false; // Variable to remember the button state in the previous iteration

    // Drivetrain controls
    int forwardBackward = Controller1.Axis3.position(); // Left joystick for forward/backward
    int turning = Controller1.Axis1.position(); // Right joystick for turning

    // Mixing for tank drive control
    int leftMotorSpeed = forwardBackward + turning;
    int rightMotorSpeed = forwardBackward - turning;

    FrontLeft.spin(forward, leftMotorSpeed, percent);
    BackLeft.spin(forward, leftMotorSpeed, percent);
    FrontRight.spin(forward, rightMotorSpeed, percent);
    BackRight.spin(forward, rightMotorSpeed, percent);

    // Catapult control - Button X
    if (Controller1.ButtonX.pressing()) {
      // Rotate the catapult to 80 degrees at a certain speed. 
      // The motor will automatically stop once it reaches the target rotation.
      Catapult.rotateTo(500, deg, 90, velocityUnits::pct); // 50% speed, for example
 
      // Wait until the motor stops (reaches the target), you can also add a timeout to prevent an infinite loop
      while(Catapult.isSpinning()) {
        // You might want to do something while waiting or just do nothing
        vex::task::sleep(20); // Sleep to prevent hogging the CPU
      }

      // Optional: pause here if you want a delay before the catapult returns
      vex::task::sleep(500); // Pause for 0.5 seconds, for example

      // Rotate back to the original position (0 degrees)
      Catapult.rotateTo(0, deg, 50, velocityUnits::pct); // Assuming the original position is 0 degrees

      // Again, wait until the motor stops
      while(Catapult.isSpinning()) {
        vex::task::sleep(20); // Sleep to prevent hogging the CPU
      }
    }

    // Pneumatics for wings - Button A (Toggle)
    // Pneumatics for wings - Button A (Toggle)
    static bool wingsExtended = false; // holds current state of wings
    bool isButtonAPressed = Controller1.ButtonA.pressing(); // checks if button is currently pressed

    if (isButtonAPressed && !wasButtonAPressed) { // if button is pressed now and was not pressed before
      wingsExtended = !wingsExtended; // toggle state
      Wings.set(wingsExtended); // set wings to current state
    }
    wasButtonAPressed = isButtonAPressed; // remember the current state for the next iteration
    // Intake control - Left Trigger
    if (Controller1.ButtonL2.pressing()) {
      // Spin the intake motor. You need to replace "Intake" with your actual motor's name
      Intake.spin(forward); // You might want to set a speed here
    } else {
      Intake.stop();
    }

    // Scooper control - Right Trigger (Pneumatic, hold-controlled)
    if (Controller1.ButtonR2.pressing()) {
      // Assuming "Scooper" is your pneumatic device for the scooper
      Scooper.set(true); // Engage the scooper
    } else {
      Scooper.set(false); // Disengage the scooper
    }

    // Sleep the task for a short amount of time to prevent wasted resources.
    vex::task::sleep(20);
  }
}

//
// Main will set up the competition functions and callbacks.
//
int main() {
  // Set up callbacks for autonomous and driver control periods.
  Competition.autonomous(autonomous);
  Competition.drivercontrol(usercontrol);

  // Run the pre-autonomous function.
  pre_auton();

  // Prevent main from exiting with an infinite loop.
  while (true) {
    wait(100, msec);
  }
}

So I want to know when you set the desired value and desired turn value in the auton, how does it work and how am I supposed to make it work? In the autonomous section, you can see that I commented out the Catapult PID part because it didn’t work when I ran it from my controller and timed run that has the auton. It didn’t even move.

Please teach me, also I know it’s not very advisable and respected to get spoon-fed help like this but I am seriously stuck. Thank you.

2

In your auto function you don’t need to run drivePID after you already start it in a thread. Because the drivePID task has an infinite loop in it, if you ever run drivePID() in the main thread all of the code after it will never be reached. It also looks like your kP might be too small.

As for your catapult PID, right now you only run the catapult PID once instead of constantly, so you should add a loop inside of the catapultPID function. You should also run the function inside of the thread instead of just directly running it, like how you started the drivePID at the top of the autonomous function. Lastly, it also looks like your catapult PID constants are all 0, I would recommend giving kP a value so you can see if the function works, then you can start tuning it

Non threaded PID

Also, imo the method of making PIDs in a thread like how the tutorial you followed does can be confusing and hard to use. Here’s some pseudocode of the way I think is easier to work with which doesn’t use threads:

// distance: The distance to drive in inches
// threshold: How close to get to the target before stopping driving
function drive(distance, threshold = 1) {
    var kP = // kP value
    var kI = // kI value
    var kD = // kD value
    var wheel_circumfrance = // The circumfrance of the wheels in inches
    var gear_ratio = // The gear ratio of the drivetrain (input teeth / output teeth)

    // Convert distance from inches to degrees
    var target_wheel_rotation = distance / wheel_circumfrance / gear_ratio * 360

    // Create some variables
    var starting_position = get_wheel_rotation()
    var total_error = 0

    while (true) {
        // Get the distance from the target position
        var current_distance = get_wheel_rotation() - starting_position
        var error = target_wheel_rotation - current_distance

        // Check if the robot is close to the end, and if it is end the loop
        if (error < threshold && error > -threshold) {
            break
        }

        // Add to the total error used for the I term
        total_error = total_error + error

        // Calculate the output of the PID
        var output = error * kP + total_error * kI + (error - last_error) * kD

        // Set the speed of the motors
        move_left_motors(output)
        move_right_motors(output)

        // Set the value of last_error used in the D term
        last_error = error
    }

    // Stop the motors
    move_left_motors(0)
    move_right_motors(0)
}

This code won’t work if you just copy/paste it because it’s pseudocode, so it’s your job to translate it into C++ code. If you can get your PIDs working how they are now I would recommend just sticking with how you already have it, but if you are still stuck after trying to fix the current version you can maybe try doing it how I have it here