View Single Post
  #11   Spotlight this post!  
Unread 11-02-2015, 09:03 AM
jgrindle's Avatar
jgrindle jgrindle is offline
Losing Sleep
AKA: John Grindle
FRC #5122 (RobOTies)
Team Role: Programmer
 
Join Date: Nov 2014
Rookie Year: 2015
Location: Old Town, ME
Posts: 34
jgrindle will become famous soon enough
Re: Kiwi Drive and Integrating PID Loops

z_beebleBrox,
I added the I term to the loop, in turn it ended up looking like:
Code:
void DriveBase::TimerStart(){
	timer->Start();
}
void DriveBase::TimerStop(){
	timer->Stop();
}
//Used to fix deadzones on Joysticks, Gyros, or anything else
void DriveBase::dzFixer(float z){
	if((z >-(deadzone)) && (z < (deadzone))){
		z = 0;
	}
}

void DriveBase::PIDDrive(float drive, float turn, float strafe)
{
	dzFixer(turn);
	if(turn == 0 && (strafe > deadzone|| strafe < deadzone))
	{
		tim = float(timer->Get());
		x = g1->GetAngle();//get gyro reading
		dzFixer(x);
		I += (error()/tim);
		output = (error()* kp) + ((x+kp)*kd) + (I*ki);// First Parentheses = Error, Second set = Derivitive of the first term, I = integral
		Drive(drive, output, strafe); //Correction made here
		tim -= float(timer->Get());
	}else{
		Drive(drive,turn,strafe);
	}

}
float DriveBase::error(){
	float err;
	err = (0-x);
	return err;
}
void DriveBase::DStop(){
	Drive(0,0,0);
}
I'm hoping this is what it should look like, I should be able to test it soon, I can tell you the results when I do. I'm hoping this is readable code
__________________

FRC 2015 Season: Programmer, Electrical, PIDTuner, Safety Captain
FRC 2015 Off-Season: Programmer, CAD Designer, Driver, Drive Team Coach, Electrical, Mechanical, PIDTuner
Reply With Quote