src/main/cpp/subsystems/DriveTrain.cpp
void DriveTrain::GyroCrab(float x, float y, float desiredAngle, bool operatorControl) {
	auto currentAngle = Robot::gyroSub->PIDGet();
	auto twist = -(desiredAngle - currentAngle);

	while (twist > 180.0) {
		twist -= 360.0;
	}
	while (twist < -180.0) {
		twist += 360.0;
	}

	constexpr float GYRO_P = 0.028; //original is 0.007
	constexpr float GYRO_MAX = 0.3;

	twist = std::min<float>(GYRO_MAX, std::max<float>(-GYRO_MAX, twist * GYRO_P)); //is this correct?
	Crab(twist, y, x, true);
}
//desiredAngle does not work in current state.
// Wheels turning inwards to robot 
// FURTHER TESTING / DEBUGGING REQUIRED
desiredAngle does not work in current state.
Wheels turning inwards to robot
FURTHER TESTING / DEBUGGING REQUIRED
desiredAngle does not work in current state . Wheels turning inwards to robot FURTHER TESTING / DEBUGGING REQUIRED
void DriveTrain::GyroCrab(float x, float y, float desiredAngle, bool operatorControl) {
	auto currentAngle = Robot::gyroSub->PIDGet();
	auto twist = -(desiredAngle - currentAngle);
	while (twist > 180.0) {
		twist -= 360.0;
	}
	while (twist < -180.0) {
		twist += 360.0;
	}
	constexpr float GYRO_P = 0.028; 
	constexpr float GYRO_MAX = 0.3;
	twist = std::min<float>(GYRO_MAX, std::max<float>(-GYRO_MAX, twist * GYRO_P)); 
	Crab(twist, y, x, true);
}
void DriveTrain :: GyroCrab ( float x , float y , float desiredAngle , bool operatorControl ) { auto currentAngle = Robot :: gyroSub -> PIDGet ( ) ; auto twist = - ( desiredAngle - currentAngle ) ; while ( twist > 180.0 ) { twist -= 360.0 ; } while ( twist < -180.0 ) { twist += 360.0 ; } constexpr float GYRO_P = 0.028 ; constexpr float GYRO_MAX = 0.3 ; twist = std :: min < float > ( GYRO_MAX , std :: max < float > ( - GYRO_MAX , twist * GYRO_P ) ) ; Crab ( twist , y , x , true ) ; }
desiredAngle does not work in current state.
desiredAngle does not work in current state .
//original is 0.007 //is this correct?