//$$$$$$$$$$// //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(); }