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; }