FTC Scaling run_to_position Speeds

Hey, our team is using the run_to_position mode on our motors/encoders but our robot is accelerating and decelerating too fast (causing the robot to suddenly stop and sometimes turn 15-20 degrees).

We tried to fix it within the method (which I don’t have access to at the moment, but I’ll rewrite a conceptual version below) but it wasn’t actually fixing the problem. Is there a way to scale the speed (or set the max acceleration) in run_to_position so the root isn’t doing this as often?

We tried to scale the speed by actually changing the speed values of the motor at different times. Basically, we set the speed to 50% during the first and last 15% of the journey and to 100% during the middle bit, but it wasn’t working. Instead, the speed just stayed at 50% the whole time. This is what our attempt to scale the speed looked like (this is a very simplified version with only one motor, non-passed-through values, etc):

int ticksToTravel = someNumber;
double speed = someNumber;
int initialPos= motor.getCurrentPosition();

motor.setTargetPosition(ticksToTravel + initialPos);
motor.setPower(0.5 * speed);

//Wait for the motor to have traversed the first 15% of the trip
while(Math.abs(initialPos - motor.getCurrentPosition()) < 0.15 * ticksToTravel) {}
motor.setPower(1 * speed);

//Wait for the motor to have traversed 85% of the trip
while(Math.abs(initialPos - motor.getCurrentPosition()) < 0.85 * ticksToTravel) {}
motor.setPower(0.5 * speed);

//Wait for the motor to finish
while(motor.isBusy()) {}

//set the speed to zero
motor.setPower(0);

I don’t know specifics about FTC, but I think what you want is a Trapezoidal Motion Profile.

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.