Hi Everyone,
I am trying to code PID for Worlds. Below is this code. Every time I run it, it is a single jitter and prints an error close to -420.
/*----------------------------------------------------------------------------*/
/* */
/* Module: main.cpp */
/* Author: sagan.karthikeyan */
/* Created: 3/28/2025, 12:22:07 AM */
/* Description: V5 project */
/* */
/*----------------------------------------------------------------------------*/
#include "../include/vex.h"
using namespace vex;
// A global instance of competition
competition Competition;
brain Brain;
// define your global instances of motors and other devices here
controller controller1 = controller(); //definining controllers
// defining motors
//Drivetrain
motor LeftFront = motor(PORT1, ratio18_1, true);
motor LeftBack = motor(PORT2, ratio18_1, true);
motor RightFront = motor(PORT11, ratio18_1, true);
motor RightBack = motor(PORT12, ratio18_1, true);
motor_group LeftDrive = motor_group(LeftFront, LeftBack);
motor_group RightDrive = motor_group(RightFront, RightBack);
drivetrain Drivetrain = drivetrain(LeftDrive, RightDrive, 259.34, 14, 11.5, inches, 2);
//Mogomech
motor MogoMech = motor(PORT20, ratio6_1, true);
//LadyBrown
motor LadyBrown = motor(PORT17, ratio6_1, true);
//Intake
motor Intake = motor(PORT18, ratio36_1, false);
motor Conveyor = motor(PORT16, ratio36_1, false);
motor_group FullIntake = motor_group(Intake, Conveyor); //Motor Group for Intake
/////////////////////WallStake Macros////////////////////////////////////////////////
//3 Wallstake Positions
int WallPOSITION_1 = 0;
int WallPOSITION_2 = 350;
int WallPOSITION_3 = 2400;
int WallPOSITION_4 = 3200;
//Starts at position 1
int Wallposition = 1;
///////////////////////////////////End of WallStake Macros/////////////////////////////////////////////////
///////////////////////////////////MogoMech Macros/////////////////////////////////////////////////////////
//2 MogoMech Positions
int MogoPOSITION_1 = 0;
int MogoPOSITION_2 = 180;
//Starts at position 1
int MogoPosition = 1;
//////////////////////////////////End of MogoMech Macros/////////////////////////////////
double wheelDiameter = 3.25;
double PI = 3.141592653589793;
double inchesToDegrees(double inches) {
return (inches / (PI * wheelDiameter)) * 360.0;
}
double turnDegreesToDriveDegrees(double turnDegrees, double trackWidthInches) {
double turnCircumference = PI * trackWidthInches;
double rotationDistance = (turnDegrees/360.0) * turnCircumference;
return(rotationDistance/ (PI * wheelDiameter)) * 360.0;
}
////////////////////////////////////////////PRE-AUTONOMOUS/////////////////////////////////////////////////////
/*---------------------------------------------------------------------------*/
/* 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) {
// All activities that occur before the competition starts
// Example: clearing encoders, setting servo positions, ...
}
//////////////////////////////////PRE-AUTOONOMOUS END////////////////////////////////////////////////////////////////
///////////////////////////PID VARIABLES START///////////////////////////////////////
//Settings
double kP = 0.06;
double kI = 0.0;
double kD = 0.05;
double turnkP = 0.1;
double turnkI = 0.0;
double turnkD = 0;
/* HOW TO TUNE SETTINGS
kP: Increase kP until you have steady minor oscillations; This fixes errors.
kD: Increase kD until the isolations are no longer isolations anymore; This makes minor corrections.
kI: Increase kI until the precision is "good enough for you." This changes speed depending if it is too fast or too slow. DON'T DO FOR DRIVETRIAN AS IT CAN BE INEFFECTIVE.*/
/*EXAMPLE VALUES:
kP = 0.0000004
During autonomous,
desiredValue = 300
desiredTurnValue = 600*/
//https://api.vexcode.cloud/v5/html
//Autotnomous Settings
int desiredValue = 0;
int desiredTurnValue = 0;
int error; //Sensor value - Desired Value - Position
int prevError = 0; //Position 20 milliseconds ago
int derivative; //Difference between error and previous error : Speed
int totalError = 0; //totalError = totalError + error
int turnError; //Sensor value - Desired Value - Position
int turnPrevError = 0; //Position 20 milliseconds ago
int turnDerivative; //Difference between error and previous error : Speed
int turnTotalError; //totalError = totalError + error
bool resetDriveSensors = false;
double maxVoltage = 12.0;
//Variables modified for use
bool enabledrivePID = false;
////////////////////////////////////////PID VARIABLES END////////////////////////////////////////////////////////
//////////////////////////////////////////DRIVEPID///////////////////////////////////////////////////////////////
int drivePID(){
while(enabledrivePID == true){
if (resetDriveSensors) {
resetDriveSensors = false;
LeftFront.setPosition(0, degrees);
LeftBack.setPosition(0, degrees);
RightFront.setPosition(0, degrees);
RightBack.setPosition(0, degrees);
}
//Get the position of both motors
int lfmp = LeftFront.position(degrees) ;
int lbmp = LeftBack.position(degrees);
int rfmp = RightFront.position(degrees) ;
int rbmp = RightBack.position(degrees);
int LeftAvg = (lfmp+lbmp)/2;
int RightAvg = (rfmp+rbmp)/2;
//Debug
Brain.Screen.clearScreen();
Brain.Screen.setCursor(1, 1);
Brain.Screen.print("LF Pos: %f", lfmp); // 👈 This is the bonus debug tip
Brain.Screen.setCursor(2, 1);
Brain.Screen.print("RF Pos: %f", rfmp);
///////////////////////////////////////////////////////////////////////
//Lateral Movement PID
///////////////////////////////////////////////////////////////////////
//Get average of the two motors
int averagePosition = (LeftAvg + RightAvg)/2;
//Potential
error = desiredValue - averagePosition;
//Derivative
derivative = error - prevError;
//Integral
//totalError += error;
//DONT USE INTEGRAL CONTROL FOR DRIVETRAIN, AS IT CAN BE INEFFECTIVE!!!
double lateralMotorPower = (error * kP + derivative * kD);
if (lateralMotorPower > maxVoltage) lateralMotorPower = maxVoltage;
if (lateralMotorPower < -maxVoltage) lateralMotorPower = -maxVoltage;
//////////////////////////////////////////////////////////// ///////////
///////////////////////////////////////////////////////////////////////
//Turning Movement PID
///////////////////////////////////////////////////////////////////////
//Get average of the two motors
int turnDifference = (LeftAvg) - (RightAvg);
//Potential
turnError = desiredTurnValue - turnDifference;
//Derivative
turnDerivative = turnError - turnPrevError;
//Integral
//turnTotalError += turnError;
//DONT USE INTEGRAL CONTROL FOR DRIVETRAIN, AS IT CAN BE INEFFECTIVE!!!
double turnMotorPower = (turnError * turnkP + turnDerivative * turnkD);
///////////////////////////////////////////////////////////////////////
//start motors
LeftFront.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
LeftBack.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
RightFront.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
RightBack.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
Brain.Screen.clearScreen();
Brain.Screen.setCursor(1, 1);
Brain.Screen.print("Error: %d", error);
Brain.Screen.setCursor(2, 1);
Brain.Screen.print("Lateral Power: %.2f", lateralMotorPower);
Brain.Screen.setCursor(3, 1);
Brain.Screen.print("Turn Power: %.2f", turnMotorPower);
//code
prevError = error;
turnPrevError = turnError;
vex::task::sleep(20);
};
return 1;
};
/////////////////////////////////////////////End of PID LOOP////////////////////////////////////////////////////
////////////////////////////////////////////Drive Wrapper Function//////////////////////////////////////////////
void driveForward(double inches) {
resetDriveSensors = true;
desiredTurnValue = inchesToDegrees(inches);
desiredValue = 0;
Brain.Screen.print("Target: %f", desiredValue);
vex::task::sleep(500);
while (abs(error) > 15) {
vex::task::sleep(20);
}
}
////////////////////////////////////////////End of Drive Wrapper Function//////////////////////////////////////////////
////////////////////////////////////////////Turning Wrapper Function//////////////////////////////////////////////
void turnRobot(double degreesToTurn, double trackWidthInches = 13.5){
resetDriveSensors = true;
desiredValue = turnDegreesToDriveDegrees(degreesToTurn, trackWidthInches);
desiredTurnValue = 0;
Brain.Screen.print("Target: %f", desiredTurnValue);
vex::task::sleep(500);
while (abs(error) > 15) {
vex::task::sleep(20);
}
}
////////////////////////////////////////////End of Turning Wrapper Function//////////////////////////////////////////////
//////////////////////////////////////////DRIVEPID END///////////////////////////////////////////////////////////////
///////////////////////////////////////////AUTONOMOUS///////////////////////////////////////////////
/*---------------------------------------------------------------------------*/
/* */
/* 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.
enabledrivePID = true;
vex::task start(drivePID);
LeftFront.setStopping(brake);
RightFront.setStopping(brake);
LeftBack.setStopping(brake);
RightBack.setStopping(brake);
int errorMargin = 20;
driveForward(-12);
//This is how you would code Autonomous; Think of the 300's as measurements.
// ..........................................................................
}
//////////////////////////////////////////AUTONOMOUS END///////////////////////////////////////////////////////////////
/////////////////////////////////////////USER CONTROL////////////////////////////////////////////////////////////////////
/*---------------------------------------------------------------------------*/
/* */
/* 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
enabledrivePID = false;
while (1) {
// 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.
//9Motor Gang Code for Split Arcade
LeftDrive.setStopping(coast);
RightDrive.setStopping(coast);
LeftDrive.spin(forward, controller1.Axis1.position(percent) + controller1.Axis3.position(percent)*12/100, volt);
RightDrive.spin(forward, controller1.Axis1.position(percent) - controller1.Axis3.position(percent)*12/100, volt);
//Ladybrown Controller Code
LadyBrown.setVelocity(500, rpm);
LadyBrown.setStopping(hold);
if (Wallposition == 1){
LadyBrown.spinTo(WallPOSITION_1, degrees, false);
}
if (Wallposition == 2){
LadyBrown.spinTo(WallPOSITION_2, degrees, false);
}
if (Wallposition == 3){
LadyBrown.spinTo(WallPOSITION_3, degrees, false);
}
if (Wallposition == 4){
LadyBrown.spinTo(WallPOSITION_4, degrees, false);
}
if (Wallposition > 4) {
Wallposition = 4;
}
if (Wallposition < 1) {
Wallposition = 1;
}
if (controller1.ButtonL1.PRESSED) {
Wallposition++;
}
if (controller1.ButtonL2.PRESSED) {
Wallposition--;
}
//MogoMech Controller Code
MogoMech.setStopping(hold);
MogoMech.setMaxTorque(100, percent);
if (controller1.ButtonY.pressing())
{
MogoMech.spin(forward, 4, volt);
}
else if (controller1.ButtonB.pressing())
{
MogoMech.spin(reverse, 4, volt);
}
else
{
MogoMech.setStopping(hold);
MogoMech.stop();
}
//Intake Controlling
FullIntake.setStopping(brake);
FullIntake.setVelocity(100, percent);
if (controller1.ButtonR2.pressing())
{
FullIntake.spin(forward);
}
else if (controller1.ButtonR1.pressing())
{
FullIntake.spin(reverse);
}
else
{
FullIntake.setStopping(brake);
FullIntake.stop();
}
// ........................................................................
wait(20, msec); // Sleep the task for a short amount of time to
// prevent wasted resources.
}
}
//////////////////////////////////////////USERCONTROL END///////////////////////////////////////////////////////////////
//
// 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();
autonomous();
// Prevent main from exiting with an infinite loop.
while (true) {
wait(100, msec);
}
}
Any help is deeply appreciated!