VexCode PID help

Hey guys, now that worlds is cancelled, along with school and sports, I have a bit of free time. I’m not an incredibly experienced programmer, as you are about to see, but I have been trying to teach myself more. My team had a great robot for tower takeover, but the main flaw was that our autonomous programs were not always consistent, and we have not been using a PID. This year we just used the rotatefor command in vexcode, using the internal encoder to measure. However, as I said, this was not as reliable as I wanted. Now that I have no reason to work on my TT robot, I am doing some CAD work and trying to learn PID to utilize for the next game. I looked at @Connor’s PID tutorial, which helped a lot, but I am trying to do something a bit different, because copying and pasting code is no fun. When I run the code below, it runs the rotations, but is not escaping somewhere because I cannot run driver control after the autonomous runs, and I cannot run that same function again afterwards. I was wondering how I could make it so it runs the function, then can run other functions for the rest of autonomous, then can run driver control. I just have 1 function for driver control as a test, this is just for testing the PID system. Any other tips for PID would help as well, I am still trying to learn. Thanks.

Here is the code:

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

// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name] [Type] [Port(s)]
// Controller1 controller
// Drivetrain drivetrain 1, 10, D
// ClawMotor motor 3
// ArmMotor motor 8
// ---- END VEXCODE CONFIGURED DEVICES ----

#include “vex.h”

using namespace vex;

// A global instance of competition
competition Competition;

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

/---------------------------------------------------------------------------/
/* Pre-Autonomous Functions /
/
/
/
You may want to perform some actions before the competition starts. /
/
Do them in the following function. You must return from this function /
/
or the autonomous and usercontrol tasks will not be started. This /
/
function is only called once after the V5 has been powered on and /
/
not every time that the robot is disabled. /
/
---------------------------------------------------------------------------*/

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, …
}
//settings

double integral=0;
double previouserror=7200;

double Kp = 0.18;//somewhat tuned-.18
double Ki = 0.000000000001;
double Kd = 0.05;
int sensorvalue=0;

bool enablePID=true;

int drivePID(){

while(sensorvalue<7200)
{
double error = 7200 - sensorvalue;
integral = integral + error;
if(error==0)
{
integral=0;
}

double derivative = error-previouserror;
previouserror=error;

double speed = (Kp * error)+(Ki * integral)+(Kd * derivative);

ArmMotor.setVelocity(speed, percent);
ArmMotor.spin(forward);

sensorvalue=ArmMotor.position(degrees);

}
return 1;
}

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

void autonomous(void) {

vex::task bill(drivePID);
vex::task::sleep(2000);
vex::task billl(drivePID);
}

/---------------------------------------------------------------------------/
/* /
/
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
enablePID=false;

while (1) {
if (Controller1.ButtonA.pressing()){
ArmMotor.spinFor(forward, 90, degrees);
}
else
{

}

// This is the main execution loop for the user control program.
// Each time through the loop your program should update motor + servo
// values based on feedback from the joysticks.

// ........................................................................
// Insert user code here. This is where you use the joystick values to
// update your motors, etc.
// ........................................................................

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) {
wait(100, msec);
}
}

this should be easier for others to read. I’ll look at the actual code later if no one responds.

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

// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name] [Type] [Port(s)]
// Controller1 controller
// Drivetrain drivetrain 1, 10, D
// ClawMotor motor 3
// ArmMotor motor 8
// ---- END VEXCODE CONFIGURED DEVICES ----

#include “vex.h”

using namespace vex;

// A global instance of competition
competition Competition;

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

/---------------------------------------------------------------------------/
/* Pre-Autonomous Functions /
/ /
/ You may want to perform some actions before the competition starts. /
/ Do them in the following function. You must return from this function /
/ or the autonomous and usercontrol tasks will not be started. This /
/ function is only called once after the V5 has been powered on and /
/ not every time that the robot is disabled. /
/---------------------------------------------------------------------------*/

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, …
}
//settings

