Hello.
This year we are trying to use a P-loop with slew control. We more or less based it off of This thread, and made it for an x drive and added some other (kind of unnecessary) things.
When we run the code it will accelerate like it should, but at the end the left side stops like normal, while the right side keeps going and doesn’t stop. Anyone know why? Thanks!
Sorry in advance for the sloppy code. Neither of us are great coders. The issue may be something really simple and stupid that I missed.
Code
// ---- START VEXCODE CONFIGURED DEVICES ----
// Robot Configuration:
// [Name] [Type] [Port(s)]
// LeftFrontDrive motor 13
// RightFrontDrive motor 20
// LeftBackDrive motor 12
// RightBackDrive motor 11
// ---- END VEXCODE CONFIGURED DEVICES ----
#include "vex.h"
using namespace vex;
//FL=Front left wheel, FR=Front right wheel, BL=Back left wheel, BR=back right wheel
//This variable is the voltage limit for the motors during acceleration
double slew=0;
//These are the targets that the wheels need to finish at
double targetFL=0;
double targetFR=0;
double targetBL=0;
double targetBR=0;
//These variables are the direction the wheels are turning, 1 for forward, -1 for backward
int dirFL;
int dirFR;
int dirBL;
int dirBR;
//This indicates the type of movement the robot is doing, true for forward/backward/strafe, false for turning
bool dir;
//This indicates whether the p loop has finished the current movement, true for finished, false for incomplete
bool finish=false;
//These are tuned in order to convert the auton code numbers to motor ticks, so we can code in inches rather than in ticks.
//Calculation: 10*moveTick=number of motor ticks to move 1 inch
double moveTick=1;
//Calculation: 1*turnTick=number of motor ticks to turn 1 degree
double turnTick=1;
//Define the P loop
void PLoop ()
{
LeftFrontDrive.spin(forward);
RightFrontDrive.spin(forward);
LeftBackDrive.spin(forward);
RightBackDrive.spin(forward);
//This variable controls how fast/slow the acceleration is
double slewConstant=0.5;
//These variables are tuned to control how fast the deceleration is
//This one is for forward/backward/strafe
double moveConstant=0.5;
//This one is for turning
double turnConstant=0.5;
//These variables are the distance until the robot meets it's target
double errorFL=0;
double errorFR=0;
double errorBL=0;
double errorBR=0;
//These variables are the final speed output to the motors
static double speedFL=0;
static double speedFR=0;
static double speedBL=0;
static double speedBR=0;
//Start motors turning
//Create an infinite loop
while(true)
{
//Set the error variable to the targfet minus the distance already traveled
errorFL = targetFL - LeftFrontDrive.position(degrees) ;
errorFR = targetFR - RightFrontDrive.position(degrees);
errorBL = targetBL - LeftBackDrive.position(degrees);
errorBR = targetBR - RightBackDrive.position(degrees);
//This if statement is used for deceleration...
//If the direction is forward/backward/strafe...
if(dir==true)
{
//...Set the speed to the error times the constant.
//Usually this will set the speed to the max, however as the error begins to decrease, it will begin to decelerate.
//Additionally, if the error switches direction (I.E. the robot overshoots the target),
//the speed will also switch direction, moving back to the target
speedFL=errorFL * moveConstant;
speedFR=errorFR * moveConstant;
speedBL=errorBL * moveConstant;
speedBR=errorBR * moveConstant;
}
//If the robot is turning...
else{
//ditto with turnConstant
speedFL=errorFL * turnConstant;
speedFR=errorFR * turnConstant;
speedBL=errorBL * turnConstant;
speedBR=errorBR * turnConstant;
}
//Increase the slew variable by the constant. (I.E. the slew variable will slowly increase as the PLoop loops)
slew=slew+slewConstant;
//The following if statements are used for acceleration...
//If the wheel is turning forwards, and the speed is greater than the slew variable
if(speedFL > slew && dirFL==1)
{
//...The speed variable equals the slew limit times direction (-1 or 1)
//(I.E. the slew variable wil limit the speed of the wheel if the speed is greater than that of the slew variable)
speedFL=slew*dirFL;
}
//If the wheel is turning backwards, and the speed is less than the slew variable
if(speedFL < slew && dirFL==-1)
{
//...The speed variable equals the slew limit times direction (-1 or 1)
//(I.E. the slew variable wil limit the speed of the wheel if the speed is greater than that of the slew variable)
speedFL=slew*dirFL;
}
//Ditto for FR
if(speedFR > slew && dirFR==1)
{
speedFR=slew*dirFR;
}
if(speedFR < slew && dirFR==-1)
{
speedFR=slew*dirFR;
}
//Ditto for BL
if(speedBL > slew && dirBL==1)
{
speedBL=slew*dirBL;
}
if(speedBL < slew && dirBL==-1)
{
speedBL=slew*dirBL;
}
//Ditto for BR
if(speedBR > slew && dirBR==1)
{
speedBR=slew*dirBR;
}
if(speedBR < slew && dirBR==-1)
{
speedBR=slew*dirBR;
}
//If all motors have a very small error...
if(-100<errorFL && errorFL<10 && -10<errorFR && errorFR <10 && -10<errorBL && errorBL<10 && -10<errorBR && errorBR<10)
{
speedFL=0;
speedFR=0;
speedBL=0;
speedBR=0;
break;
}
//Set the motors to their intended speed
LeftFrontDrive.setVelocity(speedFL, rpm);
RightFrontDrive.setVelocity(speedFR, rpm);
LeftBackDrive.setVelocity(speedBL, rpm);
RightBackDrive.setVelocity(speedBR, rpm);
}
}
//Define the drive control function (forward/backward)
void Drive (double driveDistance)
{
//Set the direction bool to true (Forward/Backward/Strafe)
dir=true;
//Reset the slew limit because this is a seprate movement
slew=0;
//Increase/decrease targets by the amount that the robot is moving (times the constant)
targetFL= targetFL+ driveDistance*moveTick;
targetFR= targetFR- driveDistance*moveTick;
targetBL= targetBL+ driveDistance*moveTick;
targetBR= targetBR- driveDistance*moveTick;
//If the robot is moving backwards...
if(driveDistance<0)
{
//...Set direction variables accordingly
dirFL=-1;
dirFR=1;
dirBL=-1;
dirBR=1;
}
//If the robot is moving forwards...
else{
//...Set direction variables
dirFL=1;
dirFR=-1;
dirBL=1;
dirBR=-1;
}
PLoop();
}
//Define the strafe control function (left/right)
void Strafe (double strafeDistance)
{
//Set the direction bool to true (Forward/Backward/Strafe)
dir=true;
//Reset the slew limit because this is a seprate movement
slew=0;
//Increase/decrease targets by the amount that the robot is moving (times the constant)
targetFL= targetFL+ strafeDistance*moveTick;
targetFR= targetFR+ strafeDistance*moveTick;
targetBL= targetBL- strafeDistance*moveTick;
targetBR= targetBR- strafeDistance*moveTick;
//If the robot is moving left...
if(strafeDistance<0)
{
//...Set direction variables accordingly
dirFL= -1;
dirFR= -1;
dirBL= 1;
dirBR= 1;
}
//If the robot is moving right...
else{
//...Set direction variables
dirFL= 1;
dirFR= 1;
dirBL= -1;
dirBR= -1;
}
PLoop();
}
//Define the turn control function
void Turn (double turnDistance)
{
//Set the direction bool to false (Turning)
dir=false;
//Reset the slew limit because this is a seprate movement
slew=0;
//Increase/decrease targets by the amount that the robot is moving (times the constant)
targetFL= targetFL+ turnDistance*turnTick;
targetFR= targetFR+ turnDistance*turnTick;
targetBL= targetBL+ turnDistance*turnTick;
targetBR= targetBR+ turnDistance*turnTick;
//If the robot is moving counterclockwise...
if(turnDistance<0)
{
//...Set direction variables accordingly
dirFL= -1;
dirFR= -1;
dirBL= -1;
dirBR= -1;
}
//If the robot is moving clockwise...
else{
//...Set direction variables
dirFL= 1;
dirFR= 1;
dirBL= 1;
dirBR= 1;
}
PLoop();
}
int main() {
vexcodeInit();
//Start P loop
// LeftFrontDrive.spinFor(forward, 90, degrees);
// RightFrontDrive.spinFor(forward, 90, degrees);
// LeftBackDrive.spinFor(forward, 90, degrees);
// RightBackDrive.spinFor(forward, 90, degrees);
Drive(1000);
}
P.S. How do I make my code in a box like others do it??
*edited by mods to add code tags