[Jacek S]

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

[Jacek S]

Hi Anders,
In all code examples I found setpower equivalent is used(in lejos segway.java and hitec nxc example, and labview ev3 program) I dont know why.

In lejos source code i found that power param for setpower is byte(in java is signed type) I changed this in monobrickfirmware to sbyte, now i dont need to change polarization(Reverse attr).

Now I’m analyzing lejos source code to find differences. And I think they are using some filters for gyro.
Maybe this is a problem.

Jacek

[Jacek S]

You don’t need to decompile. You can grab sources from github. There is a visual studio solution with several examples.

[Jacek S]

Hi Anders,
I mean labview block. I’m trying to translate block sequence from original program to c# code.
I spend many hours during weekend but still with no success. Robot is far from balancing.
Still don’t know where is diffrence. Two points where I’m not sure is gyro sensor rate read and “unregulated motor”.
“Unregulated motor” in my program is sequence: motor.On() (if power < 0) motor.Reverse and motor.SetPower(power).
The oryginal program is very tricky and depends strongly on enviroment and even on battery level.

Fragment of my program(balance method):

        private void Balance()
        {

            var lastSumMotor = sumMotor;
            sumMotor = leftMotor.GetTachoCount() + rightMotor.GetTachoCount();

            var diffMotor = (sumMotor - lastSumMotor);
            rateMotor = 0.75 * rateMotor + 0.25 * (diffMotor / avgLoopTimer);
            angleMotor = moveMotor * avgLoopTimer + angleMotor + diffMotor;

            var gyroRead = (double)gyro.Read();
            rateGyro = gyroRead - offsetGyro;
            offsetGyro = 0.001 * gyroRead + 0.999 * offsetGyro;
            angleGyro = angleGyro + avgLoopTimer * rateGyro;

            motorPower = rateGyro * GYRO_RATE + angleGyro * GYRO_ANGLE + rateMotor * MOTOR_RATE
                + angleMotor * MOTOR_POS;

            if (motorPower < 0)
            {
                motorPower *= -1d;
                if (!leftMotor.Reverse) rightMotor.Reverse = leftMotor.Reverse = true;
            }
            else
            {
                if (leftMotor.Reverse) rightMotor.Reverse = leftMotor.Reverse = false;
            }

            if (motorPower > 100d) motorPower = 100d;

            leftMotor.SetPower((byte)motorPower);
            rightMotor.SetPower((byte)motorPower);
        }

Jacek

[Jacek S]

Hi,
I did some initial test of my gyro boy.
And I encountered some problems.
First and least important is gyro sensor only works in port1.
Second is that I don’t know how to use motor methods to simulate “unregulated motor” block.
SetPower do not start motor, and I was using sequence motor.On(0) and motor.SetPower(power).
I’m not sure is equivalent. After that the motor start rotating, but when i set it to off and quit program,
then start program again motors starting after new Motor(…) constructor,
before i set them to on. I don’t know how to avoid this.
The next thing is that the gyro sensor doesn’t have “rate and angle” mode
I have read that swithching to this mode is used to remove drift effect.
Last thing. Please confirm getTacho from motor is angle in degrees.

Jacek

[Jacek S]

Hi Marta,
There is a guide with c# sample http://www.monobrick.dk/guides/how-to-establish-a-wifi-connection-with-the-ev3-brick/

BR
Jacek

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

[Jacek S]

Hi Martin,
I’m interested in writing GyroBoy program too. Now I’m only waiting for my wifi dongle.
I have read your post, and I think there are simplest approaches to do this program.
Maybe I’m wrong, but you are trying to think like in real-time os, but this is linux it is not real time.
You can’t specify timers and expect results in desired time. My approach is to think that is lego toy
not high precision robot. Program needs some simplifications, thread prioritization and other advanced thread managment functions are not needed.
Oryginal program is not ideal, I have found some bugs and glitches.

I don’t analyze all the tricky gyro stuff from oryginal program yet, but i have some initial
plan for my program:
– tasks with infinite loops for sensor sampling. Idle time (sleep) will determine sample rate.
– main thread with task handling loop
– priority queue for message handling.

I think I accomplish project in two weeks, then will share results/program with monobrick community.
PS
Sorry for my poor english.

Jacek

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

[Jacek S]

Hi,
Do I need to buy license for xamarin studio? I think xamarin free starter limitation to 64kb IL size is not sufficient (monobrick library have more than 100kb). Are there any alternatives to use monobrick library for android without buying xamarin license?

Jacek

[Author]

Posts