Reply To: Preparations for GyroBot

Hi Anders,
Thanks, I grabbed this fixed line of code yesterday.
I have no progress in my GyroBoy implementation.
I can’t get it working and I have no idea what may be wrong.
I paste my code, maybe someone can look at the code or try run.

I used decimals(to be extremely close to original labview program), but initially there were doubles

Program.cs

using System.Threading;
using System.Threading.Tasks;
using MonoBrickFirmware.UserInput;

namespace GyroBoy
{
    internal class Program
    {
        private static void Main(string[] args)
        {
            ManualResetEvent terminateProgram = new ManualResetEvent(false);
            GyroBoy gyroBoy = new GyroBoy();
            ButtonEvents buts = new ButtonEvents();

            buts.EscapePressed += () =>
            {
                terminateProgram.Set();
            };

            Task.Factory.StartNew(() => gyroBoy.Run(terminateProgram));

            terminateProgram.WaitOne();
            gyroBoy.Stop();
        }
    }
}

GyroBoy.cs

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 decimal MOTOR_RATE = 0.08m;
        private const decimal MOTOR_POS = 0.08m;

        private const decimal GYRO_RATE = 0.7m;
        private const decimal GYRO_ANGLE = 12m;

        private decimal avgLoopTimer = 0.01m;

        private decimal gyroRate = 0;
        private decimal gyroAngle = 0;
        private decimal gyroOffset = 0;

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

        private decimal motorDelta3 = 0;
        private decimal motorDelta2 = 0;
        private decimal 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) > 99m) fallDownCntr++;
                else fallDownCntr = 0;
                if (fallDownCntr >= 50)
                {
                    LcdConsole.WriteLine("Falll down...");
                    resetEvent.Set();
                    break;
                }

                avgLoopTimer = 0.7m * avgLoopTimer + 0.3m * (decimal)(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 = (decimal)gyro.Read();

            gyroRate = gyroRawRead - gyroOffset;
            gyroOffset = 0.001m * gyroRawRead + (1 - 0.001m) * 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.75m * rateMotor) + (0.25m * (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 > 100m) motorPower = 100m;
            if (motorPower < -100m) motorPower = -100m;

            // !!! 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, 11 months ago by Jacek S.
• This reply was modified 10 years, 11 months ago by Jacek S.