//Initialize everything.
void Start()
{
//Make sure the altitude is zeroed
startAltitude = transform.position.y;
//Get the brake component. In the real program this might
//Initialize an object that deals with breaks or something.
brake = this.GetComponent <Brakes>();
//Make sure the brakes are off
brake.setResistForce(0);
//Make sure the brakes are activated.
brake.on = true;
}
//FixedUpdate is called every x seconds. In the simulation it is set to run every 0.2 seconds.
//This is important to note because of Time.fixedDeltaTime. This should be changed to calculate the
//time difference between one cycle of the code and the next.
void FixedUpdate()
{
//Logging the maximum altitude.
//Is our current y position higher than maxAlt?
if (transform.position.y > maxAlt)
{
//The current altitude is the highest we've ever been!
maxAlt = transform.position.y;
}
//This is our current speed in the y direction (up)
float curSpeed = GetComponent <Rigidbody>().velocity.y;
//Logging max speed
if (maxSpeed < curSpeed)
{
altAtMaxSpeed = transform.position.y;
maxSpeed = curSpeed;
}
//If the brake is on, or applying force
//brake.on isn't checking if the brakes are deployed, it's making sure the module is active.
//We're using PID, not bang-bang or on-off control
if (!brake.on || brake.resistanceForce == 0)
{
acceleration = (curSpeed - lastSpeed) / Time.fixedDeltaTime;
}
//Logging minimum acceleration.
minAccel = Mathf.Min(acceleration, minAccel);
//Our current K constant calculated through the function
// ((a - g) * m)/ v^2 = k
//This is only for testing purposes. It should change with the braking. Higher braking percentage means higher k value.
float k = ((acceleration - Physics.gravity.y) * massConstant) / (curSpeed * curSpeed);
Debug.Log("Calculated K: " + k);
//Are we using PID? We better be.
//If we aren't we are probably doing a launch to obtain k values and such.
if (usePID)
{
//Calculate what our projected final altitude is.
//The function used to find this is
//(m / (2 * k)) * Log((m * -g + k * v^2)/(m * -g)) + y
//Where:
// m = mass in grams(?)
// k = K constant - This is calculated beforehand and is a constant in the program.
// g = acceleration due to gravity.
// v = vertical velocity
// y = current altitude
float finalAltitude = (massConstant / (2 * KConstant)) * Mathf.Log((massConstant * -Physics.gravity.y + KConstant * curSpeed * curSpeed) / (massConstant * -Physics.gravity.y)) + transform.position.y;
//Ignore this.
//-(curSpeed * curSpeed) / (2 * (-9.81f - ((curSpeed * curSpeed * dragConstant)/massConstant))) + transform.position.y;
//Print some logging stuff out.
Debug.Log("Final Alt: " + finalAltitude + " " + (massConstant * -Physics.gravity.y));
//Are we decelerating?
//If we are, the motors aren't firing anymore and braking should be applied.
if (acceleration < 0)
{
//Get the difference between how high we will go and how high we want to go.
error = finalAltitude - targetAltitude;
//Take the integral of the error
integral = integral + error * Time.fixedDeltaTime;
//Take the derivative of the error
derivative = (error - prevError) / Time.fixedDeltaTime;
//Now apply the P I and D tuning parameters.
//This will give us an output percentage which may be over 100%. This is fine, and expected.
//Just limit it in the brake function, don't limit it here.
float output = P * error + I * integral + D * derivative;
//save the current error so we can calcuate the derivative.
prevError = error;
//Set the braking percentage.
brake.setResistForce(output);
//A little bit of debugging.
Debug.Log("Output: " + output + ", Error: " + error);
}
}
//Log the altitude quick so we can calculate vertical velocity
logAltitude(getAltitude());
//Log the last speed so we can calculate acceleration.
lastSpeed = curSpeed;
}
