public Vector apply(Vector value)
{
if (_output == null)
_output = value;
else {
double dt = SystemTime.SecondsSince(_lastTime);
double a = MathEx.Exp(-dt / Tau);
_output = (1 - a) * value + a * _output;
}
_lastTime = SystemTime.Seconds;
return _output;
}
public Vector Wrap(Vector v)
{
return new Vector(X.Wrap(v.X), Y.Wrap(v.Y));
}
public bool Contains(Vector v, bool inclusive = true)
{
return X.Contains(v.X, inclusive) && Y.Contains(v.Y, inclusive);
}
public Vector Clip(Vector v)
{
return new Vector(X.Clip(v.X), Y.Clip(v.Y));
}
public VectorRange(Vector min, Vector max)
{
X = max.X > min.X ? new Range(min.X, max.X) : new Range(max.X, min.X);
Y = max.Y > min.Y ? new Range(min.Y, max.Y) : new Range(max.Y, min.Y);
}
public override void ActivePeriodic()
{
if (robot.LightGate.IsObstructed)
{
robot.Drive.ControlEnabled = false;
// robot.Drive.RotationPoint = Vector.I * 250;
robot.Drive.DriveVelocity = 800 * Vector.J;
robot.Drive.TurnVelocity = 0;
//Thread.Sleep(500);
//robot.Drive.TurnVelocity = 0;
robot.Drive.ControlEnabled = true;
}
else
{
Vector direction;
#if TRY_PLAYING_FOOTBALL
direction = 200 * filter.apply(robot.BallDetector.Get());
/*
double absAngle = MathEx.Abs(angle / 180);
double sign = MathEx.Sign(angle);
double newAngle = (exponent * absAngle - Math.Pow(absAngle, exponent)) /
(exponent - 1) * sign * 180;
double x = 100.0 * MathExGHI.Tan(MathEx.ToRadians(newAngle));
// Debug.Print("New Heading " + newAngle);
if (DoubleEx.IsNaN(x))
{
direction.SetNewVector(0,0);//300.0 * sign, 0.0);
}
else
{
direction.SetNewVector(x, (absAngle > 0.5 ? -1 : 1) * 300.0);
}*/
direction = new Vector(direction.X, direction.Y - MathEx.Abs(direction.X * 0.5));
#endif
/*
Debug.Print("Ball at " + angle);
double sign = Technobotts.MathEx.Sign(angle);
double newAngle = Math.PI * 0.2 * sign + angle * 0.8;
double x= 100.0 * MathExGHI.Tan(newAngle);
Debug.Print("New Heading " + newAngle);
if (DoubleEx.IsNaN(x))
{
direction.SetNewVector(Math.PI/200.0 * sign, 0.0);
}
else
{
direction.SetNewVector(x, 100.0);
}
*/
// direction.SetNewVector(direction.X, direction.Y);
#if WALK_PITCH
// robot.Drive.ControlEnabled = false;
direction = vectors[curState];
DateTime dtNow = DateTime.UtcNow;
if (dtNow > dtNextChange)
{
curState = (curState+1) % states;
Debug.Print("CurState=" + curState);
dtNextChange = dtNow.AddSeconds(seconds * (((curState & 1) == 1) ?1 : 5));
}
#endif
#if DONT_BUMP_WALLS
IRangeFinder[] sensors = robot.Sensors.US;
int cm;
if (direction.Y > 0 && (cm = sensors[0].DistanceCM) < 30) // negative y moves away
direction = new Vector(direction.X, (cm - 30) / 120.0 * direction.Y);
else if (direction.Y < 0 && (cm = sensors[2].DistanceCM) < 30) // positive y moves away
direction = new Vector(direction.X, (cm - 30) / 120.0 * direction.Y);
if (direction.X > 0 && (cm = sensors[1].DistanceCM) < 20) // negative x moves away
direction = new Vector((cm - 20) / 80.0 * direction.X, direction.Y);
else if (direction.X < 0 && (cm = sensors[3].DistanceCM) < 20) // positive x moves away.
direction = new Vector((cm - 20) / 80.0 * direction.X,direction.Y);
#endif
robot.Drive.RotationPoint = 0;
// robot.Drive.TurnVelocity = 0;
robot.Drive.DriveVelocity = direction;
}
}
public OrientedIntensityDetector(IIntensityDetector detector, Vector orientation)
{
Detector = detector;
Orientation = orientation;
}
public Vector GetPosition()
{
double heading = Compass.Angle;
heading /= MathEx.TwoPi;
Orientation o = Orientation.Invalid;
if (HeadingRanges[0].Contains(heading) || HeadingRanges[4].Contains(heading))
o = Orientation.North;
else if (HeadingRanges[1].Contains(heading))
o = Orientation.East;
else if (HeadingRanges[2].Contains(heading))
o = Orientation.South;
else if (HeadingRanges[3].Contains(heading))
o = Orientation.West;
if (o != Orientation.Invalid)
{
Distances d;
if (o == Orientation.North)
d = new Distances(Front.DistanceCM, Right.DistanceCM, Back.DistanceCM, Left.DistanceCM);
else if (o == Orientation.East)
d = new Distances(Left.DistanceCM, Front.DistanceCM, Right.DistanceCM, Back.DistanceCM);
else if (o == Orientation.South)
d = new Distances(Back.DistanceCM, Left.DistanceCM, Front.DistanceCM, Right.DistanceCM);
else
d = new Distances(Right.DistanceCM, Back.DistanceCM, Left.DistanceCM, Front.DistanceCM);
int robotSize = (int)(SensorSpacing / 2 * MathEx.Cos(heading * MathEx.TwoPi));
d.IncrementBy(robotSize);
Vector pos = d.ApproximatePosition;
if(lastDistances != null) {
Distances changes = d.ChangeFrom(lastDistances);
if (!Tolerance.Contains(d.ApproximateWidth - PitchSize.X))
{
if(changes.West > changes.East)
pos = new Vector(PitchSize.X - d.East, pos.Y);
else
pos = new Vector(d.West, pos.Y);
}
if (!Tolerance.Contains(d.ApproximateHeight - PitchSize.Y)) {
if(changes.South > changes.North)
pos = new Vector(pos.X, PitchSize.Y - d.North);
else
pos = new Vector(pos.X, d.South);
}
}
lastDistances = d;
return pos;
}
return null;
}
public override void ActivePeriodic()
{
Vector ballDirection = filter.apply(robot.BallDetector.Get());
Vector direction;
if (ballDirection.Y > 0 && robot.LightGate.IsObstructed)
{
direction = 800 * Vector.J;
}
else
{
direction = 800 * new Vector(ballDirection.X, ballDirection.Y - MathEx.Abs(ballDirection.X * 0.5));
}
Distances actual = robot.Sensors.USDistances;
Distances delta = actual - min;
double p = 20;
if (direction.Y > 0 && delta.Top < 0)
direction = new Vector(direction.X, p * delta.Top);
else if (direction.Y < 0 && delta.Bottom < 0)
direction = new Vector(direction.X, -p * delta.Bottom);
if (direction.X > 0 && delta.Right < 0)
direction = new Vector(p * delta.Right, direction.Y);
else if (direction.X < 0 && delta.Left < 0)
direction = new Vector(-p * delta.Left, direction.Y);
robot.Drive.RotationPoint = 0;
robot.Drive.DriveVelocity = direction;
}
public static Matrix FromPrincipleAxis(Vector axis)
{
return new Matrix(
axis.Y, axis.X,
-axis.X, axis.Y);
}
/**
* Construct a square 2x2 Matrix representing the transformation onto a new
* pair of coordinate axes
*/
public static Matrix FromCoordinateAxes(Vector yAxis, Vector xAxis)
{
return new Matrix(
xAxis.X, yAxis.X,
xAxis.Y, yAxis.Y);
}
public void reset()
{
_output = null;
}
public void set(Vector driveVelocity, double turnVelocity)
{
_driveVelocity = driveVelocity;
_turnVelocity = turnVelocity;
update();
}
public Wheel(Vector position, Matrix transformMatrix, IMotor motor)
{
Position = position;
Motor = motor;
Motor.NeutralMode = NeutralMode.Brake;
TransformMatrix = transformMatrix;
}
public Wheel(Vector position, Vector driveAxis, Vector rollAxis, IMotor motor)
: this(position, Matrix.FromCoordinateAxes(driveAxis, rollAxis), motor)
{
}
public double Dot(Vector that)
{
return X * that.X + Y * that.Y;
}
