Hi there. This is essentially a follow-up to the thread I posted here a while back. With the Vexcode IQ v2.0 release, I was finally able to start coding in C++ so I could have a PID program for my project. However, since I don’t use C++ often, I looked for some guides and happened to stumble upon @Connor’s guide for V5 PID. Naturally, I thought that it would work because both V5 and IQ share the same programming language now, but it didn’t take long for me to understand that something was off.
I spent yesterday’s evening and today’s morning as well as noon trying to figure out what was the problem, but I’m still equally as stumped now as I was before. Most likely, I think it’s just bad programming on my part (as I am notorious for making syntax errors), but I do remember a post somewhere saying that IQ has very little memory and hence just doesn’t work, which could be another reason for why the PID loop simply isn’t working. Alternatively, I could try and revert back to ROBOTC, but I’m not entirely sure if the IQ robot accepts ROBOTC code as of late.
Some more information before I share the code: my robot is a relatively small one, with two motors, two distance sensors at the front and back, two left and right bumpers on the front, as well as five TouchLEDs thrown in for decoration’s sake. I also have a color sensor that is currently not in use. A gyro would have probably been more than optimal for my situation, but unfortunately, I didn’t have access to them whatsoever.
All of that being said, attached below is my code. It’s a simple test that involves the robot going up to a wall and stopping either before it hits or when the bumper touches the wall. This is also the primary method in which I would have wanted to experiment with the PID loop through changing the kP, kI, and kD (side note: recommendations for numeric values to experiment with here would be appreciated).
// Include the IQ Library
#include "iq_cpp.h"
// Allows for easier use of the VEX Library
using namespace vex;
int Brain_precision = 0;
float myVariable;
//Settings
double kP = 2.0;
double kI = 0.04;
double kD = 0.0;
double turnkP = 2.0;
double turnkI = 0.04;
double turnkD = 0.0;
//Autonomous Settings
int desiredValue = 200;
int desiredTurnValue = 0;
int error = FrontDistance.distance(mm) - 200; // SensorValue - DesiredValue : Position
int prevError = 0; // Position 20 milliseconds ago
int derivative; // error - prevError : Speed
int totalError = 0; // totalError = totalError + error
int turnError; // SensorValue - DesiredValue : Position
int turnPrevError = 0; // Position 20 milliseconds ago
int turnDerivative; // error - prevError : Speed
int turnTotalError = 0; // totalError = totalError + error
bool enableDrivePID = true;
//variables modified for use
bool resetDriveSensors = false;
int drivePID(){
while(enableDrivePID){
if (resetDriveSensors) {
resetDriveSensors = false;
LeftMotor.setPosition(0,degrees);
RightMotor.setPosition(0,degrees);
}
int leftMotorPosition = LeftMotor.position(degrees);
int rightMotorPosition = RightMotor.position(degrees);
//////////////////////////////////////////////////////////////////
// Lateral movement PID
/////////////////////////////////////////////////////////////////
//Get average of the two motors
int averagePosition = (leftMotorPosition + rightMotorPosition)/2;
//Proportional
error = averagePosition - desiredValue;
//Derivative
derivative = error - prevError;
//Integral
//totalError += error;
double lateralMotorPower = error * kP + derivative * kD;
/////////////////////////////////////////////////////////////////
// Turning movement PID
/////////////////////////////////////////////////////////////////
//Get average of the two motors
int turnDifference = leftMotorPosition - rightMotorPosition;
//Proportional
turnError = turnDifference - desiredTurnValue;
//Derivative
turnDerivative = turnError - turnPrevError;
//Integral
//turnTotalError += turnError;
double turnMotorPower = turnError * turnkP + turnDerivative * turnkD;
////////////////////////////////////////////////////////////////////////
LeftMotor.spin(forward, lateralMotorPower + turnMotorPower, voltageUnits::volt);
RightMotor.spin(forward, lateralMotorPower - turnMotorPower, voltageUnits::volt);
prevError = error;
turnPrevError = turnError;
vex::task::sleep(20);
}
return 1;
}
void onevent_TouchLED_released_0() {
TouchLED.setBrightness(100);
TouchLED.setColor(white);
LeftMotor.spin(forward);
RightMotor.spin(forward);
LeftMotor.setVelocity(100.0, percent);
RightMotor.setVelocity(100.0, percent);
wait(3.0, seconds);
while (true) {
if (FrontDistance.distance(inches) < 10.0) {
TouchLED.setColor(blue_green);
repeat(50) {
drivePID();
if(FrontDistance.distance(inches) < 6.0){
break;
}
}
}
if (LBumper.pressing()) {
TouchLED.setColor(red_violet);
LeftMotor.setStopping(brake);
RightMotor.setStopping(brake);
}
if (RBumper.pressing()) {
TouchLED.setColor(yellow_orange);
LeftMotor.setStopping(brake);
RightMotor.setStopping(brake);
}
wait(20, msec);
}
}
int main() {
// register event handlers
TouchLED.released(onevent_TouchLED_released_0);
wait(15, msec);
resetDriveSensors = true;
}
Thanks in advance for your help! I will be happy to elaborate on any further details that I may have missed.
