PID Loop Help Requested

IndianaForever · January 11, 2025, 11:52pm

Hey all,

I have a teammate that’s been working over the past week on a PID loop for the robot’s turning. We have a competition next weekend, and we would like to get the PID to work soon, or else we’ll have to go back to our old code.

The program is intended to have the robot turn 90° clockwise and stop.

I’ve included the full program below (PID code specifically is 101-121) as well as a video of what the robot is doing; any help would be greatly appreciated.


#pragma region VEXcode Generated Robot Configuration
// Make sure all required headers are included.
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <math.h>
#include <string.h>


#include "vex.h"

using namespace vex;

// Brain should be defined by default
brain Brain;


// START V5 MACROS
#define waitUntil(condition)                                                   \
  do {                                                                         \
    wait(5, msec);                                                             \
  } while (!(condition))

#define repeat(iterations)                                                     \
  for (int iterator = 0; iterator < iterations; iterator++)
// END V5 MACROS


// Robot configuration code.



// generating and setting random seed
void initializeRandomSeed(){
  int systemTime = Brain.Timer.systemHighResolution();
  double batteryCurrent = Brain.Battery.current();
  double batteryVoltage = Brain.Battery.voltage(voltageUnits::mV);

  // Combine these values into a single integer
  int seed = int(batteryVoltage + batteryCurrent * 100) + systemTime;

  // Set the seed
  srand(seed);
}



void vexcodeInit() {

  //Initializing random seed.
  initializeRandomSeed(); 
}


// Helper to make playing sounds from the V5 in VEXcode easier and
// keeps the code cleaner by making it clear what is happening.
void playVexcodeSound(const char *soundName) {
  printf("VEXPlaySound:%s\n", soundName);
  wait(5, msec);
}

#pragma endregion VEXcode Generated Robot Configuration

//Aden[
competition Competition;

controller Controller1 = controller(primary);

bumper swingarm = bumper(Brain.ThreeWirePort.B);
bumper MoGoDetection = bumper(Brain.ThreeWirePort.E);
digital_out CornerClearCylinder = digital_out(Brain.ThreeWirePort.F); 
digital_out PtoCylinders = digital_out(Brain.ThreeWirePort.G);
digital_out MoGoClamp = digital_out(Brain.ThreeWirePort.H);
optical RingSensor = optical(PORT5);
inertial InertialL = inertial(PORT13, right);
inertial InertialR = inertial(PORT4, right);

motor FirstStageIntake = motor(PORT11, ratio6_1, false);
motor SecondStageIntake = motor(PORT12, ratio6_1, true);

motor leftMotorA = motor(PORT1, ratio6_1, true);
motor leftMotorB = motor(PORT3, ratio6_1, true);
motor_group leftDrive = motor_group(leftMotorA, leftMotorB);
motor PtoMotorLeft = motor(PORT2, ratio6_1, true);

motor rightMotorA = motor(PORT10, ratio6_1, false);
motor rightMotorB = motor(PORT9, ratio6_1, false);
motor_group rightDrive = motor_group(rightMotorA, rightMotorB);
motor PtoMotorRight = motor(PORT20, ratio6_1, false);

int Brain_precision = 0, Console_precision = 0, Controller1_precision = 0, SlowdownPID = 10, R=1, L=-1;

float Pid = 0,Pidmuilt=0.2,piD,piDmuilt=0.01,pId,pIdmuilt =0;

bool RemoteControlCodeEnabled = true;
bool DrivetrainLNeedsToBeStopped_Controller1 = true;
bool DrivetrainRNeedsToBeStopped_Controller1 = true;
bool Right = true, Left = false;
bool isPtoOnBase = true, isMoGoClampOn = false, isRedAlliance, isPos, isFirstStageOn = false, CornerClearer = false, HaltSecIntakeAtTop, doNotRunFirstStageR1 = false;

