Esempio n. 1
0
        // 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);
        }
Esempio n. 2
0
        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());
        }
Esempio n. 3
0
        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);
        }
Esempio n. 4
0
        // 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;
        }
Esempio n. 5
0
        // 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();
        }
Esempio n. 6
0
 /// <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_);
 }
Esempio n. 7
0
 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));
 }
Esempio n. 8
0
 public XYZCoordinate(XYZCoordinate copy)
 {
     x_ = copy.X;
     y_ = copy.Y;
     z_ = copy.Z;
 }
Esempio n. 9
0
 public Vector(XYZCoordinate xyzCord)
 {
     _vector = new XYZCoordinate(xyzCord);
 }
Esempio n. 10
0
 public Vector(double x, double y, double z)
 {
     _vector = new XYZCoordinate(x, y, z);
 }