public static void Main()
{
// Motors
Motor motorA = new Motor(Pins.GPIO_PIN_D10, Pins.GPIO_PIN_D12, Pins.GPIO_PIN_D11, Pins.GPIO_PIN_D4, Pins.GPIO_PIN_D5);
Motor motorB = new Motor(Pins.GPIO_PIN_D9, Pins.GPIO_PIN_D7, Pins.GPIO_PIN_D8, Pins.GPIO_PIN_D6, Pins.GPIO_PIN_D13);
motorA.Reversed = true;
motorB.Reversed = false;
// Rangers
InfraredDistanceSensor irFront = new InfraredDistanceSensor(Pins.GPIO_PIN_A4);
InfraredDistanceSensor irBack = new InfraredDistanceSensor(Pins.GPIO_PIN_A5);
// Robot and its controller
StasisRobot bot = new StasisRobot(motorA, motorB, irFront, irBack);
StasisController controller = new StasisController(bot);
Timer thinkTimer = new Timer(new TimerCallback(delegate
{
controller.Think();
}), null, 0, 15);
// Keep alive
Thread.Sleep(Timeout.Infinite);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="bot"></param>
public StasisController(StasisRobot bot,
double displacementSetPoint = 0.0,
double angleSetPoint = 90.5, // slightly tilted; should calibrate at the beginning of each run -BZ (4/12/12)
double velocitySetPoint = 0.0,
double angularVelocitySetPoint = 0.0,
double displacementProportionalValue = 0,
double velocityProportionalValue = 0,
double angleProportionalValue = 7.5,
double angleIntegrationValue = 1.25,
double angleDerivativeValue = 55.0,
double angularVelocityProportionalValue = 0,
int integratorWindow = 4)
{
this.Robot = bot;
// PID
this.DisplacementPID = new PID(setPoint: displacementSetPoint, proportionalConstant: displacementProportionalValue, integratorWindowSize: integratorWindow);
this.VelocityPID = new PID(setPoint: velocitySetPoint, proportionalConstant: velocityProportionalValue, integratorWindowSize: integratorWindow);
this.AnglePID = new PID(setPoint: angleSetPoint, proportionalConstant: angleProportionalValue, integrationConstant: angleIntegrationValue, derivativeConstant: angleDerivativeValue, integratorWindowSize: integratorWindow);
this.AngularVelocityPID = new PID(setPoint: angularVelocitySetPoint, proportionalConstant: angularVelocityProportionalValue, integratorWindowSize: integratorWindow);
this.wifiMonitor.MessageReceived += new WiFiMonitor.MessageReceivedEventHandler(WifiMonitor_MessageReceived);
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="bot"></param>
public StasisController(StasisRobot bot,
double displacementSetPoint = 0.0,
double angleSetPoint = 90.5, // slightly tilted; should calibrate at the beginning of each run -BZ (4/12/12)
double velocitySetPoint = 0.0,
double angularVelocitySetPoint = 0.0,
double displacementProportionalValue = 0,
double velocityProportionalValue = 0,
double angleProportionalValue = 7.5,
double angleIntegrationValue = 1.25,
double angleDerivativeValue = 55.0,
double angularVelocityProportionalValue = 0,
int integratorWindow = 4)
{
this.Robot = bot;
// PID
this.DisplacementPID = new PID(setPoint: displacementSetPoint, proportionalConstant: displacementProportionalValue, integratorWindowSize: integratorWindow);
this.VelocityPID = new PID(setPoint: velocitySetPoint, proportionalConstant: velocityProportionalValue, integratorWindowSize: integratorWindow);
this.AnglePID = new PID(setPoint: angleSetPoint, proportionalConstant: angleProportionalValue, integrationConstant: angleIntegrationValue, derivativeConstant:angleDerivativeValue, integratorWindowSize: integratorWindow);
this.AngularVelocityPID = new PID(setPoint: angularVelocitySetPoint, proportionalConstant: angularVelocityProportionalValue, integratorWindowSize: integratorWindow);
this.wifiMonitor.MessageReceived += new WiFiMonitor.MessageReceivedEventHandler(WifiMonitor_MessageReceived);
}
public static void Main()
{
// Motors
Motor motorA = new Motor(Pins.GPIO_PIN_D10, Pins.GPIO_PIN_D12, Pins.GPIO_PIN_D11, Pins.GPIO_PIN_D4, Pins.GPIO_PIN_D5);
Motor motorB = new Motor(Pins.GPIO_PIN_D9, Pins.GPIO_PIN_D7, Pins.GPIO_PIN_D8, Pins.GPIO_PIN_D6, Pins.GPIO_PIN_D13);
motorA.Reversed = true;
motorB.Reversed = false;
// Rangers
InfraredDistanceSensor irFront = new InfraredDistanceSensor(Pins.GPIO_PIN_A4);
InfraredDistanceSensor irBack = new InfraredDistanceSensor(Pins.GPIO_PIN_A5);
// Robot and its controller
StasisRobot bot = new StasisRobot(motorA, motorB, irFront, irBack);
StasisController controller = new StasisController(bot);
Timer thinkTimer = new Timer(new TimerCallback(delegate
{
controller.Think();
}), null, 0, 15);
// Keep alive
Thread.Sleep(Timeout.Infinite);
}
