TimeProportionedPidController(float windowSize, IOnOff relay, TimeSpan samplingRate,
Func <float> readProcess, Func <float> readOutput, Func <float> readSetPoint,
float proportionalGain, float integralGain, float derivativeGain,
ControllerDirection controllerDirection = ControllerDirection.Direct,
ControllerMode controllerMode = ControllerMode.Automatic)
{
_windowSize = windowSize;
_relay = relay;
if (readProcess == null)
{
throw new ArgumentNullException(nameof(readProcess), "Read process must not be null.");
}
if (readOutput == null)
{
throw new ArgumentNullException(nameof(readOutput), "Read output must not be null.");
}
if (readSetPoint == null)
{
throw new ArgumentNullException(nameof(readSetPoint), "Read set-point must not be null.");
}
SamplingRate = samplingRate;
SetOutputLimits(0, _windowSize);
_readProcess = readProcess;
_readOutput = readOutput;
_writeOutput = writeOutput;
_readSetPoint = readSetPoint;
ProportionalGain = proportionalGain;
IntegralGain = integralGain;
DerivativeGain = derivativeGain;
ControllerDirection = controllerDirection;
ControllerMode = controllerMode;
}
/// <summary>
/// Instantiates a new PID controller class. The <see cref="Run" /> method must be called to trigger computation.
/// </summary>
/// <param name="samplingRate">
/// The rate at which the algorithm will compute a new output once <see cref="Run" /> is
/// called.
/// </param>
/// <param name="outputMinimum">The minimum value the output can be written to. The output influences the process value.</param>
/// <param name="outputMaximum">The maximum value the output can be written to. The output influences the process value.</param>
/// <param name="readProcess">
/// A function to read the process value. The process value is what you are attempting to
/// control.
/// </param>
/// <param name="readOutput">A function to read the output. The output influences the process value.</param>
/// <param name="writeOutput">A function to write to the output. The output influences the process value.</param>
/// <param name="readSetPoint">A function to read the target set-point for the process.</param>
/// <param name="proportionalGain">The proportional gain to use.</param>
/// <param name="integralGain">The integral gain to use.</param>
/// <param name="derivativeGain">The derivative gain to use.</param>
/// <param name="controllerDirection">The direction to control the output if you want the process to increase in value..</param>
/// <param name="controllerMode">The mode to start out the PID controller in.</param>
/// <exception cref="ArgumentNullException">
/// Thrown when <see cref="readProcess" /> or <see cref="readOutput" /> or <see cref="writeOutput" /> or
/// <see cref="readSetPoint" /> are null.
/// </exception>
public PidController(TimeSpan samplingRate, float outputMinimum, float outputMaximum, Func <float> readProcess,
Func <float> readOutput,
Action <float> writeOutput, Func <float> readSetPoint, float proportionalGain, float integralGain,
float derivativeGain, ControllerDirection controllerDirection, ControllerMode controllerMode)
{
if (readProcess == null)
{
throw new ArgumentNullException(nameof(readProcess), "Read process must not be null.");
}
if (readOutput == null)
{
throw new ArgumentNullException(nameof(readOutput), "Read output must not be null.");
}
if (readSetPoint == null)
{
throw new ArgumentNullException(nameof(readSetPoint), "Read set-point must not be null.");
}
if (writeOutput == null)
{
throw new ArgumentNullException(nameof(writeOutput), "Write output must not be null.");
}
SamplingRate = samplingRate;
SetOutputLimits(outputMinimum, outputMaximum);
_readProcess = readProcess;
_readOutput = readOutput;
_writeOutput = writeOutput;
_readSetPoint = readSetPoint;
ProportionalGain = proportionalGain;
IntegralGain = integralGain;
DerivativeGain = derivativeGain;
ControllerDirection = controllerDirection;
ControllerMode = controllerMode;
}
public FormFillingContracts(ControllerDirection serviceD, ControllerClient serviceC, ControllerContractClient serviceCC,
ContractAgent serviceCA, ControllerService serviceS, ControllerPrinting serviceP, ControllerValidation validation)
{
InitializeComponent();
this.serviceD = serviceD;
this.serviceCC = serviceCC;
this.serviceC = serviceC;
this.serviceS = serviceS;
this.serviceCA = serviceCA;
this.validation = validation;
this.serviceP = serviceP;
}
static void GamepadToPancakePrinter(ControllerDirection dir, int magnitude)
{
switch (dir)
{
case ControllerDirection.Down:
pancakeDrive.SetY(magnitude * -1);
pancakeDrive.SetX(0);
break;
case ControllerDirection.Up:
pancakeDrive.SetY(magnitude);
pancakeDrive.SetX(0);
break;
case ControllerDirection.Left:
pancakeDrive.SetX(magnitude * -1);
pancakeDrive.SetY(0);
break;
case ControllerDirection.Right:
pancakeDrive.SetX(magnitude);
pancakeDrive.SetY(0);
break;
case ControllerDirection.DownLeft:
pancakeDrive.SetX(magnitude * -1);
pancakeDrive.SetY(magnitude * -1);
break;
case ControllerDirection.DownRight:
pancakeDrive.SetX(magnitude);
pancakeDrive.SetY(magnitude * -1);
break;
case ControllerDirection.UpLeft:
pancakeDrive.SetX(magnitude * -1);
pancakeDrive.SetY(magnitude);
break;
case ControllerDirection.UpRight:
pancakeDrive.SetX(magnitude);
pancakeDrive.SetY(magnitude);
break;
default:
pancakeDrive.SetX(0);
pancakeDrive.SetY(0);
break;
}
}
static void XBoxToRobotDirection(ControllerDirection dir, int magnitude)
{
switch (dir)
{
case ControllerDirection.Down: Movements.Walk("backward"); break;
case ControllerDirection.Up: Movements.Walk("forward"); break;
case ControllerDirection.Left: Movements.Turn("left"); break;
case ControllerDirection.Right: Movements.Turn("right"); break;
//case ControllerDirection.DownLeft: Movements.Turn("backleft"); break;
case ControllerDirection.DownRight: Movements.Crouch(); break;
//case ControllerDirection.UpLeft: Movements.Turn("forleft"); break;
case ControllerDirection.UpRight: Movements.Raise(); break;
default: Movements.Stand(); break;
}
}
/// <summary>
/// Instantiates a new PID controller class. The <see cref="Run" /> method must be called to trigger computation.
/// </summary>
/// <param name="samplingRate">
/// The rate at which the algorithm will compute a new output once <see cref="Run" /> is
/// called.
/// </param>
/// <param name="outputMinimum">The minimum value the output can be written to. The output influences the process value.</param>
/// <param name="outputMaximum">The maximum value the output can be written to. The output influences the process value.</param>
/// <param name="readProcess">
/// A function to read the process value. The process value is what you are attempting to
/// control.
/// </param>
/// <param name="readOutput">A function to read the output. The output influences the process value.</param>
/// <param name="writeOutput">A function to write to the output. The output influences the process value.</param>
/// <param name="readSetPoint">A function to read the target set-point for the process.</param>
/// <param name="proportionalGain">The proportional gain to use.</param>
/// <param name="integralGain">The integral gain to use.</param>
/// <param name="derivativeGain">The derivative gain to use.</param>
/// <param name="controllerDirection">The direction to control the output if you want the process to increase in value..</param>
/// <param name="controllerMode">The mode to start out the PID controller in.</param>
/// <exception cref="ArgumentNullException">
/// Thrown when <see cref="readProcess" /> or <see cref="readOutput" /> or <see cref="writeOutput" /> or
/// <see cref="readSetPoint" /> are null.
/// </exception>
public PidController(string name, TimeSpan samplingRate, float outputMinimum, float outputMaximum,
Func <int> readProcess, Func <ulong> readOutput, Action <ulong> writeOutput, Func <int> readSetPoint,
float proportionalGain, float integralGain, float derivativeGain,
ControllerDirection controllerDirection, ControllerMode controllerMode)
{
if (readProcess == null)
{
throw new ArgumentNullException(nameof(readProcess), "Read process must not be null.");
}
if (readOutput == null)
{
throw new ArgumentNullException(nameof(readOutput), "Read output must not be null.");
}
if (readSetPoint == null)
{
throw new ArgumentNullException(nameof(readSetPoint), "Read set-point must not be null.");
}
if (writeOutput == null)
{
throw new ArgumentNullException(nameof(writeOutput), "Write output must not be null.");
}
_name = name;
SamplingRate = samplingRate;
SetOutputLimits(outputMinimum, outputMaximum);
_readProcess = readProcess;
_readOutput = readOutput;
_writeOutput = writeOutput;
_readSetPoint = readSetPoint;
ProportionalGain = proportionalGain;
IntegralGain = integralGain;
DerivativeGain = derivativeGain;
ControllerDirection = controllerDirection;
ControllerMode = controllerMode;
_computeTimer = new Timer(callback => Compute(), null, Timeout.Infinite, Timeout.Infinite);
}
protected virtual void InitDirection()
{
Direction = Controller.Modules.Find <ControllerDirection>();
}
private static void inputReportReceived(HidDevice sender, HidInputReportReceivedEventArgs args)
{
//button
int[] _buttons = new int[10] {
0, 0, 0, 0, 0, 0, 0, 0, 0, 0
};
foreach (var btn in args.Report.ActivatedBooleanControls)
{
_buttons[btn.Id - 5] = 1;
}
if (_buttons[0] == 1)
{
if (Button_A_Pressed != null)
{
Button_A_Pressed();
}
}
if (_buttons[1] == 1)
{
if (Button_B_Pressed != null)
{
Button_B_Pressed();
}
}
if (_buttons[2] == 1)
{
if (Button_X_Pressed != null)
{
Button_X_Pressed();
}
}
if (_buttons[3] == 1)
{
if (Button_Y_Pressed != null)
{
Button_Y_Pressed();
}
}
if (_buttons[4] == 1)
{
if (Button_LB_Pressed != null)
{
Button_LB_Pressed();
}
}
if (_buttons[5] == 1)
{
if (Button_RB_Pressed != null)
{
Button_RB_Pressed();
}
}
if (_buttons[6] == 1)
{
if (Button_Back_Pressed != null)
{
Button_Back_Pressed();
}
}
if (_buttons[7] == 1)
{
if (Button_Start_Pressed != null)
{
Button_Start_Pressed();
}
}
long Button_LT = Math.Max(0, args.Report.GetNumericControl(0x01, 0x32).Value - 32768);
long Button_RT = Math.Max(0, (-1) * (args.Report.GetNumericControl(0x01, 0x32).Value - 32768));
//stick
double stickX = args.Report.GetNumericControl(0x01, 0x30).Value - 32768;
double stickY = args.Report.GetNumericControl(0x01, 0x31).Value - 32768;
double RStickX = args.Report.GetNumericControl(0x01, 0x33).Value - 32768;
double RStickY = args.Report.GetNumericControl(0x01, 0x34).Value - 32768;
stickX = getMagnitude(stickX);
stickY = getMagnitude(stickY);
RStickX = getMagnitude(RStickX);
RStickY = getMagnitude(RStickY);
if ((stickX == 0) && (stickY == 0))
{
MoveDir = ControllerDirection.None;
}
if (stickY < 0)
{
MoveDir = ControllerDirection.Up;
}
if (stickY > 0)
{
MoveDir = ControllerDirection.Down;
}
if (stickX < 0)
{
MoveDir = ControllerDirection.Left;
}
if (stickX > 0)
{
MoveDir = ControllerDirection.Right;
}
if ((RStickX == 0) && (RStickY == 0))
{
RMoveDir = ControllerDirection.None;
}
if (RStickY < 0)
{
RMoveDir = ControllerDirection.Up;
}
if (RStickY > 0)
{
RMoveDir = ControllerDirection.Down;
}
if (RStickX < 0)
{
RMoveDir = ControllerDirection.Left;
}
if (RStickX > 0)
{
RMoveDir = ControllerDirection.Right;
}
FristMoveDir = MoveDir;
RFristMoveDir = RMoveDir;
if (FristMoveDir != LastMoveDir)
{
if (FristMoveDir == ControllerDirection.None)
{
if (Leftstick_Stop != null)
{
Leftstick_Stop();
}
}
if (FristMoveDir == ControllerDirection.Up)
{
if (Leftstick_Up != null)
{
Leftstick_Up();
}
}
if (FristMoveDir == ControllerDirection.Down)
{
if (Leftstick_Down != null)
{
Leftstick_Down();
}
}
if (FristMoveDir == ControllerDirection.Left)
{
if (Leftstick_Left != null)
{
Leftstick_Left();
}
}
if (FristMoveDir == ControllerDirection.Right)
{
if (Leftstick_Right != null)
{
Leftstick_Right();
}
}
LastMoveDir = FristMoveDir;
}
if (RFristMoveDir != RLastMoveDir)
{
if (RFristMoveDir == ControllerDirection.None)
{
if (Rightstick_Stop != null)
{
Rightstick_Stop();
}
}
if (RFristMoveDir == ControllerDirection.Up)
{
if (Rightstick_Up != null)
{
Rightstick_Up();
}
}
if (RFristMoveDir == ControllerDirection.Down)
{
if (Rightstick_Down != null)
{
Rightstick_Down();
}
}
if (RFristMoveDir == ControllerDirection.Left)
{
if (Rightstick_Left != null)
{
Rightstick_Left();
}
}
if (RFristMoveDir == ControllerDirection.Right)
{
if (Rightstick_Right != null)
{
Rightstick_Right();
}
}
RLastMoveDir = RFristMoveDir;
}
}
