Kiwi Drive and Integrating PID Loops

[jgrindle]

Hey Everyone,
I got two main questions for today. The first being with a kiwi drive, I know that in general for driving the function would look a little like this:

void DriveBase::Drive(float joy_X, float joy_Y, float joy_Z){
	float Con1 = (joy_X + joy_Z);
	float Con2 = ((-((joy_X)/2))+((sqrt(3)/2)*(joy_Y)) + joy_Z);
	float Con3 = ((-((joy_X)/2))-((sqrt(3)/2)*(joy_Y)) + joy_Z);

	c1->Set(Con1);
	c2->Set(Con2);
	c3->Set(Con3);
}

But in design I do not have an equilateral triangle. I have what I am deeming a "Butterfly" Do I have to change any of the above code, to make this work?

Onto the next part. Along with that I have a custom PD loop that I am working on, I currently have not integrated the I term, as we have not gone over integrals in Calculus yet. I have decided against going with a regular Subsystem, because of how much of a pain they were during build season; I have less time than I did in build season, I have two weeks. Currently my code for the PD Drive is

//Used to fix deadzones on Joysticks, Gyros, or anything else
void DriveBase::dzFixer(float z){
	if((z >-(deadzone)) && (z < (deadzone))){
		z = 0;
	}
}

void DriveBase::PDDrive(float drive, float turn, float strafe, float kp, float kd)
{
	x = g1->GetAngle();
	dzFixer(x);
	dzFixer(turn);
	if(turn == 0 && (strafe > deadzone|| strafe < deadzone))
	{
		output = ((0 - x)* kp) + ((x+kp)*kd);// First Parentheses = Error, Second set = Derivitive of the first term
	}
	Drive(drive, output, strafe); //Correction made here
}

Is this sufficient? Should I go about and add an I term? If so, how would I do that? Is there an easier way?