Hi all, I have a quick question
I want to condense some of my code into one or two lines. I’m using pid and to use it in autonomous, it takes 5 lines total, and im wondering if theres a way to add a function to make it smaller.
Code
vex::task test1(drivePID);
resetDriveSensors= true;
desiredLateralValue = 500;
waitUntil(LateralTrack > 500);
vex::task::stop(test1);
is there a way to cut this down into one line? any help is appreciated.
Yes. And there may be valid reasons for doing this. What do you think the benefits of “mak[ing] it smaller” are?
I’m pretty sure you need to have the
vex::task test1(drivePID);
and
vex::task::stop(test1);
at the beginning and end of your autonomous routine. If you want to condense the other 3 lines you could make a function to enter your desired value, and within the separate function it will set the desiredLateralValue and other items.
Mainly I just want to make programming faster and more efficient. In my code I love to do a lot of set up work that way when I need to put that set up into use, I dont have to write too much code. basically, I want one customizable line to do this.
Ive done in before in a different code, but it’s different in this scenario I think
Heres where I’ve done something similar before.
void driveForward(int degrees, int speed, double waitTime){
FrontLeftDrive.resetRotation();
FrontRightDrive.resetRotation();
BackLeftDrive.resetRotation();
BackRightDrive.resetRotation();
FrontLeftDrive.startRotateTo(degrees, deg, speed, rpm);
FrontRightDrive.startRotateTo(degrees, deg, speed, rpm);
BackLeftDrive.startRotateTo(degrees, deg, speed, rpm);
BackRightDrive.rotateTo(degrees, deg, speed, rpm);
wait(waitTime, sec);
and here is where I implemented it
driveForward(500, 100, 1);
basically, I want to do this, but where I call my PID.
yeah, I figured that was the case. Moving to the other three lines, though, how would I go about making this function? would it be similar to my other example?
Yes you can do something like what you had before. Just have the function call the three lines and take whatever variables you need when you call the function.
Heres my first rendition
Before anything, I’ll define some stuff.
LateralTrack = both of my tracking wheels (LeftTrack and RightTrack) rotation divided by 2 (for average)
RLateralPID = Reverse Lateral PID
FLateralPID = Forward Lateral PID
I start by having my function call one line (int degrees) because the two lines
desiredLateralValue = 500;
waitUntil(LateralTrack > 500);
will always have the same value
in this chunk of code, I give the task a time parameter to run in (1.5 seconds) which is a complete guess
vex::task test1(drivePID);
resetDriveSensors= true;
desiredLateralValue = 500;
wait(1.5, sec);
vex::task::stop(test1);
in this chunk of code, Instead of giving the task a time parameter, I use my tracking wheels to stop the task for me so I dont have to guess how long it will take to run the task (I want the task to run for as long as my robot moves)
vex::task test01(drivePID);
resetDriveSensors= true;
desiredLateralValue = 500;
waitUntil(LateralTrack > 500);
vex::task::stop(test01);
(let me know if this would work))
Moving on, I decided that I would take the second chunk and use it for my task as it is probably more accurate and I don’t have to guess how long the task needs to run for
Below is how I’m attempting to condense it.
void FLateralPID(int degrees){
resetDriveSensors = true;
desiredLateralValue = degrees;
waitUntil(LateralTrack > degrees);
}
void RLateralPID(int degrees){
resetDriveSensors = true;
desiredLateralValue = -(degrees);
waitUntil(LateralTrack < degrees);
}
I made two functions, one for forward and one for reverse.
EXPLANATION
To start off, I added the line resetDriveSensors = true; which is defined here
bool enableDrivePID = true;
if (resetDriveSensors) {
resetDriveSensors = false;
LeftDrive.setPosition(0,degrees);
RightDrive.setPosition(0,degrees);
LeftTrack.setPosition(0,degrees);
RightTrack.setPosition(0,degrees);
}
This section resets my motor position and my tracking wheel position to zero.
–
Next, I have my desired value (this can be positive or negative depending on if I want to go forward or reverse, hence the reason I made two functions)
examples of both lines below
(desiredLateralValue = degrees;) OR (desiredLateralValue = -(degrees);)
Referring to the code above, this line will either have -(degrees) or (degrees). I wrote this so that I don’t have to worry about if the desired value is negative or positive, I just write how far I want to go.
–
The final line in this sequence depends if I’m going forward or reverse
Forward- waitUntil(LateralTrack > degrees);
Reverse- waitUntil(LateralTrack < degrees);
Like I mentioned before, this is here to tell the task when to stop running. If the value of the tracking wheels exceeds the distance I want to travel, the task stops. Same goes for reverse, it just has to be negative because technically the value is getting smaller and smaller the further I go in reverse.
Let me know if a further explanation is needed on a specific part or if you find anything wrong with my logic.
Here is where I put it into practice
vex::task test2F(drivePID);
FLateralPID(500);
vex::task::stop(test2F);
or
vex::task test2R(drivePID);
RLateralPID(500);
vex::task::stop(test2R);
As explained in previous post, i have 500 for both values because depending on which function I call, the direction is already chosen so all I have to do is give it a distance.
What’s in this function ? It feels like use of a task in this instance is unnecessary and it should just be called as a function that returns when whatever it does is complete. The sequence
start a task
block waiting for something to complete that the task controls
stop the task
is not really how tasks are supposed to be used.
int drivePID(){
if (resetDriveSensors) {
resetDriveSensors = false;
LeftDrive.setPosition(0,degrees);
RightDrive.setPosition(0,degrees);
LeftTrack.setPosition(0,degrees);
RightTrack.setPosition(0,degrees);
}
while(enableDrivePID){
//---------------------------------------------------------------------------//
//--------------------------------------------------------------------------------------------------------------------------------------------//
//Motor position
//--------------------------------------------------------------------------------------------------------------------------------------------//
//FrontLeft
//int FLDPos = FrontLeftDrive.position(degrees);
int FLDPos = fabs(FrontLeftDrive.position(degrees));
//FrontRight
//int FRDPos = FrontRightDrive.position(degrees);
int FRDPos = fabs(FrontRightDrive.position(degrees));
//BackLeft
//int BLDPos = BackLeftDrive.position(degrees);
int BLDPos = fabs(BackLeftDrive.position(degrees));
//BackRight
//int BRDPos = BackRightDrive.position(degrees);
int BRDPos = fabs(BackRightDrive.position(degrees));
//--------------------------------------------------------------------------------------------------------------------------------------------//
//Lateral PD
//--------------------------------------------------------------------------------------------------------------------------------------------//
//Averaging motors
int AVGPos = (FLDPos + FRDPos + BLDPos + BRDPos)/4;
//Potential
error = desiredLateralValue - AVGPos;
//Integral
totalError += error;
//Derivative
derivative = error - prevError;
//Lateral PID equation
double lateralSpeed = (error * lateralkP + totalError * lateralkI + derivative * lateralkD);
//--------------------------------------------------------------------------------------------------------------------------------------------//
//Motion
//--------------------------------------------------------------------------------------------------------------------------------------------//
FrontLeftDrive.spin(forward, lateralSpeed, voltageUnits::volt);
FrontRightDrive.spin(forward, lateralSpeed, voltageUnits::volt);
BackLeftDrive.spin(forward, lateralSpeed, voltageUnits::volt);
BackRightDrive.spin(forward, lateralSpeed, voltageUnits::volt);
//--------------------------------------------------------------------------------------------------------------------------------------------//
//Wrap-Up
//--------------------------------------------------------------------------------------------------------------------------------------------//
prevError = error;
vex::task::sleep(20);
}
return 1;
}
sorry about the code blocks, theyre being weird
edit: code tags added by mods
When I made my first PID loop and put it into practice I had formatted it like this
vex::task test1(drivePID);
resetDriveSensors= true;
desiredLateralValue = 500;
wait(1.5, sec);
vex::task::stop(test1);
If that wait 1.5 seconds was ever less than the time it took for my robot to move 500 degrees in any direction, the bot would just infinitely drive in whatever direction I coded it to go.
I want to use my tracking wheels instead of that time value so that whenever im trying to code, I dont have to test it line by line to make sure that the time is correct. too small and it doesnt stop driving, too great and Im wasting time by waiting for longer than I need to.