//$$$$$$$$$$//
//Uncomment the below section if using m3pi robots//
/// <summary>Robot speed and angle control.</summary>
/*private int[] controller(double speed, DoublePoint heading, double referenceSpeed, DoublePoint referenceHeading)
* {
* int[] tempMotorSignals = new int[2];
* const int zeroControl = 2;
* const int minControl = 25;
* const int maxControl = 127;
* const int maxSpeedControl = 70;
*
* // Regulator P constants.
* double pAngle = 20;
* double pSpeed = 1.2;
*
* // Calculate angels from vectors.
* double angle = Math.Atan2(heading.Y, heading.X);
* double referenceAngle = Math.Atan2(referenceHeading.Y, referenceHeading.X);
*
* // Calculate errors and correct for if angle changes sign.
* double angleError = referenceAngle - angle;
* if (Math.Abs(angleError) > Math.PI)
* {
* angleError = angleError - Math.Sign(angleError) * 2 * Math.PI;
* }
* //double speedError = referenceSpeed - speed // NO REAL SPEED CONTROLLER!!!
* double speedError = referenceSpeed;
*
* double angleControl = pAngle * angleError;
* double speedControl = pSpeed * speedError;
*
* // Saturate speed contol if larger than max allowed value
* if (Math.Abs(speedControl) > maxSpeedControl)
* {
* speedControl = Math.Sign(speedControl) * maxSpeedControl;
* }
*
* // Merge angle and speed part of control signal.
* tempMotorSignals[0] = (int)(speedControl + angleControl / 2);
* tempMotorSignals[1] = (int)(speedControl - angleControl / 2);
*
* // Fix contol signals if smaller/larger than min/max allowed value
* if (Math.Abs(tempMotorSignals[0]) < minControl && Math.Abs(tempMotorSignals[0]) > zeroControl || Math.Abs(tempMotorSignals[1]) < minControl && Math.Abs(tempMotorSignals[1]) > zeroControl)
* {
* if (Math.Abs(tempMotorSignals[0]) < Math.Abs(tempMotorSignals[1]))
* {
* tempMotorSignals[1] = tempMotorSignals[1] + Math.Sign(tempMotorSignals[1]) * (minControl - Math.Abs(tempMotorSignals[0]));
* tempMotorSignals[0] = Math.Sign(tempMotorSignals[0]) * minControl;
* }
* else
* {
* tempMotorSignals[0] = tempMotorSignals[0] + Math.Sign(tempMotorSignals[0]) * (minControl - Math.Abs(tempMotorSignals[1]));
* tempMotorSignals[1] = Math.Sign(tempMotorSignals[1]) * minControl;
* }
* }
*
* // Saturate control signal or set to zero if its smaller than the min allowed value.
* if (Math.Abs(tempMotorSignals[0]) > maxControl)
* {
* tempMotorSignals[0] = Math.Sign(tempMotorSignals[0]) * maxControl;
* }
* else if (Math.Abs(tempMotorSignals[0]) <= zeroControl)
* {
* tempMotorSignals[0] = 0;
* }
*
* if (Math.Abs(tempMotorSignals[1]) > maxControl)
* {
* tempMotorSignals[1] = Math.Sign(tempMotorSignals[1]) * maxControl;
* }
* else if (Math.Abs(tempMotorSignals[1]) <= zeroControl)
* {
* tempMotorSignals[1] = 0;
* }
*
* return tempMotorSignals;
*
* }*/
// ----------------- Update GUI method -----------------
public void setStrategy(ControlStrategy strategy)
{
currentStrategy = strategy.cloneStrategy();
}
