Tsukurobo_Arm

#include "OpenSivSerial.hpp"

struct Arm
{
	double l1 = 150.0;
	double l2 = 100.0;
	Vec2 pos = Vec2(200.0, 200.0);
	Vec2 delta = Vec2(10.0, 0.0);

	void draw(double a1, double a2)
	{
		auto p1 = pos;
		auto p2 = p1.movedBy(Cos(a1) * l1, -Sin(a1) * l1);
		auto p3 = p2.movedBy(10.0, 0.0);
		auto p4 = p3.movedBy(Cos(a2) * l2, Sin(a2) * l2);
		auto p5 = p4.movedBy(0.0, 30.0);

		Line(p1, p2).draw(4.0, ColorF(Palette::Green, 0.5));
		RectF(-10, -10, 50, 30).movedBy(p2).draw(ColorF(Palette::Blue, 0.2));
		Line(p3, p4).draw(4.0, ColorF(Palette::Red, 0.5));
		RectF(-30, -10, 30, 10).movedBy(p5).draw(ColorF(Palette::Blue, 0.2));
	}

	Vec2 armPos(double a1, double a2)
	{
		auto p1 = pos;
		auto p2 = p1.movedBy(Cos(a1) * l1, -Sin(a1) * l1);
		auto p3 = p2.movedBy(10.0, 0.0);
		auto p4 = p3.movedBy(Cos(a2) * l2, Sin(a2) * l2);
		auto p5 = p4.movedBy(0.0, 30.0);

		return p5;
	}
};

void Main()
{
	Serial serial;

	serial.setup(3, 115200);

	auto t = Transformer2D(Mat3x2::Scale(1.5), true);
	Arm arm;

	while (System::Update())
	{
		ClearPrint();

		double s = 0.0;
		double aveAng1 = 0.0;
		double aveAng2 = 0.0;


		for (double a = 0.0; a < 90.0; a += 3.0)
		{
			for (double b = 0.0; b < 180.0; b += 3.0)
			{
				Line(arm.armPos(ToRadians(a), ToRadians(b)), arm.armPos(ToRadians(a), ToRadians(b + 3.0))).draw(0.5, ColorF(Palette::Yellow, 0.5));
				Line(arm.armPos(ToRadians(a), ToRadians(b)), arm.armPos(ToRadians(a + 3.0), ToRadians(b))).draw(0.5, ColorF(Palette::Yellow, 0.5));
			}
		}

		for (double a = 0.0; a < 90.0; a += 3.0)
		{
			for (double b = 0.0; b < 180.0; b += 3.0)
			{
				auto p = arm.armPos(ToRadians(a), ToRadians(b));
				auto f = 1.0 - Min(Cursor::PosF().distanceFrom(p) * 0.03, 1.0);

				s += f;
				aveAng1 += f * ToRadians(a);
				aveAng2 += f * ToRadians(b);

				Circle(p, 2.0).draw(ColorF(Math::Lerp(Palette::Yellow, Palette::Red, f), 0.5));

			}
		}

		/*
		for (double a = 0.0; a < 90.0; a += 3.0)
		{
			for (double b = 0.0; b < 180.0; b += 3.0)
			{
				arm.ang1 = ToRadians(a);
				arm.ang2 = ToRadians(b);
				Circle(arm.armPos(), 1).draw(Palette::Red);
			}
		}

		Rect(90, 180).draw(ColorF(0.5, 0.5));
		*/
		std::string str2 = Unicode::Narrow(U"move " + ToString(double(KeyUp.pressed() ? 0.1 : (KeyDown.pressed() ? -0.1 : 0.0))) + U" " + ToString(double(KeyRight.pressed() ? 0.1 : (KeyLeft.pressed() ? -0.1 : 0.0))) + U" 0!");
		std::string str("LT!");
		if (KeyEnter.down()) serial.write(str.c_str(), str.size());
		if (KeySpace.down())
		{
			for (int i = 0; i < str2.size(); ++i)
			{
				serial.write<char>(str2[i]);

				//for(int j = 0; j < 4; j++)
				//System::Update();
			}

		}

		static double a1 = 1.0;
		static double a2 = 1.0;

		if (s)
		{
			a1 = Math::Lerp(a1, aveAng1 / s, 0.2);
			a2 = Math::Lerp(a2, aveAng2 / s, 0.2);

			Circle(arm.armPos(a1, a2), 3).draw(Palette::Red);
			arm.draw(a1, a2);

			Print << a1 << U" " << a2;
		}

		String m;
		auto srv = U"a " + ToString((int(ToDegrees(a2)) + 60) * 10) + U" " + ToString((int(ToDegrees(a1)) + 150) * 10) + U" " + ToString(KeyUp.pressed() ? 1500 : 800) + U" " + ToString(KeyUp.pressed() ? 1500 : 2200) + U"!";

		Print << srv;
		Print << m;

		static int g = 0;
		if (++g == 10)
		{
			g = 0;

			serial.write(srv);
		}
		if (g == 5)
		{
			double th = 0;
			if (KeyNum1.pressed()) th = Math::QuarterPi * 0;
			else if (KeyNum2.pressed()) th = Math::QuarterPi * 1;
			else if (KeyNum3.pressed()) th = Math::QuarterPi * 2;
			else if (KeyNum6.pressed()) th = Math::QuarterPi * 3;
			else if (KeyNum9.pressed()) th = Math::QuarterPi * 4;
			else if (KeyNum8.pressed()) th = Math::QuarterPi * 5;
			else if (KeyNum7.pressed()) th = Math::QuarterPi * 6;
			else if (KeyNum4.pressed()) th = Math::QuarterPi * 7;
			else th = -1;

			if (th >= 0) m = U"move " + ToString(Cos(th) * 0.1) + U" " + ToString(Sin(th) * 0.1) + U" 0!";
			else m = U"move 0 0 0!";

			serial.write(m);
		}
	}
	serial.close();
}