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