5 changed files
src/com/_604robotics/robotnik/prefabs/motion | ||
MotionConstraints.java + | ||
MotionState.java + | ||
TrapezoidalMotionProfile.java + | ||
test/robotnik/prefabs/motion | ||
TrapezoidalMotionProfileTest.java + | ||
build.gradle | ||
MotionConstraints.java
/src/com/_604robotics/robotnik/prefabs/motion/MotionConstraints.java+12/src/com/_604robotics/robotnik/prefabs/motion/MotionConstraints.java
Add comment 1 Plus package com._604robotics.robotnik.prefabs.motion;
Add comment 2 Plus
Add comment 3 Plus public class MotionConstraints {
Add comment 4 Plus public double maxVelocity;
Add comment 5 Plus public double maxAcceleration;
Add comment 6 Plus
Add comment 7 Plus public MotionConstraints(double maxVelocity, double maxAcceleration){
Add comment 8 Plus this.maxVelocity = maxVelocity;
Add comment 9 Plus this.maxAcceleration = maxAcceleration;
Add comment 10 Plus }
Add comment 11 Plus }
Add comment 12 Plus
MotionState.java
/src/com/_604robotics/robotnik/prefabs/motion/MotionState.java+16/src/com/_604robotics/robotnik/prefabs/motion/MotionState.java
Add comment 1 Plus package com._604robotics.robotnik.prefabs.motion;
Add comment 2 Plus
Add comment 3 Plus public class MotionState {
Add comment 4 Plus public double position;
Add comment 5 Plus public double velocity;
Add comment 6 Plus
Add comment 7 Plus public MotionState(double position, double velocity){
Add comment 8 Plus this.position = position;
Add comment 9 Plus this.velocity = velocity;
Add comment 10 Plus }
Add comment 11 Plus
Add comment 12 Plus public MotionState(){
Add comment 13 Plus this(0.0, 0.0);
Add comment 14 Plus }
Add comment 15 Plus }
Add comment 16 Plus
TrapezoidalMotionProfile.java
/src/com/_604robotics/robotnik/prefabs/motion/TrapezoidalMotionProfile.java+73/src/com/_604robotics/robotnik/prefabs/motion/TrapezoidalMotionProfile.java
Add comment 1 Plus package com._604robotics.robotnik.prefabs.motion;
Add comment 2 Plus
Add comment 3 Plus import javax.lang.model.element.VariableElement;
Add comment 4 Plus
Add comment 5 Plus public class TrapezoidalMotionProfile {
Add comment 6 Plus private MotionState initial;
Add comment 7 Plus private MotionState goal;
Add comment 8 Plus
Add comment 9 Plus private double maxVel;
Add comment 10 Plus private double maxAccel;
Add comment 11 Plus
Add comment 12 Plus private double areaAccel;
Add comment 13 Plus
Add comment 14 Plus private double t_1;
Add comment 15 Plus private double t_2 = 0.0;
Add comment 16 Plus
Add comment 17 Plus public TrapezoidalMotionProfile(MotionState initial, MotionState goal, MotionConstraints constraints){
Add comment 18 Plus this.initial = initial;
Add comment 19 Plus this.goal = goal;
Add comment 20 Plus
Add comment 21 Plus this.maxVel = Math.abs(constraints.maxVelocity);
Add comment 22 Plus this.maxAccel = Math.abs(constraints.maxAcceleration);
Add comment 23 Plus
Add comment 24 Plus if(goal.position < initial.position){
Add comment 25 Plus maxAccel = -maxAccel;
Add comment 26 Plus maxVel = -maxVel;
Add comment 27 Plus }
Add comment 28 Plus
Add comment 29 Plus t_1 = Math.abs((maxVel - initial.velocity) / maxAccel);
Add comment 30 Plus
Add comment 31 Plus areaAccel = areaTriangle(t_1, maxVel);
Add comment 32 Plus
Add comment 33 Plus if((Math.abs(areaAccel) * 2) > Math.abs(goal.position)){
Add comment 34 Plus areaAccel = 0.5 * goal.position;
Add comment 35 Plus
Add comment 36 Plus t_1 = Math.abs(((-0.5 * initial.velocity) + Math.sqrt((0.25 * Math.pow(initial.velocity, 2)) - (2 * maxAccel * (-areaAccel)))) / (maxAccel));
Add comment 37 Plus } else {
Add comment 38 Plus t_2 = Math.abs((Math.abs(goal.position) - (2 * Math.abs(areaAccel))) / (maxVel));
Add comment 39 Plus }
Add comment 40 Plus
Add comment 41 Plus }
Add comment 42 Plus
Add comment 43 Plus public MotionState sample(double t){
Add comment 44 Plus double position;
Add comment 45 Plus double velocity;
Add comment 46 Plus
Add comment 47 Plus if(t <= t_1){
Add comment 48 Plus velocity = maxAccel * t + initial.position;
Add comment 49 Plus position = areaTriangle(t, velocity);
Add comment 50 Plus } else if(t_1 < t && t <= (t_1 + t_2)){
Add comment 51 Plus velocity = maxVel;
Add comment 52 Plus position = areaAccel + (velocity * (t - t_1));
Add comment 53 Plus } else if ((t_1 + t_2) < t && t <= (getTotalTime())){
Add comment 54 Plus velocity = (-maxAccel * t) - (-maxAccel * (t_1 + t_2)) + ((maxAccel * t_1) + initial.position);
Add comment 55 Plus position = areaAccel + (maxVel * t_2) + (areaAccel - areaTriangle((getTotalTime() - t), velocity));
Add comment 56 Plus } else {
Add comment 57 Plus position = goal.position;
Add comment 58 Plus velocity = goal.velocity;
Add comment 59 Plus }
Add comment 60 Plus
Add comment 61 Plus return new MotionState(position, velocity);
Add comment 62 Plus }
Add comment 63 Plus
Add comment 64 Plus public double getTotalTime(){
Add comment 65 Plus return t_2 + 2 * t_1;
Add comment 66 Plus }
Add comment 67 Plus
Add comment 68 Plus private double areaTriangle(double b, double h){
Add comment 69 Plus return 0.5 * b * h;
Add comment 70 Plus }
Add comment 71 Plus
Add comment 72 Plus }
Add comment 73 Plus
TrapezoidalMotionProfileTest.java
/test/robotnik/prefabs/motion/TrapezoidalMotionProfileTest.java/test/robotnik/prefabs/motion/TrapezoidalMotionProfileTest.java