static void evalSecondDeriv(state r, double t)
{
vec accel = LinearAcceleration(r, t);
secondDeriv[0] = accel.i;
secondDeriv[1] = accel.j;
secondDeriv[2] = accel.k;
}
//Steal cans
void AutonomousType13() {
SmartDashboard::PutString("STATUS:", "STARTING AUTO 13");
robotDrive.MecanumDrive_Cartesian(0, 0.2, 0);
if (WaitF(1.2))
return;
robotDrive.MecanumDrive_Cartesian(0, 0, 0);
canGrabber.SetSpeed(1);
if (WaitF(4))
return;
canGrabber.SetSpeed(0);
LinearAcceleration(1, 0, 1, 0);
SmartDashboard::PutString("STATUS:", "AUTO 13 COMPLETE");
}
/*!
* A generic fourth-order Runge-Kutta numerical integration engine for second
* order differential equations.
*
* Second Derivative (eg, acceleration) = secondDeriv
* First Derivative (eg, velocity) = firstDeriv
* Function (eg, position = function
*
* initial values = *_n
* next guess = *_n1
*
* One Eulers step:
* secondDeriv = ::get from program::
* firstDeriv_n1 = firstDeriv_n + secondDeriv*timestep
* function_n1 = function_n + firstDeriv*timestep
*
* rk4firstDeriv = Guesses for the first Derivitive
* rk4secondDeriv = Guesses for the second Derivitve
*
* You are not expected to understand this.
*/
state rk4(state r, float h)
{
int i;
double average;
double t = r.met;
// Prime the pump
initilize(r);
/* First Steps */
evalSecondDeriv(r, t); //Begining
for (i = 0; i < DOF; i++)
{
rk4firstDeriv[0][i] = firstDeriv_n[i]; //Using initial values
rk4secondDeriv[0][i] = secondDeriv[i];
}
/* Second Steps */
r = updateState(r, 0, 0.5*h); //Midpoint
evalFirstDeriv(r, t + 0.5*h); //Midpoint
evalSecondDeriv(r, t + 0.5*h); //Midpoint
for (i = 0; i < DOF; i++)
{
rk4firstDeriv[1][i] = firstDeriv[i];
rk4secondDeriv[1][i] = secondDeriv[i];
}
/* Third Steps */
r = updateState(r, 1, 0.5*h); //Midpoint
evalFirstDeriv(r, t + 0.5*h); //Midpoint
evalSecondDeriv(r, t + 0.5*h); //Midpoint
for (i = 0; i < DOF; i++)
{
rk4firstDeriv[2][i] = firstDeriv[i];
rk4secondDeriv[2][i] = secondDeriv[i];
}
/* Fourth Steps */
r = updateState(r, 2, h); //Endpoint
evalFirstDeriv(r, t + h); //Endpoint
evalSecondDeriv(r, t + h); //Endpoint
for (i = 0; i < DOF; i++)
{
rk4firstDeriv[3][i] = firstDeriv[i];
rk4secondDeriv[3][i] = secondDeriv[i];
}
/* Add it up */
for (i = 0; i < DOF; i++)
{
average = rk4firstDeriv[0][i]
+ 2*rk4firstDeriv[1][i]
+ 2*rk4firstDeriv[2][i]
+ rk4firstDeriv[3][i];
function_n1[i] = function_n[i] + (h*average)/6.0;
average = rk4secondDeriv[0][i]
+ 2*rk4secondDeriv[1][i]
+ 2*rk4secondDeriv[2][i]
+ rk4secondDeriv[3][i];
firstDeriv_n1[i] = firstDeriv_n[i] + (h*average)/6.0;
}
/* Update State */
r = setFirstDeriv(r, firstDeriv_n1);
r = setFunction(r, function_n1);
r.a = LinearAcceleration(r, t + h);
return r;
}
