Okay, what I’m coding is a control algorithm for a engine on a robot (in practice this code is going four times and reason it has to be fast is because they all simultaneous).
The code is in two parts, the control algorithm which is what will be used in the end, and a motor part which only coded to replicate the engine to make sure the controller works (if that makes sense). For this reason I’ve simply put it in a function.
I know there are problems with the actual maths (im working on them atm) I’m inexperienced in code and really after pointers about better ways of doing things cleaner ways etc if tht makes sense,
The graph this gives is a straight line which I know is wrong but when combined with other parts it works (if that’s makes sense)
Although the maths is incorrect the correct maths will be in the same structure as below.
Thanks In Advance
James
Code:
#include <stdio.h>
#include < math.h>
#include <stdlib.h>
/*Function Calls*/
void Preliminary(void);
void Engine(void);
void Speed(void);
int Time;
/*Controller Value*/
float Speed_Demand;
float Starting_Speed;
float Correction_Time;
float P_Coefficient;
float I_Coefficient;
float D_Coefficient;
float P_Term;
float D_Term;
float I_Term;
float Integral_Value;
float Previous_Integral_Value;
/*Speed Controller*/
float Speed_Error;
float Previous_Speed_Error;
float Required_Acceleration;
float Torque_Error;
float Required_Torque;
/*Engine Value*/
float Inertia;
/*Engine Simulation*/
float Throttle_Position;
float Engine_Time_Constant;
float Throttle_Factor;
float Torque_Without_Varience;
float Previous_Torque_Without_Torque_Varience;
float Engine_Speed;
float Torque_Varience;
float Engine_Torque;
float Amount_Affected_by_Torque;
/*Speed Value*/
float Actual_Speed;
/*Speed Simulation*/
float Previous_Speed;
float Acceleration;
float Tolerance;
float Tolerance_Input;
float Valve_Demand;
main()
{
FILE *fp;
fp=fopen("c:\\test.csv", "w");
/*Constants Toughout Simulation*/
P_Coefficient = -0.2;
I_Coefficient = 0.02;
D_Coefficient = 10;
Inertia = 5;
Throttle_Position = 10;
Engine_Time_Constant = 40;
Tolerance_Input = 0;
Amount_Affected_by_Torque = 0;
Speed_Demand = 1500;
Starting_Speed = 1400;
/*Initial Set Up Values*/
Time=0;
Previous_Speed = Starting_Speed;
Previous_Torque_Without_Torque_Varience=0;
Correction_Time = 0.1;
/*Used only for simulation*/
Preliminary();
/*Prints Initial Heading In File*/
fprintf(fp, "Time, Actual_Speed\n");
while(Time<10000)
{
/*Used only for simulation*/
Engine();
Speed();
/* Control System Code */
Speed_Error = Speed_Demand - Actual_Speed;
Required_Acceleration = Speed_Error / Correction_Time;
Required_Torque = Required_Acceleration * Inertia;
Torque_Error = Required_Torque - Engine_Torque;
P_Term = Torque_Error * P_Coefficient;
if(P_Term <-100)
P_Term = -100;
if(P_Term >100)
P_Term = 100;
I_Term = P_Term * I_Coefficient;
Integral_Value = I_Term + Previous_Integral_Value;
D_Term = D_Coefficient * (Previous_Speed_Error - Speed_Error);
Valve_Demand = P_Term + Integral_Value + D_Term;
if(Valve_Demand <0)
Valve_Demand = 0;
if(Valve_Demand >100)
Valve_Demand = 100;
/*Outputs the values to File*/
fprintf(fp, "%d, %f\n ",Time, Actual_Speed);
/*Sets Values for Next Pass*/
Previous_Speed_Error = Speed_Error;
Previous_Integral_Value = Integral_Value;
Time = Time+1;
}
/*Closes the File*/
fclose(fp);
}
/*Used Solely for Simulation*/
void Preliminary(void)
{
/*Engine Simulation*/
Throttle_Factor = 5/Engine_Time_Constant;
}
void Engine(void)
{
Torque_Without_Varience=Valve_Demand*Throttle_Factor + (1 -Throttle_Factor)*Previous_Torque_Without_Torque_Varience;
Engine_Speed = (Torque_Without_Varience-Engine_Torque)/Inertia;
Torque_Varience = (Torque_Without_Varience/100)*Amount_Affected_by_Torque*(sin(Time/(1000/(Engine_Speed/60))));
Engine_Torque=Throttle_Position*Throttle_Factor + (1 -Throttle_Factor)*Previous_Torque_Without_Torque_Varience+Torque_Varience;
Previous_Torque_Without_Torque_Varience = Engine_Torque;
}
void Speed(void)
{
Acceleration = Engine_Torque/Inertia;
Actual_Speed = Acceleration +Previous_Speed;
if (Actual_Speed <0)
Actual_Speed=0;
Tolerance=(((Actual_Speed/100)*Tolerance_Input)*1000);
Tolerance = (rand(Actual_Speed))/1000;
Actual_Speed = Acceleration +Previous_Speed+Tolerance;
if (Actual_Speed <0)
Actual_Speed=0;
Previous_Speed=Actual_Speed;
}