PID Turning in Place

Omnom · March 22, 2022, 8:56pm

Hello.
I’ve been trying to code a PID auton for our robot but I’ve run into some bugs. Every time we set a value to desiredTurnValue the robot drives in two separate arcs. How can I change this to turn in place?
Here is the code:

/*----------------------------------------------------------------------------*/
/*                                                                            */
/*    Module:       main.cpp                                                  */
/*    Author:       Connor                                                    */
/*    Created:      3/15/2022                                                 */
/*    Description:  PID Program                                               */
/*                                                                            */
/*----------------------------------------------------------------------------*/

#include "vex.h"
#include <cmath>

using namespace vex;

// A global instance of competition
competition Competition;

void pre_auton(void) {
  vexcodeInit();
   

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

//Settings
double kP = 0.1; //Proportional Gain (how fast the system responds)
double kI = 0.05; //Reset (repeats per amount of time)
double kD = 0.3; //Derivative (how far to predict change)
double turnkP = 0.1;
double turnkI = 0.01; 
double turnkD = 0.1;
int maxTurnIntegral = 300; // These cap the integrals
int maxIntegral = 300;
int integralBound = 3; //If error is outside the bounds, then apply the integral. This is a buffer with +-integralBound degrees

//Autonomous Settings
int desiredValue = 200;
int desiredTurnValue = 0;

int error; //SensorValue - DesiredValue : Position
int prevError = 0; //Position 20 miliseconds ago
int derivative; // error - prevError : Speed
int totalError = 0; //totalError = totalError + error

int turnError; //SensorValue - DesiredValue : Position
int turnPrevError = 0; //Position 20 miliseconds ago
int turnDerivative; // error - prevError : Speed
int turnTotalError = 0; //totalError = totalError + error

bool resetDriveSensors = false;

//Variables modified for use
bool enableDrivePID = true;

//Pasted from a C++ resource
double signnum_c(double x) {
  if (x > 0.0) return 1.0;
  if (x < 0.0) return -1.0;
  return x;
}

int drivePID(){
  
  while(enableDrivePID){

    if (resetDriveSensors) {
      resetDriveSensors = false;
      LeftBack.setPosition(0,degrees);
      RightBack.setPosition(0,degrees);
    }


    //Get the position of both motors
    int leftMotorPosition = LeftBack.position(degrees);
    int rightMotorPosition = RightBack.position(degrees);

 //Lateral movement PID
    //Get average of the two motors
    int averagePosition = (leftMotorPosition + rightMotorPosition)/2;

    //Potential
    error = averagePosition - desiredValue;

    //Derivative
    derivative = error - prevError;

    //Integral
    if(abs(error) < integralBound){
    totalError+=error; 
    }  else {
    totalError = 0; 
    }
    //totalError += error;

    //This would cap the integral
    totalError = abs(totalError) > maxIntegral ? signnum_c(totalError) * maxIntegral : totalError;

    double lateralMotorPower = error * kP + derivative * kD + totalError * kI;


 //Turning Movement PID
    //Get average of the two motors
    int turnDifference = leftMotorPosition - rightMotorPosition;

    //Potential
    turnError = turnDifference - desiredTurnValue;

    //Derivative
    turnDerivative = turnError - turnPrevError;

    //Integral
    if(abs(error) < integralBound){
    turnTotalError+=turnError; 
    }  else {
    turnTotalError = 0; 
    }
    //turnTotalError += turnError;

    //This would cap the integral
    turnTotalError = abs(turnTotalError) > maxIntegral ? signnum_c(turnTotalError) * maxIntegral : turnTotalError;

    double turnMotorPower = turnError * turnkP + turnDerivative * turnkD + turnTotalError * turnkI;

    LeftDrive.spin(forward, -lateralMotorPower + (-turnMotorPower), voltageUnits::volt);
    RightDrive.spin(forward, -lateralMotorPower + (turnMotorPower), voltageUnits::volt);

    prevError = error;
    turnPrevError = turnError;
    vex::task::sleep(20);

  }

  return 1;
}

void autonomous(void) {
 vex::task initpid(drivePID);

  MainArm.setBrake(hold);
  //Forward
  resetDriveSensors = true;
  desiredValue = 820;
  BackClamp.open();
  wait(.5,seconds);
 
  
  //Grab
  ArmClamp1.open();
  ArmClamp2.open();
  wait(2,seconds);



  //Drive Back
  resetDriveSensors = true;
  desiredValue = -700;
  wait(1,seconds);

  //Release
  ArmClamp1.close();
  ArmClamp2.close();

  resetDriveSensors = true;
  desiredTurnValue = 450;
  wait(1,seconds);
  All.stop();

  resetDriveSensors = true;

  Drive.spin(fwd);


}

void usercontrol(void) {

  enableDrivePID = false;

 //Settings
  double turnImportance = 0.5;

  while (1) {


  //Driver Control
    double turnVal = Controller1.Axis1.position(percent);
    double forwardVal = Controller1.Axis3.position(percent);

    double turnVolts = turnVal * 0.12;
    double forwardVolts = forwardVal * 0.12 * (1 - (std::abs(turnVolts)/12.0) * turnImportance);

    //0 - 12 = -12
    //0 + 12 = 12(due to cap)


    LeftDrive.spin(forward, forwardVolts + turnVolts, voltageUnits::volt);
    RightDrive.spin(forward, forwardVolts - turnVolts, voltageUnits::volt);


   //Arm Control
    bool topRightButton = Controller1.ButtonR1.pressing();
    bool bottomRightButton = Controller1.ButtonR2.pressing();

    if (topRightButton){
      MainArm.spin(forward, 12.0, voltageUnits::volt);
    }
    else if (bottomRightButton){
      MainArm.spin(forward, -12.0, voltageUnits::volt);
    }
    else{
      MainArm.stop(brake);
    }

   //Intake Control
    bool topLeftButton = Controller1.ButtonL1.pressing();
    bool bottomLeftButton = Controller1.ButtonL2.pressing();

    if (ArmDown.pressing() == 0){
      RingConveyor.spin(forward, 12.0, voltageUnits::volt);
    }
    else if(topLeftButton){
      RingConveyor.spin(forward, 12.0, voltageUnits::volt);
    }
    else if (bottomLeftButton){
      RingConveyor.spin(forward, -12.0, voltageUnits::volt);
    }
    else{
      RingConveyor.spin(forward, 0, voltageUnits::volt);
    }

   //Front Clamp Control
    if (Controller1.ButtonY.pressing()){
      ArmClamp1.open();
      ArmClamp2.open();
    }
    else if (Controller1.ButtonA.pressing()){
       ArmClamp1.close();
       ArmClamp2.close();
    }
    else{
      wait(20, msec);
    }


    //Back Clamp Control
    if (Controller1.ButtonUp.pressing()){
      BackClamp.open();
    }
    else if (Controller1.ButtonDown.pressing()){
       BackClamp.close();
    }
    else{
      wait(20, msec);
    }

    //Platform
    if(Controller1.ButtonB.pressing()){
     RightBack.setBrake(hold);
     RightFront.setBrake(hold);
     RightMid.setBrake(hold);
     LeftBack.setBrake(hold);
     LeftFront.setBrake(hold);
     LeftMid.setBrake(hold);

   }
    else if (Controller1.ButtonX.pressing()){
     RightBack.setBrake(coast);
     RightFront.setBrake(coast);
     RightMid.setBrake(coast);
     LeftBack.setBrake(coast);
     LeftFront.setBrake(coast);
     LeftMid.setBrake(coast);
   }
    wait(20, msec); 
  }
}

int main() {
  Competition.autonomous(autonomous);
  Competition.drivercontrol(usercontrol);

  pre_auton();

  while (true) {
    wait(100, msec);
  }
}

TheRichDarth · March 24, 2022, 2:18pm

Your description of the problem is somewhat vague but if I’m understanding you correctly, the left motors both move and then the right motors move rather than both moving simultaneously. In your code I actually do see the reason this could happen. You forgot to include the wait for completion parameter in the leftDrive.spin function call.
Your Code:

LeftDrive.spin(forward, -lateralMotorPower + (-turnMotorPower), voltageUnits::volt);
RightDrive.spin(forward, -lateralMotorPower + (turnMotorPower), voltageUnits::volt);

Should be:

LeftDrive.spin(forward, -lateralMotorPower + (-turnMotorPower), voltageUnits::volt, false);
RightDrive.spin(forward, -lateralMotorPower + (turnMotorPower), voltageUnits::volt);

This is because there is an additional parameter after the ones you’re using in the .spin function. This parameter controls whether or not the code will wait for the first function call to complete before running the next one. Right now, your code tells the robot to move the left motors then stop and then move the right motors.

7686B_Ian · March 24, 2022, 5:51pm

We will try this tonight :))

7686B_Ian · March 31, 2022, 4:21pm

Well it still doesn’t work- it only turns one wheel

Anomalocaris · March 31, 2022, 11:54pm

A couple of things from a quick skim:

Use parameters for your target values, not global variables.
Split your drive and turn PID into 2 separate functions. It makes the code much simpler to follow and debug.
Just call the PID function in auton and use whether or not you’ve reached the target as your while loop condition, not enableDrivePID. Again, this makes the code much more streamline and easier to debug.
Your error should be target - currentValue, not the other way around

Try rewriting the code to make it simpler, and you should have an easier time pinpointing errors