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; }