void PIDTurning(double PID_Turning_Degrees, int Clockwise, int PID_Max_Speed) {
  //while((InertialL.heading(degrees)-PID_Turning_Degrees) > 1) && (InertialL.heading(degrees)-PID_Turning_Degrees) < -1){
    while(true){
    float PrevPid = Pid;
    Pid = (InertialL.heading(degrees)-PID_Turning_Degrees);
    pId =pId+Pid;
    piD=Pid-PrevPid;
    float Turnspeed=Pid*Pidmuilt+pId*pIdmuilt+piD*piDmuilt;

    leftDrive.setVelocity(Turnspeed, percent);
    rightDrive.setVelocity(Turnspeed, percent);
    leftDrive.spin(forward);
    rightDrive.spin(reverse);
    if(isPtoOnBase){
      PtoMotorLeft.setVelocity(Turnspeed, percent);
      PtoMotorRight.setVelocity(Turnspeed, percent);
      PtoMotorLeft.spin(forward);
      PtoMotorRight.spin(reverse);
    }
  }
}

void onevent_Controller1ButtonUp_pressed_0() {
  isPtoOnBase = false;
  PtoCylinders.set(true);
  PtoMotorLeft.setStopping(coast);
  PtoMotorRight.setStopping(coast);
  PtoMotorLeft.setVelocity(100.0, percent);
  PtoMotorRight.setVelocity(100.0, percent);
  PtoMotorLeft.spin(reverse);
  PtoMotorRight.spin(reverse);
  waitUntil((!Controller1.ButtonUp.pressing()));
  PtoMotorLeft.stop();
  PtoMotorRight.stop();
}

// "when Controller1 ButtonB pressed" hat block
void onevent_Controller1ButtonB_pressed_0() {
  if (!isFirstStageOn && !doNotRunFirstStageR1) {
    FirstStageIntake.stop();
    SecondStageIntake.stop();
    isFirstStageOn = true;
    doNotRunFirstStageR1 = true;
  }
  else {
    FirstStageIntake.spin(forward);
    SecondStageIntake.spin(forward);
    isFirstStageOn = false;
    doNotRunFirstStageR1 = false;
  }
}

// "when Controller1 ButtonR1 pressed" hat block
void onevent_Controller1ButtonR1_pressed_0() {
  if (isFirstStageOn) {
    FirstStageIntake.spin(forward);
    isFirstStageOn = false;
  }
  else {
    FirstStageIntake.stop();
    isFirstStageOn = true;
  }
}

// "when Controller1 ButtonDown pressed" hat block
void onevent_Controller1ButtonDown_pressed_0() {
  if (!isPtoOnBase) {
    PtoMotorLeft.setVelocity(90.0, percent);
    PtoMotorRight.setVelocity(90.0, percent);
    PtoMotorLeft.spin(forward);
    PtoMotorRight.spin(forward);
    waitUntil(swingarm.pressing());
    PtoMotorLeft.stop();
    PtoMotorRight.stop();
    isPtoOnBase = true;
    PtoCylinders.set(false);
  }
}

// "when Controller1 ButtonR2 pressed" hat block
void onevent_Controller1ButtonR2_pressed_0() {
  if (doNotRunFirstStageR1) {
    SecondStageIntake.spin(forward);
    doNotRunFirstStageR1 = false;
  }
  else {
    SecondStageIntake.stop();
    doNotRunFirstStageR1 = true;
  }
}

// "when Controller1 ButtonA pressed" hat block
void onevent_Controller1ButtonA_pressed_0() {
  // Reverse the second stage of the intake
  while (Controller1.ButtonA.pressing()) {
    SecondStageIntake.spin(reverse, 10.0, volt);
  wait(5, msec);
  }
  SecondStageIntake.stop();
}

// "when Controller1 ButtonL2 pressed" hat block
void onevent_Controller1ButtonL2_pressed_0() {
  FirstStageIntake.spin(reverse);
  SecondStageIntake.spin(reverse);
}

// "when Controller1 ButtonX pressed" hat block
void onevent_Controller1ButtonX_pressed_0() {
  // Reverse the first stage of the intake
  while (Controller1.ButtonX.pressing()) {
    isFirstStageOn = false;
    doNotRunFirstStageR1 = true;
    FirstStageIntake.spin(reverse, 10.0, volt);
  wait(5, msec);
  }
  FirstStageIntake.stop();
  doNotRunFirstStageR1 = false;
}

