cscの作法 その582

概要

cscの作法、調べてみた。
OpenRCF v2.8、見つけたので、windows11で、MSbuildしてみた。
シュミレーションやってみた。
PID制御やってみた。

写真

サンプルコード

using System;
using System.Windows;
using OpenRCF;
using Vector = OpenRCF.Vector;
using OpenTK;
using OpenTK.Graphics;
using OpenTK.Graphics.OpenGL;


namespace OpenRCF
{
	class InvertedPendulum {
		private readonly static float g = 9.80665f;
		private Cuboid Pole = new Cuboid(0.1f, 0.1f, 0.8f);
		private Pillar Joint = new Pillar();
		private Cuboid Base = new Cuboid(0.4f, 0.4f, 0.2f);
		private float poleLength = 0.5f;
		private float poleMass = 0.2f;
		private float baseMass = 0.2f;
		private bool isDynamicsEnabled = false;
		private void CalcDynamics() {
			float CosTh = (float) Math.Cos(Theta);
			float SinTh = (float) Math.Sin(Theta);
			float force = Force - PoleMass * g * CosTh * SinTh;
			Velocity += SamplingTime * force / (BaseMass + PoleMass);
			Position += SamplingTime * Velocity;
			Joint.Position[0] = Position;
			float poleForce = PoleMass / (BaseMass + PoleMass) * force;
			Torque = 0.5f * poleLength * (PoleMass * g * SinTh - poleForce * CosTh);
			Omega += SamplingTime * Torque / Inertia;
			Theta += SamplingTime * Omega;
			if (Theta > 1.6)
				return;
			if (Theta < -1.6)
				return;
			Joint.Rotate.SetRy(Theta);
		}
		public float Inertia {
			get;
			private set;
		}
		public float Position,
			Velocity;
		public float Theta,
			Omega,
			Torque;
		public float Force;
		public float samplingTime = 0.05f;
		public float SamplingTime {
			get {
				return samplingTime;
			}
			set {
				if (isDynamicsEnabled)
				{
					Console.WriteLine("Error: SamplingTime setting must be before StartDynamics().");
				}
				else if (value < 0.001f)
				{
					Console.WriteLine("Error: SamplingTime is too small.");
				}
				else
				{
					samplingTime = value;
				}
			}
		}
		public float PoleLength {
			get {
				return poleLength;
			}
			set {
				poleLength = value;
				Inertia = 0.25f * poleMass * poleLength * poleLength;
				Pole.SetSize(0.1f * poleLength, 0.1f * poleLength, poleLength);
				Pole.SetPositionOffset(0, 0, 0.5f * Pole.SizeZ);
				Joint.Radius = 0.075f * poleLength;
				Joint.Height = 0.12f * poleLength;
			}
		}
		public float PoleMass {
			get {
				return poleMass;
			}
			set {
				if (0 < value)
				{
					poleMass = value;
					Inertia = 0.25f * poleMass * poleLength * poleLength;
				}
				else
				{
					Console.WriteLine("Error: Mass must be greater than 0.");
				}
			}
		}
		public float BaseMass {
			get {
				return baseMass;
			}
			set {
				if (0 < value)
					baseMass = value;
				else
					Console.WriteLine("Error: Mass must be greater than 0.");
			}
		}
		public InvertedPendulum(float poleLength = 0.8f) {
			PoleLength = poleLength;
			Pole.Position = Joint.Position;
			Pole.Rotate = Joint.Rotate;
			Base.Position = Joint.Position;
			Base.SetPositionOffset(0, 0, -0.5f * Base.SizeZ);
			Joint.SetRotateOffset(0.5f * (float) Math.PI, 0, 0);
			Joint.Color.SetLightGray();
			Joint.Position[2] = Base.SizeZ;
		}
		public void SetBaseSize(float sizeX, float sizeY, float sizeZ) {
			Base.SetSize(sizeX, sizeY, sizeZ);
			Base.SetPositionOffset(0, 0, -0.5f * Base.SizeZ);
			Joint.Position[2] = Base.SizeZ;
		}
		public void Draw() {
			Pole.Draw();
			Joint.Draw();
			Base.Draw();
		}
		public void StartDynamics() {
			if (isDynamicsEnabled == false)
			{
				Parallel.RunEndless(CalcDynamics, (uint) (1000 * SamplingTime));
				isDynamicsEnabled = true;
			}
			else
			{
				Console.WriteLine("Warning: Dynamics is already enabled.");
			}
		}
		public void Reset() {
			Force = 0;
			Position = 0;
			Velocity = 0;
			Theta = 0;
			Omega = 0;
			isDynamicsEnabled = false;
		}
	}
	partial class MainWindow: Window {
		InvertedPendulum InvertedPendulum = new InvertedPendulum();
		float err2 = 0;
		float err = 0;
		float P = 0;
		float I = 0;
		float D = 0;
		
		void Agent() {
			float Kp = 10.0f;
			float Ki = 0.2f;
			float Kd = 0.2f;
			float dt = 0.05f;
			P = 0 + InvertedPendulum.Theta;
			I += P * dt;
			D = (P - err2) / dt;
			InvertedPendulum.Force = Kp * P + Ki * I + Kd * D;
			err2 = err;
			err = P;
		}
		void Setup() {
			InvertedPendulum.PoleLength = 0.8f;
			InvertedPendulum.PoleMass = 0.2f;
			InvertedPendulum.BaseMass = 0.2f;
			Button1.Content = "start";
			Button2.Content = "push";
			Button3.Content = "agent";
			Button4.Content = "reset";
			Button5.Content = "none";
		}
		void Loop() {

		}
		void Draw() {
			InvertedPendulum.Draw();
		}
		void Button1_Click(object sender, RoutedEventArgs e) {
			InvertedPendulum.StartDynamics();
		}
		void Button2_Click(object sender, RoutedEventArgs e) {
			InvertedPendulum.Theta += 0.01f;
		}
		void Button3_Click(object sender, RoutedEventArgs e) {
			Parallel.RunEndless(Agent, 50);
		}
		void Button4_Click(object sender, RoutedEventArgs e) {
			InvertedPendulum.Reset();
		}
		void Button5_Click(object sender, RoutedEventArgs e) {
			InvertedPendulum.Force = 0.01f;
		}
	}
}

以上。