Browse Source
add ardutracker support add load/save setting in Tracker move getserialports to serial class hopefully fix dtr issue for good.master
Michael Oborne
13 years ago
14 changed files with 539 additions and 99 deletions
@ -0,0 +1,155 @@
@@ -0,0 +1,155 @@
|
||||
using System; |
||||
using System.Collections.Generic; |
||||
using System.Linq; |
||||
using System.Text; |
||||
|
||||
namespace ArdupilotMega.Antenna |
||||
{ |
||||
class ArduTracker : ITrackerOutput |
||||
{ |
||||
public SerialPort ComPort { get; set; } |
||||
/// <summary> |
||||
/// 0-360 |
||||
/// </summary> |
||||
public double TrimPan { get; set; } |
||||
/// <summary> |
||||
/// -90 - 90 |
||||
/// </summary> |
||||
public double TrimTilt { get; set; } |
||||
|
||||
public int PanStartRange { get; set; } |
||||
public int TiltStartRange { get; set; } |
||||
public int PanEndRange { get; set; } |
||||
public int TiltEndRange { get; set; } |
||||
public int PanPWMRange { get; set; } |
||||
public int TiltPWMRange { get; set; } |
||||
|
||||
public bool PanReverse { get { return _panreverse == 1; } set { _panreverse = value == true ? -1 : 1; } } |
||||
public bool TiltReverse { get { return _tiltreverse == 1; } set { _tiltreverse = value == true ? -1 : 1; } } |
||||
|
||||
int _panreverse = 1; |
||||
int _tiltreverse = 1; |
||||
|
||||
int currentpan = 1500; |
||||
int currenttilt = 1500; |
||||
|
||||
public bool Init() |
||||
{ |
||||
|
||||
if ((PanStartRange - PanEndRange) == 0) |
||||
{ |
||||
System.Windows.Forms.CustomMessageBox.Show("Invalid Pan Range", "Error"); |
||||
return false; |
||||
} |
||||
|
||||
if ((TiltStartRange - TiltEndRange) == 0) |
||||
{ |
||||
System.Windows.Forms.CustomMessageBox.Show("Invalid Tilt Range", "Error"); |
||||
return false; |
||||
} |
||||
|
||||
try |
||||
{ |
||||
ComPort.Open(); |
||||
} |
||||
catch (Exception ex) { System.Windows.Forms.CustomMessageBox.Show("Connect failed " + ex.Message, "Error"); return false; } |
||||
|
||||
return true; |
||||
} |
||||
public bool Setup() |
||||
{ |
||||
|
||||
|
||||
return true; |
||||
} |
||||
|
||||
double wrap_180(double input) |
||||
{ |
||||
if (input > 180) |
||||
return input - 360; |
||||
if (input < -180) |
||||
return input + 360; |
||||
return input; |
||||
} |
||||
|
||||
double wrap_range(double input, double range) |
||||
{ |
||||
if (input > range) |
||||
return input - 360; |
||||
if (input < -range) |
||||
return input + 360; |
||||
return input; |
||||
} |
||||
|
||||
public bool Pan(double Angle) |
||||
{ |
||||
double range = Math.Abs(PanStartRange - PanEndRange); |
||||
|
||||
// get relative center based on tracking center |
||||
double rangeleft = PanStartRange - TrimPan; |
||||
double rangeright = PanEndRange - TrimPan; |
||||
double centerpos = 1500; |
||||
|
||||
// get the output angle the tracker needs to point and constrain the output to the allowed options |
||||
short PointAtAngle = Constrain(wrap_180(Angle - TrimPan), PanStartRange, PanEndRange); |
||||
|
||||
// conver the angle into a 0-pwmrange value |
||||
int target = (int)((((PointAtAngle / range) * 2.0) * (PanPWMRange / 2) * _panreverse + centerpos)); |
||||
|
||||
// Console.WriteLine("P " + Angle + " " + target + " " + PointAtAngle); |
||||
|
||||
currentpan = target; |
||||
|
||||
return false; |
||||
} |
||||
|
||||
public bool Tilt(double Angle) |
||||
{ |
||||
double range = Math.Abs(TiltStartRange - TiltEndRange); |
||||
|
||||
short PointAtAngle = Constrain((Angle - TrimTilt), TiltStartRange, TiltEndRange); |
||||
|
||||
int target = (int)((((PointAtAngle / range) * 2.0) * (TiltPWMRange / 2) * _tiltreverse + 1500)); |
||||
|
||||
// Console.WriteLine("T " + Angle + " " + target + " " + PointAtAngle); |
||||
|
||||
currenttilt = target; |
||||
|
||||
return false; |
||||
} |
||||
|
||||
public bool PanAndTilt(double pan, double tilt) |
||||
{ |
||||
Tilt(tilt); |
||||
Pan(pan); |
||||
|
||||
string command = string.Format("!!!PAN:{0:0000},TLT:{1:0000}\n", currentpan, currenttilt); |
||||
|
||||
Console.Write(command); |
||||
|
||||
ComPort.Write(command); |
||||
|
||||
return false; |
||||
} |
||||
|
||||
public bool Close() |
||||
{ |
||||
try |
||||
{ |
||||
ComPort.Close(); |
||||
} |
||||
catch { } |
||||
return true; |
||||
} |
||||
|
||||
short Constrain(double input, double min, double max) |
||||
{ |
||||
if (input < min) |
||||
return (short)min; |
||||
if (input > max) |
||||
return (short)max; |
||||
return (short)input; |
||||
} |
||||
|
||||
} |
||||
} |
||||
@ -0,0 +1,46 @@
@@ -0,0 +1,46 @@
|
||||
using System; |
||||
using System.Collections.Generic; |
||||
using System.Linq; |
||||
using System.Text; |
||||
|
||||
namespace ArdupilotMega |
||||
{ |
||||
class PIDTunning |
||||
{ |
||||
|
||||
public static void twiddle(double[] initialgains, Func<double[],double> run, double tol = 0.001) |
||||
{ |
||||
int n_params = 3; |
||||
double err= 0; |
||||
double[] dparams = initialgains; //{1.0f,1.0f,1.0f}; |
||||
double[] paramss = {0.0f,0.0f,0.0f}; |
||||
double best_error = run(paramss); |
||||
int n = 0; |
||||
|
||||
while (dparams.Sum() > tol) { |
||||
for (int i = 0; i < n_params; i++){ |
||||
paramss[i] += dparams[i]; |
||||
err = run(paramss); |
||||
if (err < best_error){ |
||||
best_error = err; |
||||
dparams[i] *= 1.1; |
||||
} |
||||
else { |
||||
paramss[i] -= 2.0 * dparams[i]; |
||||
err = run(paramss); |
||||
if (err < best_error){ |
||||
best_error = err; |
||||
dparams[i] *= 1.1; |
||||
} |
||||
else { |
||||
paramss[i] += dparams[i]; |
||||
dparams[i] *= 0.9; |
||||
} |
||||
} |
||||
n += 1; |
||||
Console.WriteLine("Twiddle #" + n + " " + paramss.ToString() + " -> " + best_error); |
||||
} |
||||
} |
||||
} |
||||
} |
||||
} |
||||
Binary file not shown.
Loading…
Reference in new issue