PID Turning in Place

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;

}

return 1;
}

void autonomous(void) {

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);
}
}
``````
2 Likes

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.

``````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.

3 Likes

We will try this tonight :))

1 Like

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

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

2 Likes