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));
}
}
}
Follow