Recently, I’ve been working on a function that controls my robot’s drivetrain in autonomous using two encoders on each side for position PID (albeit only using the proportional part) control and a gyro to keep the robot from drifting. While this function works as intended, once I called the function again it just malfunctioned and turned (video for reference). I’m using PROS, and this is a Cortex robot
This is the code I was running in
auto.c
:
#include "main.h"
void autonomous() {
autoSmartRobot(1800, 1800, 127, 0, 0);
autoSmartRobot(-1800, -1800, 127, 0, 0); //problematic part
}
And this is the
autoSmartRobot()
function definition:
void autoSmartRobot(int driveLeft, int driveRight, int limit, int ballIntake, int flyWheelRPM) {
const int driveThreshold = 180;
//reset encoders and gyro
encoderReset(leftDriveEncoder);
encoderReset(rightDriveEncoder);
gyroReset(gyro);
int leftDiff = driveLeft - encoderGet(leftDriveEncoder);
int rightDiff = driveRight - encoderGet(rightDriveEncoder);
//run robot subsystems, and when drivetrain target is reached stop drivetrain
while (abs(leftDiff) > driveThreshold && abs(rightDiff) > driveThreshold) {
autoStraightDriveTrain(driveLeft, driveRight);
autoBallIntake(ballIntake);
autoManualFlyWheelControl(flyWheelRPM);
printf("left %d right %d\n", encoderGet(leftDriveEncoder), encoderGet(rightDriveEncoder));
leftDiff = driveLeft - encoderGet(leftDriveEncoder);
rightDiff = driveRight - encoderGet(rightDriveEncoder);
delay(20);
}
driveTrainSetSpeed(0, 0);
}
And my
autoStraightDriveTrain()
function definition:
void autoStraightDriveTrain(int leftTarget, int rightTarget) {
int leftCurrent = encoderGet(leftDriveEncoder);
int rightCurrent = encoderGet(rightDriveEncoder);
int robotHeading = gyroGet(gyro);
int leftSpeed = positionPID(leftCurrent, leftTarget, POS_PID_DRIVETRAIN_L);
int rightSpeed = positionPID(rightCurrent, rightTarget, POS_PID_DRIVETRAIN_R);
leftSpeed = min(127, max(-127, leftSpeed));
rightSpeed = min(127, max(-127, rightSpeed));
leftSpeed += robotHeading * 25;
rightSpeed -= robotHeading * 25;
driveTrainSetSpeed(leftSpeed, rightSpeed);
}
Any ideas on how to fix this issue? I was thinking it was due to the negative drivetrain parameters in the second
autoSmartRobot()
call, but when I ran the second
autoSmartRobot()
call by itself (ie I commented out the first call) it worked fine.