[Jacek S]

I don’t think this is an issue since the kernel module is thread safe. Have you seen this fail?

Yes I have seen text displayed in the same line many times, when I had a lot of threads.

(I think the problem is with scrollPos variable)
Jacek

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

[Jacek S]

Hi,

Few days ago I was trying to fix it on my own.

This is my version of pooling mechanism:

	    private bool started = false;
		private void PollFunction (Object source, ElapsedEventArgs e)
		{
			if (!Monitor.TryEnter (this)) {
				//cancel has the lock
				return;
			}
			//Do the polling
			if (IsRunning ()) {
			    start.Set ();
			    started = true;
			} 
			else 
			{
			    if (started)
			    {
			        stop.Set ();
			        started = false;
			    }
			}
			Monitor.Exit(this);
		}

	    protected void StartPooling()
	    {
                waitHandle.Reset();
                start.Set();
		stop.Set();
		started = false;
                stop.Reset();
                start.Reset();
	        timer.Start();
	    }

		protected WaitHandle WaitForMotorsToStartAndStop()
		{
			//Optimize the poll function to save this exstra thread
			(new Thread(() => {
				start.WaitOne();
				stop.WaitOne();
				timer.Stop();
				start.Reset();
				stop.Reset();
				waitHandle.Set();
		    })).Start();
			return waitHandle;
		}

And start pooling call in the speedprofile method:

		public WaitHandle SpeedProfile(sbyte speed, UInt32 rampUpSteps, UInt32 constantSpeedSteps, UInt32 rampDownSteps, bool brake)
		{
			output.SetPower (0);
                        StartPooling();
			output.SetStepSpeed (speed, rampUpSteps, constantSpeedSteps, rampDownSteps, brake);
			return WaitForMotorsToStartAndStop();
		}

I also noticed that LcdConsole.WriteLine method is not thread safe. There should be lock block:

		static public void WriteLine(string format, params Object[] arg)
		{
			if (cw == null)
				cw = new ConsoleWriter();
		    lock (cw)
		    {
		        cw.WriteLine(string.Format(format, arg));
		    }
		}

Jacek

[Jacek S]

Ok I will add. Thanks

[Jacek S]

Ok, great. I will try. Thanks

[Jacek S]

Hi,

I need RGB values to convert to HSV. This is the way that colors can be determined easily.
I have made modification to the firmware, and it is working for me. Now I can easily detect orange and other colors.

I added support for uart.mode4 to EV3ColorSensor.cs:

       
  public RGBColor ReadRGB()
  {
      if (uartMode != UARTMode.Mode4) Mode = ColorMode.RGB;
      byte[] rawBytes = ReadBytes(6);
      return new RGBColor(rawBytes[0], rawBytes[2], rawBytes[4]);
  }

[Jacek S]

Hi,

Any progress? I trying to solve the same problem using the ev3 sensor.
I know it is possible with the new lego firmware 1.06H. Sensor can be switched into RGB mode.

Question for Anders: Which version of lego firmware is the base for the monobrick firmware?

Thanks,
Jacek

[Jacek S]

Hi,
I will post my results, after I finish some basic functionality like driving forward, backward, turning and remote steering using tcp.

Jacek

[Jacek S]

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

[Jacek S]

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

Jacek

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

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

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

[Jacek S]

Hi,
Your program never ends because you must start main loop in separate thread and place wait event at the end of main thread.

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

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

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

            var myLogic = new MyLogic();

            Task.Factory.StartNew(() => myLogic.Run());

            terminateProgram.WaitOne();
        }
    }
}

Jacek

[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, 10 months ago by Jacek S.

[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

[Author]

Posts