// Simulated state
public bool UpdateState(double rudderAngle, double propSpeed)
{
// Update propspeed
_propSpeed = propSpeed;
// Update heading
_angVelocity.Z = _angVelocity.Z - _propSpeed * SPEED_CONSTANT * (rudderAngle) * DELTA_T;
_heading = _headingPrev + _angVelocity.Z * DELTA_T;
// Update velocity in x and y
_velocity.X = _propSpeed * SPEED_CONSTANT * Math.Cos(_heading);
_velocity.Y = _propSpeed * SPEED_CONSTANT * Math.Sin(_heading);
_velocity.Z = 0 * SPEED_CONSTANT;
_depth = 0;
// Update current position in x and y
_position.X = _positionPrev.X + _velocity.X * DELTA_T;
_position.Y = _positionPrev.Y + _velocity.Y * DELTA_T;
_position.Z = 0;
// Update previous values
_positionPrev = _position;
_velocityPrev = _velocity;
_accelerationPrev = _acceleration;
return(true);
}
static void Main(string[] args)
{
// Create simulated AUV
AUVStateSim simIver = new AUVStateSim("Small Iver", null, null, 20, 5, new XYZCoordinate(1, 2, 3), new XYZCoordinate(2, 3, 4), new XYZCoordinate(0, 0, 0));
// Logging parameters
var csv = new StringBuilder();
int i = 0;
int iterations = 1000;
// Simulation parameters
var positionDes = new XYZCoordinate();
positionDes.X = 1.0;
positionDes.Y = 1.0;
positionDes.Z = 0;
simIver._propSpeed = 0.3;
// Shark randomness
Random random = new Random();
while (i < iterations)
{
// Calculate rudder Ang
simIver.UpdateState(-simIver.calcRudderAng(positionDes), simIver._propSpeed);
// Log data
var newLine = string.Format("{0},{1},{2},{3},{4},{5}", simIver._position.X, simIver._position.Y, positionDes.X, positionDes.Y, simIver._heading, simIver._rudderAngle);
csv.AppendLine(newLine);
// Point tracking stop condition
if (positionDes.DistanceTo(simIver._position) < .1)
{
// newIver._propSpeed = 0;
}
// Move desired point
positionDes.X += 0.01;
positionDes.Y += Math.Sin(positionDes.X) * 0.005 - 0.005;
++i;
}
string filePath = "\\Users\\Eyassu\\Desktop\\LAIR\\SimData\\PTrackingTest.csv";
File.WriteAllText(filePath, csv.ToString());
}
public double calcRudderAng(XYZCoordinate positionDes)
{
// Rudder angle in radians
double angOffset = Math.Atan2(positionDes.Y - _position.Y, positionDes.X - _position.X);
_rudderAngle = Kp_ANG * (AngleDifference.angleDiff(angOffset, _heading));
if (_rudderAngle > Math.PI / 4)
{
_rudderAngle = Math.PI / 4;
}
else if (_rudderAngle < -Math.PI / 4)
{
_rudderAngle = -Math.PI / 4;
}
return(_rudderAngle);
}
// Constructor
public AUVStateSim(string name = "null",
double[] initLatLong = null,
double[] latLong = null,
double heading = 0, double depth = 0,
XYZCoordinate position = new XYZCoordinate(),
XYZCoordinate velocity = new XYZCoordinate(),
XYZCoordinate acceleration = new XYZCoordinate())
{
_name = name;
_initLatLong = initLatLong == null ? DEFAULT_LATLONG : initLatLong;
_latLong = latLong == null ? DEFAULT_LATLONG : latLong;
_position = position;
_velocity = velocity;
_acceleration = acceleration;
_heading = heading;
_depth = depth;
}
// Constructor
public AUVState(string name = "null",
double[] initLatLong = null,
double[] latLong = null,
double heading = 0, double depth = 0,
XYZCoordinate position = new XYZCoordinate(),
XYZCoordinate velocity = new XYZCoordinate(),
XYZCoordinate acceleration = new XYZCoordinate(),
List <int> servoStates = null,
List <double> compassStates = null,
string mode = "",
string errorState = "",
ackMessage ackMsg = null)
{
_name = name;
_initLatLong = initLatLong == null ? DEFAULT_LATLONG : initLatLong;
_latLong = latLong == null ? DEFAULT_LATLONG : latLong;
_position = position;
_velocity = velocity;
_acceleration = acceleration;
_heading = heading;
_depth = depth;
_servoStates = servoStates == null ? DEFAULT_LIST_INT : servoStates;
_compassStates = compassStates == null ? DEFAULT_LIST_DOUBLE : compassStates;
_mode = mode;
_errorState = errorState;
_ackMsg = ackMsg == null ? DEFAULT_ACKMESSAGE : ackMsg;
_serialPort = new SerialPort();
this.SerialSetup();
}
/// <summary>
/// True if x, y and z are all equal
/// </summary>
/// <param name="coord"></param>
/// <returns></returns>
public bool Equals(XYZCoordinate coord)
{
return(coord.x_ == x_ && coord.y_ == y_ && coord.z_ == z_);
}
public double DistanceTo(XYZCoordinate arg)
{
return(Math.Pow((Math.Pow(x_ - arg.X, 2) +
Math.Pow(y_ - arg.Y, 2) +
Math.Pow(z_ - arg.Z, 2)), 0.5));
}
public XYZCoordinate(XYZCoordinate copy)
{
x_ = copy.X;
y_ = copy.Y;
z_ = copy.Z;
}
public Vector(XYZCoordinate xyzCord)
{
_vector = new XYZCoordinate(xyzCord);
}
public Vector(double x, double y, double z)
{
_vector = new XYZCoordinate(x, y, z);
}