I started on this a few days ago to try to optimize some autonomous. While I figured I wouldn’t finish it before our state competition this week, I got it most of the way. Even if I fix it too late it will help next year, and it just bothers me when I can’t figure it out completely. The idea is to slow down the velocity of the drivetrain when approaching a target distance using only integrated encoder (that is all we have currently on the robot). I got it to work with decent accuracy. My issue is that once the robot has finished the loop it stops but the loop never ends so no other lines will run. If there is any code after the drive line it will not run since the drive function has not stopped. The weird part is it drives the correct distance and stops so my presumption of the issue being the while does not hold then.
Notes: our robot has 4 motors on the drivetrain with only 4 wheels. I’m converting everything to ticks when checking it against encoder values which are also in ticks. The distance you input is in feet. Currently we have a turning pid based off the inertial sensor (I did not develop that one though I do understand it and a lot of this is based on that one) that works so this should work with it.
This probably is not the most efficient way to go about doing this as I’m aware so I may have made a tiny error and didn’t see it.
Trying to look at this on my phone, and the braces aren’t lining up well, so it’s hard to see just what the while loop encompasses. I will try to get on the computer once kids are in bed and see if it’s better there.
In the meantime, a couple of readability tips, that can save your sanity in a few places, and also help weed out order of operations errors.
In the while loop, where it’s got the calculation that includes wheel diameter, pull that calculation out to a variable before the start of the loop. This makes it more legible, and also prevents the code from calculating it every time through the loop evaluation (optimization would make this go away, but I’m not sure if the Vex compiler does optimization).
Second, on big loops, I like to add // comments at the end of a loop (for, while, etc), so that you’re not totally dependent on indenting of code to see what lines up to where. Would also make it easier to digest on mobile format.
#include "vex.h"
using namespace vex;
//declare motor groups and drivetrain
//drivetrain is not used here but in turn pid
//motor groups allow us to code left and right side without worrying about individual motors
motor_group LeftDriveSmart = motor_group(FrontLeft, BackLeft);
motor_group RightDriveSmart = motor_group(FrontRight, BackRight);
smartdrive Drivetrain = smartdrive(LeftDriveSmart, RightDriveSmart, TurnGyroSmart, 319.19, 320, 40, mm, 1);
//declaration of global variables
int targetDistance =0;
//gains
double kI=.009;
double kD =.01;
double kP = 0.15;
//drive threshold for integral (2inches)
//haven't fully tested so may be inacurrate
double driveThreshold =2;
double tickDistance =0;
//drivetrain wheel diameter in inches
double wheelDiameter = 4;
//pi
double pi = 3.14159265358979;
// total ticks in encoder
double eTicks = 900;
//function to say drive x distance in ft
void driveTo(double targetDistance) {
//reset motor encoders so they are all zero
//this way the ticks match up without worrying about turning
BackLeft.resetRotation();
BackRight.resetRotation();
FrontLeft.resetRotation();
FrontRight.resetRotation();
//declaration of local variables
double integral = 0;
double error = 0;
double derivative = 0;
double prevError =0;
//converting target distance into ticks
tickDistance = fabs(targetDistance/(wheelDiameter * pi) *eTicks);
//while loop
//checks desired distance against sensor of current distance driven
while(fabs(tickDistance)>fabs(FrontRight.rotation(rotationUnits::deg))/(wheelDiameter*pi*1)*2.5) {
//error is tick distance - sensor
error = tickDistance -fabs(FrontRight.rotation(rotationUnits::deg))/(wheelDiameter*pi*1)*2.5;
//assign derivative
derivative = error - prevError;
//assign previous error as the error before
prevError = error;
//if error is less than threshold and error is not 0 add integral + error to integral
if (fabs(error)<driveThreshold&&error !=0) {
integral += error;
//else nothing
} else {
integral = 0;
}
//end of if for integral
//declare and assign powerdrive (I.E. velocity control PID)
double powerDrive = error *kP + derivative * kD + integral*kI;
//if the distance is positive drive forward
if(targetDistance >0) {
LeftDriveSmart.spin(forward,powerDrive,voltageUnits::volt);
RightDriveSmart.spin(forward,powerDrive,voltageUnits::volt);
}
//end of positive drive if
//if the distance is negative drive reverse
if (targetDistance<0) {
LeftDriveSmart.spin(reverse,powerDrive,voltageUnits::volt);
RightDriveSmart.spin(reverse,powerDrive,voltageUnits::volt);
}
//end of negative drive if
this_thread::sleep_for(15);
}//end of while loop
//tell motors to stop if target is achieved
LeftDriveSmart.stop();
RightDriveSmart.stop();
//print data and assign last values
error = tickDistance - fabs(FrontRight.rotation(rotationUnits::deg))/(wheelDiameter*pi*1)*2.5;
derivative = error - prevError;
Controller1.Screen.clearScreen();
Controller1.Screen.setCursor(1, 1);
Controller1.Screen.setCursor(1, 13);
Controller1.Screen.newLine();
Controller1.Screen.print("error: %.5f", error);
Controller1.Screen.newLine();
Controller1.Screen.print("derivative %.5f", error);
Controller1.Screen.newLine();
}//end of function
//test button
void test() {
driveTo(-2);
driveTo(2);
}//end of test button
int main() {
// Initializing Robot Configuration. DO NOT REMOVE!
vexcodeInit();
Controller1.ButtonA.pressed(test);
}
For the future I commented through to indicate what things do and where brackets are assigned. Not very comprehensive I’ll admit since I just threw it together, but hope it helps.
One thing that stands out, not sure that it’s the smoking gun here, but your while() comparison may not be evaluating exactly what you’re looking for. One recommendation I always make to new programmers is to never trust the compiler and standard when it comes to order of operations. ALWAYS parenthesize every operation that you want to happen in the order you need it to happen in. I’ve taken the liberty of taking a stab at what you might be intending for comparison. Note that pulling out the calculation for the wheel velocity also helps remove some parentheses that can cloud the issue. Take a look at this updated function and see if anything more jumps out at you in regards to what you’re looking for. I’ll take another look through after I post this to see if I can spot a logic error that’s keeping it “locked up” inside the loop.
I’mma take some stabs in the dark here to hopefully inspire some ideas for you, as it’s late and my brain is just about done with this day. One last been-there-done-that programmer rant. Comments are always key, but so is consistent spacing. The compiler doesn’t care about whitespace, so use it liberally so that you know what you’re looking at, and where things flow in logic. One bad indent can make you think some code should be executing when it’s actually still part of another logic loop, and it’s very easy to miss (ask me how I know… ). It’s very worth the time to make indentations line up.
So, in order for the loop to exit, the tick distance must be less than (or technically equal to) the degrees of rotation divided by the wheel distance. Is it possible that you’re effectively super close to the border condition, but not quite there to get over the line? In other words, you’re so close, that you can’t generate a new motor command that’s going to move the robot enough to get the tick distance to flip to the other side, but then you’re still just far enough away that it thinks it has to stay in the loop?
One thing with PIDs is making sure you have just enough Kp to be able to push a command effectively, or enough Ki or Kd to be able to accumulate when you’re this close to the finish line but you need to scooch forward enough to get over it. You likely need to play with a combination of things, including the driveThreshold value that’s zeroing out your integral (that would otherwise grow enough to start moving your robot again), and your Kp.
Having implemented a couple of really crazy PID applications in industrial products (one of which is flying around in air cargo last I still heard), tuning your PID & threshold values is a mystical art to get things right where you want them.
Thought I would mention for future reference of this post: always check units. The issue turned out to be some thresholds since they were arbitrary numbers were too small because I was thinking in inches, but they should have been in ticks. As we did run into an issue when driving in reverse once I had fixed the previous issue, I will say this as advice since I didn’t realize it till another team today at our state competition pointed it out, it matters how your motor is driving the wheel for the math. While I haven’t tested changing it, with how our robot is built upon retrospect it makes sense that the math for reversing would be wrong since it seemed slightly wrong forward as well, but it was less of an issue with how the robot drives.
Lesson learned always check units and think through math with how your mechanics work.
). It’s very worth the time to make indentations line up.