Hello! I’ve been working on this PID control all day and I can’t figure out how it’s gonna work. I have a four motor drivetrain and everything is messed up within the program. I followed @Connor tutorial on this and changed a few things to compensate for the extra 2 motors. Whenever I call the (desiredTurnValue) function and put ANY value whether it = 1 or 1000 it turns forever. Whenever I call the (desiredValue) function the motors go opposite directions into each other and the result is the robot staying in the same spot while all of the motors are spinning. Here is the code: PID12PM.zip (6.9 KB)
/*----------------------------------------------------------------------------*/
/* */
/* Module: main.cpp */
/* Author: VEX */
/* Created: Thu Sep 26 2019 */
/* Description: Clawbot Competition Template */
/* */
/*----------------------------------------------------------------------------*/
#include "vex.h"
// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name] [Type] [Port(s)]
// ---- END VEXCODE CONFIGURED DEVICES ----
using namespace vex;
// A global instance of competition
competition Competition;
// define your global instances of motors and other devices here
vex::motor BottomLeftDriveMotor(vex::PORT4, vex::gearSetting::ratio18_1, true);
vex::motor BottomRightDriveMotor(vex::PORT1, vex::gearSetting::ratio18_1, true);
vex::motor FrontLeftDriveMotor(vex::PORT10, vex::gearSetting::ratio18_1, true);
vex::motor FrontRightDriveMotor(vex::PORT9, vex::gearSetting::ratio18_1, true);
vex::controller Controller1;
vex::motor LiftR(vex::PORT6, vex::gearSetting::ratio18_1, true);
vex::motor LiftL(vex::PORT7, vex::gearSetting::ratio18_1, true);
vex::inertial inertial1(vex::PORT5);
motor_group lift= motor_group(LiftR, LiftL);
extern motor_group lift;
// 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 kP = 0.1;
double kI = 0.1;
double kD = 0.1;
double turnkP = 0.5;
double turnkI = 0.0;
double turnkD = 0.0;
//auton settings
int desiredValue = 200;
int desiredTurnValue = 0;
int error; //SensorValue - DesiredVaule : Position
int prevError = 0;//Position 20 msecs ago
int derivative; // error - prevError : Speed
int totalError = 0; //totalError = totalError + error
int turnError; //SensorValue - DesiredValue : Position
int turnPrevError = 0;//Position 20 msecs ago
int turnDerivative; // error - prevError : Speed
int turnTotalError = 0; //totalError = totalError + error
bool resetDriveSensors = false;
//variables (modifiable)
bool enableDrivePID = true;
int drivePID(){
while(enableDrivePID){
if (resetDriveSensors) {
resetDriveSensors = false;
BottomLeftDriveMotor.setPosition(0, degrees);
BottomRightDriveMotor.setPosition(0, degrees);
FrontLeftDriveMotor.setPosition(0, degrees);
FrontRightDriveMotor.setPosition(0, degrees);
}
//position of all 4 motors
int BottomLeftDriveMotorPosition = BottomLeftDriveMotor.position(degrees);
int BottomRightDriveMotorPosition = BottomRightDriveMotor.position(degrees);
int FrontLeftDriveMotorPosition = FrontLeftDriveMotor.position(degrees);
int FrontRightDriveMotorPosition = FrontRightDriveMotor.position(degrees);
//////////////////////////////////////////////////////////////////////////////////////
//Lateral Movement PID
//////////////////////////////////////////////////////////////////////////////////////
//avg of motors
int averagePosition = (BottomLeftDriveMotorPosition + BottomRightDriveMotorPosition
+ FrontLeftDriveMotorPosition + FrontRightDriveMotorPosition)/4;
//Potential
error = averagePosition - desiredValue;
//Derivative
derivative = error - prevError;
//Integral
totalError += error;
double lateralMotorPower = error * kP + derivative * kD + totalError * kI;
//////////////////////////////////////////////////////////////////////////////////////
//Turning Movement PID
//////////////////////////////////////////////////////////////////////////////////////
//avg of motors
int turnDifference = FrontLeftDriveMotorPosition - FrontRightDriveMotorPosition + BottomLeftDriveMotorPosition + BottomRightDriveMotorPosition;
//Potential
turnError = desiredTurnValue - turnDifference;
//Derivative
turnDerivative = turnError - turnPrevError;
//Integral
turnTotalError += turnError;
double turnMotorPower = turnError * turnkP + turnDerivative * turnkD + turnTotalError * turnkI;
FrontRightDriveMotor.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
FrontLeftDriveMotor.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
BottomLeftDriveMotor.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
BottomRightDriveMotor.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
FrontRightDriveMotor.setStopping(brake);
FrontLeftDriveMotor.setStopping(brake);
BottomLeftDriveMotor.setStopping(brake);
BottomRightDriveMotor.setStopping(brake);
//code
prevError = error;
turnPrevError = turnError;
vex::task::sleep(20);
}
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) {
// ..........................................................................
// Insert autonomous user code here.
// ..........................................................................
vex::task MainFunction(drivePID);
resetDriveSensors = true;
desiredValue = 1;
vex::task::sleep(1000);
}
/*---------------------------------------------------------------------------*/
/* */
/* 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
while (1) {
enableDrivePID = false;
// 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);
}
}
edit: mods added code tags, please try and remember to use them
Both your left motors have to be + and both your right motors have to be -.
should be FrontLeftDriveMotorPosition - FrontRightDriveMotorPosition + BottomLeftDriveMotorPosition - BottomRightDriveMotorPosition.
Your left drive motors need to be assigned the same speed as each other and your right drive motors need to be assigned the same speed as each other. This is not okay:
It might help if you grouped the left and right motors together:
Thank you for that. When doing the corrections you suggested the robot had some changes. When I tested desiredValue and turnValue they do the same exact thing. They both turn but don’t stay in the same position moving around a bit. I turned it on it’s back and I can see that the left side of the drivetrain continuosly drives forward while the right side goes forward and backward maybe trying to correct itself? I put the pI “integral” up to 0.001 on both and it’s not working. Any suggestions?
Updated code:
[/----------------------------------------------------------------------------/
/* /
/ Module: main.cpp /
/ Author: VEX /
/ Created: Thu Sep 26 2019 /
/ Description: Clawbot Competition Template /
//
/----------------------------------------------------------------------------*/
#include "vex.h"
// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name] [Type] [Port(s)]
// ---- END VEXCODE CONFIGURED DEVICES ----
using namespace vex;
// A global instance of competition
competition Competition;
// define your global instances of motors and other devices here
vex::motor BottomLeftDriveMotor(vex::PORT4, vex::gearSetting::ratio18_1, true);
vex::motor BottomRightDriveMotor(vex::PORT1, vex::gearSetting::ratio18_1, true);
vex::motor FrontLeftDriveMotor(vex::PORT10, vex::gearSetting::ratio18_1, true);
vex::motor FrontRightDriveMotor(vex::PORT9, vex::gearSetting::ratio18_1, true);
vex::controller Controller1;
vex::motor LiftR(vex::PORT6, vex::gearSetting::ratio18_1, true);
vex::motor LiftL(vex::PORT7, vex::gearSetting::ratio18_1, true);
vex::inertial inertial1(vex::PORT5);
motor_group lift= motor_group(LiftR, LiftL);
extern motor_group lift;
// 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 kP = 0.0;
double kI = 0.001;
double kD = 0.0;
double turnkP = 0.0;
double turnkI = 0.001;
double turnkD = 0.0;
//auton settings
int desiredValue = 150;
int desiredTurnValue = 0;
int error; //SensorValue - DesiredVaule : Position
int prevError = 0;//Position 20 msecs ago
int derivative; // error - prevError : Speed
int totalError = 0; //totalError = totalError + error
int turnError; //SensorValue - DesiredValue : Position
int turnPrevError = 0;//Position 20 msecs ago
int turnDerivative; // error - prevError : Speed
int turnTotalError = 0; //totalError = totalError + error
bool resetDriveSensors = false;
//variables (modifiable)
bool enableDrivePID = true;
int drivePID(){
while(enableDrivePID){
if (resetDriveSensors) {
resetDriveSensors = false;
BottomLeftDriveMotor.setPosition(0, degrees);
BottomRightDriveMotor.setPosition(0, degrees);
FrontLeftDriveMotor.setPosition(0, degrees);
FrontRightDriveMotor.setPosition(0, degrees);
}
//position of all 4 motors
int BottomLeftDriveMotorPosition = BottomLeftDriveMotor.position(degrees);
int BottomRightDriveMotorPosition = BottomRightDriveMotor.position(degrees);
int FrontLeftDriveMotorPosition = FrontLeftDriveMotor.position(degrees);
int FrontRightDriveMotorPosition = FrontRightDriveMotor.position(degrees);
//////////////////////////////////////////////////////////////////////////////////////
//Lateral Movement PID
//////////////////////////////////////////////////////////////////////////////////////
//avg of motors
int averagePosition = (BottomLeftDriveMotorPosition + BottomRightDriveMotorPosition
+ FrontLeftDriveMotorPosition + FrontRightDriveMotorPosition)/4;
//Potential
error = averagePosition - desiredValue;
//Derivative
derivative = error - prevError;
//Integral
totalError += error;
double lateralMotorPower = error * kP + derivative * kD + totalError * kI;
//////////////////////////////////////////////////////////////////////////////////////
//Turning Movement PID
//////////////////////////////////////////////////////////////////////////////////////
//avg of motors
int turnDifference = FrontLeftDriveMotorPosition + FrontRightDriveMotorPosition - BottomLeftDriveMotorPosition + BottomRightDriveMotorPosition;
//Potential
turnError = desiredTurnValue - turnDifference;
//Derivative
turnDerivative = turnError - turnPrevError;
//Integral
turnTotalError += turnError;
double turnMotorPower = turnError * turnkP + turnDerivative * turnkD + turnTotalError * turnkI;
FrontRightDriveMotor.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
FrontLeftDriveMotor.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
BottomLeftDriveMotor.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
BottomRightDriveMotor.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
FrontRightDriveMotor.setStopping(brake);
FrontLeftDriveMotor.setStopping(brake);
BottomLeftDriveMotor.setStopping(brake);
BottomRightDriveMotor.setStopping(brake);
//code
prevError = error;
turnPrevError = turnError;
vex::task::sleep(20);
}
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) {
// ..........................................................................
// Insert autonomous user code here.
// ..........................................................................
vex::task MainFunction(drivePID);
resetDriveSensors = true;
desiredTurnValue = 150;
vex::task::sleep(1000);
}
/*---------------------------------------------------------------------------*/
/* */
/* 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
while (1) {
enableDrivePID = false;
// 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);
}
}
Let’s start this off simple, to ensure that the lateral pid isn’t influencing the turn pid.
set all turn constants to 0, set ki, and kd to 0. Keep kp at 0.1 and set desired to 1000 (this is 1000 ticks).
What happens when you run your code?
Another note: It appears that when you added your motor, you made both the left and right motors reversed. Keep your left motors at false for reversed and your right motors true for reversed. This is assuming direct drive.
Just tested the changes you suggested. desiredValue set to any value outputs the robot going forward or backward, (using negative to go forward and positive to go backward) this is the same for desiredTurnValue. I see 1 or 2 adjustments to keep the robot straight but thats about it. The robot drives FOREVER. any help?
Updated code here:
*----------------------------------------------------------------------------*/
/* */
/* Module: main.cpp */
/* Author: VEX */
/* Created: Thu Sep 26 2019 */
/* Description: Clawbot Competition Template */
/* */
/*----------------------------------------------------------------------------*/
#include "vex.h"
// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name] [Type] [Port(s)]
// ---- END VEXCODE CONFIGURED DEVICES ----
using namespace vex;
// A global instance of competition
competition Competition;
// define your global instances of motors and other devices here
vex::motor BottomLeftDriveMotor(vex::PORT4, vex::gearSetting::ratio18_1, false);
vex::motor BottomRightDriveMotor(vex::PORT1, vex::gearSetting::ratio18_1, true);
vex::motor FrontLeftDriveMotor(vex::PORT10, vex::gearSetting::ratio18_1, false);
vex::motor FrontRightDriveMotor(vex::PORT9, vex::gearSetting::ratio18_1, true);
vex::controller Controller1;
vex::motor LiftR(vex::PORT6, vex::gearSetting::ratio18_1, true);
vex::motor LiftL(vex::PORT7, vex::gearSetting::ratio18_1, true);
vex::inertial inertial1(vex::PORT5);
motor_group lift= motor_group(LiftR, LiftL);
extern motor_group lift;
// 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 kP = 0.1;
double kI = 0.0;
double kD = 0.0;
double turnkP = 0.0;
double turnkI = 0.0;
double turnkD = 0.0;
//auton settings
int desiredValue = 1000;
int desiredTurnValue = 0;
int error; //SensorValue - DesiredVaule : Position
int prevError = 0;//Position 20 msecs ago
int derivative; // error - prevError : Speed
int totalError = 0; //totalError = totalError + error
int turnError; //SensorValue - DesiredValue : Position
int turnPrevError = 0;//Position 20 msecs ago
int turnDerivative; // error - prevError : Speed
int turnTotalError = 0; //totalError = totalError + error
bool resetDriveSensors = false;
//variables (modifiable)
bool enableDrivePID = true;
int drivePID(){
while(enableDrivePID){
if (resetDriveSensors) {
resetDriveSensors = false;
BottomLeftDriveMotor.setPosition(0, degrees);
BottomRightDriveMotor.setPosition(0, degrees);
FrontLeftDriveMotor.setPosition(0, degrees);
FrontRightDriveMotor.setPosition(0, degrees);
}
//position of all 4 motors
int BottomLeftDriveMotorPosition = BottomLeftDriveMotor.position(degrees);
int BottomRightDriveMotorPosition = BottomRightDriveMotor.position(degrees);
int FrontLeftDriveMotorPosition = FrontLeftDriveMotor.position(degrees);
int FrontRightDriveMotorPosition = FrontRightDriveMotor.position(degrees);
//////////////////////////////////////////////////////////////////////////////////////
//Lateral Movement PID
//////////////////////////////////////////////////////////////////////////////////////
//avg of motors
int averagePosition = (BottomLeftDriveMotorPosition + BottomRightDriveMotorPosition
+ FrontLeftDriveMotorPosition + FrontRightDriveMotorPosition)/4;
//Potential
error = averagePosition - desiredValue;
//Derivative
derivative = error - prevError;
//Integral
totalError += error;
double lateralMotorPower = error * kP + derivative * kD + totalError * kI;
//////////////////////////////////////////////////////////////////////////////////////
//Turning Movement PID
//////////////////////////////////////////////////////////////////////////////////////
//avg of motors
int turnDifference = -FrontLeftDriveMotorPosition + FrontRightDriveMotorPosition - BottomLeftDriveMotorPosition + BottomRightDriveMotorPosition;
//Potential
turnError = turnDifference - desiredTurnValue;
//Derivative
turnDerivative = turnError - turnPrevError;
//Integral
turnTotalError += turnError;
double turnMotorPower = turnError * turnkP + turnDerivative * turnkD + turnTotalError * turnkI;
FrontRightDriveMotor.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
FrontLeftDriveMotor.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
BottomLeftDriveMotor.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
BottomRightDriveMotor.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
FrontRightDriveMotor.setStopping(brake);
FrontLeftDriveMotor.setStopping(brake);
BottomLeftDriveMotor.setStopping(brake);
BottomRightDriveMotor.setStopping(brake);
//code
prevError = error;
turnPrevError = turnError;
vex::task::sleep(20);
}
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) {
// ..........................................................................
// Insert autonomous user code here.
// ..........................................................................
vex::task MainFunction(drivePID);
resetDriveSensors = true;
desiredTurnValue = 100;
vex::task::sleep(1000);
resetDriveSensors = true;
}
/*---------------------------------------------------------------------------*/
/* */
/* 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
while (1) {
enableDrivePID = false;
// 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 just tested what you told me. Atleast they stop now
Anyway when I called the desiredValue function it just made the robot drive forward but I found I had to do a lot of units like 500 to 2000 for it to actually look like its driving a good amount. When i called the desiredTurnValue function it drove forward but also used the PID because it was correcting itself. any ideas? im confused. note: i can send a video if needed
Updated code:
/*----------------------------------------------------------------------------*/
/* */
/* Module: main.cpp */
/* Author: VEX */
/* Created: Thu Sep 26 2019 */
/* Description: Clawbot Competition Template */
/* */
/*----------------------------------------------------------------------------*/
#include "vex.h"
// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name] [Type] [Port(s)]
// ---- END VEXCODE CONFIGURED DEVICES ----
using namespace vex;
// A global instance of competition
competition Competition;
// define your global instances of motors and other devices here
vex::motor BottomLeftDriveMotor(vex::PORT4, vex::gearSetting::ratio18_1, false);
vex::motor BottomRightDriveMotor(vex::PORT1, vex::gearSetting::ratio18_1, true);
vex::motor FrontLeftDriveMotor(vex::PORT10, vex::gearSetting::ratio18_1, false);
vex::motor FrontRightDriveMotor(vex::PORT9, vex::gearSetting::ratio18_1, true);
vex::controller Controller1;
vex::motor LiftR(vex::PORT6, vex::gearSetting::ratio18_1, true);
vex::motor LiftL(vex::PORT7, vex::gearSetting::ratio18_1, true);
vex::inertial inertial1(vex::PORT5);
motor_group lift= motor_group(LiftR, LiftL);
extern motor_group lift;
// 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 kP = 0.1;
double kI = 0.0;
double kD = 0.0;
double turnkP = 0.0;
double turnkI = 0.0;
double turnkD = 0.0;
//auton settings
int desiredValue = 1000;
int desiredTurnValue = 0;
int error; //SensorValue - DesiredVaule : Position
int prevError = 0;//Position 20 msecs ago
int derivative; // error - prevError : Speed
int totalError = 0; //totalError = totalError + error
int turnError; //SensorValue - DesiredValue : Position
int turnPrevError = 0;//Position 20 msecs ago
int turnDerivative; // error - prevError : Speed
int turnTotalError = 0; //totalError = totalError + error
bool resetDriveSensors = false;
//variables (modifiable)
bool enableDrivePID = true;
int drivePID(){
while(enableDrivePID){
if (resetDriveSensors) {
resetDriveSensors = false;
BottomLeftDriveMotor.setPosition(0, degrees);
BottomRightDriveMotor.setPosition(0, degrees);
FrontLeftDriveMotor.setPosition(0, degrees);
FrontRightDriveMotor.setPosition(0, degrees);
}
//position of all 4 motors
int BottomLeftDriveMotorPosition = BottomLeftDriveMotor.position(degrees);
int BottomRightDriveMotorPosition = BottomRightDriveMotor.position(degrees);
int FrontLeftDriveMotorPosition = FrontLeftDriveMotor.position(degrees);
int FrontRightDriveMotorPosition = FrontRightDriveMotor.position(degrees);
//////////////////////////////////////////////////////////////////////////////////////
//Lateral Movement PID
//////////////////////////////////////////////////////////////////////////////////////
//avg of motors
int averagePosition = (BottomLeftDriveMotorPosition + BottomRightDriveMotorPosition
+ FrontLeftDriveMotorPosition + FrontRightDriveMotorPosition)/4;
//Potential
error = desiredValue - averagePosition;
//Derivative
derivative = error - prevError;
//Integral
totalError += error;
double lateralMotorPower = error * kP + derivative * kD + totalError * kI;
//////////////////////////////////////////////////////////////////////////////////////
//Turning Movement PID
//////////////////////////////////////////////////////////////////////////////////////
//avg of motors
int turnDifference = -FrontLeftDriveMotorPosition + FrontRightDriveMotorPosition - BottomLeftDriveMotorPosition + BottomRightDriveMotorPosition;
//Potential
turnError = turnDifference - desiredTurnValue;
//Derivative
turnDerivative = turnError - turnPrevError;
//Integral
turnTotalError += turnError;
double turnMotorPower = turnError * turnkP + turnDerivative * turnkD + turnTotalError * turnkI;
FrontRightDriveMotor.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
FrontLeftDriveMotor.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
BottomLeftDriveMotor.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
BottomRightDriveMotor.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
FrontRightDriveMotor.setStopping(brake);
FrontLeftDriveMotor.setStopping(brake);
BottomLeftDriveMotor.setStopping(brake);
BottomRightDriveMotor.setStopping(brake);
//code
prevError = error;
turnPrevError = turnError;
vex::task::sleep(20);
}
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) {
// ..........................................................................
// Insert autonomous user code here.
// ..........................................................................
vex::task MainFunction(drivePID);
resetDriveSensors = true;
desiredValue = 1500;
vex::task::sleep(1000);
resetDriveSensors = true;
}
/*---------------------------------------------------------------------------*/
/* */
/* 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
while (1) {
enableDrivePID = false;
// 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);
}
}
Which is when I tell the program to desiredTurnValue = 100;
It doesn’t turn at all and just goes forward and corrects itself a bit. I dont know what to do.
Updated code:
/*----------------------------------------------------------------------------*/
/* */
/* Module: main.cpp */
/* Author: VEX */
/* Created: Thu Sep 26 2019 */
/* Description: Clawbot Competition Template */
/* */
/*----------------------------------------------------------------------------*/
#include "vex.h"
// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name] [Type] [Port(s)]
// ---- END VEXCODE CONFIGURED DEVICES ----
using namespace vex;
// A global instance of competition
competition Competition;
// define your global instances of motors and other devices here
vex::motor BottomLeftDriveMotor(vex::PORT4, vex::gearSetting::ratio18_1, false);
vex::motor BottomRightDriveMotor(vex::PORT1, vex::gearSetting::ratio18_1, true);
vex::motor FrontLeftDriveMotor(vex::PORT10, vex::gearSetting::ratio18_1, false);
vex::motor FrontRightDriveMotor(vex::PORT9, vex::gearSetting::ratio18_1, true);
vex::controller Controller1;
vex::motor LiftR(vex::PORT6, vex::gearSetting::ratio18_1, true);
vex::motor LiftL(vex::PORT7, vex::gearSetting::ratio18_1, true);
vex::inertial inertial1(vex::PORT5);
motor_group lift= motor_group(LiftR, LiftL);
extern motor_group lift;
// 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 kP = 0.1;
double kI = 0.0;
double kD = 0.0;
double turnkP = 0.0;
double turnkI = 0.0;
double turnkD = 0.0;
//auton settings
int desiredValue = 1000;
int desiredTurnValue = 0;
int error; //SensorValue - DesiredVaule : Position
int prevError = 0;//Position 20 msecs ago
int derivative; // error - prevError : Speed
int totalError = 0; //totalError = totalError + error
int turnError; //SensorValue - DesiredValue : Position
int turnPrevError = 0;//Position 20 msecs ago
int turnDerivative; // error - prevError : Speed
int turnTotalError = 0; //totalError = totalError + error
bool resetDriveSensors = false;
//variables (modifiable)
bool enableDrivePID = true;
int drivePID(){
while(enableDrivePID){
if (resetDriveSensors) {
resetDriveSensors = false;
BottomLeftDriveMotor.setPosition(0, degrees);
BottomRightDriveMotor.setPosition(0, degrees);
FrontLeftDriveMotor.setPosition(0, degrees);
FrontRightDriveMotor.setPosition(0, degrees);
}
//position of all 4 motors
int BottomLeftDriveMotorPosition = BottomLeftDriveMotor.position(degrees);
int BottomRightDriveMotorPosition = BottomRightDriveMotor.position(degrees);
int FrontLeftDriveMotorPosition = FrontLeftDriveMotor.position(degrees);
int FrontRightDriveMotorPosition = FrontRightDriveMotor.position(degrees);
//////////////////////////////////////////////////////////////////////////////////////
//Lateral Movement PID
//////////////////////////////////////////////////////////////////////////////////////
//avg of motors
int averagePosition = (BottomLeftDriveMotorPosition + BottomRightDriveMotorPosition
+ FrontLeftDriveMotorPosition + FrontRightDriveMotorPosition)/4;
//Potential
error = desiredValue - averagePosition;
//Derivative
derivative = error - prevError;
//Integral
totalError += error;
double lateralMotorPower = error * kP + derivative * kD + totalError * kI;
//////////////////////////////////////////////////////////////////////////////////////
//Turning Movement PID
//////////////////////////////////////////////////////////////////////////////////////
//avg of motors
int turnDifference = -FrontLeftDriveMotorPosition + FrontRightDriveMotorPosition - BottomLeftDriveMotorPosition + BottomRightDriveMotorPosition;
//Potential
turnError = turnDifference - desiredTurnValue;
//Derivative
turnDerivative = turnError - turnPrevError;
//Integral
turnTotalError += turnError;
double turnMotorPower = turnError * turnkP + turnDerivative * turnkD + turnTotalError * turnkI;
FrontRightDriveMotor.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
FrontLeftDriveMotor.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
BottomLeftDriveMotor.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
BottomRightDriveMotor.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
FrontRightDriveMotor.setStopping(brake);
FrontLeftDriveMotor.setStopping(brake);
BottomLeftDriveMotor.setStopping(brake);
BottomRightDriveMotor.setStopping(brake);
//code
prevError = error;
turnPrevError = turnError;
vex::task::sleep(20);
Brain.Screen.print(BottomLeftDriveMotorPosition);
Brain.Screen.newLine();
Brain.Screen.print(BottomRightDriveMotorPosition);
Brain.Screen.newLine();
Brain.Screen.print(FrontLeftDriveMotorPosition);
Brain.Screen.newLine();
Brain.Screen.print(FrontRightDriveMotorPosition);
Brain.Screen.newLine();
Brain.Screen.print(turnDifference);
Brain.Screen.newLine();
Brain.Screen.print(turnError);
Brain.Screen.newLine();
Brain.Screen.print(turnDerivative);
Brain.Screen.newLine();
Brain.Screen.print(turnMotorPower);
}
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) {
// ..........................................................................
// Insert autonomous user code here.
// ..........................................................................
vex::task MainFunction(drivePID);
resetDriveSensors = true;
desiredTurnValue = 100;
vex::task::sleep(1000);
resetDriveSensors = true;
}
/*---------------------------------------------------------------------------*/
/* */
/* 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
while (1) {
enableDrivePID = false;
// 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);
}
}
now i messed with the values of “turnkP” and now it goes forward for a bit trying to correct itself and then turns forever. idk why its doing this please help
I set turnkP to 0.01 or 0.1 and it does the same thing @Connor ?
should i be using int turnDifference = ((FrontLeftDriveMotorPosition + BottomLeftDriveMotorPosition/2) - (FrontRightDriveMotorPosition + BottomRightDriveMotorPosition)/2);
I just added it and now my robot goes foward and turns a bit at the end. im so confused
This is because turnkP is 0, so setting desiredTurnValue to anything will have no effect because you’re multiplying your turn error by 0. Maybe you could just try turning kP or/and turnkP way up, like 0.5 or 1, and see what happens.
This is because your target is in the unit “motor degrees”, where a motor degree is 1/360 of a full revolution. So if you say desiredValue = 360 it will try to spin the motors one full turn. If you calculated out, one revolution would probably drive the robot forward about 6 inches (plus or minus 3).
I would also suggest you initialize desiredValue and desiredTurnValue to 0, and also set your motor positions to 0 before auton (maybe in pre_auton()?).
I set the desiredValue to 0 and the desiredTurnValue to 0. I also put kP as 1 and turnkP as 1.
I have videos of the robot performing these If i can email them to you would that help?
The robot kinda spasms out. @Connor@242EProgrammer
