private void Init()
{
const double kp = 1;
const double ki = 0.01;
const double kd = 0.8;
_pidController = new Library.PidController(
new Factory <IGain>(() => new ProportionalGain(kp)),
new Factory <IGain>(() => new IntegralGain(ki)),
new Factory <IGain>(() => new DerivativeGain(kd, 5))
);
var tvcMountStartAngle = 0d;
var rocketStartAngle = 40;
var rocketStartAngleRate = 70;
_rocketAngle = rocketStartAngle;
_tvcMountAngle = 0;
_timeProvider = new TimeProvider();
_tvcMount = new TvcMountController(_timeProvider, tvcMountStartAngle);
var rocketMass = 0.5d;
var height = 1;
var thrust = 7;
_rocket = new Rocket(_tvcMount, height, rocketMass, thrust, rocketStartAngle, rocketStartAngleRate);
_timer = new DispatcherTimer();
_timer.Interval = TimeSpan.FromMilliseconds(50);
_timer.Tick += Timer_Tick;
_timer.Start();
}
static void Main(string[] args)
{
const double kp = 1;
const double ki = 0.01;
const double kd = 0.8;
var pidController = new Library.PidController(
new Factory <IGain>(() => new ProportionalGain(kp)),
new Factory <IGain>(() => new IntegralGain(ki)),
new Factory <IGain>(() => new DerivativeGain(kd, 5))
);
var tvcMountStartAngle = 0d;
var rocketStartAngle = 40;
var rocketStartAngleRate = 40;
var timeProvider = new TimeProvider();
var tvcMount = new TvcMountController(timeProvider, tvcMountStartAngle);
var rocketMass = 0.5d;
var height = 1;
var thrust = 7;
var rocket = new Rocket(tvcMount, height, rocketMass, thrust, rocketStartAngle, rocketStartAngleRate);
timeProvider.Start();
Task.Run(() =>
{
var sp = 0d;
while (true)
{
var pv = rocket.Angle;
var err = pv - sp;
var cv = pidController.In(err);
tvcMount.MountAngle = cv;
Thread.Sleep(50);
}
});
while (true)
{
System.Console.WriteLine($"Time: {timeProvider.Time}, Rocket Angle: {rocket.Angle}, TVC Mount Angle: {tvcMount.MountAngle}");
Thread.Sleep(500);
}
}