// "when Controller1 ButtonL2 released" hat block
void onevent_Controller1ButtonL2_released_0() {
  FirstStageIntake.stop();
  SecondStageIntake.stop();
}

// "when Controller1 ButtonL1 pressed" hat block
void onevent_Controller1ButtonL1_pressed_0() {
  // Mobile goal clamp toggle
  if (isMoGoClampOn) {
    isMoGoClampOn = false;
    MoGoClamp.set(false);
  }
  else {
    isMoGoClampOn = true;
    MoGoClamp.set(true);
  }
}

// "when Controller1 ButtonY pressed" hat block
void onevent_Controller1ButtonY_pressed_0() {
  if (CornerClearer) {
    CornerClearer = false;
    CornerClearCylinder.set(false);
  }
  else {
    CornerClearer = true;
    CornerClearCylinder.set(true);
  }
}

// define a task that will handle monitoring inputs from Controller1
int rc_auto_loop_function_Controller1() {
  while(true) {
    if(RemoteControlCodeEnabled) {
      // stop the motors if the brain is calibrating
      if (InertialR.isCalibrating() || InertialL.isCalibrating()) {
        rightDrive.stop();
        PtoMotorRight.stop();
        leftDrive.stop();
        PtoMotorLeft.stop();
        while (InertialR.isCalibrating() || InertialL.isCalibrating()) {
          wait(25, msec);
        }
      }
      
      // calculate the drivetrain motor velocities from the controller joystick axies
      // left = Axis3 + Axis1
      // right = Axis3 - Axis1
      int drivetrainLeftSideSpeed = Controller1.Axis3.position() + Controller1.Axis1.position();
      int drivetrainRightSideSpeed = Controller1.Axis3.position() - Controller1.Axis1.position();
      
      // check if the value is inside of the deadband range
      if (drivetrainLeftSideSpeed < 5 && drivetrainLeftSideSpeed > -5) {
        // check if the left motor has already been stopped
        if (DrivetrainLNeedsToBeStopped_Controller1) {
          // stop the left drive motor
          leftDrive.stop();
          if (isPtoOnBase){
            PtoMotorLeft.stop();
          }
          // tell the code that the left motor has been stopped
          DrivetrainLNeedsToBeStopped_Controller1 = false;
        }
      } else {
        // reset the toggle so that the deadband code knows to stop the left motor nexttime the input is in the deadband range
        DrivetrainLNeedsToBeStopped_Controller1 = true;
      }

      // check if the value is inside of the deadband range
      if (drivetrainRightSideSpeed < 5 && drivetrainRightSideSpeed > -5) {
        // check if the right motor has already been stopped
        if (DrivetrainRNeedsToBeStopped_Controller1) {
          // stop the right drive motor
          rightDrive.stop();
          if (isPtoOnBase){
            PtoMotorRight.stop();
          }
          // tell the code that the right motor has been stopped
          DrivetrainRNeedsToBeStopped_Controller1 = false;
        }
      } else {
        // reset the toggle so that the deadband code knows to stop the right motor next time the input is in the deadband range
        DrivetrainRNeedsToBeStopped_Controller1 = true;
      }
      
      // only tell the left drive motor to spin if the values are not in the deadband range
      if (DrivetrainLNeedsToBeStopped_Controller1) {
        leftDrive.setVelocity(drivetrainLeftSideSpeed, percent);
        leftDrive.spin(forward);
        if (isPtoOnBase){
          PtoMotorLeft.setVelocity(drivetrainLeftSideSpeed, percent);
          PtoMotorLeft.spin(forward);
        }
      }
      // only tell the right drive motor to spin if the values are not in the deadband range
      if (DrivetrainRNeedsToBeStopped_Controller1) {
        rightDrive.setVelocity(drivetrainRightSideSpeed, percent);
        rightDrive.spin(forward);
        if (isPtoOnBase){
          PtoMotorRight.setVelocity(drivetrainRightSideSpeed, percent);
          PtoMotorRight.spin(forward);
        }
      }
    }
    // wait before repeating the process
    wait(20, msec);
  }
  return 0;
}

