<commit_before>#pragma config(Sensor, dgtl1,  autonCont,      sensorTouch)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#define DRIVE_THRESHOLD_FORWARD 15 // Joystick forward threshold
#define DRIVE_THRESHOLD_TURN 15 // Joystick turn threshold
#define DRIVE_THRESHOLD_STRAFE 15 // Joystick strafe threshold
#define INITIAL_DRIVE_POWER 25 //The power that drive with logic will start it's linear function at for drive power
#define JOYSTICK_MOVEMENT_THRESHOLD 15 //The amount the joystick has to move for it to be used in the linear function to calculate RPM


/////////////////////////////////////////////////////////////////////////////////////////
//
//                        Normal drive methods (No sensors)
//
/////////////////////////////////////////////////////////////////////////////////////////

// Cleaner way of setting all four drive motor speeds.
// Drive with checks to rule out errors with joystick controls
// PARAMETERS:
//	int: -127 to 127, speed for the front left motor
//	int: -127 to 127, speed for the back left motor
//	int: -127 to 127, speed for the front right motor
//	int: -127 to 127, speed for the back right motor
void driveRaw(int speedFL, int speedBL, int speedFR, int speedBR) {
	motor[driveFL] = speedFL; // Front left
	motor[driveBL] = speedBL; // Back left
	motor[driveFR] = speedFR; // Front right
	motor[driveBR] = speedBR; // Back right
}



// Set mecanum wheel motor speeds based on forward, turn, and strafe speeds
// PARAMETERS:
//	int: -127 to 127, speed to drive forward or backward respectively
//	int: -127 to 127, speed to turn left or right respectively
//	int: -127 to 127, speed to strafe left or right respectively
void drive(int speedForward, int speedTurn, int speedStrafe) {
	int speedFL = speedForward + speedTurn + speedStrafe;
	int speedBL = speedForward + speedTurn - speedStrafe;
	int speedFR = speedForward - speedTurn - speedStrafe;
	int speedBR = speedForward - speedTurn + speedStrafe;
	driveRaw(speedFL, speedBL, speedFR, speedBR);
}

void driveBackwards(int speedForward, int speedTurn, int speedStrafe) {
	int speedBR = speedForward + speedTurn + speedStrafe; // FL equivalent
	int speedFR = speedForward + speedTurn - speedStrafe; // BL equivalent
	int speedBL = speedForward - speedTurn - speedStrafe; // FR equivalent
	int speedFL = speedForward - speedTurn + speedStrafe; // BR equivalent
	driveRaw(speedFL, speedBL, speedFR, speedBR);
}

// Drive with checks to rule out errors with joystick controls
// NOTE:
// 	Multipliers should always be (0 <= x <= 1.0). This avoids going too fast or slow and going out of proportion
// PARAMETERS:
//	int: -127 to 127, speed to drive forward or backward respectively
//	int: -127 to 127, speed to turn left or right respectively
//	int: -127 to 127, speed to strafe left or right respectively
//	float: What to reduce forward/backward speed to (0.7 -> 70% of input)
//	float: What to reduce left/right turn speed to (0.7 -> 70% of input)
//	float: What to reduce left/right strafe speed to (0.7 -> 70% of input)
void driveWithLogic(int speedForward, int speedTurn, int speedStrafe, bool reverse, float forwardMultiplier, float turnMultiplier, float strafeMultiplier) {
	int multipliedSpeedForward = speedForward; // ((float) speedForward)*forwardMultiplier;
	int multipliedSpeedTurn = speedTurn; //((float) speedTurn)*turnMultiplier;
	int multipliedSpeedStrafe = speedStrafe; //((float) speedStrafe)*strafeMultiplier;

	if (abs(multipliedSpeedForward) <= DRIVE_THRESHOLD_FORWARD) multipliedSpeedForward = 0;
	if (abs(multipliedSpeedTurn) <= DRIVE_THRESHOLD_TURN) multipliedSpeedTurn = 0;
	if (abs(multipliedSpeedStrafe) <= DRIVE_THRESHOLD_STRAFE) multipliedSpeedStrafe = 0;

	if(abs(multipliedSpeedForward) <= JOYSTICK_MOVEMENT_THRESHOLD) multipliedSpeedForward = 0;
	if(abs(multipliedSpeedTurn) <= JOYSTICK_MOVEMENT_THRESHOLD) multipliedSpeedTurn = 0;
	if(abs(multipliedSpeedStrafe) <= JOYSTICK_MOVEMENT_THRESHOLD) multipliedSpeedStrafe = 0;

	// TODO Cooler stuff than just thresholds :P
	// Consider it TODONE

	//uses linear interpolation or lerp to fix the logarithmic nature of a motor's RPM to motor speed ratio into linear growth
	float slope = ((105.0-(float)INITIAL_DRIVE_POWER)/(127.0-(float)JOYSTICK_MOVEMENT_THRESHOLD));
	float yInt = 105.0-(slope*127.0);
	multipliedSpeedForward = RPMToMotor((multipliedSpeedForward*slope)+yInt);
	multipliedSpeedTurn = RPMToMotor((multipliedSpeedTurn*slope)+yInt);
	multipliedSpeedStrafe = RPMToMotor((multipliedSpeedStrafe*slope)+yInt);

	if (!reverse)
		drive(multipliedSpeedForward, multipliedSpeedTurn, multipliedSpeedStrafe); // Pass off the checked values to drive
	else
		driveBackwards(multipliedSpeedForward, multipliedSpeedTurn, multipliedSpeedStrafe); // Pass off the checked values to drive
}

