public void DrawTrajectory(LineRenderer lineRenderer, List <TrajectoryPoint> points, float angleSpread)
{
lineRenderer.positionCount = points.Count;
lineRenderer.widthMultiplier = 0.5f;
for (int i = 0; i < points.Count; i++)
{
TrajectoryPoint point = points[i];
lineRenderer.SetPosition(i, point.Point);
}
if (angleSpread > 0 && points.Count > 0)
{
Vector2 supVector = Quaternion.Euler(0, 0, angleSpread) *
(Vector2.up * Vector2.Distance(points[0].Point, points[1].Point));
lineRenderer.widthMultiplier = 1f;
lineRenderer.widthCurve = AnimationCurve.Linear(0f, lineRenderer.widthMultiplier * 0f,
1f, lineRenderer.widthMultiplier * Mathf.Abs(supVector.x) / CustomPhysic.getInstance().Timestep);
}
else
{
lineRenderer.widthCurve = AnimationCurve.Constant(0f, 1f, lineRenderer.widthMultiplier * .5f);
}
}
public void ShouldDeleteInCascateWithMultipleOneToMany()
{
Db.CreateOneToMany <Trajectory, TrajectoryPoint>(tp => tp.Trajectory.Id, true);
var trajectory1 = new Trajectory();
var trajectory2 = new Trajectory();
var tp1 = new TrajectoryPoint(trajectory1);
var tp2 = new TrajectoryPoint(trajectory2);
var tp3 = new TrajectoryPoint(trajectory2);
var tp4 = new TrajectoryPoint(trajectory1);
Db.Insert(trajectory1);
Db.Insert(trajectory2);
Db.Insert(tp1);
Db.Insert(tp2);
Db.Insert(tp3);
Db.Insert(tp4);
Db.Delete(trajectory1);
Assert.True(1 == Db.Count <Trajectory>());
Assert.True(2 == Db.Count <TrajectoryPoint>());
Assert.Equal(trajectory2.Id, Db.GetOne <Trajectory>(trajectory2.Id).Id);
foreach (var trajectoryPoint in Db.GetAll <TrajectoryPoint>())
{
Assert.Equal(trajectory2.Id, trajectoryPoint.Trajectory.Id);
}
}
public override string CreateSymbol(TrajectoryPoint point)
{
if (point is TrajectoryPoint3D p3d)
{
var difX = p3d.X - X;
var difY = p3d.Y - Y;
var difZ = p3d.Z - Z;
var dis = Math.Sqrt(difX * difX + difY * difY + difZ * difZ);
if (dis <= Radius)
{
return($"A{ID + 1}_Hit");
}
else if (dis <= ToleranceRadius)
{
return($"A{ID + 1}_Tolerance");
}
else
{
return(null);
}
}
throw new ArgumentException("The point has to be 3 dimensional");
}
public override string CreateSymbol(TrajectoryPoint point)
{
if (point is TrajectoryPoint3D p3d)
{
var p = Vector <double> .Build.DenseOfArray(new double[] { p3d.X, p3d.Y, p3d.Z });
var pp = p - ellipsoid.Centroid;
var disV = pp * (ellipsoid.A * pp);
var disVtol = pp * (Atol * pp);
if (disV <= 1)
{
return($"A{ID + 1}_Hit");
}
else if (disVtol <= 1)
{
return($"A{ID + 1}_Tolerance");
}
else
{
return(null);
}
}
throw new ArgumentException("The point has to be 3 dimensional");
}
public void TestTrajectoryPoint()
{
var point = new TrajectoryPoint(TimeSpan.FromMilliseconds(0.5),
new Measurement <WeightUnit>(55, WeightUnit.Grain),
new Measurement <DistanceUnit>(100, DistanceUnit.Yard),
new Measurement <VelocityUnit>(2430, VelocityUnit.FeetPerSecond),
2.15937,
new Measurement <DistanceUnit>(1.54, DistanceUnit.Inch),
new Measurement <DistanceUnit>(-2.55, DistanceUnit.Inch));
point.OptimalGameWeight.In(WeightUnit.Pound).Should().BeApproximately(65, 0.5);
point.DropAdjustment.Should().Be(new Measurement <AngularUnit>(1.54, AngularUnit.InchesPer100Yards));
point.WindageAdjustment.Should().Be(new Measurement <AngularUnit>(-2.55, AngularUnit.InchesPer100Yards));
SerializerRoundtrip serializer = new SerializerRoundtrip();
var xml = serializer.Serialize(point);
var point2 = serializer.Deserialize <TrajectoryPoint>(xml);
point2.Time.Should().Be(point.Time);
point2.Distance.Should().Be(point.Distance);
point2.Velocity.Should().Be(point.Velocity);
point2.Mach.Should().Be(point.Mach);
point2.Energy.Should().Be(point.Energy);
point2.OptimalGameWeight.Should().Be(point.OptimalGameWeight);
point2.Drop.Should().Be(point.Drop);
point2.DropAdjustment.Should().Be(point.DropAdjustment);
point2.Windage.Should().Be(point.Windage);
point2.WindageAdjustment.Should().Be(point.WindageAdjustment);
}
/// <summary>
/// Updates the trajectory and the point cloud from sensor data.
/// </summary>
private void UpdateSensors()
{
float highestSensorSpeed = 0;
TrajectoryPoint trajectoryPoint = new TrajectoryPoint(_position, _theta)
{
FillColor = Color.Green
};
// Finds the points discovered by sensors for a trajectory point
foreach (Sensor sensor in Sensors)
{
sensor.FindPoint();
if (sensor.HasFoundPoint)
{
trajectoryPoint.PointsFoundBySensors.Add(sensor.CloudPoint);
}
if (sensor.Speed > highestSensorSpeed)
{
highestSensorSpeed = sensor.Speed;
}
}
Trajectory.Points.Add(trajectoryPoint);
}
public TrajectorySamplePoint sample(double distance)
{
if (distance >= last_interpolant())
{
return(new TrajectorySamplePoint(mTrajectory.getPoint(mTrajectory.length() - 1)));
}
if (distance <= 0.0)
{
return(new TrajectorySamplePoint(mTrajectory.getPoint(0)));
}
for (int i = 1; i < distances.Length; ++i)
{
TrajectoryPoint s = mTrajectory.getPoint(i);
if (distances[i] >= distance)
{
TrajectoryPoint prev_s = mTrajectory.getPoint(i - 1);
if (epsilonEquals(distances[i], distances[i - 1]))
{
return(new TrajectorySamplePoint(s));
}
else
{
return(new TrajectorySamplePoint(prev_s.mState.interpolate(s.mState,
(distance - distances[i - 1]) / (distances[i] - distances[i - 1])), i - 1, i));
}
}
}
throw new Exception();
}
public List <TrajectoryPoint> LoadTrajectory(string trajPath)
// Loads sound trajectories from CSV files
{
var trajL = new List <TrajectoryPoint>();
StreamReader inp_stm = new StreamReader(trajPath);
while (!inp_stm.EndOfStream)
{
string inp_ln = inp_stm.ReadLine();
string[] fields = inp_ln.Split(',');
float phi, x, y, z, t;
float.TryParse(fields[0], out phi);
float.TryParse(fields[1], out x);
float.TryParse(fields[2], out y);
float.TryParse(fields[3], out z);
float.TryParse(fields[4], out t);
TrajectoryPoint point = new TrajectoryPoint(x, z + 1, y, t); // Adjust coordinate system to unity -> flipped Y and Z
trajL.Add(point);
}
inp_stm.Close();
if (trajL.Count == 1)
{
Debug.Log("Error in loading trajectory file");
}
return(trajL);
}
//ToDo: könnte für lineare Interpolation optimiert werden (Verzicht auf binäre Suche) (dann nicht mehr static)
public static TrajectoryPoint[] getEquidistantPoints(IStrokeInterpolation s, int nNewPoints)
{
if (nNewPoints < 2)
{
throw new ArgumentException("Stroke muss in mindestens 2 Abschnitte eingeteilt werden", "nNewPoints");
}
var result = new TrajectoryPoint[nNewPoints];
//first one is clear
result[0] = s.getByArcLength(0);
var stepSize = s.ArcLength / (nNewPoints - 1);
var curArcLen = stepSize;
for (int j = 1; j < nNewPoints - 1; j++)
{
result[j] = s.getByArcLength(curArcLen);
curArcLen += stepSize;
}
//last one is also clear
result[nNewPoints - 1] = s.getByArcLength(s.ArcLength);
return(result);
}
public override string CreateSymbol(TrajectoryPoint point)
{
var difX = X - point.X;
var difY = Y - point.Y;
var dis = Math.Sqrt(difX * difX + difY * difY);
return($"A{ID + 1}_{dis:F4}");
}
void Drive()
{
FillBtns(ref _btns);
float y = -1 * _gamepad.GetAxis(1);
Deadband(ref y);
_talon.ProcessMotionProfileBuffer();
/* button handler, if btn5 pressed launch MP, if btn7 pressed, enter percent output mode */
if (_btns[5] && !_btnsLast[5])
{
/* disable MP to clear IsLast */
_talon.Set(ControlMode.MotionProfile, 0);
Watchdog.Feed();
Thread.Sleep(10);
/* buffer new pts in HERO */
TrajectoryPoint point = new TrajectoryPoint();
_talon.ClearMotionProfileHasUnderrun();
_talon.ClearMotionProfileTrajectories();
for (uint i = 0; i < MotionProfile.kNumPoints; ++i)
{
point.position = (float)MotionProfile.Points[i][0] * kTicksPerRotation; //convert from rotations to sensor units
point.velocity = (float)MotionProfile.Points[i][1] * kTicksPerRotation / 600; //convert from RPM to sensor units per 100 ms.
point.headingDeg = 0; //not used in this example
point.isLastPoint = (i + 1 == MotionProfile.kNumPoints) ? true : false;
point.zeroPos = (i == 0) ? true : false;
point.profileSlotSelect0 = 0;
point.profileSlotSelect1 = 0; //not used in this example
point.timeDur = TrajectoryPoint.TrajectoryDuration.TrajectoryDuration_10ms;
_talon.PushMotionProfileTrajectory(point);
}
/* send the first few pts to Talon */
for (int i = 0; i < 5; ++i)
{
Watchdog.Feed();
Thread.Sleep(10);
_talon.ProcessMotionProfileBuffer();
}
/*start MP */
_talon.Set(ControlMode.MotionProfile, 1);
}
else if (_btns[7] && !_btnsLast[7])
{
_talon.Set(ControlMode.PercentOutput, 0);
}
/* if not in motion profile mode, update output percent */
if (_talon.GetControlMode() != ControlMode.MotionProfile)
{
_talon.Set(ControlMode.PercentOutput, y);
}
/* copy btns => btnsLast */
System.Array.Copy(_btns, _btnsLast, _btns.Length);
}
private static TrajectoryPoint FindByDistance(IEnumerable <TrajectoryPoint> trajectory, Measurement <DistanceUnit> distance)
{
TrajectoryPoint previous = null;
foreach (var point in trajectory)
{
if (point.Distance > distance)
{
return(previous);
}
previous = point;
}
return(null);
}
IEnumerator Move(Vector2 velocity)
{
float timestep = _customPhysic.Timestep;
ballVelocity = velocity;
while (!dropped)
{
TrajectoryPoint nextPoint = _customPhysic.GetNextTrajectoryPoint(transform.position, ballVelocity);
transform.position = nextPoint.Point;
ballVelocity = nextPoint.Velocity;
yield return(new WaitForSeconds(timestep));
}
}
public static void Load(string path)
{
Loaded = true;
Trajectory = new Trajectory();
PointCloud = new PointCloud();
string[] lines = File.ReadAllLines(path);
foreach (string line in lines)
{
if (!line.Contains(";") && line.Length != 0) // Ignores comments
{
string[] pointLine = line.Split(' ');
// Point coordinates
string[] coordinates = pointLine[0].Split('_');
float x = float.Parse(coordinates[0]);
float y = float.Parse(coordinates[1]);
// Theta
float theta = float.Parse(pointLine[1]);
// Adds a trajectory point object to the trajectory
TrajectoryPoint trajectoryPoint = new TrajectoryPoint(new Vector2f(x, y), theta)
{
FillColor = Color.Red
};
Trajectory.Points.Add(trajectoryPoint);
// Points found by sensors
for (byte pointFoundIndex = 0; pointFoundIndex < pointLine.Length - 2; pointFoundIndex++)
{
string[] pointFoundCoordinates = pointLine[pointFoundIndex + 2].Split('_');
float pointFoundX = float.Parse(pointFoundCoordinates[0]);
float pointFoundY = float.Parse(pointFoundCoordinates[1]);
// Adds a point object to the point cloud
CloudPoint point = new CloudPoint(new Vector2f(pointFoundX, pointFoundY))
{
FillColor = Color.Red
};
PointCloud.Points.Add(point);
}
}
}
}
public void ShouldThrowExceptionWhenFkDependencyNotSatisfy()
{
Db.CreateOneToMany <Trajectory, TrajectoryPoint>(tp => tp.Trajectory.Id);
var trajectory = new Trajectory();
var tp1 = new TrajectoryPoint(trajectory);
var ex = Assert.Throws <InvalidOperationException>(() => { Db.Insert(tp1); });
Assert.Equal("Invalid FK", ex.Message);
Assert.True(0 == Db.Count <Trajectory>());
Assert.True(0 == Db.Count <TrajectoryPoint>());
}
private IEnumerator ShootProcess(List <TrajectoryPoint> bulletTrajectory, Transform bulletPrefab)
{
bulletPrefab.gameObject.SetActive(false);
Transform bullet = GameObject.Instantiate(bulletPrefab.gameObject, bulletPrefab.parent).gameObject.transform;
bullet.gameObject.SetActive(true);
List <TrajectoryPoint> bt = bulletTrajectory;
TrajectoryPoint from = bt[0];
TrajectoryPoint to = bt[1];
TrajectoryPoint end = bt.Last();
float safetyTime = 50f;
bullet.position = from.position;
while (bullet.position != end.position && safetyTime >= 0f)
{
safetyTime -= Time.deltaTime;
bullet.position = Vector3.MoveTowards(bullet.position, to.position, bulletSpeed * Time.deltaTime);
if (bullet.position == to.position)
{
if (to.wall != null)
{
to.wall?.GotHitted();
}
if (to.position == end.position)
{
// skip and go to end
}
else
{
int fromIndex = bt.IndexOf(from);
from = bt[fromIndex + 1];
to = bt[fromIndex + 2];
}
}
yield return(null);
}
Destroy(bullet.gameObject);
shootCor = null;
yield return(null);
}
public void ShouldUpdateIfFkSatisfy()
{
Db.CreateOneToMany <Trajectory, TrajectoryPoint>(tp => tp.Trajectory.Id);
var trajectory = new Trajectory();
var trajectory2 = new Trajectory();
var tp1 = new TrajectoryPoint(trajectory);
var tp2 = new TrajectoryPoint(trajectory);
var tp3 = new TrajectoryPoint(trajectory);
Db.Insert(trajectory);
Db.Insert(trajectory2);
Db.Insert(tp1);
Db.Insert(tp2);
Db.Insert(tp3);
Assert.True(2 == Db.Count <Trajectory>());
Assert.True(3 == Db.Count <TrajectoryPoint>());
foreach (var trajectoryPoint in Db.GetAll <TrajectoryPoint>())
{
Assert.Equal(trajectory.Id, trajectoryPoint.Trajectory.Id);
}
tp2.Trajectory = trajectory2;
Db.Update(tp2);
var countForTrajectory = 0;
var countForTrajectory2 = 0;
foreach (var trajectoryPoint in Db.GetAll <TrajectoryPoint>())
{
if (trajectory.Id == trajectoryPoint.Trajectory.Id)
{
countForTrajectory++;
}
if (trajectory2.Id == trajectoryPoint.Trajectory.Id)
{
countForTrajectory2++;
}
}
Assert.Equal(2, countForTrajectory);
Assert.Equal(1, countForTrajectory2);
}
public List <TrajectoryPoint> GetTrajectory(Vector2 ballPosition, Vector2 force)
{
CustomPhysic customPhysic = CustomPhysic.getInstance();
List <TrajectoryPoint> trajectoryPoints = new List <TrajectoryPoint>();
TrajectoryPoint currentTrajectoryPoint = new TrajectoryPoint(ballPosition, force);
trajectoryPoints.Add(currentTrajectoryPoint);
for (int i = 0; i < maxSteps; i++)
{
currentTrajectoryPoint = customPhysic.GetNextTrajectoryPoint(currentTrajectoryPoint.Point, currentTrajectoryPoint.Velocity);
trajectoryPoints.Add(currentTrajectoryPoint);
}
return(trajectoryPoints);
}
public void ShouldReturnCopyFromSearch()
{
var trajectory1 = new Trajectory();
var tp1 = new TrajectoryPoint(trajectory1);
Db.Insert(trajectory1);
Db.Insert(tp1);
var founded = Db.Search <TrajectoryPoint>(tp => tp.Trajectory.Id == trajectory1.Id).First();
founded.Trajectory = new Trajectory();
var newFounded = Db.Search <TrajectoryPoint>(tp => tp.Trajectory.Id == trajectory1.Id).First();
Assert.Equal(founded.Id, newFounded.Id);
Assert.NotEqual(founded.Trajectory.Id, newFounded.Trajectory.Id);
}
public override string CreateSymbol(TrajectoryPoint point)
{
var difX = point.X - X;
var difY = point.Y - Y;
var dis = Math.Sqrt(difX * difX + difY * difY);
if (dis <= Radius)
{
return($"A{ID + 1}_Hit");
}
else if (dis <= ToleranceRadius)
{
return($"A{ID + 1}_Tolerance");
}
else
{
return(null);
}
}
private IEnumerable <ReticleElement> CalculateBdc(ReticleDefinition reticle, IEnumerable <TrajectoryPoint> trajectory, Measurement <DistanceUnit> zero, bool closeBdc, DistanceUnit distanceUnits, string color)
{
TrajectoryPoint previousPoint = null;
foreach (var point in trajectory)
{
if (!closeBdc && point.Distance < zero)
{
previousPoint = point;
continue;
}
if (closeBdc && point.Distance >= zero)
{
break;
}
if (previousPoint != null)
{
foreach (var bdcPoint in reticle.BulletDropCompensator)
{
if ((previousPoint.DropAdjustment >= bdcPoint.Position.Y &&
point.DropAdjustment <= bdcPoint.Position.Y) ||
(previousPoint.DropAdjustment <= bdcPoint.Position.Y &&
point.DropAdjustment >= bdcPoint.Position.Y))
{
var x = bdcPoint.Position.X + bdcPoint.TextOffset;
var y = bdcPoint.Position.Y - bdcPoint.TextHeight / 2;
yield return(new ReticleText()
{
Position = new ReticlePosition()
{
X = x, Y = y
},
TextHeight = bdcPoint.TextHeight,
Text = Math.Round(point.Distance.In(distanceUnits)).ToString(),
Color = color,
});
}
}
}
previousPoint = point;
}
}
public void ShouldDeleteInCascate()
{
Db.CreateOneToMany <Trajectory, TrajectoryPoint>(tp => tp.Trajectory.Id);
var trajectory = new Trajectory();
var tp1 = new TrajectoryPoint(trajectory);
var tp2 = new TrajectoryPoint(trajectory);
var tp3 = new TrajectoryPoint(trajectory);
Db.Insert(trajectory);
Db.Insert(tp1);
Db.Insert(tp2);
Db.Insert(tp3);
Db.Delete(trajectory);
Assert.True(0 == Db.Count <Trajectory>());
Assert.True(0 == Db.Count <TrajectoryPoint>());
}
void followTrajectory()
{
var point = PixelsToUnits(targetTrajPoint);
transform.position = Vector3.MoveTowards(transform.position, point, moveSpeed * Time.deltaTime);
if (transform.position == point)
{
currentTrajPoint++;
targetTrajPoint = points[currentTrajPoint];
}
head.transform.rotation = Quaternion.LookRotation(head.transform.forward) * Quaternion.Euler(targetTrajPoint.Pitch * Mathf.Rad2Deg, targetTrajPoint.Yaw * Mathf.Rad2Deg, targetTrajPoint.Roll * Mathf.Rad2Deg);
var aux = new Vector3(targetTrajPoint.Pitch * Mathf.Rad2Deg, targetTrajPoint.Yaw * Mathf.Rad2Deg, targetTrajPoint.Roll * Mathf.Rad2Deg);
float headRot = head.transform.rotation.eulerAngles.y;
float delta = headRot - bodyRot;
if (delta > 180)
{
delta -= 360;
}
if (delta < -180)
{
delta += 360;
}
if (Mathf.Abs(delta) > FOV)
{
if ((delta > FOV || delta < -180) && delta < 180)
{
bodyRot = headRot - FOV;
}
delta = headRot - bodyRot;
if ((delta < FOV || delta > 180))
{
bodyRot = headRot + FOV;
}
}
Debug.Log("Head: " + headRot + " Body:" + bodyRot);
transform.rotation = Quaternion.Euler(0, bodyRot, 0);
}
Vector3 PixelsToUnits(TrajectoryPoint point)
{
var w = viewCamera.pixelHeight;
var h = viewCamera.pixelWidth;
//Debug.Log("CAM WIDTH: " + w + " CAM HEIGHT" + h);
//READ FRAME INFO
var pixels = new Vector3(point.x, point.y);
//Debug.Log("FRAME: " + point.frame + " PIXEL POS: " + pixels);
//FRAMEPIXELS TO CAMPIXELS
var newX = point.x * viewCamera.pixelWidth / imageSize.x;
var newY = point.y * viewCamera.pixelHeight / imageSize.y;
//Debug.Log("NEW PIXELS " + newX + " " + newY);
//TRANSFORM PIXELS TO UNITs
var newpoint = viewCamera.ScreenToWorldPoint(new Vector3(newX, newY, 3));
//Debug.Log("NEW" + newpoint);
var targetUnits = new Vector3(newpoint.x, transform.position.y, newpoint.z);
return(targetUnits);
}
//ToDo: könnte für lineare Interpolation optimiert werden (Verzicht auf binäre Suche) (dann nicht mehr static)
public static TrajectoryPoint[] getPointsByDistance(IStrokeInterpolation s, double distance)
{
if (distance <= double.Epsilon)
{
throw new ArgumentException("Distanz muss größer sein", "distance");
}
var nPoints = (int)(s.ArcLength / distance);
var result = new TrajectoryPoint[nPoints];
var curArcLen = distance;
for (int i = 0; i < nPoints; i++)
{
result[i] = s.getByArcLength(curArcLen);
curArcLen += distance;
}
//what about points exactly at or close to the end?
return(result);
}
public void ShouldInsertOneToManyIfFkIsSatisfy()
{
Db.CreateOneToMany <Trajectory, TrajectoryPoint>(tp => tp.Trajectory.Id);
var trajectory = new Trajectory();
var tp1 = new TrajectoryPoint(trajectory);
var tp2 = new TrajectoryPoint(trajectory);
var tp3 = new TrajectoryPoint(trajectory);
Db.Insert(trajectory);
Db.Insert(tp1);
Db.Insert(tp2);
Db.Insert(tp3);
Assert.True(1 == Db.Count <Trajectory>());
Assert.True(3 == Db.Count <TrajectoryPoint>());
foreach (var trajectoryPoint in Db.GetAll <TrajectoryPoint>())
{
Assert.Equal(trajectory.Id, trajectoryPoint.Trajectory.Id);
}
}
public void ShouldSearch()
{
var trajectory1 = new Trajectory();
var trajectory2 = new Trajectory();
var tp1 = new TrajectoryPoint(trajectory1);
var tp2 = new TrajectoryPoint(trajectory2);
var tp3 = new TrajectoryPoint(trajectory2);
var tp4 = new TrajectoryPoint(trajectory1);
var tp5 = new TrajectoryPoint(trajectory2);
var tp6 = new TrajectoryPoint(trajectory1);
Db.Insert(trajectory1);
Db.Insert(trajectory2);
Db.Insert(tp1);
Db.Insert(tp2);
Db.Insert(tp3);
Db.Insert(tp4);
Db.Insert(tp5);
Db.Insert(tp6);
var founded = Db.Search <TrajectoryPoint>(tp => tp.Trajectory.Id == trajectory2.Id).ToList();
var expected = new List <TrajectoryPoint>()
{
tp2, tp3, tp5
};
Assert.Equal(expected.Count, founded.Count);
for (int i = 0; i < founded.Count; i++)
{
Assert.Equal(expected[i].Id, founded[i].Id);
Assert.Equal(expected[i].Trajectory.Id, founded[i].Trajectory.Id);
}
}
public void ShouldThrowExceptionWhenFkDependencyNotSatisfy()
{
Db.CreateOneToMany <Trajectory, TrajectoryPoint>(tp => tp.Trajectory.Id);
var trajectory = new Trajectory();
var tp1 = new TrajectoryPoint(trajectory);
var tp2 = new TrajectoryPoint(trajectory);
var tp3 = new TrajectoryPoint(trajectory);
Db.Insert(trajectory);
Db.Insert(tp1);
Db.Insert(tp2);
Db.Insert(tp3);
Assert.True(1 == Db.Count <Trajectory>());
Assert.True(3 == Db.Count <TrajectoryPoint>());
foreach (var trajectoryPoint in Db.GetAll <TrajectoryPoint>())
{
Assert.Equal(trajectory.Id, trajectoryPoint.Trajectory.Id);
}
tp2.Trajectory = new Trajectory();
var ex = Assert.Throws <InvalidOperationException>(() => { Db.Update(tp2); });
Assert.Equal("Invalid FK", ex.Message);
Assert.True(1 == Db.Count <Trajectory>());
Assert.True(3 == Db.Count <TrajectoryPoint>());
foreach (var trajectoryPoint in Db.GetAll <TrajectoryPoint>())
{
Assert.Equal(trajectory.Id, trajectoryPoint.Trajectory.Id);
}
}
/// <summary>
/// Pushes a new <see cref="TrajectoryPoint"/> to the Talon for Motion Profiling.
/// </summary>
/// <param name="trajPt">The TrajectoryPoint to send.</param>
/// <returns>True if successful, otherwise false.</returns>
public bool PushMotionProfileTrajectory(TrajectoryPoint trajPt)
{
if (IsMotionProfileTopLevelBufferFull())
return false;
int targPos = ScaleRotationsToNativeUnits(m_feedbackDevice, trajPt.Position);
int targVel = ScaleRotationsToNativeUnits(m_feedbackDevice, trajPt.Velocity);
int profileSlotSelect = trajPt.ProfileSlotSelect > 0 ? 1 : 0;
int timeDurMs = trajPt.TimeDurMs;
if (timeDurMs > 255)
timeDurMs = 255;
if (timeDurMs < 0)
timeDurMs = 0;
CTR_Code status = C_TalonSRX_PushMotionProfileTrajectory(m_talonPointer, targPos, targVel, profileSlotSelect, timeDurMs, trajPt.VelocityOnly ? 1 : 0,
trajPt.IsLastPoint ? 1 : 0, trajPt.ZeroPos ? 1 : 0);
CheckCTRStatus(status);
return true;
}
public static void Main()
{
/* Set neutral mode */
Hardware._rightTalon.SetNeutralMode(NeutralMode.Brake);
Hardware._leftVictor.SetNeutralMode(NeutralMode.Brake);
Hardware._leftTalon.SetNeutralMode(NeutralMode.Brake);
/** Feedback Sensor Configuration [Remote Sum] */
/* Configure the left Talon's selected sensor as local QuadEncoder */
Hardware._leftTalon.ConfigSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, // Local Feedback Source
Constants.PID_PRIMARY, // PID Slot for Source [0, 1]
Constants.kTimeoutMs); // Configuration Timeout
/* Configure the Remote Talon's selected sensor as a remote sensor for the right Talon */
Hardware._rightTalon.ConfigRemoteFeedbackFilter(Hardware._leftTalon.GetDeviceID(), // Device ID of Source
RemoteSensorSource.RemoteSensorSource_TalonSRX_SelectedSensor, // Remote Feedback Source
Constants.REMOTE_0, // Source number [0, 1]
Constants.kTimeoutMs); // Configuration Timeout
/* Setup Sum signal to be used for Distance */
Hardware._rightTalon.ConfigSensorTerm(SensorTerm.SensorTerm_Sum0, FeedbackDevice.RemoteSensor0, Constants.kTimeoutMs); // Feedback Device of Remote Talon
Hardware._rightTalon.ConfigSensorTerm(SensorTerm.SensorTerm_Sum1, FeedbackDevice.QuadEncoder, Constants.kTimeoutMs); // Quadrature Encoder of current Talon
/* Configure Sum [Sum of both QuadEncoders] to be used for Primary PID Index */
Hardware._rightTalon.ConfigSelectedFeedbackSensor(FeedbackDevice.SensorSum, Constants.PID_PRIMARY, Constants.kTimeoutMs);
/* Scale Feedback by 0.5 to half the sum of Distance */
Hardware._rightTalon.ConfigSelectedFeedbackCoefficient(0.5f, // Coefficient
Constants.PID_PRIMARY, // PID Slot of Source
Constants.kTimeoutMs); // Configuration Timeout
/* Configure output and sensor direction */
Hardware._rightTalon.SetInverted(true);
Hardware._leftVictor.SetInverted(false); // Output on victor
Hardware._rightTalon.SetSensorPhase(true);
Hardware._leftTalon.SetSensorPhase(true); // Talon only used for QuadEncoder sensor
/* Set status frame periods to ensure we don't have stale data */
Hardware._rightTalon.SetStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, 20, Constants.kTimeoutMs);
Hardware._rightTalon.SetStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, 20, Constants.kTimeoutMs);
Hardware._rightTalon.SetStatusFramePeriod(StatusFrameEnhanced.Status_10_Targets, 20, Constants.kTimeoutMs);
Hardware._leftTalon.SetStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, 10, Constants.kTimeoutMs);
/* Configure neutral deadband */
Hardware._rightTalon.ConfigNeutralDeadband(Constants.kNeutralDeadband, Constants.kTimeoutMs);
Hardware._leftVictor.ConfigNeutralDeadband(Constants.kNeutralDeadband, Constants.kTimeoutMs);
/* Motion Magic/Profile Configurations */
Hardware._rightTalon.ConfigMotionAcceleration(2000, Constants.kTimeoutMs);
Hardware._rightTalon.ConfigMotionCruiseVelocity(2000, Constants.kTimeoutMs);
/* Configure max peak output [Open and closed loop modes]
* Can use configClosedLoopPeakOutput() for only closed Loop modes
*/
Hardware._rightTalon.ConfigPeakOutputForward(+1.0f, Constants.kTimeoutMs);
Hardware._rightTalon.ConfigPeakOutputReverse(-1.0f, Constants.kTimeoutMs);
Hardware._leftVictor.ConfigPeakOutputForward(+1.0f, Constants.kTimeoutMs);
Hardware._leftVictor.ConfigPeakOutputReverse(-1.0f, Constants.kTimeoutMs);
/* FPID Gains for Motion Profile */
Hardware._rightTalon.Config_kP(Constants.kSlot_MotProf, Constants.kGains_MotProf.kP, Constants.kTimeoutMs);
Hardware._rightTalon.Config_kI(Constants.kSlot_MotProf, Constants.kGains_MotProf.kI, Constants.kTimeoutMs);
Hardware._rightTalon.Config_kD(Constants.kSlot_MotProf, Constants.kGains_MotProf.kD, Constants.kTimeoutMs);
Hardware._rightTalon.Config_kF(Constants.kSlot_MotProf, Constants.kGains_MotProf.kF, Constants.kTimeoutMs);
Hardware._rightTalon.Config_IntegralZone(Constants.kSlot_MotProf, (int)Constants.kGains_MotProf.kIzone, Constants.kTimeoutMs);
Hardware._rightTalon.ConfigClosedLoopPeakOutput(Constants.kSlot_MotProf, Constants.kGains_MotProf.kPeakOutput, Constants.kTimeoutMs);
Hardware._rightTalon.ConfigAllowableClosedloopError(Constants.kSlot_MotProf, 0, Constants.kTimeoutMs);
/* 1ms per loop. PID loop can be slowed down if need be. */
int closedLoopTimeMs = 1;
Hardware._rightTalon.ConfigSetParameter(CTRE.Phoenix.LowLevel.ParamEnum.ePIDLoopPeriod, closedLoopTimeMs, 0x00, 0, Constants.kTimeoutMs); // Primary
Hardware._rightTalon.ConfigSetParameter(CTRE.Phoenix.LowLevel.ParamEnum.ePIDLoopPeriod, closedLoopTimeMs, 0x00, 1, Constants.kTimeoutMs); // Turn (Auxiliary)
/* False means talon's local output is PID0 + PID1, and other side Talon is PID0 - PID1
* True means talon's local output is PID0 - PID1, and other side Talon is PID0 + PID1
*/
Hardware._rightTalon.ConfigAuxPIDPolarity(false, Constants.kTimeoutMs);
/* Increase the rate at which control frame is sent/recieved */
Hardware._rightTalon.ChangeMotionControlFramePeriod(5);
/* Configure base duration time of all trajectory points, which is then added to each points time duration */
Hardware._rightTalon.ConfigMotionProfileTrajectoryPeriod(0, Constants.kTimeoutMs);
/* Latched values to detect on-press events for buttons */
bool[] _btns = new bool[Constants.kNumButtonsPlusOne];
bool[] btns = new bool[Constants.kNumButtonsPlusOne];
/* Initialize */
bool _state = false;
bool _firstCall = true;
bool _direction = false;
bool _MPComplete = false;
ZeroSensors();
while (true)
{
/* Enable motor controller output if gamepad is connected */
if (Hardware._gamepad.GetConnectionStatus() == CTRE.Phoenix.UsbDeviceConnection.Connected)
{
CTRE.Phoenix.Watchdog.Feed();
}
/* Joystick processing */
float leftY = -1 * Hardware._gamepad.GetAxis(1);
float rightX = +1 * Hardware._gamepad.GetAxis(2);
CTRE.Phoenix.Util.Deadband(ref leftY);
CTRE.Phoenix.Util.Deadband(ref rightX);
/* Button processing */
Hardware._gamepad.GetButtons(ref btns);
if (btns[1] && !_btns[1])
{
/* Enter/Exit reverse direction Motion Profile */
_state = !_state;
_firstCall = true;
_direction = false; // Go reverse if X-Button pressed
}
else if (btns[3] && !_btns[3])
{
/* Enter/Exit forward direction Motion Profile */
_state = !_state;
_firstCall = true;
_direction = true; // Go forward if B-Button pressed
}
else if (btns[2] && !_btns[2])
{
/* Zero all sensors used in example, only manual when in ArcadeDrive state */
if (_state == false)
{
ZeroSensors();
}
}
System.Array.Copy(btns, _btns, Constants.kNumButtonsPlusOne);
/* Push/Clear Trajectory points */
Hardware._rightTalon.ProcessMotionProfileBuffer();
Thread.Sleep(5);
/* Update motion profile status every loop */
Hardware._rightTalon.GetMotionProfileStatus(ref _motionProfileStatus);
if (!_state)
{
if (_firstCall)
{
Debug.Print(("This is basic Arcade Drive with Arbitrary Feed-forward.\n") +
("Enter/Exit Motion Profile Mode using Button 1 or 3. (X-Button or B-Button)\n") +
("Button 1 is reverse direction and Button 3 is forward direction. FeedForward applied from left thumbstick.\n"));
}
/* Use Arbitrary FeedForward to create an Arcade Drive Control by modifying the forward output */
Hardware._rightTalon.Set(ControlMode.PercentOutput, leftY, DemandType.ArbitraryFeedForward, -rightX);
Hardware._leftVictor.Set(ControlMode.PercentOutput, leftY, DemandType.ArbitraryFeedForward, +rightX);
}
else
{
if (_firstCall)
{
Debug.Print("Motion Profile will start once all trajectory points have been pushed into the buffer.\n" +
"Custom FeedForward can be applied by the left thumb stick's Y-axis.\n");
ZeroSensors();
/* Disable Motion Profile to clear IsLast */
_motionProfileSet = SetValueMotionProfile.Disable;
Hardware._rightTalon.Set(ControlMode.MotionProfile, (int)_motionProfileSet);
Thread.Sleep(10);
/* Reset trajectory points*/
Hardware._rightTalon.ClearMotionProfileHasUnderrun();
Hardware._rightTalon.ClearMotionProfileTrajectories();
TrajectoryPoint point = new TrajectoryPoint();
/* Fill trajectory points */
for (uint i = 0; i < MotionProfile.kNumPoints; ++i)
{
/* Determine direction */
float direction = _direction ? +1 : -1;
/* Calculate Point's position, velocity, and heading */
point.position = direction * (float)MotionProfile.Points[i][0] * Constants.kSensorUnitsPerRotation; // Convert from rotations to sensor units
point.velocity = direction * (float)MotionProfile.Points[i][1] * Constants.kSensorUnitsPerRotation / 600; // Convert from RPM to sensor units per 100 ms.
point.headingDeg = 0; // Not used in this example
/* Define whether a point is first or last in trajectory buffer */
point.isLastPoint = (i + 1 == MotionProfile.kNumPoints) ? true : false;
point.zeroPos = (i == 0) ? true : false;
/* Slot Index provided through trajectory points rather than SelectProfileSlot() */
point.profileSlotSelect0 = Constants.kSlot_MotProf;
/* All points have the same duration of 10ms in this example */
point.timeDur = TrajectoryPoint.TrajectoryDuration.TrajectoryDuration_10ms;
/* Push point into buffer that will be proccessed with ProcessMotionProfileBuffer() */
Hardware._rightTalon.PushMotionProfileTrajectory(point);
}
/* Send a few points for initialization */
for (int i = 0; i < 5; ++i)
{
Hardware._rightTalon.ProcessMotionProfileBuffer();
}
_motionProfileSet = SetValueMotionProfile.Enable;
_MPComplete = false;
}
/* Telemetry */
if (_motionProfileStatus.activePointValid && _motionProfileStatus.isLast && _MPComplete == false)
{
Debug.Print("Motion Profile complete, holding final trajectory point.\n");
_MPComplete = true;
}
if (_MPComplete == false)
{
Instrument();
}
/* Calcluate FeedForward from gamepad */
float feedForward = leftY * 0.50f;
/* Configured for Motion Profile on Quad Encoders' Sum and Arbitrary FeedForward on rightY */
Hardware._rightTalon.Set(ControlMode.MotionProfile, (int)_motionProfileSet, DemandType.ArbitraryFeedForward, feedForward);
Hardware._leftVictor.Follow(Hardware._rightTalon);
}
_firstCall = false;
Thread.Sleep(5);
}
}
public static void Main()
{
/* Factory Default all hardware to prevent unexpected behaviour */
Hardware._rightTalon.ConfigFactoryDefault();
Hardware._leftTalon.ConfigFactoryDefault();
Hardware._leftVictor.ConfigFactoryDefault();
Hardware._pidgey.ConfigFactoryDefault();
/* Set neutral mode */
Hardware._rightTalon.SetNeutralMode(NeutralMode.Brake);
Hardware._leftVictor.SetNeutralMode(NeutralMode.Brake);
Hardware._leftTalon.SetNeutralMode(NeutralMode.Brake);
/** Feedback Sensor Configuration [Remote Sum and Yaw] */
/* Configure the left Talon's selected sensor as local QuadEncoder */
Hardware._leftTalon.ConfigSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, // Local Feedback Source
Constants.PID_PRIMARY, // PID Slot for Source [0, 1]
Constants.kTimeoutMs); // Configuration Timeout
/* Configure the Remote Talon's selected sensor as a remote sensor for the right Talon */
Hardware._rightTalon.ConfigRemoteFeedbackFilter(Hardware._leftTalon.GetDeviceID(), // Device ID of Source
RemoteSensorSource.RemoteSensorSource_TalonSRX_SelectedSensor, // Remote Feedback Source
Constants.REMOTE_0, // Source number [0, 1]
Constants.kTimeoutMs); // Configuration Timeout
/* Configure the Pigeon IMU to the other Remote Slot on the Right Talon */
Hardware._rightTalon.ConfigRemoteFeedbackFilter(Hardware._pidgey.GetDeviceID(),
RemoteSensorSource.RemoteSensorSource_Pigeon_Yaw,
Constants.REMOTE_1,
Constants.kTimeoutMs);
/* Setup Sum signal to be used for Distance */
Hardware._rightTalon.ConfigSensorTerm(SensorTerm.SensorTerm_Sum0, FeedbackDevice.RemoteSensor0, Constants.kTimeoutMs); // Feedback Device of Remote Talon
Hardware._rightTalon.ConfigSensorTerm(SensorTerm.SensorTerm_Sum1, FeedbackDevice.QuadEncoder, Constants.kTimeoutMs); // Quadrature Encoder of current Talon
/* Configure Sum [Sum of both QuadEncoders] to be used for Primary PID Index */
Hardware._rightTalon.ConfigSelectedFeedbackSensor(FeedbackDevice.SensorSum, Constants.PID_PRIMARY, Constants.kTimeoutMs);
/* Scale Feedback by 0.5 to half the sum of Distance */
Hardware._rightTalon.ConfigSelectedFeedbackCoefficient(0.5f, // Coefficient
Constants.PID_PRIMARY, // PID Slot of Source
Constants.kTimeoutMs); // Configuration Timeout
/* Configure Remote Slot 1 [Pigeon IMU's Yaw] to be used for Auxiliary PID Index */
Hardware._rightTalon.ConfigSelectedFeedbackSensor(FeedbackDevice.RemoteSensor1, Constants.PID_TURN, Constants.kTimeoutMs);
/* Scale the Feedback Sensor using a coefficient (Configured for 3600 units of resolution) */
Hardware._rightTalon.ConfigSelectedFeedbackCoefficient(Constants.kTurnTravelUnitsPerRotation / Constants.kPigeonUnitsPerRotation,
Constants.PID_TURN,
Constants.kTimeoutMs);
/* Configure output and sensor direction */
Hardware._rightTalon.SetInverted(true);
Hardware._leftVictor.SetInverted(false); // Output on victor
Hardware._rightTalon.SetSensorPhase(true);
Hardware._leftTalon.SetSensorPhase(true); // Talon used for QuadEncoder sensor
/* Set status frame periods to ensure we don't have stale data */
Hardware._rightTalon.SetStatusFramePeriod(StatusFrameEnhanced.Status_12_Feedback1, 10, Constants.kTimeoutMs);
Hardware._rightTalon.SetStatusFramePeriod(StatusFrameEnhanced.Status_13_Base_PIDF0, 10, Constants.kTimeoutMs);
Hardware._rightTalon.SetStatusFramePeriod(StatusFrameEnhanced.Status_14_Turn_PIDF1, 10, Constants.kTimeoutMs);
Hardware._rightTalon.SetStatusFramePeriod(StatusFrameEnhanced.Status_10_Targets, 10, Constants.kTimeoutMs);
Hardware._pidgey.SetStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 5, Constants.kTimeoutMs);
Hardware._leftTalon.SetStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, 5, Constants.kTimeoutMs);
/* Configure neutral deadband */
Hardware._rightTalon.ConfigNeutralDeadband(Constants.kNeutralDeadband, Constants.kTimeoutMs);
Hardware._leftVictor.ConfigNeutralDeadband(Constants.kNeutralDeadband, Constants.kTimeoutMs);
/* Motion Magic/Profile Configurations */
Hardware._rightTalon.ConfigMotionAcceleration(2000, Constants.kTimeoutMs);
Hardware._rightTalon.ConfigMotionCruiseVelocity(2000, Constants.kTimeoutMs);
/* Configure max peak output [Open and closed loop modes]
* Can use configClosedLoopPeakOutput() for only closed Loop modes
*/
Hardware._rightTalon.ConfigPeakOutputForward(+1.0f, Constants.kTimeoutMs);
Hardware._rightTalon.ConfigPeakOutputReverse(-1.0f, Constants.kTimeoutMs);
Hardware._leftVictor.ConfigPeakOutputForward(+1.0f, Constants.kTimeoutMs);
Hardware._leftVictor.ConfigPeakOutputReverse(-1.0f, Constants.kTimeoutMs);
/* FPID Gains for Motion Profile */
Hardware._rightTalon.Config_kP(Constants.kSlot_MotProf, Constants.kGains_MotProf.kP, Constants.kTimeoutMs);
Hardware._rightTalon.Config_kI(Constants.kSlot_MotProf, Constants.kGains_MotProf.kI, Constants.kTimeoutMs);
Hardware._rightTalon.Config_kD(Constants.kSlot_MotProf, Constants.kGains_MotProf.kD, Constants.kTimeoutMs);
Hardware._rightTalon.Config_kF(Constants.kSlot_MotProf, Constants.kGains_MotProf.kF, Constants.kTimeoutMs);
Hardware._rightTalon.Config_IntegralZone(Constants.kSlot_MotProf, (int)Constants.kGains_MotProf.kIzone, Constants.kTimeoutMs);
Hardware._rightTalon.ConfigClosedLoopPeakOutput(Constants.kSlot_MotProf, Constants.kGains_MotProf.kPeakOutput, Constants.kTimeoutMs);
Hardware._rightTalon.ConfigAllowableClosedloopError(Constants.kSlot_MotProf, 0, Constants.kTimeoutMs);
/* FPID Gains for Arc of Motion Profile */
Hardware._rightTalon.Config_kP(Constants.kSlot_Turning, Constants.kGains_Turning.kP, Constants.kTimeoutMs);
Hardware._rightTalon.Config_kI(Constants.kSlot_Turning, Constants.kGains_Turning.kI, Constants.kTimeoutMs);
Hardware._rightTalon.Config_kD(Constants.kSlot_Turning, Constants.kGains_Turning.kD, Constants.kTimeoutMs);
Hardware._rightTalon.Config_kF(Constants.kSlot_Turning, Constants.kGains_Turning.kF, Constants.kTimeoutMs);
Hardware._rightTalon.Config_IntegralZone(Constants.kSlot_Turning, (int)Constants.kGains_Turning.kIzone, Constants.kTimeoutMs);
Hardware._rightTalon.ConfigClosedLoopPeakOutput(Constants.kSlot_Turning, Constants.kGains_Turning.kPeakOutput, Constants.kTimeoutMs);
Hardware._rightTalon.ConfigAllowableClosedloopError(Constants.kSlot_Turning, 0, Constants.kTimeoutMs);
/* 1ms per loop. PID loop can be slowed down if need be. */
int closedLoopTimeMs = 1;
Hardware._rightTalon.ConfigSetParameter(CTRE.Phoenix.LowLevel.ParamEnum.ePIDLoopPeriod, closedLoopTimeMs, 0x00, 0, Constants.kTimeoutMs); // Primary
Hardware._rightTalon.ConfigSetParameter(CTRE.Phoenix.LowLevel.ParamEnum.ePIDLoopPeriod, closedLoopTimeMs, 0x00, 1, Constants.kTimeoutMs); // Turn (Auxiliary)
/* False means talon's local output is PID0 + PID1, and other side Talon is PID0 - PID1
* True means talon's local output is PID0 - PID1, and other side Talon is PID0 + PID1
*/
Hardware._rightTalon.ConfigAuxPIDPolarity(false, Constants.kTimeoutMs);
/* Increase the rate at which control frame is sent/recieved */
Hardware._rightTalon.ChangeMotionControlFramePeriod(5);
/* Configure base duration time of all trajectory points, which is then added to each points time duration */
Hardware._rightTalon.ConfigMotionProfileTrajectoryPeriod(0, Constants.kTimeoutMs);
/* Latched values to detect on-press events for buttons */
bool[] _btns = new bool[Constants.kNumButtonsPlusOne];
bool[] btns = new bool[Constants.kNumButtonsPlusOne];
/* Initialize */
int _state = 0;
bool _firstCall = true;
bool _direction = false;
bool _MPComplete = false;
double _targetHeading = 0;
ZeroSensors();
while (true)
{
/* Enable motor controller output if gamepad is connected */
if (Hardware._gamepad.GetConnectionStatus() == CTRE.Phoenix.UsbDeviceConnection.Connected)
{
CTRE.Phoenix.Watchdog.Feed();
}
/* Joystick processing */
float leftY = -1 * Hardware._gamepad.GetAxis(1);
float leftX = +1 * Hardware._gamepad.GetAxis(0);
float rightX = +1 * Hardware._gamepad.GetAxis(2);
CTRE.Phoenix.Util.Deadband(ref leftY);
CTRE.Phoenix.Util.Deadband(ref rightX);
/* Button processing */
Hardware._gamepad.GetButtons(btns);
if (btns[1] && !_btns[1])
{
/* Enter/Exit reverse direction Motion Profile */
_firstCall = true; // State change, do first call operation
if (_state == 0)
{
_state = 1; // Toggle state
_direction = false; // Go forward if B-Button pressed
}
else
{
_state = 0;
}
}
else if (btns[3] && !_btns[3])
{
/* Enter/Exit forward direction Motion Profile */
_firstCall = true;
if (_state == 0)
{
_state = 1;
_direction = true;
}
else
{
_state = 0;
}
}
else if (btns[2] && !_btns[2])
{
if (_state == 1)
{
_firstCall = true;
_state = 2;
}
}
else if (btns[4] && !_btns[4])
{
ZeroSensors(); // Zero sensors
}
System.Array.Copy(btns, _btns, Constants.kNumButtonsPlusOne);
/* Push/Clear Trajectory points */
Hardware._rightTalon.ProcessMotionProfileBuffer();
Thread.Sleep(5);
/* Update motion profile status every loop */
Hardware._rightTalon.GetMotionProfileStatus(_motionProfileStatus);
if (_state == 0)
{
if (_firstCall)
{
Debug.Print("This is basic Arcade Drive with Arbitrary Feed-forward.\n" +
"Enter/Exit Motion Profile Arc using Button 1 or 3. (X-Button or B-Button)\n" +
"Button 1 is reverse direction and Button 3 is forward direction.\n");
}
/* Use Arbitrary FeedForward to create an Arcade Drive Control by modifying the forward output */
Hardware._rightTalon.Set(ControlMode.PercentOutput, leftY, DemandType.ArbitraryFeedForward, -rightX);
Hardware._leftVictor.Set(ControlMode.PercentOutput, leftY, DemandType.ArbitraryFeedForward, +rightX);
}
else if (_state == 1)
{
if (_firstCall)
{
Debug.Print("You have entered Motion Profile Arc, now select the desired final heading.\n" +
"Select your targetheading by using the left stick X-axis [3600, -3600] and press A to start Motion Profile.\n" +
"Press either Button 1 or 2 to return back to ArcadeDriveAuxilary.\n");
}
/* Update target heading with joystick, which will be latched once user requests next state */
_targetHeading = Constants.kTurnTravelUnitsPerRotation * leftX * -1;
Debug.Print("Heading: " + _targetHeading);
}
else if (_state == 2)
{
if (_firstCall)
{
Debug.Print("Motion Profile Arc will start once all trajectory points have been pushed into the buffer.\n");
ZeroPosition();
/* Disable Motion Profile to clear IsLast */
_motionProfileSet = SetValueMotionProfile.Disable;
Hardware._rightTalon.Set(ControlMode.MotionProfile, (int)_motionProfileSet);
Thread.Sleep(10);
/* Reset trajectory points*/
Hardware._rightTalon.ClearMotionProfileHasUnderrun();
Hardware._rightTalon.ClearMotionProfileTrajectories();
TrajectoryPoint point = new TrajectoryPoint();
/* Pull the final target distance, we will use this for heading generation */
double finalPositionRot = MotionProfile.Points[MotionProfile.kNumPoints - 1][0];
/* Fill trajectory points */
for (uint i = 0; i < MotionProfile.kNumPoints; ++i)
{
/* Calculations */
double direction = _direction ? +1 : -1;
double positionRot = MotionProfile.Points[i][0];
double velocityRPM = MotionProfile.Points[i][1];
double heading = _targetHeading * positionRot / finalPositionRot; // Scale heading progress to position progress
/* Point's Position, Velocity, and Heading */
point.position = direction * positionRot * Constants.kSensorUnitsPerRotation; // Convert from rotations to sensor units
point.velocity = direction * velocityRPM * Constants.kSensorUnitsPerRotation / 600; // Convert from RPM to sensor units per 100 ms.
point.headingDeg = heading; // Convert to degrees
/* Define whether a point is first or last in trajectory buffer */
point.isLastPoint = (i + 1 == MotionProfile.kNumPoints) ? true : false;
point.zeroPos = (i == 0) ? true : false;
/* Slot Index provided through trajectory points rather than SelectProfileSlot() */
point.profileSlotSelect0 = Constants.kSlot_MotProf;
point.profileSlotSelect1 = Constants.kSlot_Turning;
/* All points have the same duration of 10ms in this example */
point.timeDur = TrajectoryPoint.TrajectoryDuration.TrajectoryDuration_10ms;
/* Push point into buffer that will be proccessed with ProcessMotionProfileBuffer() */
Hardware._rightTalon.PushMotionProfileTrajectory(point);
}
/* Send a few points for initialization */
for (int i = 0; i < 5; ++i)
{
Hardware._rightTalon.ProcessMotionProfileBuffer();
}
/* Enable Motion Profile */
_motionProfileSet = SetValueMotionProfile.Enable;
_MPComplete = false;
}
if (_motionProfileStatus.activePointValid && _motionProfileStatus.isLast && _MPComplete == false)
{
Debug.Print("Motion Profile complete, holding final trajectory point.\n");
_MPComplete = true;
}
/* Configured for Motion Profile Arc on Quad Encoders' Sum and Pigeon's Heading, */
Hardware._rightTalon.Set(ControlMode.MotionProfileArc, (int)_motionProfileSet);
Hardware._leftVictor.Follow(Hardware._rightTalon, FollowerType.AuxOutput1);
}
_firstCall = false;
Thread.Sleep(10);
}
}
/// <summary>
/// Gets the status of the currently running the motion profile.
/// </summary>
/// <returns>The status of the motion profile.</returns>
public MotionProfileStatus GetMotionProfileStatus()
{
int flags = 0;
int profileSelect = 0;
int pos = 0;
int vel = 0;
int topRem = 0;
int topCnt = 0;
int btmCnt = 0;
int outEnable = 0;
CTR_Code status = C_TalonSRX_GetMotionProfileStatus(m_talonPointer, ref flags, ref profileSelect,
ref pos, ref vel, ref topRem, ref topCnt, ref btmCnt, ref outEnable);
CheckCTRStatus(status);
bool hasUnderrun = (flags & kMotionProfileFlag_HasUnderrun) > 0 ? true : false;
bool isUnderrun = (flags & kMotionProfileFlag_IsUnderrun) > 0 ? true : false;
bool activePointValid = (flags & kMotionProfileFlag_ActTraj_IsValid) > 0 ? true : false;
bool isLastPoint = (flags & kMotionProfileFlag_ActTraj_IsLast) > 0 ? true : false;
bool isVelocityOnly = (flags & kMotionProfileFlag_ActTraj_VelOnly) > 0 ? true : false;
double position = ScaleNativeUnitsToRotations(m_feedbackDevice, pos);
double velocity = ScaleNativeUnitsToRpm(m_feedbackDevice, vel);
SetValueMotionProfile outputEnable = (SetValueMotionProfile)outEnable;
bool zeroPos = false;
int timeDurMs = 0;
TrajectoryPoint activePoint = new TrajectoryPoint()
{
IsLastPoint = isLastPoint,
VelocityOnly = isVelocityOnly,
Position = position,
Velocity = velocity,
ProfileSlotSelect = profileSelect,
ZeroPos = zeroPos,
TimeDurMs = timeDurMs
};
return new MotionProfileStatus(topRem, topCnt, btmCnt, hasUnderrun, isUnderrun,
activePointValid, activePoint, outputEnable);
}
/**
* Push another trajectory point into the top level buffer (which is emptied into
* the Talon's bottom buffer as room allows).
* @param trajPt the trajectory point to insert into buffer.
* @return true if trajectory point push ok. CTR_BufferFull if buffer is full
* due to kMotionProfileTopBufferCapacity.
*/
public bool PushMotionProfileTrajectory(TrajectoryPoint trajPt)
{
/* convert positiona and velocity to native units */
Int32 targPos = ScaleRotationsToNativeUnits(m_feedbackDevice, trajPt.position);
Int32 targVel = ScaleVelocityToNativeUnits(m_feedbackDevice, trajPt.velocity);
/* bounds check signals that require it */
int profileSlotSelect = (trajPt.profileSlotSelect > 0) ? 1 : 0;
byte timeDurMs = 255;
if(trajPt.timeDurMs < 255)
timeDurMs = (byte)trajPt.timeDurMs; /* cap time to 255ms */
/* send it to the top level buffer */
int status = m_impl.PushMotionProfileTrajectory( (int)targPos,
(int)targVel,
(int)profileSlotSelect,
(int)timeDurMs,
trajPt.velocityOnly ? 1:0,
trajPt.isLastPoint ? 1 : 0,
trajPt.zeroPos ? 1 : 0);
return (status == CAN_OK) ? true : false;
}
public MotionProfileStatus(int topRem, int topCnt, int btmCnt, bool hasUnder,
bool isUnder, bool activeValid, TrajectoryPoint activePoint, SetValueMotionProfile outEnable)
{
TopBufferRem = topRem;
TopBufferCnt = topCnt;
BtmBufferCnt = btmCnt;
HasUnderrun = hasUnder;
IsUnderrun = isUnder;
ActivePointValid = activeValid;
ActivePoint = activePoint;
OutputEnable = outEnable;
}