double integral = 0;
double previouserror = 7200;

double Kp = 0.18; //somewhat tuned-.18
double Ki = 0.000000000001;
double Kd = 0.05;
int sensorvalue = 0;

bool enablePID = true;

int drivePID() {

  while (sensorvalue < 7200) {
    double error = 7200 - sensorvalue;
    integral = integral + error;
    if (error == 0) {
      integral = 0;
    }

    double derivative = error - previouserror;
    previouserror = error;

    double speed = (Kp * error) + (Ki * integral) + (Kd * derivative);

    ArmMotor.setVelocity(speed, percent);
    ArmMotor.spin(forward);

    sensorvalue = ArmMotor.position(degrees);

  }
  return 1;
}

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

void autonomous(void) {

  vex::task bill(drivePID);
  vex::task::sleep(2000);
  vex::task billl(drivePID);
}

/---------------------------------------------------------------------------/
/* /
/ 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
  enablePID = false;

  while (1) {
    if (Controller1.ButtonA.pressing()) {
      ArmMotor.spinFor(forward, 90, degrees);
    } else {

    }

    // This is the main execution loop for the user control program.
    // Each time through the loop your program should update motor + servo
    // values based on feedback from the joysticks.

    // ........................................................................
    // Insert user code here. This is where you use the joystick values to
    // update your motors, etc.
    // ........................................................................

    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) {
    wait(100, msec);
  }
}
3 Likes

why is it called drivePID if it controls ArmMotor?

this looks to be starting two tasks doing the same thing.

void autonomous(void) {

  vex::task bill(drivePID);
  vex::task::sleep(2000); // this belongs in the task, but with shorter interval
  vex::task billl(drivePID); // this instance of the  task never ends 
}

for now don’t make drivePID a task. get it working as a function that takes the setpoint as a global variable so you can make it a task later.

the while loop needs a wait(n) where n is usually between 20 and 100 ms. Without a delay you’ll be accumulating the integral way too fast (but you do have a very small number for Ki). Kd is also dependent on the time interval since, well, de/dt. Also, the sensor values (encoders) don’t update within the time of a short loop (it seams its about 5ms).

look at @Connor PID tutorial here. he shows you how to make it a task. when that is done i can help you optimize the PID code for live tuning and preventing run-away speed values.

ed: just noticed that usercontrol spinFor does not have a velocity so it may be 0

2 Likes

Ok so I rewatched Connor’s tutorial, and it helped. I figured I’d try downloading his program and testing that, but when I did it did not do anything. Why is that?

@JD6 I’m still not clear if this PID is for a Drive or an Arm?

Did you set ki, kp, kd to something other than 0?

Its for drive; I just used armmotor because that’s the port I had filled in

So I have downloaded Connor’s program, labeled drive PID, and it’s not doing anything and I am unclear why. I figured that program should work by doing something. Also, just for reference, the PID is really just for learning so I can apply it, I don’t care if it is for arm or drive. I juts figured drive is best because that’s where my autonomous have had the most error in the past.

Did you make sure the motor ports are updated to your ports and if you don’t have Arm or Claw to remove them.

If kp, ki, and kd are all 0 then lateralMotorPower will be zero. I suggest starting with kp around .01 (That’s just a guess since this is using degrees and 360deg is about 12.5" on a 4" wheel. So it should start out at 3.6V out of 12V if moving only 12.5").

Eventually, we will be adding clamp to this PID for the iTerm and each final calculation.

2 Likes

I edited out the other motors. I currently have 2 drive motors, left and right. The motors spin in driver control. I have values set for the K constants. What I am noticing is that the function is not working with the “while enable drive PID.” I tried running autonomous and nothing moved. Taking out the PID, I left the loop and just turned on the motors but nothing worked, even while pausing after the motors turn on. Once the loop is removed, the motors spin. However, PID requires a loop. I am not sure why the motors are not running within the loop. The function is being called during autonomous, but for some reason, the loop is off and I have no idea why.

A couple of thing I’ve noted is that the code intermixes int and double in calculations. I suggest changing them all to double and anywhere a whole number is add a decimal point and 0 so 200 would be 200.0

the API shows velocity and position are doubles
https://api.vexcode.cloud/v5/html/classvex_1_1motor.html

I’ll try to run that code on my chassis tomorrow and let you know how it goes. but i’m going to have to make so it can run from usercontrol since my compswitch is on loan.

2 Likes

Ok, thanks for your help. It’s just weird that it doesn’t work with the loop. If I take it out it works fine, and speed can be changed by adjusting constants, but then it will never change speed or stop, or have any input. Also, just thinking about it, how is the loop supposed to stop? It says while enabled, but when does it ever unable? Shouldn’t it be while the averageposition is less than the desired value?

error calculation was flipped, it should be
error = desiredValue - averagePosition;

we can add clamping and fix the loop exit another day.

2 Likes

Yes, I have the error calculation fixed. I think it’s just the loop that needs to be fixed.

Ok. I post tested code tonight but the PID task should look like this:

bool enableDrivePID = false; // NOTE THIS CHANGE

bool pidAbort = false;      // added for testing during usercontrol
double deadBand = 5.0; // this is the close enough to target value

int drivePID() {

  while (1) {

    // Reset values for each move segment
    if (resetDriveSensors) {
      resetDriveSensors = false;
      totalError = 0.0;
      prevError = 0.0; 
      turnTotalError = 0.0;
      turnPrevError = 0.0;
      LeftFrontMotor.setPosition(0, degrees);
      RightFrontMotor.setPosition(0, degrees);
    }

    if (enableDrivePID) {
       // PID CODE GOES HERE

       // stop PID from controlling motors when at target
       if ((fabs(error) < deadBand) || pidAbort) {
         enableDrivePID = false;
       }
    } // enableDrivePID
    vex::task::sleep(20);
 }   // while

to make using the drivePID easier put all the controls in a function

void pidMoveToPosition (double setPoint, double setTurn, bool wait4done=true) {
      resetDriveSensors = true;
      desiredValue = setPoint;
      desiredTurnValue = setTurn;
      enableDrivePID = true;
      while (enableDrivePID & wait4done) {
        pidAbort = Controller1.ButtonB.pressing();  // abort PID by press button B
        vex::task::sleep(20);
      }
      pidAbort = false;
  return; 
}

to test it comment out all code in autonomous and start the task in usercontrol

void usercontrol(void) {
 
  vex::task t1(drivePID);

  // ADD THIS CODE - it doesn't mater if it is before or after Driver Control code
  // start PID by pressing button A
   if (Controller1.ButtonA.pressing()) {
     pidMoveToPosition(720,0,true);
     pidMoveToPosition(-720,0,true);
   }

ed: since you are trying to learn, attempt to get it working and note places you have questions.

3 Likes

Ok, thanks for your help. I’ve been doing some more research and will try this out soon.

Sorry, one question that I have before diving in; how will the drivePID task terminate, since it is in a while loop that lasts forever?

If you don’t plan on using it during drive control then stop it using
static void vex::task::stop ( const task & t )
or if you do plan to use it in driver suspend it
static void vex::task::suspend ( const task & t )
where “t” is the task name “t1” im my example.

You don’t have to kill or suspend the task with this method. It does get scheduled so it will run every 20ms, see it is not enabled, and exit. This is what you’d do for a PID task controlling an arm. It doesn’t interfere with user control.

The reason you would want a drive task working during user control is so you can live tune ki and kd, though I don’t think you need ki for drive (unless you want a really small kp). Once you get your values tuned you can delete the driver control usage and add task stop.

1 Like

Ok, I’ll try this and see how it works. Thanks.

Okay so I tried implementing what you have above but it still isn’t doing anything. I’m still not clear how to get it to do anything. I’ll post what I have now so you can see, maybe I’m doing something stupid.

Blockquote//////////////////////////////////////////////////////////////////////////////////////
/----------------------------------------------------------------------------/
/* /
/
Module: main.cpp /
/
Author: VEX /
/
Created: Thu Sep 26 2019 /
/
Description: Competition Template /
/
/
/
----------------------------------------------------------------------------/
// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name] [Type] [Port(s)]
// LeftMotor motor 1
// RightMotor motor 8
// Controller1 controller
// ---- END VEXCODE CONFIGURED DEVICES ----
#include “vex.h”
using namespace vex;
// A global instance of competition
competition Competition;
// define your global instances of motors and other devices here
/
---------------------------------------------------------------------------/
/
Pre-Autonomous Functions /
/
/
/
You may want to perform some actions before the competition starts. /
/
Do them in the following function. You must return from this function /
/
or the autonomous and usercontrol tasks will not be started. This /
/
function is only called once after the V5 has been powered on and /
/
not every time that the robot is disabled. /
/
---------------------------------------------------------------------------/
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, …
}
double kP = 0.0;
double kI = 0.0;
double kD = 0.0;
double turnkP = 0.0;
double turnkI = 0.0;
double turnkD = 0.0;
//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
int averagePosition=0;
int integralBound=3;
bool resetDriveSensors = false;
//Variables modified for use
bool enableDrivePID = false; // NOTE THIS CHANGE
bool pidAbort = false; // added for testing during usercontrol
double deadBand = 5.0; // this is the close enough to target value
int drivePID() {
//while (1) {
{
// Reset values for each move segment
if (resetDriveSensors) {
resetDriveSensors = false;
totalError = 0.0;
prevError = 0.0;
turnTotalError = 0.0;
turnPrevError = 0.0;
LeftMotor.setPosition(0, degrees);
RightMotor.setPosition(0, degrees);
}
if (enableDrivePID) {
// PID CODE GOES HERE
//Get the position of both motors
int leftMotorPosition = LeftMotor.position(degrees);
int rightMotorPosition = RightMotor.position(degrees);
///////////////////////////////////////////
//Lateral movement PID
/////////////////////////////////////////////////////////////////////
//Get average of the two motors
averagePosition = (leftMotorPosition + rightMotorPosition)/2;
//Potential
error = desiredValue - averagePosition;
//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;
/////////////////////////////////////////////////////////////////////
LeftMotor.spin(forward, lateralMotorPower, voltageUnits::volt);
RightMotor.spin(forward, lateralMotorPower, voltageUnits::volt);
//end of “PID code”
// stop PID from controlling motors when at target
if ((error < deadBand) || pidAbort) {//absolute value was removed for warning error
enableDrivePID = false;
}
}//if enable drive PID
vex::task::sleep(20);
}//while
return 1;
}//drive PID task
void pidMoveToPosition (double setPoint, double setTurn, bool wait4done=true) {
resetDriveSensors = true;
desiredValue = setPoint;
desiredTurnValue = setTurn;
enableDrivePID = true;
while (enableDrivePID & wait4done) {
pidAbort = Controller1.ButtonB.pressing(); // abort PID by press button B
vex::task::sleep(20);
}
pidAbort = false;
return;
}
/
---------------------------------------------------------------------------/
/
/
/
Autonomous Task /
/
/
/
This task is used to control your robot during the autonomous phase of /
/
a VEX Competition. /
/
/
/
You must modify the code to add your own robot specific commands here. /
/
---------------------------------------------------------------------------/
void autonomous(void) {
// …
// Insert autonomous user code here.
// …
}
/
---------------------------------------------------------------------------/
/
/
/
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) {
vex::task t1(drivePID);
// ADD THIS CODE - it doesn’t mater if it is before or after Driver Control code
// start PID by pressing button A
if (Controller1.ButtonA.pressing()) {
pidMoveToPosition(720,0,true);
pidMoveToPosition(-720,0,true);
}
}
//
// 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);
}
}
//////////////////////////////////////////////////////////////////////////////////////