Implement PID to VEXCode V5

Hello, my teammate who has been working on creating and tuning a pid had just finished his work and now i am wondering how to implement it. Do i have to rewrite the functions from vex or something else? Any help is welcomed!

This is my current non-pid code:

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

// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name]               [Type]        [Port(s)]
// Controller1          controller                    
// Drivetrain           drivetrain    13, 14, 11, 12, 10
// Catapult             motor_group   6, 7            
// Intake               motor_group   15, 16          
// Pneumatics           digital_out   A               
// ---- END VEXCODE CONFIGURED DEVICES ----

#include "vex.h"

using namespace vex;

// A global instance of competition
competition Competition;
bool intake, Pneu;

/*----------------------------------------------------------------------------*/
// define the drivetrain
// this one is a smart drive which uses the gyro
// gyro and all motors were defined using graphical config
// we have 4 inch wheels
// drive is 16 inches wide between the wheels
// drive has a 16 inch wheelbase (between fron and back wheel axles)
//
// this is how the above definition would be if no gyro is used
//drivetrain    robotDrive( leftDrive, rightDrive, 12.56, 16, 16, distanceUnits::in );



// define your global instances of motors and other devices here

/*---------------------------------------------------------------------------*/
/*                          Pre-Autonomous Functions                         */
/*---------------------------------------------------------------------------*/

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

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

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              Autonomous Task                              */
/*                                                                           */
/*---------------------------------------------------------------------------*/

void autonomous(void) {
  // ..........................................................................
  // Insert autonomous user code here.
  // ..........................................................................
  Drivetrain.setStopping(brake);
  Catapult.setVelocity(100.0, percent);
  Pneumatics.set(false);
  Drivetrain.setDriveVelocity(80.0, percent);
  Drivetrain.setTurnVelocity(40.0, percent);
  Catapult.spinFor(forward, 140.0, degrees);
  Catapult.setVelocity(100.0, percent);
  Drivetrain.turnToHeading(55.0, degrees);
  wait(0.5, seconds);
  Drivetrain.driveFor(forward, 835.0, mm);
  Drivetrain.turnToHeading(3.0, degrees);
  wait(0.2, seconds);
  Intake.spinFor(forward, 2.0, turns);
  Drivetrain.driveFor(forward, 300.0, mm, false);
  wait(0.5, seconds);
  Drivetrain.driveFor(reverse, 330.0, mm, true);
  // comment
  Drivetrain.turnToHeading(295.0, degrees, true);
  wait(0.2, seconds);
  // comment
  Drivetrain.driveFor(forward, 1075.0, mm, true);
  Drivetrain.turnToHeading(325.0, degrees, true);
  Drivetrain.driveFor(forward, 200.0, mm, true);
  Drivetrain.setTurnVelocity(30.0, percent);
  Drivetrain.turnToHeading(0.0, degrees, true);
  wait(0.1, seconds);
  Drivetrain.driveFor(forward, 400.0, mm, true);
  Drivetrain.turnToHeading(90.0, degrees, true);
  Pneumatics.set(true);
  wait(0.5, seconds);
  Drivetrain.driveFor(forward, 900.0, mm, true);
}

/*---------------------------------------------------------------------------*/
/*                                                                           */
/*                              User Control Task                            */
/*                                                                           */
/*---------------------------------------------------------------------------*/

void usercontrol(void) {
  // User control code here, inside the loop
  while (1) {
    Drivetrain.setDriveVelocity(100.0, percent);
    Drivetrain.setTurnVelocity(100.0, percent);
    Drivetrain.setStopping(coast);
    Catapult.setVelocity(100.0, percent);
    wait(20, msec); // Sleep the task for a short amount of time to
                    // prevent wasted resources.
  }
}

//
// 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) {

    //***********************************************************************
    //SPIN INTAKE
    //will this work????? the code with bool intake = reverse would constantly tell motors to stop wouldnt they?
    
    if (Controller1.ButtonR2.pressing()) {
      Intake.spin(forward);
      intake = false;
    } else if (Controller1.ButtonR1.pressing()) {
      Intake.spin(reverse);
      intake = false;
    } else if (!intake) {
      Intake.stop();
      intake = true;
    }

    //***********************************************************************
    //Pneumatics toggle
    if (Controller1.ButtonRight.pressing()) {
      if (Pneu) {
        Pneumatics.set(true);
        Pneu = false;
        wait(0.1, seconds);
      }
      else {
        Pneumatics.set(false);
        Pneu = true;
        wait(0.1, seconds);
      }
    }
    //***********************************************************************
    wait(100, msec);
  }
}

I would ask your teammate, who wrote the code.

1 Like

In addition to asking your teammate, you could also try and understand it yourself through the George Gillard document . Also, your driver control code should not go inside of the while true loop of main and should instead be inside the while true loop of driver control.

1 Like

First of all what kind of PID did your teammate make as a pid is just a control function, is it for turning, driving, etc.

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.