I want to add a strafePID to my existing lateralPID and turnPID .
How would I go about doing this?
code posted below
//START PRE-AUTON//
//------------------------------------------------------------------------------------------------------------------------------------------------------//
void pre_auton(void){
vexcodeInit();
}
//Motor groups
//--------------------------------------------------------------------------------------------------------------------------------------------//
motor_group FrontDrive =
motor_group(FrontLeftDrive, FrontRightDrive);
motor_group BackDrive =
motor_group(BackLeftDrive, BackRightDrive);
motor_group LeftDrive =
motor_group(FrontLeftDrive, BackLeftDrive);
motor_group RightDrive =
motor_group(FrontRightDrive, BackRightDrive);
motor_group Drive =
motor_group(FrontLeftDrive, FrontRightDrive, BackLeftDrive, BackRightDrive);
//--------------------------------------------------------------------------------------------------------------------------------------------//
//Settings
//--------------------------------------------------------------------------------------------------------------------------------------------//
//Constants
//---------------------------------------------------------------------------//
//Lateral constants
double kP = 0.0;
double kI = 0.0;
double kD = 0.0;
//Turn constants
double turnkP = 0.0;
double turnkI = 0.0;
double turnkD = 0.0;
//---------------------------------------------------------------------------//
//Auton settings
//---------------------------------------------------------------------------//
//Lateral Value
int desiredLateralValue = 0;
//Turn Value
int desiredTurnValue = 0;
//---------------------------------------------------------------------------//
//--------------------------------------------------------------------------------------------------------------------------------------------//
//Variables
//--------------------------------------------------------------------------------------------------------------------------------------------//
//Lateral Variables
//---------------------------------------------------------------------------//
int error; //SensorValue - DesiredValue : Positional value
int prevError = 0; //Position 20ms ago
int derivative; // error - prevError : speed
int totalError = 0; //totalError = totalError + error
//---------------------------------------------------------------------------//
//Turn Variables
//---------------------------------------------------------------------------//
int turnError; //SensorValue - DesiredValue : Positional value
int turnPrevError; //Position 20ms ago
int turnDerivative; // error - prevError : speed
int turnTotalError; //totalError = totalError + error
//---------------------------------------------------------------------------//
bool resetDriveSensors = false;
//Variables modified for use
//---------------------------------------------------------------------------//
bool enableDrivePID = true;
int drivePID(){
while(enableDrivePID){
if (resetDriveSensors) {
resetDriveSensors = false;
FrontLeftDrive.setPosition(0,degrees);
FrontRightDrive.setPosition(0,degrees);
BackLeftDrive.setPosition(0,degrees);
BackRightDrive.setPosition(0,degrees);
}
//---------------------------------------------------------------------------//
//--------------------------------------------------------------------------------------------------------------------------------------------//
//Motor position
//--------------------------------------------------------------------------------------------------------------------------------------------//
//FrontLeft
//---------------------------------------------------------------------------//
int FLDPos = FrontLeftDrive.position(degrees);
//---------------------------------------------------------------------------//
//FrontRight
//---------------------------------------------------------------------------//
int FRDPos = FrontRightDrive.position(degrees);
//---------------------------------------------------------------------------//
//BackLeft
//---------------------------------------------------------------------------//
int BLDPos = BackLeftDrive.position(degrees);
//---------------------------------------------------------------------------//
//BackRight
//---------------------------------------------------------------------------//
int BRDPos = BackRightDrive.position(degrees);
//---------------------------------------------------------------------------//
//--------------------------------------------------------------------------------------------------------------------------------------------//
//Lateral PID
//--------------------------------------------------------------------------------------------------------------------------------------------//
//Averaging motors
//---------------------------------------------------------------------------//
int AVGPos = (FLDPos + FRDPos + BLDPos + BRDPos)/4;
//---------------------------------------------------------------------------//
//Proportional
//---------------------------------------------------------------------------//
error = AVGPos - desiredLateralValue;
//---------------------------------------------------------------------------//
//Integral
//---------------------------------------------------------------------------//
totalError += error;
//---------------------------------------------------------------------------//
//Derivative
//---------------------------------------------------------------------------//
derivative = error - prevError;
//---------------------------------------------------------------------------//
//Lateral PID equation
//---------------------------------------------------------------------------//
double lateralMotorPower = (error * kP + totalError * kI + derivative * kD);
//---------------------------------------------------------------------------//
//--------------------------------------------------------------------------------------------------------------------------------------------//
//Turn PID
//--------------------------------------------------------------------------------------------------------------------------------------------//
//Finding turn difference
//---------------------------------------------------------------------------//
int turnDifference = ((FLDPos + BLDPos) - (FRDPos + BRDPos));
//---------------------------------------------------------------------------//
//Proportional
//---------------------------------------------------------------------------//
turnError = turnDifference - desiredTurnValue;
//---------------------------------------------------------------------------//
//Integral
//---------------------------------------------------------------------------//
turnTotalError += turnError;
//---------------------------------------------------------------------------//
//Derivative
//---------------------------------------------------------------------------//
turnDerivative = turnError - turnPrevError;
//---------------------------------------------------------------------------//
//Turn PID equation
//---------------------------------------------------------------------------//
double turnMotorPower = (turnError * turnkP + turnTotalError * turnkI + turnDerivative * turnkD);
//---------------------------------------------------------------------------//
//--------------------------------------------------------------------------------------------------------------------------------------------//
//Motion
//--------------------------------------------------------------------------------------------------------------------------------------------//
//FrontLeft
//---------------------------------------------------------------------------//
FrontLeftDrive.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
//---------------------------------------------------------------------------//
//FrontRight
//---------------------------------------------------------------------------//
FrontRightDrive.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
//---------------------------------------------------------------------------//
//BackLeft
//---------------------------------------------------------------------------//
BackLeftDrive.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
//---------------------------------------------------------------------------//
//BackRight
//---------------------------------------------------------------------------//
BackRightDrive.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
//---------------------------------------------------------------------------//
//--------------------------------------------------------------------------------------------------------------------------------------------//
//Wrap-Up
//--------------------------------------------------------------------------------------------------------------------------------------------//
prevError = error;
vex::task::sleep(20);
}
return 1;
}
//--------------------------------------------------------------------------------------------------------------------------------------------//
//------------------------------------------------------------------------------------------------------------------------------------------------------//
//END PRE-AUTON//
Connor
July 21, 2022, 9:32am
2
Howdy,
I would think you would want just lateralPID and turnPID , with lateralPID controlling all straight-line horizontal movement. But maybe you do that by adding a strafePID to a lateralPID.
1 Like
Connor
July 21, 2022, 1:41pm
4
When doing forward/backward, you get positive average of all motor encoders (fl + bl + fr + br) / 4
When doing side to side I would assume you get the difference of the front and back (fl - bl + fr - br)/4
So…
int AVGPos_Strafe = (FLDPos + FRDPos - BLDPos - BRDPos)/4;
1 Like
I have an x drive. basically my question is that if a want another axis of pid, do I have to write the whole loop twice or is there a way I can add strafe to my existing lateral and turn pid
I’m going to write something up and I’ll post the code again.
Ok, I’ve written up something that I think will work. code here
//START PRE-AUTON//
//------------------------------------------------------------------------------------------------------------------------------------------------------//
void pre_auton(void){
vexcodeInit();
}
//Motor groups
//--------------------------------------------------------------------------------------------------------------------------------------------//
motor_group FrontDrive =
motor_group(FrontLeftDrive, FrontRightDrive);
motor_group BackDrive =
motor_group(BackLeftDrive, BackRightDrive);
motor_group LeftDrive =
motor_group(FrontLeftDrive, BackLeftDrive);
motor_group RightDrive =
motor_group(FrontRightDrive, BackRightDrive);
motor_group Drive =
motor_group(FrontLeftDrive, FrontRightDrive, BackLeftDrive, BackRightDrive);
//--------------------------------------------------------------------------------------------------------------------------------------------//
//Settings
//--------------------------------------------------------------------------------------------------------------------------------------------//
//simple shortcuts
//---------------------------------------------------------------------------//
//Right preset turn values
int R45deg = 45;
int R90deg = 90;
int R135deg = 135;
int R180deg = 180;
//Left preset turn values
int L45deg = -45;
int L90deg = -90;
int L135deg = -135;
int L180deg = -180;
//---------------------------------------------------------------------------//
//Constants
//---------------------------------------------------------------------------//
//Lateral constants
double kP = 0.0;
double kI = 0.0;
double kD = 0.0;
//Turn constants
double turnkP = 0.0;
double turnkI = 0.0;
double turnkD = 0.0;
//Turn constants
double strafekP = 0.0;
double strafekI = 0.0;
double strafekD = 0.0;
//---------------------------------------------------------------------------//
//Auton settings
//---------------------------------------------------------------------------//
//Lateral Value
int desiredLateralValue = 0;
//Strafe Value
int desiredStrafeValue = 0;
//Turn Value
int desiredTurnValue = 0;
//---------------------------------------------------------------------------//
//--------------------------------------------------------------------------------------------------------------------------------------------//
//Variables
//--------------------------------------------------------------------------------------------------------------------------------------------//
//Lateral variables
//---------------------------------------------------------------------------//
int error; //SensorValue - DesiredValue : Positional value
int prevError = 0; //Position 20ms ago
int derivative; // error - prevError : speed
int totalError = 0; //totalError = totalError + error
//---------------------------------------------------------------------------//
//Turn variables
//---------------------------------------------------------------------------//
int turnError; //SensorValue - DesiredValue : Positional value
int turnPrevError; //Position 20ms ago
int turnDerivative; // error - prevError : speed
int turnTotalError; //totalError = totalError + error
//---------------------------------------------------------------------------//
//Strafe variables
//---------------------------------------------------------------------------//
int strafeError; //SensorValue - DesiredValue : Positional value
int strafePrevError; //Position 20ms ago
int strafeDerivative; // error - prevError : speed
int strafeTotalError; //totalError = totalError + error
//---------------------------------------------------------------------------//
bool resetDriveSensors = false;
//Variables modified for use
//---------------------------------------------------------------------------//
bool enableDrivePID = true;
int drivePID(){
while(enableDrivePID){
if (resetDriveSensors) {
resetDriveSensors = false;
FrontLeftDrive.setPosition(0,degrees);
FrontRightDrive.setPosition(0,degrees);
BackLeftDrive.setPosition(0,degrees);
BackRightDrive.setPosition(0,degrees);
}
//---------------------------------------------------------------------------//
//--------------------------------------------------------------------------------------------------------------------------------------------//
//Motor position
//--------------------------------------------------------------------------------------------------------------------------------------------//
//FrontLeft
//---------------------------------------------------------------------------//
int FLDPos = FrontLeftDrive.position(degrees);
//---------------------------------------------------------------------------//
//FrontRight
//---------------------------------------------------------------------------//
int FRDPos = FrontRightDrive.position(degrees);
//---------------------------------------------------------------------------//
//BackLeft
//---------------------------------------------------------------------------//
int BLDPos = BackLeftDrive.position(degrees);
//---------------------------------------------------------------------------//
//BackRight
//---------------------------------------------------------------------------//
int BRDPos = BackRightDrive.position(degrees);
//---------------------------------------------------------------------------//
//--------------------------------------------------------------------------------------------------------------------------------------------//
//Lateral PID
//--------------------------------------------------------------------------------------------------------------------------------------------//
//Averaging motors
//---------------------------------------------------------------------------//
int AVGPos = (FLDPos + FRDPos + BLDPos + BRDPos)/4;
//---------------------------------------------------------------------------//
//Proportional
//---------------------------------------------------------------------------//
error = AVGPos - desiredLateralValue;
//---------------------------------------------------------------------------//
//Integral
//---------------------------------------------------------------------------//
totalError += error;
//---------------------------------------------------------------------------//
//Derivative
//---------------------------------------------------------------------------//
derivative = error - prevError;
//---------------------------------------------------------------------------//
//Lateral PID equation
//---------------------------------------------------------------------------//
double lateralMotorPower = (error * kP + totalError * kI + derivative * kD);
//---------------------------------------------------------------------------//
//--------------------------------------------------------------------------------------------------------------------------------------------//
//Turn PID
//--------------------------------------------------------------------------------------------------------------------------------------------//
//Finding turn difference
//---------------------------------------------------------------------------//
int turnDifference = ((FLDPos + BLDPos) - (FRDPos + BRDPos));
//---------------------------------------------------------------------------//
//Proportional
//---------------------------------------------------------------------------//
turnError = turnDifference - desiredTurnValue;
//---------------------------------------------------------------------------//
//Integral
//---------------------------------------------------------------------------//
turnTotalError += turnError;
//---------------------------------------------------------------------------//
//Derivative
//---------------------------------------------------------------------------//
turnDerivative = turnError - turnPrevError;
//---------------------------------------------------------------------------//
//Turn PID equation
//---------------------------------------------------------------------------//
double turnMotorPower = (turnError * turnkP + turnTotalError * turnkI + turnDerivative * turnkD);
//---------------------------------------------------------------------------//
//--------------------------------------------------------------------------------------------------------------------------------------------//
//Strafe PID
//--------------------------------------------------------------------------------------------------------------------------------------------//
//Averaging motors
//---------------------------------------------------------------------------//
int strafeAVGPos = (FLDPos + FRDPos - BLDPos - BRDPos)/4;
//---------------------------------------------------------------------------//
//Proportional
//---------------------------------------------------------------------------//
strafeError = strafeAVGPos - desiredStrafeValue;
//---------------------------------------------------------------------------//
//Integral
//---------------------------------------------------------------------------//
strafeTotalError += strafeError;
//---------------------------------------------------------------------------//
//Derivative
//---------------------------------------------------------------------------//
strafeDerivative = strafeError - strafePrevError;
//---------------------------------------------------------------------------//
//Strafe PID equation
//---------------------------------------------------------------------------//
double strafeMotorPower = (strafeError * strafekP + strafeTotalError * strafekI + derivative * strafekD);
//---------------------------------------------------------------------------//
//Motion
//--------------------------------------------------------------------------------------------------------------------------------------------//
//FrontLeft
//---------------------------------------------------------------------------//
FrontLeftDrive.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
//---------------------------------------------------------------------------//
//FrontRight
//---------------------------------------------------------------------------//
FrontRightDrive.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
//---------------------------------------------------------------------------//
//BackLeft
//---------------------------------------------------------------------------//
BackLeftDrive.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
//---------------------------------------------------------------------------//
//BackRight
//---------------------------------------------------------------------------//
BackRightDrive.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
//---------------------------------------------------------------------------//
//--------------------------------------------------------------------------------------------------------------------------------------------//
//Wrap-Up
//--------------------------------------------------------------------------------------------------------------------------------------------//
prevError = error;
vex::task::sleep(20);
}
return 1;
}
//--------------------------------------------------------------------------------------------------------------------------------------------//
//------------------------------------------------------------------------------------------------------------------------------------------------------//
//END PRE-AUTON//
My question now is this- How should I fit
//Strafe PID equation
//---------------------------------------------------------------------------//
double strafeMotorPower = (strafeError * strafekP + strafeTotalError * strafekI + derivative * strafekD);
//---------------------------------------------------------------------------//
into
//Motion
//--------------------------------------------------------------------------------------------------------------------------------------------//
//FrontLeft
//---------------------------------------------------------------------------//
FrontLeftDrive.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
//---------------------------------------------------------------------------//
//FrontRight
//---------------------------------------------------------------------------//
FrontRightDrive.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
//---------------------------------------------------------------------------//
//BackLeft
//---------------------------------------------------------------------------//
BackLeftDrive.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
//---------------------------------------------------------------------------//
//BackRight
//---------------------------------------------------------------------------//
BackRightDrive.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
//---------------------------------------------------------------------------//
//--------------------------------------------------------------------------------------------------------------------------------------------//
I think this will work. Each motor on the drive has two directions, and based on those directions, you either subtract or add the MotorPower for said wheels to go in the desired direction.
//Motion
//--------------------------------------------------------------------------------------------------------------------------------------------//
//FrontLeft
//---------------------------------------------------------------------------//
FrontLeftDrive.spin(forward, lateralMotorPower + turnMotorPower + strafeMotorPower, voltageUnits::volt);
//---------------------------------------------------------------------------//
//FrontRight
//---------------------------------------------------------------------------//
FrontRightDrive.spin(forward, lateralMotorPower - turnMotorPower - strafeMotorPower, voltageUnits::volt);
//---------------------------------------------------------------------------//
//BackLeft
//---------------------------------------------------------------------------//
BackLeftDrive.spin(forward, lateralMotorPower + turnMotorPower - strafeMotorPower, voltageUnits::volt);
//---------------------------------------------------------------------------//
//BackRight
//---------------------------------------------------------------------------//
BackRightDrive.spin(forward, lateralMotorPower - turnMotorPower + strafeMotorPower, voltageUnits::volt);
//---------------------------------------------------------------------------//
//--------------------------------------------------------------------------------------------------------------------------------------------//
For FrontLeft, I added strafeMotorPower because that motor needs to go forward to strafe right, and reverse to strafe left. I used this logic to write the rest of the motor movements.
Let me know if you see any errors.