int velCap; //velCap limits the change in velocity and must be global
int targetLeft;
int targetRight;
void drivePIDFn(void*){
FL.setPosition(0,degrees); //reset base encoders
FR.setPosition(0,degrees);
int errorLeft;
int errorRight;
float kp = 0.075;
float kpTurn = 0.2;
int acc = 5;
int voltageLeft = 0;
int voltageRight = 0;
int signLeft;
int signRight;
wait(20,msec);
while(targetLeft>1){
errorLeft = targetLeft - FL.position(deg); //error is target minus actual value
errorRight = targetRight - FR.position(deg);
signLeft = errorLeft / abs(errorLeft); // + or - 1
signRight = errorRight / abs(errorRight);
if(signLeft == signRight){
voltageLeft = errorLeft * kp; //intended voltage is error times constant
voltageRight = errorRight * kp;
}
else{
voltageLeft = errorLeft * kpTurn; //same logic with different turn value
voltageRight = errorRight * kpTurn;
}
velCap = velCap + acc; //slew rate
if(velCap > 115){
velCap = 115; //velCap cannot exceed 115
}
if(abs(voltageLeft) > velCap){ //limit the voltage
voltageLeft = velCap * signLeft;
}
if(abs(voltageRight) > velCap){ //ditto
voltageRight = velCap * signRight;
}
FL.spin(fwd, voltageLeft, vex::velocityUnits::pct); //set the motors to the intended speed
BL.spin(fwd, voltageLeft, vex::velocityUnits::pct);
BR.spin(fwd, voltageRight, vex::velocityUnits::pct);
FR.spin(fwd, voltageRight, vex::velocityUnits::pct);
wait(20,msec);
}
FL.stop();
BL.stop();
BR.stop();
FR.stop();
}
void drive(int left, int right){
targetLeft = targetLeft + left;
targetRight = targetRight + right;
velCap = 0; //reset velocity cap for slew rate
}
void autonomous(void) {
drive(100, 100);
}
so, I pretty much took this directly from the motion profiling code that @anomaly posted, but I changed some things around to make them work correctly. I have this in a competition template, but It will not even move when I try to run it. I do have a good amount of programming experience and have looked over this code multiple times and have not seen any reason for it to not work. Does anyone have any suggestions @mvas@theol0403 ?
Firstly, this isn’t a motion profile. This is a simple pLoop. You also have logic for turning, but you never implement it after you calculate the output power (so you might as well delete it or create a separate function for turning altogether).
Here is a more efficient way to have multiple controllers (for all your subsystems) driven by one function. The output of multiple controllers can be added if they run in parallel –– this is determined by how many degrees of freedom the control system has (a tank drive chassis has two DOF for example).
This is a data structure that creates PID type objects and the following function allows you to pass in the objects containing all the PID variable values to get the output for the controller.
Declaring a PID object is as simple as calling the class constructor and initilizing all the variables. For example:
PID drivePID = pidInit (.4, 0.01, .17, 5, 30);
PID turnPID = pidInit (.01, 0.001, .0197, 5, 30);
Then you can call the PID function to receive the calculated output for the desired controller. pidCalculate (turnPID, target, processVariable); and send the output returned by the function directly to the motors. Or in your case, you can filter the output through a slew rate controller but I’ll leave that to you to figure it. You’re also gonna need to declare the data structure in a header file so it can have a global scope. But I’ll leave that for you to figure it –– let me know if you have any questions
Thank you for the response! This is helpful and I will definitely try to use it, but I just wanted to know why my original code wasn’t working in a competition template.
I understand that, but there’s no hurt in learning how to further improve your code. Also, you never even call the drivePIDFn() in the autonomous function. You need to call it if you ever want it to execute.
Don’t ever apologize for wanting to learn… We all need to start somewhere.
I highly recommend you read up on C++ and this website also has really good sources.
As for your function:
You probably wanna pass the target in as a parameter (like what’s shown above) instead of having a generic pointer for your parameter (not like this data type will be dynamic anyways). And you call a function the same way you said it:
drivePIDFn(...) –– use whatever target value you want. Essentially, when you call a function in a program, it is a condensed way of rewriting all the code in the function in the exact spot that the function was called. So in other words, whenever drivePIDFn(...); appears in the program, think of it like you’re copying and pasting the function body in the spot where the function was called. The function body being everything inside of the curly braces {} in the function defintion.
As you can see, it wouldn’t make sense to rewrite the same code over and over. That is the purpose of a function. Once you understand that premise, I think it should be trivial to get your program up and running.
So would I use drivePIDFn(100,100)? I don’t think that would work
Edit: could I just use drivePIDFn(drive(100,100));
Edit 2: I just tried to put drivePIDFn(); by itself in the autonomous section, and I also tried drivePIDFn(drive(100,100)); and it still didn’t work.
I really wouldn’t mess with pointers and references (i.e. void*) if you don’t have extensive C++ knowledge. The simplified version that I edited is easier for you to understand because it deals with less complicated concepts. Just change the parameter and adjust the function body accordingly. In any case, if you don’t understand that then I think you need to back track a few steps before you jump into using this code. Start with what you understand and work up from there. As always, you can ask as many questions as you want.
int velCap; //velCap limits the change in velocity and must be global
int targetLeft;
int targetRight;
void drivePIDFn(int left, int right){
targetLeft = targetLeft + left;
targetRight = targetRight + right;
FL.setPosition(0,degrees); //reset base encoders
FR.setPosition(0,degrees);
int errorLeft;
int errorRight;
float kp = 0.075;
float kpTurn = 0.2;
int acc = 5;
int voltageLeft = 0;
int voltageRight = 0;
int signLeft;
int signRight;
wait(20,msec);
while(targetLeft>1){
errorLeft = targetLeft - FL.position(deg); //error is target minus actual value
errorRight = targetRight - FR.position(deg);
signLeft = errorLeft / abs(errorLeft); // + or - 1
signRight = errorRight / abs(errorRight);
if(signLeft == signRight){
voltageLeft = errorLeft * kp; //intended voltage is error times constant
voltageRight = errorRight * kp;
}
else{
voltageLeft = errorLeft * kpTurn; //same logic with different turn value
voltageRight = errorRight * kpTurn;
}
velCap = velCap + acc; //slew rate
if(velCap > 115){
velCap = 115; //velCap cannot exceed 115
}
if(abs(voltageLeft) > velCap){ //limit the voltage
voltageLeft = velCap * signLeft;
}
if(abs(voltageRight) > velCap){ //ditto
voltageRight = velCap * signRight;
}
FL.spin(fwd, voltageLeft, vex::velocityUnits::pct); //set the motors to the intended speed
BL.spin(fwd, voltageLeft, vex::velocityUnits::pct);
BR.spin(fwd, voltageRight, vex::velocityUnits::pct);
FR.spin(fwd, voltageRight, vex::velocityUnits::pct);
wait(20,msec);
}
}
void autonomous(void) {
drivePIDFn(100, 100);
}
Ok, so I changed it around based on what you said, and it still doesn’t work. I read through your posts multiple times and I think I did what you said.
The parameter should just be (float target)… Remember, you don’t need two separate variables if both sides of the chassis will be getting the same power output. You’re only dealing in one dimension here.
This can be deleted as well as any logic for turning (turning should have it’s own function) for the sake of simplicity/readability. You don’t want to use a slew rate controller for turns anyways because the settling times will be significantly greater and it’s just unnecessary (you won’t be able to reach max speed for the most part).
Change to this –– it’s essentially saying when the robot is 1 unit away from the target, then exit the loop (this depends on the units you’re using, in this case 1 represents a tick of the encoder since you didn’t convert to anything else). Otherwise, keep calculating the output power.
Change the error calculation to use the average of the two sides. Calculating the error of either side is effectively useless without using tracking wheels. This is because you won’t be able to account for wheel slippage so theoretically the power sent to either side should be the same. If you want to correct for the chassis drifting off course, you’re going to want to use more robust logic.
Delete this. You only need one variable to store the power output and you only need to calculate it once. The same goes for any other redundancies in the program which I’ll leave for you to figure out. It should run anyways.
You still don’t need two separate variables for each side of the chassis respectively to account for that.
I encourage you to use a gyro for this, otherwise, you’ll need tracking wheels. There’s no good way to do it with the IMEs because it’s quite difficult to measure slippage –– at leasst not as accurate as a tracking wheel or a gyro.
The basic logic is as foilows: setDrive(power+gyro, power-gyro)
where power is the calculated linear output, and gyro is the value returned from the gyro (relative to the heading at the start of the movement, so you’ll have to zero it before the loop starts). Notice, as the robot veers off course the gyro value will increase –– adding and subtracting that value from each side of the chassis will allow the robot to turn against the direction of resistance. As the robot approaches the target heading, gyro will approach 0, and the power will be the only thing taking over until the target is reached. The same exact logic I explained before follows except now you have a variable tracking your relative heading.
Alternatively, you can combine two PID controllers in parallel as I explained above.
Here’s an example of what the logic I explained should look like (this is from my TT season):
Yes and no… Slew rate will mitigate slippage enough so that you can assume there isn’t any –– but what happens if the robot bumps a corner or even a ball or any other thing you can think of. Depending on how well tuned your loops are and how much margin of error your robot has, this can turn into a serious issue. The video I sent shows how the robot would respond if it were to get stuck on an obstacle (my hand for example). To truly eliminate the problem of wheel slippage requires unpowered tracking wheels or a really expensive accelerometer.
When you operate under the assumption that the robot will not encounter any significant external forces and the PID controller is robust enough to mitigate any unpredictable force –– only then will it be justified to not use tracking wheels. I personally don’t believe those are all correct assumptions –– at least not when you want the robot to repeat a route multiple times in 1 minute increments (aka skills).
Ok I will try to implement tracking wheels. I think we have space. But either way, I still cannot seem to get it to work in a competition template. This is the only portion that I actually need help on since I have tried mostly everything. I do not know how to properly call the function to make it go forward. I literally have no clue whatsoever as to why it does not drive when I call this in the autonomous function:
I see you haven’t been following my suggestions –– you don’t need two parameters… Anyways, how do you know your problem has to do with the competition template at all? How do you know there isn’t a whole array of other issues? Without sharing all of your code, there is no way for anyone here to know. In any case, I feel as though I’ve given you as much feedback needed to get it working. Someone else could and should give their advice (this isn’t a pm)
I’m sorry, it was just a typing mistake by me. I meant drivePIDFn(100); there could be another issue. I will take a look at it when I am at my computer. Thanks for all your help.
