cscの作法 その583

概要

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

練習問題

ドローンを、8文字飛行させよ。

サンプルコード

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


namespace OpenRCF
{
	partial class MainWindow: Window {
		Sphere Drone = new Sphere(0.25f);
		Sphere p1 = new Sphere(0.05f);
		Sphere p2 = new Sphere(0.05f);
		Sphere p3 = new Sphere(0.05f);
		Sphere p4 = new Sphere(0.05f);
		float[] point1 = new float[3] {0, -1.0f, 1.0f};
		float[] point2 = new float[3] {0, -1.0f, 3.0f};
		float[] point3 = new float[3] {0, 2.5f, 1.0f};
		float[] point4 = new float[3] {0, 2.5f, 3.0f};
		bool isMoved = false;
		int state = 4;
		class Agent {
			float err2 = 0;
			float err = 0;
			float P = 0;
			float I = 0;
			float D = 0;
			public float get_action(float setPoint, float input) {
				float Kp = 0.02f;
				float Ki = 0.002f;
				float Kd = 0.002f;
				float dt = 0.05f;
				this.P = setPoint - input;
				this.I += this.P * dt;
				this.D = (this.P - this.err2) / dt;
				float u = Kp * this.P + Ki * this.I + Kd * this.D;
				this.err2 = this.err;
				this.err = this.P;
				return u;
			}
		}
		float getDistance(float x, float y, float x2, float y2) {
			float distance = (float) Math.Sqrt((x2 - x) * (x2 - x) + (y2 - y) * (y2 - y));
			return distance;
		}
		Agent agent0 = new Agent();
		Agent agent1 = new Agent();
		void to(float[] point) {
			float x = Drone.Position[1];
			float y = Drone.Position[2];
			float u0 = agent0.get_action(point[1], x);
			float u1 = agent1.get_action(point[2], y);
			//Console.WriteLine(Drone.Position[0]);
			Drone.Position[1] += u0;
			Drone.Position[2] += u1;
		}
		void Setup() {
			Drone.Position[2] = Drone.Radius;
			p1.Position = point1;
			p2.Position = point2;
			p3.Position = point3;
			p4.Position = point4;
			Button1.Content = "start";
			Button2.Content = "stop";
			Button3.Content = "none";
			Button4.Content = "none";
			Button5.Content = "none";
		}
		void Loop() {
			if (isMoved)
			{
				float dis;
				switch (state)
				{
				case 4:
					dis = getDistance(point4[1], point4[2], Drone.Position[1], Drone.Position[2]);
					if (dis < 0.3)
					{
						state = 3;
						break;
					}
					Console.WriteLine(4);
					to(point4);
				break;
				case 3:
					dis = getDistance(point3[1], point3[2], Drone.Position[1], Drone.Position[2]);
					if (dis < 0.3)
					{
						state = 2;
						break;
					}
					Console.WriteLine(3);
					to(point3);
				break;
				case 2:
					dis = getDistance(point2[1], point2[2], Drone.Position[1], Drone.Position[2]);
					if (dis < 0.3)
					{
						state = 1;
						break;
					}
					Console.WriteLine(2);
					to(point2);
				break;
				case 1:
					dis = getDistance(point1[1], point1[2], Drone.Position[1], Drone.Position[2]);
					if (dis < 0.3)
					{
						state = 4;
						break;
					}
					Console.WriteLine(1);
					to(point1);
				break;
				}
			}
		}
		void Draw() {
			Drone.Draw();
			p1.Draw();
			p2.Draw();
			p3.Draw();
			p4.Draw();
		}
		void Button1_Click(object sender, RoutedEventArgs e) {
			isMoved = true;
		}
		void Button2_Click(object sender, RoutedEventArgs e) {
			Drone.Position[2] = Drone.Radius;
			isMoved = false;
		}
		void Button3_Click(object sender, RoutedEventArgs e) {
		}
		void Button4_Click(object sender, RoutedEventArgs e) {
		}
		void Button5_Click(object sender, RoutedEventArgs e) {
		}
	}
}

以上。