task rc_auto_loop_task_Controller1(rc_auto_loop_function_Controller1);

// ]Aden

event IntakeReset = event();
event message1 = event();


// "when started" hat block
int whenStarted1() {
  // PTO motor initializations
  PtoMotorLeft.setMaxTorque(100.0, percent);
  PtoMotorLeft.setStopping(coast);
  PtoMotorRight.setMaxTorque(100.0, percent);
  PtoMotorRight.setStopping(coast);
  // Intake motor initializations
  FirstStageIntake.setMaxTorque(100.0, percent);
  FirstStageIntake.setStopping(coast);
  FirstStageIntake.setVelocity(100.0, percent);
  SecondStageIntake.setMaxTorque(100.0, percent);
  SecondStageIntake.setStopping(coast);
  SecondStageIntake.setVelocity(100.0, percent);
  // Determines whether we are competing as Red or Blue alliance
  Brain.Screen.setPenWidth(20);
  Brain.Screen.setFont(prop30);
  repeat(30) {
    Brain.Screen.setPenColor(white);
    if (Brain.Screen.xPosition() < 240.0) {
      Brain.Screen.setPenColor(green);
      isRedAlliance = true;
    }
    Brain.Screen.setFillColor(red);
    Brain.Screen.drawRectangle(30, 30, 170, 170);
    Brain.Screen.setCursor(3, 4);
    Brain.Screen.print("RED");
    Brain.Screen.setPenColor(white);
    if (Brain.Screen.xPosition() > 240.0) {
      Brain.Screen.setPenColor(green);
      isRedAlliance = false;
    }
    Brain.Screen.setFillColor(blue);
    Brain.Screen.drawRectangle(260, 30, 170, 170);
    Brain.Screen.setCursor(3, 20);
    Brain.Screen.print("BLUE");
    wait(0.25, seconds);
    wait(5, msec);
  }
  Brain.Screen.clearScreen();
  Brain.Screen.setFont(prop60);
  repeat(30) {
    Brain.Screen.setPenColor(white);
    if (Brain.Screen.xPosition() < 240.0) {
      Brain.Screen.setPenColor(green);
      isPos = true;
    }
    if (isRedAlliance) {
      Brain.Screen.setFillColor(red);
    }
    else {
      Brain.Screen.setFillColor(blue);
    }
    Brain.Screen.drawRectangle(30, 30, 170, 170);
    Brain.Screen.setCursor(2, 4);
    Brain.Screen.print("+");
    Brain.Screen.setPenColor(white);
    if (Brain.Screen.xPosition() > 240.0) {
      Brain.Screen.setPenColor(green);
      isPos = false;
    }
    Brain.Screen.drawRectangle(260, 30, 170, 170);
    Brain.Screen.setCursor(2, 12);
    Brain.Screen.print("_");
    wait(0.25, seconds);
    wait(5, msec);
  }
  Brain.Screen.clearScreen();
  Brain.Screen.setFont(prop40);
  Brain.Screen.setCursor(1, 10);
  Brain.Screen.setFillColor(transparent);
  Brain.Screen.print("OK\?");
  if (isRedAlliance) {
    Brain.Screen.setFillColor(red);
  }
  else {
    Brain.Screen.setFillColor(blue);
  }
  Brain.Screen.setPenWidth(0);
  Brain.Screen.drawRectangle(160, 105, 160, 30);
  if (isPos) {
    Brain.Screen.drawRectangle(220, 40, 30, 160);
  }
  waitUntil(Brain.Screen.pressing());
  Brain.Screen.clearScreen();
  wait(1.0, seconds);
  Brain.Screen.print("calibrating");
  InertialL.calibrate();
  InertialR.calibrate();
  wait(6,seconds);
  Brain.Screen.clearScreen();
  if (isRedAlliance) {
    Brain.Screen.setFillColor(red);
  }
  else {
    Brain.Screen.setFillColor(blue);
  }
  Brain.Screen.drawRectangle(160, 105, 160, 30);
  if (isPos) {
    Brain.Screen.drawRectangle(220, 40, 30, 160);
  }
  return 0;
}

