PID problems when switching from Motor Group -> Drivetrain

Hey, recently I’ve started to program PID for the Tipping Point game, skills and match-auto period.

I started my PID program today, and the first part works. The robot stops working after I turn the PID off through “enableDrivePID = false” and when I re-enable it using “enableDrivePID = true”. The PID and everything before this point works smoothly and properly. (around line 142)

I’m not sure for the reason of it not working past this point, it may be because I used motor groups for the PID and then use drivetrain to turn to heading - in
robot-config.cpp I configured the program to use both motor groups and drivetrain + inertial.

(Below is the Program)

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

// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name] [Type] [Port(s)]
// Arm motor_group 6, 5
// Latch motor 8
// Back motor 16
// Controller1 controller
// Drivetrain drivetrain 2, 13, 15, 7, 4
// ---- 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();
LeftMotor.setStopping(hold);
RightMotor.setStopping(hold);
Arm.setStopping(hold);
Latch.setStopping(hold);
Back.setStopping(hold);

LeftMotor.setVelocity(100, percent);
RightMotor.setVelocity(100, percent);
Arm.setVelocity(100, percent);
Back.setVelocity(100, percent);
Latch.setVelocity(100, percent);

// All activities that occur before the competition starts
// Example: clearing encoders, setting servo positions, …
}
///////////////////////////// pid stuff /////////////////////////////
//settings //

//lateral PID
double kP = -0.02; //1 tune until steady minor issolation

// tune normally has large sig-figs

int desiredValue;

// lateral PID
int error = 0; // sensor value (current) - desired value // (positional value) deltaX // sensorValue - DesiredValue : Position
////////////////////////////////////////////////////////////////////////////////////////////////////////

bool resetDriveSensors = false;

//variable modified for use
bool enableDrivePID = true;

int drivePID(){
while(1==1){
while(enableDrivePID) {
if (resetDriveSensors) {
resetDriveSensors = false;

LeftMotor.setPosition(0,degrees);
RightMotor.setPosition(0,degrees);

}

// get position of both motors
int leftMotorPosition = LeftMotor.position(degrees);
int rightMotorPosition = RightMotor.position(degrees);

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Lateral Movement PID
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// get the mean of the left and right motor (2m drive)
int averagePosition = (leftMotorPosition + rightMotorPosition)/2;

//Potential
error = averagePosition - desiredValue;

double lateralMotorPower = (error * kP); // can /12
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
LeftMotor.spin(forward, lateralMotorPower, voltageUnits::volt);
RightMotor.spin(forward, lateralMotorPower, voltageUnits::volt);
vex::task::sleep(20);

}

return 1;
}
}

void autonomous(void) {

vex::task randomname(drivePID); // randomname is to change task (variable)

resetDriveSensors = true;

Drivetrain.setHeading(0, degrees);
Back.spinFor(forward, 800, degrees);

resetDriveSensors = true;
desiredValue = -500; // how far forward it goes
vex::task::sleep(1500);

Back.spinFor(reverse, 800, degrees);

enableDrivePID = true;
resetDriveSensors = true;
desiredValue = 500; // how far forward it goes
vex::task::sleep(1500);
//Forward

enableDrivePID = false;
Drivetrain.turnToHeading(90, degrees);

enableDrivePID = true;
Controller1.Screen.print(“Got past”);
resetDriveSensors = true;
desiredValue = 2000; // how far forward it goes
vex::task::sleep(1500);

//Forward

// …
// 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) {
int deadband = 5;
// User control code here, inside the loop
while (1) {
int RightStick = Controller1. Axis3. position() - Controller1. Axis1. position();
int LeftStick = Controller1. Axis3. position() + Controller1. Axis1. position();

if(abs(LeftStick)<deadband){
  LeftMotor.setVelocity(0, percent);
}
else{
  LeftMotor.setVelocity(LeftStick, percent);
}

if(abs(RightStick)<deadband){
  RightMotor.setVelocity(0, percent);
}
else{
  RightMotor.setVelocity(RightStick, percent);
}
RightMotor.spin(forward);
LeftMotor.spin(forward);






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

}
}

I am reviewing your code but I would start with the following.

  1. I would change the while(enableDrivePID) to if(enableDrivePID) beacuse it is already inside of a while loop.
  2. Move the return 1; in that task to outside of the while loop - under the next brace. (I am not very experienced with c++ tasks but it may be sending a success response to the caller when it shouldn’t be).
int drivePID(){
	while(1==1){
		if(enableDrivePID) {  //<-- 1. Here
		if (resetDriveSensors) {
		resetDriveSensors = false;

		LeftMotor.setPosition(0,degrees);
		RightMotor.setPosition(0,degrees);
		}

		// get position of both motors
		int leftMotorPosition = LeftMotor.position(degrees);
		int rightMotorPosition = RightMotor.position(degrees);

		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
		//Lateral Movement PID
		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
		// get the mean of the left and right motor (2m drive)
		int averagePosition = (leftMotorPosition + rightMotorPosition)/2;

		//Potential
		error = averagePosition - desiredValue;

		double lateralMotorPower = (error * kP); // can /12
		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

		////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
		LeftMotor.spin(forward, lateralMotorPower, voltageUnits::volt);
		RightMotor.spin(forward, lateralMotorPower, voltageUnits::volt);


		}
		vex::task::sleep(20);  //move this outside of the if but still inside the while loop.
	//return 1;  //omit this line.
	}
    return 1;  //<-- 2. Here
}
3 Likes

I had the same suggestions; if it breaks out of the inner while, the task will return 1 (non-zero returns are typically intended to mean error states), so the task/thread would stop the first time out of the inner loop, I believe.

3 Likes

Thanks for the solutions guys!
I´ll try the solutions later today and let you guys know.

Thanks!

1 Like