<commit_before>﻿using System;
using System.Collections.Generic;
using System.Threading;
using System.Threading.Tasks;
using Lego.Ev3.Core;
using System.Drawing;
using AI_In_Robotics.Utils;

namespace AI_In_Robotics.Classes
{
    public class Motion
    {
        private readonly Brick _brick;
        private const uint MotorMoveTimeMs = 250;
        private const double MoveDistancePerCicle = 2.05;
        private const uint RotaionDegStep = 360 / 12;
        private readonly Queue<Action> Commands = new Queue<Action>();

        public Motion(Brick brick)
        {
            _brick = brick;
            _brick.BrickChanged += _brick_BrickChanged;

            _brick.Ports[Settings.gyroPort].SetMode(GyroscopeMode.Calibrate);
            Thread.Sleep(2000);
            _brick.Ports[Settings.gyroPort].SetMode(GyroscopeMode.Angle);
        }

        private double getGyro()
        {
            return _brick.Ports[Settings.gyroPort].SIValue;
        }

        public void motionTest()
        {
            //Console.WriteLine(getGyro());
            //PIDMove(10);
            PIDTurn(90);
            PIDTurn(90);
            PIDTurn(90);
            PIDTurn(90);
        }

        public void PIDMove(double moveDistanceCm)
        {
            //System.IO.StreamWriter file = new System.IO.StreamWriter("D:MotorPIDtest.txt");
            double moveCiclesCount = 0;
            double kp = 0.8;
            double Ki = 0.12;
            double Kd = 0.06;
            double gyroOffset = getGyro();
            double Tp = 0;
            if (moveDistanceCm > 0)
            {
                Tp = 25;
            }
            else
            {
                Tp = -25;
            }
            double integral = 0;
            double lastError = 0;
            double derivative = 0;

            while (moveCiclesCount < moveDistanceCm/MoveDistancePerCicle)
            {
                double gyroValue = getGyro();
                double error = gyroValue - gyroOffset;
                integral = integral + error;
                derivative = error - lastError;
                double turn = kp * error + Ki * integral + Kd * derivative;
                double powerRight = Tp - turn;
                double powerLeft = Tp + turn;

                moveMoters(powerRight, powerLeft);

                lastError = error;
                moveCiclesCount++;
            }
        }

        public void PIDTurn(double rotateDeg)
        {
            //System.IO.StreamWriter file = new System.IO.StreamWriter("D:MotorPIDtest.txt");
            double moveCiclesCount = 0;
            double kp = 0.5;
            double Ki = 0.05;
            double Kd = 0.1;
            double gyroOffset = getGyro() + rotateDeg;
            double Tp = 0;
            double integral = 0;
            double error = rotateDeg;
            double lastError = 0;
            double derivative = 0;

            while ((moveCiclesCount < 20) || (Math.Abs(error) > 1))
            {
                double gyroValue = getGyro();
                error = gyroValue - gyroOffset;
                integral = integral + error;
                derivative = error - lastError;
                double turn = kp * error + Ki * integral + Kd * derivative;
                double powerRight = Tp - turn;
                double powerLeft = Tp + turn;

                moveMoters(powerRight, powerLeft);

                lastError = error;
                moveCiclesCount++;
            }

            Console.WriteLine("Turn done");
        }