// "when driver control" hat block
int ondriver_drivercontrol_0() {
  PtoCylinders.set(false);
  leftMotorA.setStopping(coast);
  leftMotorB.setStopping(coast);
  PtoMotorLeft.setStopping(coast);
  rightMotorA.setStopping(coast);
  rightMotorB.setStopping(coast);
  PtoMotorRight.setStopping(coast);
  return 0;
}

void VEXcode_driver_task() {
  // Start the driver control tasks....
  vex::task drive0(ondriver_drivercontrol_0);
  while(Competition.isDriverControl() && Competition.isEnabled()) {this_thread::sleep_for(10);}
  drive0.stop();
  return;
}
int onauton_autonomous_0(){
  PIDTurning(90,1,10);
  return(0);
}

// "when autonomous" hat block
// int onauton_autonomous_0() {
//   leftDrive.setStopping(brake);
//   rightDrive.setStopping(brake);
//   if (isRedAlliance) {
//     if (!isPos) {
//       leftDrive.setVelocity(20.0, percent);
//       rightDrive.setVelocity(20.0, percent);
//       PtoCylinders.set(true);
//       MoGoClamp.set(false);
//       PtoMotorLeft.setVelocity(100.0, percent);
//       PtoMotorRight.setVelocity(100.0, percent);
//       leftDrive.spinFor(forward, 1.43, turns, false);
//       rightDrive.spinFor(forward, 1.43, turns, true);
//       PIDTurning(63, true, 3);
//       PtoMotorLeft.spin(reverse);
//       PtoMotorRight.spin(reverse);
//       wait(0.7, seconds);
//       PtoMotorLeft.spin(forward);
//       PtoMotorRight.spin(forward);
//       waitUntil(swingarm.pressing());
//       PtoMotorLeft.stop();
//       PtoMotorRight.stop();
//       leftDrive.spinFor(reverse, 2.55, turns, false);
//       rightDrive.spinFor(reverse, 2.55, turns, true);
//       PIDTurning(66, Right, 3);
//       leftDrive.spinFor(reverse, 2.55, turns, false);
//       rightDrive.spinFor(reverse, 2.55, turns, true);
//       wait(0.05, seconds);
//       MoGoClamp.set(true);
//       PIDTurning(180.0, Right, 6);
//       FirstStageIntake.spin(forward, 10.0, volt);
//       SecondStageIntake.spin(forward);
//       leftDrive.spinFor(forward, 3.18, turns, false);
//       rightDrive.spinFor(forward, 3.18, turns, true);
//       wait(1.5, seconds);
//       FirstStageIntake.stop();
//       leftDrive.setVelocity(40.0, percent);
//       rightDrive.setVelocity(40.0, percent);
//       PIDTurning(165, Right, 5);
//       SecondStageIntake.stop();
//       leftDrive.spinFor(reverse, 3.9, turns, false);
//       rightDrive.spinFor(reverse, 3.9, turns, true);
//     }
//     if (isPos) {
//       leftDrive.setVelocity(25.0, percent);
//       rightDrive.setVelocity(25.0, percent);
//       PtoCylinders.set(true);
//       MoGoClamp.set(false);
//       leftDrive.spinFor(reverse, 4.54, turns, false);
//       rightDrive.spinFor(reverse, 4.54, turns, true);
//       PIDTurning(31, Left, 6);
//       leftDrive.setVelocity(15.0, percent);
//       rightDrive.setVelocity(15.0, percent);
//       leftDrive.spinFor(reverse, 0.84, turns, false);
//       rightDrive.spinFor(reverse, 0.84, turns, true);
//       leftDrive.setVelocity(25.0, percent);
//       rightDrive.setVelocity(25.0, percent);
//       MoGoClamp.set(true);
//       wait(0.1, seconds);
//       SecondStageIntake.spin(forward);
//       wait(0.5, seconds);
//       PtoMotorLeft.stop();
//       PIDTurning(13, Right, 6);
//       leftDrive.spinFor(reverse, 28.7, degrees, false);
//       rightDrive.spinFor(reverse, 28.7, degrees, true);
//       // coming back after first goal
//       FirstStageIntake.spin(forward, 10.0, volt);
//       leftDrive.spinFor(forward, 0.88, turns, false);
//       rightDrive.spinFor(forward, 0.88, turns, true);
//       SecondStageIntake.spinFor(forward, 360.0, degrees, true);
//       wait(2.0, seconds);
//       MoGoClamp.set(false);
//       // first goal release
//       PIDTurning(92, Left, 6);
//       leftDrive.spinFor(reverse, 1.2, turns, false);
//       rightDrive.spinFor(reverse, 1.2, turns, true);
//       MoGoClamp.set(true);
//       wait(0.1, seconds);
//       SecondStageIntake.spin(forward, 10.0, volt);
//       PIDTurning(43, Left, 6);
//       leftDrive.spinFor(reverse, 1.1, turns, false);
//       rightDrive.spinFor(reverse, 1.1, turns, true);
//     }
//   }
//   if (!isRedAlliance) {
//     if (!isPos) {
//       leftDrive.setVelocity(20.0, percent);
//       rightDrive.setVelocity(20.0, percent);
//       PtoCylinders.set(true);
//       MoGoClamp.set(false);
//       PtoMotorLeft.setVelocity(100.0, percent);
//       PtoMotorRight.setVelocity(100.0, percent);
//       leftDrive.spinFor(forward, 0.76, turns, false);
//       rightDrive.spinFor(forward, 0.76, turns, true);
//       PIDTurning(63.0, Left, 6);
//       PtoMotorLeft.spin(reverse);
//       PtoMotorRight.spin(reverse);
//       wait(0.7, seconds);
//       PtoMotorLeft.spin(forward);
//       PtoMotorRight.spin(forward);
//       waitUntil(swingarm.pressing());
//       PtoMotorLeft.stop();
//       PtoMotorRight.stop();
//       leftDrive.spinFor(reverse, 2.55, turns, false);
//       rightDrive.spinFor(reverse, 2.55, turns, true);
//       wait(0.05, seconds);
//       MoGoClamp.set(true);
//       PIDTurning(180.0, Left, 6);
//       FirstStageIntake.spin(forward, 10.0, volt);
//       SecondStageIntake.spin(forward);
//       leftDrive.spinFor(forward, 1.6, turns, false);
//       rightDrive.spinFor(forward, 1.6, turns, true);
//       wait(1.5, seconds);
//       FirstStageIntake.stop();
//       leftDrive.setVelocity(40.0, percent);
//       rightDrive.setVelocity(40.0, percent);
//       PIDTurning(195, Right, 6);
//       wait(0.5, seconds);
//       SecondStageIntake.stop();
//       leftDrive.spinFor(reverse, 2.19, turns, false);
//       rightDrive.spinFor(reverse, 2.19, turns, true);
//     }
//     if (isPos) {      
//       leftDrive.setVelocity(25.0, percent);
//       rightDrive.setVelocity(25.0, percent);
//       PtoCylinders.set(true);
//       MoGoClamp.set(false);
//       leftDrive.spinFor(reverse, 2.27, turns, false);
//       rightDrive.spinFor(reverse, 2.27, turns, true);
//       PIDTurning(31.0, Right, 6);
//       leftDrive.setVelocity(15.0, percent);
//       rightDrive.setVelocity(15.0, percent);
//       leftDrive.spinFor(reverse, 0.92, turns, false);
//       rightDrive.spinFor(reverse, 0.92, turns, true);
//       leftDrive.setVelocity(25.0, percent);
//       rightDrive.setVelocity(25.0, percent);
//       MoGoClamp.set(true);
//       wait(0.1, seconds);
//       SecondStageIntake.spin(forward);
//       PIDTurning(13, Left, 6);
//       SecondStageIntake.stop();
//       leftDrive.spinFor(forward, 29, degrees, false);
//       rightDrive.spinFor(forward, 29, degrees, true);
//       FirstStageIntake.spin(forward, 10.0, volt);
//       leftDrive.spinFor(forward, 0.88, turns, false);
//       rightDrive.spinFor(forward, 0.88, turns, true);
//       wait(0.5, seconds);
//       MoGoClamp.set(false);
//       PIDTurning(89.0, Right, 6);
//       leftDrive.spinFor(reverse, 1.2, turns, false);
//       rightDrive.spinFor(reverse, 1.2, turns, true);
//       MoGoClamp.set(true);
//       wait(0.1, seconds);
//       SecondStageIntake.spin(forward, 10.0, volt);
//       PIDTurning(43.0, Right, 6);
//       leftDrive.spinFor(reverse, 1.1, turns, false);
//       rightDrive.spinFor(reverse, 1.1, turns, true);
//     }
//   }
//   return 0;
// }

