Pros inertial turning Pid isnt Working

Programming Support PROS Support
,
1

The PID i have programed to turn with the inertial sensor is behaving oddly, and I have no real idea why. Is anyone able to tell me where I’ve gone wrong?

void turning_pid(int target, int mode = 0, float tolerance = 0.1){
	float kP = 0.5;
	float kI = 0.1;
	float KD = 0.4;

	float P;
	float I;
	float aI;
	float D;
	float aD;

	float value;

	float power;

	float lasterror;

	bool firstloop = 1;

	bool left_negative;


	//true = left, false = right
	bool turning_direction = true;


	while(1){

		if(mode == 0){
			value = field_imu.get_heading();
		}
		else if(mode == 1){
			value = turning_imu.get_heading();
		}

		float error = target - value;

		//for right turns
		if(error < 0){
			error = (-error);
			if(error > 180){
				error = (360-value) + target;
			}
			else{
				error += 180;
			}
		}

		if(firstloop){
			turning_direction = error < 180;
			firstloop = 0;
		}

		P = error * kP;
		I = I + error;
		aI = I * kI;
		D = error - lasterror;
		aD = D * KD;

		power  = P + aI + aD;

		std::cout << error << std::endl;

		if(turning_direction){
			left_mtrs = !power;
			right_mtrs = power;
			if(error < tolerance){
				left_mtrs.brake();
				right_mtrs.brake();
				if(mode == 1){
					turning_imu.set_heading(0);
				}
				break;
			}
		}

		else if(!turning_direction){
			right_mtrs = !power;
			left_mtrs = power;
			if((error - 180) < tolerance){
				left_mtrs.brake();
				right_mtrs.brake();
				if(mode == 1){
					turning_imu.set_heading(0);
				}
				break;
			}
		}

		lasterror = error;

		pros::delay(20);
	}
	std::cout << "end" << std::endl;
}

Thanks in advance

2

mostly fixed, it was just turning the wrong direction, but i can’t figure out why it isnt doing a point turn.

3

nvm about the swing turns im an idiot