        private void moveMoters(double powerRight, double powerLeft)
        {
            if (powerRight >= 0)
            {
                _brick.DirectCommand.SetMotorPolarityAsync(Settings.rightMotorPort, Polarity.Forward);
                Thread.Sleep(2);
            }
            else
            {
                _brick.DirectCommand.SetMotorPolarityAsync(Settings.rightMotorPort, Polarity.Backward);
                Thread.Sleep(2);
                powerRight = powerRight * -1;
            }

            if (powerRight > 100)
            {
                powerRight = 100;
            }

            if (powerLeft >= 0)
            {
                _brick.DirectCommand.SetMotorPolarityAsync(Settings.leftMotorPort, Polarity.Forward);
                Thread.Sleep(2);
            }<commit_msg>Code added to motion class for error reading to repport<commit_after>﻿using System;
using System.Collections.Generic;
using System.Threading;
using System.Threading.Tasks;
using Lego.Ev3.Core;
using System.Drawing;
using AI_In_Robotics.Utils;

namespace AI_In_Robotics.Classes
{
    public class Motion
    {
        private readonly Brick _brick;
        private const uint MotorMoveTimeMs = 250;
        private const double MoveDistancePerCicle = 2.05;
        private const uint RotaionDegStep = 360 / 12;
        private readonly Queue<Action> Commands = new Queue<Action>();

        public Motion(Brick brick)
        {
            _brick = brick;
            _brick.BrickChanged += _brick_BrickChanged;

            _brick.Ports[Settings.gyroPort].SetMode(GyroscopeMode.Calibrate);
            Thread.Sleep(2000);
            _brick.Ports[Settings.gyroPort].SetMode(GyroscopeMode.Angle);
        }

        private double getGyro()
        {
            return _brick.Ports[Settings.gyroPort].SIValue;
        }

        public void motionTest()
        {
            //Console.WriteLine(getGyro());
            //PIDMove(100);
            PIDTurn(90);
            //PIDTurn(90);
            //PIDTurn(90);
            //PIDTurn(90);
        }

        public void PIDMove(double moveDistanceCm)
        {
            System.IO.StreamWriter fileError = new System.IO.StreamWriter("D:MotorPIDerror.txt");
            System.IO.StreamWriter fileRightPower = new System.IO.StreamWriter("D:MotorRightPower.txt");
            System.IO.StreamWriter fileLeftPower = new System.IO.StreamWriter("D:MotorLeftPower.txt");
            System.IO.StreamWriter fileTime = new System.IO.StreamWriter("D:MotorPIDtime.txt");
            double moveCiclesCount = 0;
            double kp = 1.44;
            double Ki = 0.487;
            double Kd = 1.064;
            double gyroTargetValue = getGyro();
            double movePower = 0;
            if (moveDistanceCm > 0)
            {
                movePower = 25;
            }
            else
            {
                movePower = -25;
            }
            double integral = 0;
            double lastError = 0;
            double derivative = 0;

            while (moveCiclesCount < moveDistanceCm/MoveDistancePerCicle)
            {
                double gyroValue = getGyro();
                double error = gyroValue - gyroTargetValue;
                integral = integral + error;
                derivative = error - lastError;
                double turnPower = kp * error + Ki * integral + Kd * derivative;
                double powerRight = movePower - turnPower;
                double powerLeft = movePower + turnPower;

                moveMoters(powerRight, powerLeft);

                fileError.WriteLine(error);
                fileRightPower.WriteLine(powerRight);
                fileLeftPower.WriteLine(powerLeft);
                fileTime.WriteLine(moveCiclesCount);
                lastError = error;
                moveCiclesCount++;
            }

            fileError.Close();
            fileRightPower.Close();
            fileLeftPower.Close();
            fileTime.Close();
        }

        public void PIDTurn(double rotateDeg)
        {
            System.IO.StreamWriter fileError = new System.IO.StreamWriter("D:MotorPIDerror.txt");
            System.IO.StreamWriter fileRightPower = new System.IO.StreamWriter("D:MotorRightPower.txt");
            System.IO.StreamWriter fileLeftPower = new System.IO.StreamWriter("D:MotorLeftPower.txt");
            System.IO.StreamWriter fileTime = new System.IO.StreamWriter("D:MotorPIDtime.txt");
            double moveCiclesCount = 0;
            double kp = 1.44;
            double Ki = 0.487;
            double Kd = 1.064;
            double gyroTargetValue = getGyro() + rotateDeg;
            double movePower = 0;
            double integral = 0;
            double error = rotateDeg;
            double lastError = 0;
            double derivative = 0;

            //while ((moveCiclesCount < 20) || (Math.Abs(error) > 1))
            while ((moveCiclesCount < 100))
            {
                double gyroValue = getGyro();
                error = gyroValue - gyroTargetValue;
                integral = integral + error;
                derivative = error - lastError;
                double turnPower = kp * error + Ki * integral + Kd * derivative;
                double powerRight = movePower - turnPower;
                double powerLeft = movePower + turnPower;

                moveMoters(powerRight, powerLeft);

                fileError.WriteLine(error);
                fileRightPower.WriteLine(powerRight);
                fileLeftPower.WriteLine(powerLeft);
                fileTime.WriteLine(moveCiclesCount);
                lastError = error;
                moveCiclesCount++;
            }

            fileError.Close();
            fileRightPower.Close();
            fileLeftPower.Close();
            fileTime.Close();
            Console.WriteLine("Turn done");
        }

        private void moveMoters(double powerRight, double powerLeft)
        {
            if (powerRight >= 0)
            {
                _brick.DirectCommand.SetMotorPolarityAsync(Settings.rightMotorPort, Polarity.Forward);
                Thread.Sleep(2);
            }
            else
            {
                _brick.DirectCommand.SetMotorPolarityAsync(Settings.rightMotorPort, Polarity.Backward);
                Thread.Sleep(2);
                powerRight = powerRight * -1;
            }

            if (powerRight > 100)
            {
                powerRight = 100;
            }

            if (powerLeft >= 0)
            {
                _brick.DirectCommand.SetMotorPolarityAsync(Settings.leftMotorPort, Polarity.Forward);
                Thread.Sleep(2);
            }
{'fraction_non_alphanumeric': 0.086841330585811, 'fraction_numerical': 0.015013246982631734, 'mean_word_length': 1.5146804835924006, 'pattern_counts': {'":': 0, '<': 12, '<?xml version=': 0, '>': 17, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5892340', 'n_tokens_mistral': 2922, 'n_tokens_neox': 2627, 'n_words': 568}