cscの作法 その586

概要

cscの作法、調べてみた。
OpenRCF v2.8、見つけたので、windows11で、MSbuildしてみた。
練習問題やってみた。

練習問題

Lidarで、障害物を回避せよ。

写真

サンプルコード

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 rover = new Sphere(0.25f);
		LiDAR lidar = new LiDAR();
		Cuboid wall = new Cuboid();
		Sphere p1 = new Sphere(0.05f);
		float[] point1 = new float[3] {3.0f, 0, 0.2f};
		bool isMoved = false;
		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 = rover.Position[0];
			float y = rover.Position[1];
			float u0 = agent0.get_action(point[0], x);
			float u1 = agent1.get_action(point[1], y);
			rover.Position[0] += u0;
			rover.Position[1] += u1;
		}
		void Setup() {
			rover.Position[0] = -3.0f;
			rover.Position[2] = rover.Radius;
			p1.Position = point1;
			wall = new Cuboid(0.3f, 1.0f, 0.5f);
			wall.Position[2] = 0.25f;
			lidar.Position.Follow(rover.Position);
			lidar.Rotate.Follow(rover.Rotate);
			lidar.SetPositionOffset(0, 0, 0);
			lidar.LaserLength = 0.6f;
			float[][] pointCloud = lidar.PointCloud(wall);
			Button1.Content = "start";
			Button2.Content = "reset";
			Button3.Content = "none";
			Button4.Content = "none";
			Button5.Content = "none";
		}
		void Loop() {
			if (isMoved)
			{
				float[][] pointCloud = lidar.PointCloud(wall);
				if (pointCloud[63][0] < 0.59f)
				{
					rover.Position[1] += 1.2f;
					return;
				}
				Console.WriteLine(pointCloud[73][0]);
				float dis = getDistance(point1[0], point1[1], rover.Position[0], rover.Position[1]);
				if (dis < 0.3)
				{
					isMoved = false;
					return;
				}
				to(point1);
				if (wall.IsCollided(rover))
				{
					rover.Color.SetRed();
					isMoved = false;
				}
				else
					rover.Color.SetGray();
			}
		}
		void Draw() {
			rover.Draw();
			wall.Draw();
			p1.Draw();
			lidar.Draw();
		}
		void Button1_Click(object sender, RoutedEventArgs e) {
			isMoved = true;
		}
		void Button2_Click(object sender, RoutedEventArgs e) {
			rover.Position[0] = -3.0f;
			rover.Position[1] = 0;
			rover.Position[2] = rover.Radius;
			rover.Color.SetGray();
			float[][] pointCloud = lidar.PointCloud(wall);
			isMoved = false;
		}
		void Button3_Click(object sender, RoutedEventArgs e) {
		}
		void Button4_Click(object sender, RoutedEventArgs e) {
		}
		void Button5_Click(object sender, RoutedEventArgs e) {
		}
	}
}

以上。