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