We want the robot to move back 36 inches, but it only moves back less than a foot, why?
double dst1,dst2;
double forward_Translation(double distanceTravel) {
double motorRot = 3.5/2.5;//1 rotation of the wheel
double dis1 = distanceTravel/(M_PI*(wheel_radius*2))*4/3;//How many times the wheel has to rotate
double dis2 = motorRot*dis1;//Number rotations to reach destination
return dis2*360;
}
void reset_Motors() {
set_Velocity(0);
L1.setStopping(brake);
L2.setStopping(brake);
R1.setStopping(brake);
R2.setStopping(brake);
}
void linear_PID(double P, double I, double D, double wanted_Position, bool forw) {
double error = 0;
double prev_Error = 0;
double integral = 0;// Integral
double derivative = 0;
while (true) {
// Gathering Necessary Information
int lPos = (avrL());
int rPos = (avrR());
double aPos = (lPos+rPos)/2;
// P
prev_Error = error;
error = wanted_Position- aPos;
//I
integral += fabs(error);
//if (error <= 0||error > 100) {
if (fabs(error) > 100) {
integral = 0;
}
double integralS = integral * I;
if(integralS > 5) {
integralS = 5;
}
//D
derivative = error - prev_Error;
double speed = (error * P) + integralS + derivative * D;
// print statistics to later graph, use desmos
printf("Wanted Position: %f \n", wanted_Position);
printf("Average Position: %f \n", aPos);
printf("Error: %f \n", error);
printf("Proportion: %f \n", error * P);
printf("Integral: %f \n", integral*I);
printf("Derivative: %f \n", derivative*D);
printf("Speed: %f \n", speed);
printf("\n");
// limit speed to not go too fast
if (speed > 12) {
speed = 12;
}
if(speed<-12)
speed=-12;
//if (forw) {
L1.spin(forward,speed,vex::voltageUnits::volt);
L2.spin(forward,speed,vex::voltageUnits::volt);
R1.spin(forward,speed,vex::voltageUnits::volt);
R2.spin(forward,speed,vex::voltageUnits::volt);
/* } else {
L1.spin(reverse,speed,vex::voltageUnits::volt);
L2.spin(reverse,speed,vex::voltageUnits::volt);
R1.spin(reverse,speed,vex::voltageUnits::volt);
R2.spin(reverse,speed,vex::voltageUnits::volt);
}
*/ vex::task::sleep(15);
if (fabs(error) <= 5) {
break;
}
}
reset_Motors();
}
void activateClawAuton() {
clawActivate();
// vex::task::sleep(900);
while (true) {
// Gathering Necessary Information
int lPos = (avrL());
int rPos = (avrR());
double aPos = (lPos+rPos)/2;
if(aPos>=dst1)
{
clawActivate();
break;
}
vex::task::sleep(10);
}
// clawActivate();
}
void autonLeft() {
// double
dst1=forward_Translation(52);
vex::thread t(activateClawAuton);
linear_PID(0.08,0.01,0.01,forward_Translation(52),true);
linear_PID(0.08,0.01,0.01,forward_Translation(-36),false);
move_Rotation(-70);
//move_Linear(-12,10);
//Back.spinFor(forward,250,degrees);
}
``` not all code, just essential functions