/// <summary>
/// Main constructor
/// </summary>
/// <param name="leftForwardPwm">PWM port responsible for left motor moving forward</param>
/// <param name="leftBackPwm">PWM port responsible for left motor moving backward</param>
/// <param name="rightForwardPwm">PWM port responsible for right motor moving forward</param>
/// <param name="rightBackPwm">PWM port responsible for right motor moving backward</param>
/// <param name="gearPwm">PWM port responsible for controling gear changing servo</param>
public MotorController(IPwmPort rightForwardPwm,
IPwmPort rightBackPwm,
IPwmPort leftForwardPwm,
IPwmPort leftBackPwm,
IPwmPort gearPwm,
IDigitalInputPort leftCounterPort,
IDigitalInputPort righCounterPort,
BNO055 posSens,
CameraGimbal gimb,
Action clQueue)
{
quarterCircle = fullCircle / 4;
halfCircle = fullCircle / 2;
positionSensor = posSens;
gimbal = gimb;
clearQueue = clQueue;
circumference = teethCount * chainPitch / magnetsCount;
leftMotor = new Motor(leftForwardPwm, leftBackPwm);
rightMotor = new Motor(rightForwardPwm, rightBackPwm);
gearbox = new GearBox(gearPwm);
rightCounter = new HallEffectCounter(righCounterPort);
leftCounter = new HallEffectCounter(leftCounterPort);
rightCounter.Name = Side.Right;
leftCounter.Name = Side.Left;
#if DEBUG
rightCounter.RegisterForCount(CountChanged);
leftCounter.RegisterForCount(CountChanged);
#endif
rightCounter.RegisterForLimitReached(MoveForwardStop);
leftCounter.RegisterForLimitReached(MoveForwardStop);
positionSensor.RegisterForHeadingChanged(HeadingChanged);
moveForwardResetEventLeft = new AutoResetEvent(false);
moveForwardResetEventRight = new AutoResetEvent(false);
turnResetEvent = new AutoResetEvent(false);
turnTokenSource = new CancellationTokenSource();
turnPid = new IdealPidController();
turnPid.ProportionalComponent = 1.8f;
turnPid.IntegralComponent = 0.05f;
turnPid.DerivativeComponent = 0.01f;
turnPid.OutputMax = 50;
turnPid.OutputMin = -50;
stabilizePid = new IdealPidController();
stabilizePid.ProportionalComponent = 1.8f;
stabilizePid.IntegralComponent = 0.05f;
stabilizePid.DerivativeComponent = 0.01f;
stabilizePid.OutputMax = stabilizeTurnRate;
stabilizePid.OutputMin = -stabilizeTurnRate;
}
/// <summary>
/// Event handler for stopping
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void MoveForwardStop(object sender, EventArgs e)
{
HallEffectCounter counter = sender as HallEffectCounter;
if (counter.Name == Side.Left)
{
moveForwardResetEventLeft.Set();
}
else
{
moveForwardResetEventRight.Set();
}
counter.DisableTarget();
}