// Overriding method. See driveWithLogic(int, int, int, float, float, float).
// NOTE:
// 	-> Sets multipliers to 1.0
void driveWithLogic(int speedForward, int speedTurn, int speedStrafe, bool reverse) {
	driveWithLogic(speedForward, speedTurn, speedStrafe, reverse, 1.0, 1.0, 1.0);
}

// Set each side of the drive to a certain speed.
// PARAMETERS:
//  int: How fast to move the left side (-127 to 127)
//  int: How fast to move the right side (-127 to 127)
void driveTank(int speedLeft, int speedRight){
	if(abs(speedLeft) <= DRIVE_THRESHOLD_FORWARD) speedLeft = 0;
	if(abs(speedRight) <= DRIVE_THRESHOLD_FORWARD) speedRight = 0;
	driveRaw(speedLeft,speedLeft,speedRight,speedRight);
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                        Smart drive methods (Encoders & PID)
//
/////////////////////////////////////////////////////////////////////////////////////////

float RPMValues[10] = {0,0,0,0,0,0,0,0,0,0};
long lastTickCount[10] = {0,0,0,0,0,0,0,0,0,0};
float RPMReadRate = 20.0;

task getRPMValues() {
	while(true){
		long tickCount[10] = {0,nMotorEncoder[driveFR],nMotorEncoder[driveBR],0,0,0,0,nMotorEncoder[driveFL],nMotorEncoder[driveBL],0};
		for(int i = 0; i < 10; i++){
			RPMValues[i] = (((((float)tickCount[i])-((float)lastTickCount[i]))*(60000.0/RPMReadRate))/627.2); // Made for torque geared motors
			lastTickCount[i] = tickCount[i];
		}
		/* Logging RPM
		datalogDataGroupStart();
		datalogAddValue(0,RPMValues[1]);
		datalogAddValue(1,RPMValues[2]);
		datalogAddValue(2,RPMValues[7]);
		datalogAddValue(3,RPMValues[8]);
		datalogAddValue(4,50);
		datalogDataGroupEnd();
		*/
		wait1Msec((int)RPMReadRate);

	}

}

// Sets the encoders on all of our drive motors back to 0.
void clearDriveEncoders() {
	nMotorEncoder[driveFR] = 0;
	nMotorEncoder[driveFL] = 0;
	nMotorEncoder[driveBL]= 0;
	nMotorEncoder[driveBR] = 0;
}

tMotor motorsToChange[4];
bool motorToMotorReverse[4];
long tickTarget[4];
//Setup the weights for the various stages of pid
float kP = 1.0; //Proportional Gain
float kI = 0.0; //Integral Gain
float kD = 0.0; //Derivitive Gain
float kL = 50.0; //Apparently this is there to be the integral limit, I think we missed it when working last time

task drivePID() {
	//Initialize the PID variables
	long error[4] = {0, 0, 0, 0};
	long pError[4] = {0, 0, 0, 0};
	long p[4] = {0, 0, 0, 0};
	long i[4] = {0, 0, 0, 0};
	long d[4] = {0, 0, 0, 0};
	string debugString = ""; //FOR DEBUG, REMOVE LATER
	writeDebugStreamLine("STARTING PID");

	while(true){
		long driveAvg = 0;
		long errorAvg = 0;
		for (int index = 0; index < 4; index ++){
			error[index] = tickTarget[index] - nMotorEncoder[motorsToChange[index]];
			//DEBUG
			debugString = "";
			sprintf(debugString,"Er[%i]:%i",index,error[index]);
			writeDebugStreamLine(debugString);
			//DEBUG
			p[index] = error[index]; //P is simply the error
			i[index] = abs(i[index] + error[index]) < kL ? i[index] + error[index] : sgn(i[index] + error[index])*kL; //I is the sum of errors
			d[index] = error[index] - pError[index]; //D is the change in error or delta error
			driveAvg += abs(p[index]*kP + i[index]*kI + d[index]*kD);
			errorAvg += abs(error[index]);
		}
		//Find the average drive speed and error
		driveAvg = round(driveAvg/4);
		errorAvg = round(errorAvg/4);
		debugString = "";
		sprintf(debugString,"DrAv:%i \n ErAv:%i",driveAvg,errorAvg);
		writeDebugStreamLine(debugString);
		float slope = ((105.0-(float)INITIAL_DRIVE_POWER)/(127.0-(float)JOYSTICK_MOVEMENT_THRESHOLD));
		float yInt = 105.0-(slope*127.0);
		int speedForward = RPMToMotor((driveAvg*slope)+yInt);
		for(int index = 0; index < 4; index ++){
			debugString = "";
			sprintf(debugString,"MoSp:%i",(abs(errorAvg) > 5) ? ((motorToMotorReverse[index]) ? (speedForward*-1) : (speedForward)) : (0));
			writeDebugStreamLine(debugString);
			//if the error is low turn off the motor, otherwise set to average, and reverse if this motor is meant to be reversed
			motor[motorsToChange[index]] = (abs(errorAvg) > 5) ? ((motorToMotorReverse[index]) ? (speedForward*-1) : (speedForward)) : (0);
		}
		//if the error is low end the PID loop
		if(abs(errorAvg) < 5){
			writeDebugStreamLine("STOPPING PID");
			break;
		}
		wait1Msec(25);
	}
}

// Clean way to set variables for driveMotorToTargetPID
// PARAMETERS:
//  tMotor: array of motors to use
//  long: ticks to move<commit_msg>may have fixed PID, but still some changes need to be done in the future<commit_after>#pragma config(Sensor, dgtl1,  autonCont,      sensorTouch)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#define DRIVE_THRESHOLD_FORWARD 15 // Joystick forward threshold
#define DRIVE_THRESHOLD_TURN 15 // Joystick turn threshold
#define DRIVE_THRESHOLD_STRAFE 15 // Joystick strafe threshold
#define INITIAL_DRIVE_POWER 25 //The power that drive with logic will start it's linear function at for drive power
#define JOYSTICK_MOVEMENT_THRESHOLD 15 //The amount the joystick has to move for it to be used in the linear function to calculate RPM


/////////////////////////////////////////////////////////////////////////////////////////
//
//                        Normal drive methods (No sensors)
//
/////////////////////////////////////////////////////////////////////////////////////////

// Cleaner way of setting all four drive motor speeds.
// Drive with checks to rule out errors with joystick controls
// PARAMETERS:
//	int: -127 to 127, speed for the front left motor
//	int: -127 to 127, speed for the back left motor
//	int: -127 to 127, speed for the front right motor
//	int: -127 to 127, speed for the back right motor
void driveRaw(int speedFL, int speedBL, int speedFR, int speedBR) {
	motor[driveFL] = speedFL; // Front left
	motor[driveBL] = speedBL; // Back left
	motor[driveFR] = speedFR; // Front right
	motor[driveBR] = speedBR; // Back right
}



// Set mecanum wheel motor speeds based on forward, turn, and strafe speeds
// PARAMETERS:
//	int: -127 to 127, speed to drive forward or backward respectively
//	int: -127 to 127, speed to turn left or right respectively
//	int: -127 to 127, speed to strafe left or right respectively
void drive(int speedForward, int speedTurn, int speedStrafe) {
	int speedFL = speedForward + speedTurn + speedStrafe;
	int speedBL = speedForward + speedTurn - speedStrafe;
	int speedFR = speedForward - speedTurn - speedStrafe;
	int speedBR = speedForward - speedTurn + speedStrafe;
	driveRaw(speedFL, speedBL, speedFR, speedBR);
}

void driveBackwards(int speedForward, int speedTurn, int speedStrafe) {
	int speedBR = speedForward + speedTurn + speedStrafe; // FL equivalent
	int speedFR = speedForward + speedTurn - speedStrafe; // BL equivalent
	int speedBL = speedForward - speedTurn - speedStrafe; // FR equivalent
	int speedFL = speedForward - speedTurn + speedStrafe; // BR equivalent
	driveRaw(speedFL, speedBL, speedFR, speedBR);
}

// Drive with checks to rule out errors with joystick controls
// NOTE:
// 	Multipliers should always be (0 <= x <= 1.0). This avoids going too fast or slow and going out of proportion
// PARAMETERS:
//	int: -127 to 127, speed to drive forward or backward respectively
//	int: -127 to 127, speed to turn left or right respectively
//	int: -127 to 127, speed to strafe left or right respectively
//	float: What to reduce forward/backward speed to (0.7 -> 70% of input)
//	float: What to reduce left/right turn speed to (0.7 -> 70% of input)
//	float: What to reduce left/right strafe speed to (0.7 -> 70% of input)
void driveWithLogic(int speedForward, int speedTurn, int speedStrafe, bool reverse, float forwardMultiplier, float turnMultiplier, float strafeMultiplier) {
	int multipliedSpeedForward = speedForward; // ((float) speedForward)*forwardMultiplier;
	int multipliedSpeedTurn = speedTurn; //((float) speedTurn)*turnMultiplier;
	int multipliedSpeedStrafe = speedStrafe; //((float) speedStrafe)*strafeMultiplier;

	if (abs(multipliedSpeedForward) <= DRIVE_THRESHOLD_FORWARD) multipliedSpeedForward = 0;
	if (abs(multipliedSpeedTurn) <= DRIVE_THRESHOLD_TURN) multipliedSpeedTurn = 0;
	if (abs(multipliedSpeedStrafe) <= DRIVE_THRESHOLD_STRAFE) multipliedSpeedStrafe = 0;

	if(abs(multipliedSpeedForward) <= JOYSTICK_MOVEMENT_THRESHOLD) multipliedSpeedForward = 0;
	if(abs(multipliedSpeedTurn) <= JOYSTICK_MOVEMENT_THRESHOLD) multipliedSpeedTurn = 0;
	if(abs(multipliedSpeedStrafe) <= JOYSTICK_MOVEMENT_THRESHOLD) multipliedSpeedStrafe = 0;

	// TODO Cooler stuff than just thresholds :P
	// Consider it TODONE

	//uses linear interpolation or lerp to fix the logarithmic nature of a motor's RPM to motor speed ratio into linear growth
	float slope = ((105.0-(float)INITIAL_DRIVE_POWER)/(127.0-(float)JOYSTICK_MOVEMENT_THRESHOLD));
	float yInt = 105.0-(slope*127.0);
	multipliedSpeedForward = RPMToMotor((multipliedSpeedForward*slope)+yInt);
	multipliedSpeedTurn = RPMToMotor((multipliedSpeedTurn*slope)+yInt);
	multipliedSpeedStrafe = RPMToMotor((multipliedSpeedStrafe*slope)+yInt);

	if (!reverse)
		drive(multipliedSpeedForward, multipliedSpeedTurn, multipliedSpeedStrafe); // Pass off the checked values to drive
	else
		driveBackwards(multipliedSpeedForward, multipliedSpeedTurn, multipliedSpeedStrafe); // Pass off the checked values to drive
}

// Overriding method. See driveWithLogic(int, int, int, float, float, float).
// NOTE:
// 	-> Sets multipliers to 1.0
void driveWithLogic(int speedForward, int speedTurn, int speedStrafe, bool reverse) {
	driveWithLogic(speedForward, speedTurn, speedStrafe, reverse, 1.0, 1.0, 1.0);
}

// Set each side of the drive to a certain speed.
// PARAMETERS:
//  int: How fast to move the left side (-127 to 127)
//  int: How fast to move the right side (-127 to 127)
void driveTank(int speedLeft, int speedRight){
	if(abs(speedLeft) <= DRIVE_THRESHOLD_FORWARD) speedLeft = 0;
	if(abs(speedRight) <= DRIVE_THRESHOLD_FORWARD) speedRight = 0;
	driveRaw(speedLeft,speedLeft,speedRight,speedRight);
}

/////////////////////////////////////////////////////////////////////////////////////////
//
//                        Smart drive methods (Encoders & PID)
//
/////////////////////////////////////////////////////////////////////////////////////////

float RPMValues[10] = {0,0,0,0,0,0,0,0,0,0};
long lastTickCount[10] = {0,0,0,0,0,0,0,0,0,0};
float RPMReadRate = 20.0;

task getRPMValues() {
	while(true){
		long tickCount[10] = {0,nMotorEncoder[driveFR],nMotorEncoder[driveBR],0,0,0,0,nMotorEncoder[driveFL],nMotorEncoder[driveBL],0};
		for(int i = 0; i < 10; i++){
			RPMValues[i] = (((((float)tickCount[i])-((float)lastTickCount[i]))*(60000.0/RPMReadRate))/627.2); // Made for torque geared motors
			lastTickCount[i] = tickCount[i];
		}
		/* Logging RPM
		datalogDataGroupStart();
		datalogAddValue(0,RPMValues[1]);
		datalogAddValue(1,RPMValues[2]);
		datalogAddValue(2,RPMValues[7]);
		datalogAddValue(3,RPMValues[8]);
		datalogAddValue(4,50);
		datalogDataGroupEnd();
		*/
		wait1Msec((int)RPMReadRate);

	}

}

// Sets the encoders on all of our drive motors back to 0.
void clearDriveEncoders() {
	nMotorEncoder[driveFR] = 0;
	nMotorEncoder[driveFL] = 0;
	nMotorEncoder[driveBL]= 0;
	nMotorEncoder[driveBR] = 0;
}

tMotor motorsToChange[4] = {driveFL,driveFR,driveBR,driveBL};
bool motorToMotorReverse[4] = {false, false, false, false};
long tickTarget[4];
//Setup the weights for the various stages of pid
float kP = 1.0; //Proportional Gain
float kI = 0.0; //Integral Gain
float kD = 0.0; //Derivitive Gain
float kL = 50.0; //Apparently this is there to be the integral limit, I think we missed it when working last time

task drivePID() {
	//Initialize the PID variables
	long error[4] = {0, 0, 0, 0};
	long pError[4] = {0, 0, 0, 0};
	long p[4] = {0, 0, 0, 0};
	long i[4] = {0, 0, 0, 0};
	long d[4] = {0, 0, 0, 0};
	string debugString = ""; //FOR DEBUG, REMOVE LATER
	writeDebugStreamLine("STARTING PID");

	while(true){
		long driveAvg = 0;
		long errorAvg = 0;
		for (int index = 0; index < 4; index ++){
			error[index] = tickTarget[index] - nMotorEncoder[motorsToChange[index]];
			//DEBUG
			debugString = "";
			sprintf(debugString,"Er[%i]:%i",index,error[index]);
			writeDebugStreamLine(debugString);
			//DEBUG
			p[index] = error[index]; //P is simply the error
			i[index] = abs(i[index] + error[index]) < kL ? i[index] + error[index] : sgn(i[index] + error[index])*kL; //I is the sum of errors
			d[index] = error[index] - pError[index]; //D is the change in error or delta error
			driveAvg += abs(p[index]*kP + i[index]*kI + d[index]*kD);
			errorAvg += abs(error[index]);
		}
		//Find the average drive speed and error
		driveAvg = round(driveAvg/4);
		errorAvg = round(errorAvg/4);
		debugString = "";
		sprintf(debugString,"DrAv:%i \nErAv:%i",driveAvg,errorAvg);
		writeDebugStreamLine(debugString);
		float slope = ((105.0-(float)INITIAL_DRIVE_POWER)/(127.0-(float)JOYSTICK_MOVEMENT_THRESHOLD));
		float yInt = 105.0-(slope*127.0);
		int speedForward = RPMToMotor((driveAvg*slope)+yInt);
		for(int index = 0; index < 4; index ++){
			debugString = "";
			sprintf(debugString,"MoSp:%i",(abs(errorAvg) > 5) ? ((motorToMotorReverse[index]) ? (speedForward*-1) : (speedForward)) : (0));
			writeDebugStreamLine(debugString);
			//if the error is low turn off the motor, otherwise set to average, and reverse if this motor is meant to be reversed
			motor[motorsToChange[index]] = (abs(errorAvg) > 5) ? ((motorToMotorReverse[index]) ? (speedForward*-1) : (speedForward)) : (0);
		}
		//if the error is low end the PID loop
		if(abs(errorAvg) < 5){
			writeDebugStreamLine("STOPPING PID");
			break;
		}
		wait1Msec(25);
	}
}

// Clean way to set variables for driveMotorToTargetPID
// PARAMETERS:
//  tMotor: array of motors to use
//  long: ticks to move
{'fraction_non_alphanumeric': 0.15546467201205402, 'fraction_numerical': 0.02862831620298122, 'mean_word_length': 3.6611487333834964, 'pattern_counts': {'":': 0, '<': 33, '<?xml version=': 0, '>': 15, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '1423502', 'n_tokens_mistral': 6677, 'n_tokens_neox': 6042, 'n_words': 1817}