Preparations for GyroBot

[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.

[Anders Søborg]

Hi Jacek

There are a few things that I have noticed about your code.

You are using decimals – why? I can’t tell for sure but on this hardware it might not be the best approach if you want fast calculations.

You are using a thread.sleep in your control loop – On Windows the minimum sleep time is something like 20-30 ms – what it is on the EV3′s ARM processor I am not aware off but you could do some measurements to find out. But Thread.Sleep is not guaranteed to sleep for exactly the specified amount of time. That leads me to the next question.” What performance are you getting”? Are your control loop running a a constant rate? Is it running at the rate you except? All the constants what you have are not correct if you run with a different sample time that the “original” LAB view program

Anders

[Jacek S]

Hi Anders,
I changed to decimal because LAB view numeric is decimal value. I don’t notice any performance problem with this simple calculations.
In lab view program loop takes 30ms in my takes only 1-2ms(without sleep). I have tried many configurations of sleep values and constants from sleep(28) (to be close to LABview program )
to sleep(8) like in lejos programs and hitechnic nxc program.

Jacek

[Anders Søborg]

Hi Jacek

What about gyro sensor calibration? or gyro sensor offset calculation? Could you describe the result or maybe record a video?

Anders

[Jacek S]

Hi,
I don’t know how to calibrate sensor. In lejos code I found they are using mode=4 for hardware calibration
EV3GyroSensor.java RESETMODE = 4 and SWITCHDELAY = 200ms

  /**
   * Hardware calibration of the Gyro sensor. <br>
   * The sensor should be motionless during calibration.
   */
  public void reset() {
    // TODO: Test if angle is reset to zero due to calibration
    // TODO: find out how to get out of calibration mode
    switchMode(RESETMODE, SWITCHDELAY);
  }

I think is no need to calculate offset. In labview and other examples they are using const values. In this case is 0.001

I’ll make a video soon.

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

[Anders Søborg]

Looking forward to the video

Anders

[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.

[Anders Søborg]

Hi

Great to see what is actually going on…when I see it running it seems that your response-time is way to slow… this can be cause by your control loop running at a wrong rate or the controller’s repose is way to slow. So you need to tune some of the parameters.

Another option could be that the set-point you are trying to reach is way off – the sensor input could be wrong.

In general getting a robot to balance upright is difficult. You are trying to make a highly unstable system – stable. I would say that your approach of trying to copy a Lab-View program that is running with a different sample rate and has a lot of magic numbers is doomed to go wrong. If I where you I would start over and implement a PID control loop so you know what is going on.

But before that I would start by verifying that the sensor input is correct – if this isn’t the case you will never get your robot working. Make a program where you manually move the robot to a upright position and make it fall – record those data with a fixed sample-rate and plot the curve – what are you seeing? Does the sensor input look alright?

Hope to hear from you soon.

Anders

[Jacek S]

Thank You for your time. You are my last motivation to get this working:)

You have confirmed my conclusions. Even I change parameters ( I checked many combinations) i was able to increase reaction power, but i think the response was always delayed.

I’m newbie to lego mindstorms and I was thinking this will be easy excercise to port this program. I have 3 examples of working seagway bot for 3 diffrent enviroments. In all of them the calculations was the same (very similar). In two of them parameters are the same. I know is hard to make this very unstable system – stable, but my plan was to use working solutions with alghorithms that are ready to use.
I have spent a lot of time to analyze all the magic but this not working as expected.

Now I’m very determined to get this working. In the next week I will record all parameters to file, and create some plots.

Best Regards
Jacek

[Anders Søborg]

Hi again

I really really want you to succeed as well…. Ok so it seems that the control loop should be working. There are from my point of view two possible causes why this is not working. Sensor input being wrong or output power to the motor is wrong (you are getting another output power that you except) – or it could even be a combination of the two.

So I guess a plot of the sensor input and also a motor speed versus the input power would also be a good thing (A open loop step-response of the motor at different powers) just to verify that everything looks OK.

Anders

[Jacek S]

Hi Anders,
I have uploaded data and plots.
http://sdrv.ms/1c839kY gyroboy.xslx

The reads from gyro looks like random numbers in some parts, but I’m not sure maybe this is good behaviour.

I will make more tests tomorrow.

Jacek

[Jacek S]

Hi Anders,
Do you think the latest uart bug You fixed may have caused my gyrobot problems?

Jacek

[Anders Søborg]

Hi Jacek

Yes but it is not possible to use the latest from the Git repository as LCD won’t work since it is for a new image that we are testing… you will have to wait a week or two

Anders

[Jacek S]

Hi Anders,
I get corrected methods only. Now is working!!!
Thanks
Jacek

[Anders Søborg]

Hi

This is amazing – could you please post some video and pictures.

Anders

[Author]

Posts