Home→Forums→MonoBrick EV3 Firmware→Preparations for GyroBot→Reply To: Preparations for GyroBot
February 6, 2014 at 21:52
#3944

Jacek S
Participant
Hi,
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));
}
}
}
Follow