Reply To: Preparations for GyroBot

[Jacek S]

Hi,

http://sdrv.ms/1c839kY

code:

using System;
using System.Collections.Generic;
using System.Threading;
using MonoBrickFirmware.Display;
using MonoBrickFirmware.Movement;
using MonoBrickFirmware.Sensors;

namespace GyroBoy
{
    public class GyroBoy
    {
        private GyroSensor gyro = null;
        private Motor leftMotor = null;
        private Motor rightMotor = null;
        private Lcd lcd = new Lcd();

        private const double MOTOR_RATE = 0.1;
        private const double MOTOR_POS = 0.07;

        private const double GYRO_RATE = 1.15;
        private const double GYRO_ANGLE = 7.5;

        private double avgLoopTimer = 0.02;

        private double gyroRate = 0;
        private double gyroAngle = 0;
        private double gyroOffset = 0;

        private double gyroRawRead = 0;
        
        private double motorPos = 0;
        private double motorAngle = 0;
        private double motorRate = 0;
        private double motorPower = 0;

        private double motorDelta3 = 0;
        private double motorDelta2 = 0;
        private double motorDelta1 = 0;

        private void Init()
        {
            gyro = new GyroSensor(SensorPort.In1, GyroMode.AngularVelocity);
            LcdConsole.WriteLine("GYRO init... done");

            leftMotor = new Motor(MotorPort.OutD); leftMotor.Off();
            rightMotor = new Motor(MotorPort.OutA); rightMotor.Off();
            LcdConsole.WriteLine("MOTORS init... done");
        }

        private void Reset()
        {
            gyro.Reset();
            LcdConsole.WriteLine("GYRO reset... done");
            int gyroRate = 0;
            int gyroReads = 0;
            //reset gyro to eliminate drift effect.
            while (true)
            {
                Thread.Sleep(10);
                gyroRate += gyro.Read();
                if (++gyroReads == 200)
                {
                    if (gyroRate > 0)
                    {
                        gyro.Reset();
                        LcdConsole.WriteLine("GYRO drift reset... done");
                    }
                    else break;
                    gyroRate = 0;
                    gyroReads = 0;
                }
            }

            //in some examples i found initial offset calculation, but i dont think this is needed
            //CalcGyroOffset();

            leftMotor.On(0);
            rightMotor.On(0);

            leftMotor.ResetTacho();
            rightMotor.ResetTacho();
            LcdConsole.WriteLine("MOTORS reset... done");
        }

        public void Stop()
        {
            if (leftMotor != null && rightMotor != null)
            {
                leftMotor.SetPower(0);
                rightMotor.SetPower(0);

                leftMotor.Off();
                rightMotor.Off();
            }
        }

        public void Run(ManualResetEvent resetEvent)
        {
            LcdConsole.WriteLine("Start GyroBoy");
            Init();
            Reset();
            Thread.Sleep(1500);
            LcdConsole.WriteLine("Start balancing...");

            var fallDownCntr = 0;

            lcd.Clear();

            while (true)
            {
                var startTime = DateTime.Now;

                Balance();
                Thread.Sleep(8);

                PrintDebug();

                if (Math.Abs(motorPower) > 99) fallDownCntr++;
                else fallDownCntr = 0;
                if (fallDownCntr >= 50)
                {
                    LcdConsole.WriteLine("Falll down...");
                    resetEvent.Set();
                    break;
                }

                avgLoopTimer = 0.7 * avgLoopTimer + 0.3 *(double)(DateTime.Now - startTime).TotalSeconds;
            }
        }

        private void PrintDebug()
        {
            lcd.Clear();
            lcd.WriteText(Font.MediumFont, new Point(5, 15), "GYRO ANGLE: " + gyroAngle.ToString("0.0000"), true);
            lcd.WriteText(Font.MediumFont, new Point(5, 35), "GYRO RATE: " + gyroRate.ToString("0.0000"), true);
            lcd.WriteText(Font.MediumFont, new Point(5, 55), "LOOP TIME: " + avgLoopTimer.ToString("0.0000"), true);
            lcd.Update();
        }

        private void Balance()
        {
            gyroRawRead = (double)gyro.Read();

            gyroRate = gyroRawRead - gyroOffset;
            gyroOffset = 0.001 * gyroRawRead + (1 - 0.001) * gyroOffset;
            gyroAngle += avgLoopTimer * gyroRate;

            var motorLastPos = motorPos;
            motorPos = leftMotor.GetTachoCount() + rightMotor.GetTachoCount();

            var motorDelta = motorPos - motorLastPos;
            motorRate = (motorDelta + motorDelta1 + motorDelta2 + motorDelta3) / (4 * avgLoopTimer);
            //second option for rate calculation I dont noticed any differences
            //rateMotor = (0.75 * rateMotor) + (0.25 * (deltaMotor / avgLoopTimer));
            motorAngle += motorDelta;

            motorDelta3 = motorDelta2; motorDelta2 = motorDelta1; motorDelta1 = motorDelta;

            motorPower = gyroRate * GYRO_RATE
                + gyroAngle * GYRO_ANGLE
                + motorRate * MOTOR_RATE
                + motorAngle * MOTOR_POS;

            if (motorPower > 100) motorPower = 100;
            if (motorPower < -100) motorPower = -100;

            // !!! this is modified version of setPower that accepts sbyte. You need to use Reverse property and Abs(motorPower)
            leftMotor.SetPower((sbyte)(motorPower));
            rightMotor.SetPower((sbyte)(motorPower));
        }
    }
}

• This reply was modified 10 years, 2 months ago by Jacek S.
• This reply was modified 10 years, 2 months ago by Jacek S.
• This reply was modified 10 years, 2 months ago by Jacek S.
• This reply was modified 10 years, 2 months ago by Jacek S.