void VEXcode_auton_task() {
  // Start the auton control tasks....
  vex::task auto0(onauton_autonomous_0);
  while(Competition.isAutonomous() && Competition.isEnabled()) {this_thread::sleep_for(10);}
  auto0.stop();
  return;
}

int main() {
  vex::competition::bStopTasksBetweenModes = false;
  Competition.autonomous(VEXcode_auton_task);
  Competition.drivercontrol(VEXcode_driver_task);

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

  // register event handlers
  Controller1.ButtonUp.pressed(onevent_Controller1ButtonUp_pressed_0);
  Controller1.ButtonB.pressed(onevent_Controller1ButtonB_pressed_0);
  Controller1.ButtonR1.pressed(onevent_Controller1ButtonR1_pressed_0);
  Controller1.ButtonDown.pressed(onevent_Controller1ButtonDown_pressed_0);
  Controller1.ButtonR2.pressed(onevent_Controller1ButtonR2_pressed_0);
  Controller1.ButtonA.pressed(onevent_Controller1ButtonA_pressed_0);
  Controller1.ButtonL2.pressed(onevent_Controller1ButtonL2_pressed_0);
  Controller1.ButtonX.pressed(onevent_Controller1ButtonX_pressed_0);
  Controller1.ButtonL2.released(onevent_Controller1ButtonL2_released_0);
  Controller1.ButtonL1.pressed(onevent_Controller1ButtonL1_pressed_0);
  Controller1.ButtonY.pressed(onevent_Controller1ButtonY_pressed_0);

  wait(15, msec);
  whenStarted1();
}

IMG_1379-ezgif.com-crop

Custom13 · January 12, 2025, 1:11am

The while(true) loop needs an exit condition based on error tolerance.
The PID gains (Pidmuilt, pIdmuilt, piDmuilt) likely need tuning - try starting with just P control around 0.5-0.8 and gradually add D and I terms.
Add a small deadband (like ±2 degrees) to prevent oscillation at the target angle. Would you like specific values to try for these parameters?

jpearman · January 12, 2025, 1:12am

The pid while loop needs a delay

Team21510B · January 12, 2025, 2:57pm

The whole loop needs an exit condition as stated.
Defining prevPID at the end of the loop will make more logical sense
Specify forward or reversed in the same line as the speed. For example: Left spin(fwd, speed, pct).
Maybe add parentheses around the sections of the speed calculation in case it’s messing up the order of operations.

Trishul · January 12, 2025, 2:59pm

@Custom13 How do I add a small headband, because in my drive straight PID, my PID is oscillating at the end to find the straight position it was at before for the Turn part of it.

GitHub - Trishul-creator/Autonomous-Project-32630B - my github

Custom13 · January 13, 2025, 1:48am

@Trishul To add a deadband to your drive straight PID, you can apply a simple threshold check. Add this condition to your PID loop:

float deadband = 2.0; // Adjust this value as needed
if (fabs(error) < deadband) {
    leftDrive.stop();
    rightDrive.stop();
    break; // Exit PID loop when within deadband
}

IndianaForever · January 13, 2025, 9:37pm

Thanks for the help everyone! My teammate got the PID to work.