Skip to content
This repository has been archived by the owner on Apr 23, 2023. It is now read-only.

Commit

Permalink
distance sensor localisation working i think
Browse files Browse the repository at this point in the history
  • Loading branch information
@chop0
chop0 committed Apr 8, 2022
1 parent 2e18fb3 commit 67c3b0e
Show file tree
Hide file tree
Showing 11 changed files with 319 additions and 159 deletions.
57 changes: 22 additions & 35 deletions ...orward/robot/sensors/DistanceSensors.java → ...obot/sensors/DistanceSensorLocaliser.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,28 +2,28 @@

import static com.kuriosityrobotics.firstforward.robot.util.Constants.Field.FULL_FIELD;
import static java.lang.Math.pow;
import static java.lang.Math.toDegrees;
import static java.lang.Math.toRadians;

import com.kuriosityrobotics.firstforward.robot.LocationProvider;
import com.kuriosityrobotics.firstforward.robot.modules.Module;
import com.kuriosityrobotics.firstforward.robot.sensors.kf.ExtendedKalmanFilter;
import com.kuriosityrobotics.firstforward.robot.util.math.Pose;
import com.qualcomm.robotcore.hardware.DistanceSensor;
import com.kuriosityrobotics.firstforward.robot.util.wrappers.SharpIRDistance;
import com.qualcomm.robotcore.hardware.AnalogInput;
import com.qualcomm.robotcore.hardware.HardwareMap;

import org.apache.commons.geometry.euclidean.twod.Lines;
import org.apache.commons.geometry.euclidean.twod.Segment;
import org.apache.commons.geometry.euclidean.twod.Vector2D;
import org.apache.commons.numbers.core.Precision;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.ojalgo.matrix.Primitive64Matrix;

import java.util.ArrayList;
import java.util.List;

public class DistanceSensors implements Module, LocationProvider {
public static final double SENSOR_X_DISPLACEMENT = 5.354;
public class DistanceSensorLocaliser implements Module, LocationProvider {
public static final double SENSOR_X_DISPLACEMENT_RIGHT = 5.354;
public static final double SENSOR_X_DISPLACEMENT_LEFT = 4.75;
public static final double BACK_SENSORS_Y_DISPLACEMENT = 5.6345;
public static final double FRONT_SENSORS_Y_DISPLACEMENT = 2.9505;
private static final double DISTANCE_SENSOR_MARGIN_OF_ERROR = .01;
Expand All @@ -32,21 +32,24 @@ public class DistanceSensors implements Module, LocationProvider {

private final LocationProvider locationProvider;
private final ExtendedKalmanFilter filter;
private final DistanceSensor frontLeft, backLeft, frontRight, backRight;
private final SharpIRDistance frontLeft, backLeft/*, frontRight, backRight*/;

private double x, y, heading;

public DistanceSensors(HardwareMap hardwareMap, LocationProvider locationProvider, ExtendedKalmanFilter filter) {
public DistanceSensorLocaliser(LocationProvider locationProvider, ExtendedKalmanFilter filter,
SharpIRDistance frontLeft, SharpIRDistance backLeft) {
this.locationProvider = locationProvider;
this.filter = filter;

this.frontLeft = hardwareMap.get(DistanceSensor.class, "frontLeft");
this.backLeft = hardwareMap.get(DistanceSensor.class, "backLeft");
this.frontRight = hardwareMap.get(DistanceSensor.class, "frontRight");
this.backRight = hardwareMap.get(DistanceSensor.class, "backRight");
this.frontLeft = frontLeft;
this.backLeft = backLeft;
// this.frontRight = hardwareMap.get(DistanceSensor.class, "frontRight");
// this.backRight = hardwareMap.get(DistanceSensor.class, "backRight");
}

private void processPartialState(Double[] sensorData) {
if (sensorData == null)
return;
heading = sensorData[2];

Primitive64Matrix H;
Expand Down Expand Up @@ -80,24 +83,24 @@ private void processPartialState(Double[] sensorData) {

public void update() {
{
var bestWallLeft = DistanceSensorPair.LEFT.bestWall(locationProvider.getPose());
var bestWallLeft = DistanceSensorPair.LEFT.bestWall(Pose.of(filter.getVariance()), locationProvider.getPose());
if (bestWallLeft != null)
processPartialState(DistanceSensorPair.LEFT.getPartialState(
frontLeft.getDistance(DistanceUnit.INCH),
backLeft.getDistance(DistanceUnit.INCH),
frontLeft.getDistance(),
backLeft.getDistance(),
bestWallLeft
));
}

{
/* {
var bestWallRight = DistanceSensorPair.RIGHT.bestWall(locationProvider.getPose());
if (bestWallRight != null)
processPartialState(DistanceSensorPair.RIGHT.getPartialState(
frontRight.getDistance(DistanceUnit.INCH),
backRight.getDistance(DistanceUnit.INCH),
bestWallRight
));
}
}*/
}

@Override
Expand All @@ -107,7 +110,7 @@ public boolean isOn() {

@Override
public String getName() {
return "DistanceSensors";
return "DistanceSensorLocaliser";
}

@Override
Expand All @@ -117,13 +120,13 @@ public Pose getPose() {

@Override
public Pose getVelocity() {
throw new UnsupportedOperationException("DistanceSensors cannot provide robot velocity.");
throw new UnsupportedOperationException("DistanceSensorLocaliser cannot provide robot velocity.");
}

@Override
public List<String> getTelemetryData() {
return new ArrayList<>() {{
add("Robot pose: " + getPose());
add(getPose().toString("Robot pose"));
}};
}

Expand All @@ -132,20 +135,4 @@ public int maxFrequency() {
return 10;
}

public enum Wall {
RIGHT(Lines.segmentFromPoints(Vector2D.of(0, 0), Vector2D.of(0, FULL_FIELD), p)),
LEFT(Lines.segmentFromPoints(Vector2D.of(FULL_FIELD, 0), Vector2D.of(FULL_FIELD, FULL_FIELD), p)),
FRONT(Lines.segmentFromPoints(Vector2D.of(0, FULL_FIELD), Vector2D.of(FULL_FIELD, FULL_FIELD), p)),
BACK(Lines.segmentFromPoints(Vector2D.of(0, 0), Vector2D.of(FULL_FIELD, 0), p));

private final Segment segment;

Wall(Segment segment) {
this.segment = segment;
}

public Segment getSegment() {
return segment;
}
}
}
92 changes: 53 additions & 39 deletions ...de/src/main/java/com/kuriosityrobotics/firstforward/robot/sensors/DistanceSensorPair.java
View file
Original file line number Diff line number Diff line change
@@ -1,24 +1,30 @@
package com.kuriosityrobotics.firstforward.robot.sensors;

import static com.kuriosityrobotics.firstforward.robot.util.Constants.Field.FULL_FIELD;
import static com.kuriosityrobotics.firstforward.robot.util.Constants.Units.INCH_PER_MM;
import static com.kuriosityrobotics.firstforward.robot.util.math.MathUtil.inRange;
import static com.kuriosityrobotics.firstforward.robot.util.math.MathUtil.rotate;
import static java.lang.Math.PI;
import static java.lang.Math.atan2;
import static java.lang.Math.cos;
import static java.lang.Math.sqrt;

import com.kuriosityrobotics.firstforward.robot.sensors.kf.ExtendedKalmanFilter;
import com.kuriosityrobotics.firstforward.robot.util.math.Point;
import com.kuriosityrobotics.firstforward.robot.util.math.Pose;

import org.apache.commons.geometry.euclidean.twod.Lines;
import org.apache.commons.geometry.euclidean.twod.Ray;
import org.apache.commons.geometry.euclidean.twod.Vector2D;

public enum DistanceSensorPair {
RIGHT(
Vector2D.of(-DistanceSensors.SENSOR_X_DISPLACEMENT, -DistanceSensors.BACK_SENSORS_Y_DISPLACEMENT),
Vector2D.of(-DistanceSensors.SENSOR_X_DISPLACEMENT, DistanceSensors.FRONT_SENSORS_Y_DISPLACEMENT)),
LEFT(Vector2D.of(DistanceSensors.SENSOR_X_DISPLACEMENT, -DistanceSensors.BACK_SENSORS_Y_DISPLACEMENT),
Vector2D.of(DistanceSensors.SENSOR_X_DISPLACEMENT, DistanceSensors.FRONT_SENSORS_Y_DISPLACEMENT));
Vector2D.of(DistanceSensorLocaliser.SENSOR_X_DISPLACEMENT_RIGHT, -DistanceSensorLocaliser.BACK_SENSORS_Y_DISPLACEMENT),
Vector2D.of(DistanceSensorLocaliser.SENSOR_X_DISPLACEMENT_RIGHT, DistanceSensorLocaliser.FRONT_SENSORS_Y_DISPLACEMENT)),
LEFT(Vector2D.of(-DistanceSensorLocaliser.SENSOR_X_DISPLACEMENT_LEFT, -DistanceSensorLocaliser.BACK_SENSORS_Y_DISPLACEMENT),
Vector2D.of(-DistanceSensorLocaliser.SENSOR_X_DISPLACEMENT_LEFT, DistanceSensorLocaliser.FRONT_SENSORS_Y_DISPLACEMENT));

private static final double MIN_SENSOR_DISTANCE = 1;
private static final double MAX_SENSOR_DISTANCE = 7.75;

private static Vector2D c(Point p) {
return Vector2D.of(p.x, p.y);
Expand All @@ -32,23 +38,23 @@ private static Vector2D c(Point p) {
}

public static double getWallHeading(double frontSensor, double backSensor) {
return atan2(frontSensor - backSensor, DistanceSensors.FRONT_SENSORS_Y_DISPLACEMENT + DistanceSensors.BACK_SENSORS_Y_DISPLACEMENT);
return atan2(frontSensor - backSensor, DistanceSensorLocaliser.FRONT_SENSORS_Y_DISPLACEMENT + DistanceSensorLocaliser.BACK_SENSORS_Y_DISPLACEMENT);
}

public static double getWallDistance(double frontSensor, double backSensor) {
double wallHeading = getWallHeading(frontSensor, backSensor);
double d1 = frontSensor * cos(wallHeading);
double d2 = backSensor * cos(wallHeading);
double A = DistanceSensors.FRONT_SENSORS_Y_DISPLACEMENT / (DistanceSensors.FRONT_SENSORS_Y_DISPLACEMENT + DistanceSensors.BACK_SENSORS_Y_DISPLACEMENT);
double B = DistanceSensors.BACK_SENSORS_Y_DISPLACEMENT / (DistanceSensors.FRONT_SENSORS_Y_DISPLACEMENT + DistanceSensors.BACK_SENSORS_Y_DISPLACEMENT);
double A = DistanceSensorLocaliser.FRONT_SENSORS_Y_DISPLACEMENT / (DistanceSensorLocaliser.FRONT_SENSORS_Y_DISPLACEMENT + DistanceSensorLocaliser.BACK_SENSORS_Y_DISPLACEMENT);
double B = DistanceSensorLocaliser.BACK_SENSORS_Y_DISPLACEMENT / (DistanceSensorLocaliser.FRONT_SENSORS_Y_DISPLACEMENT + DistanceSensorLocaliser.BACK_SENSORS_Y_DISPLACEMENT);

double d = A * d1 + B * d2;
double dOffset = DistanceSensors.SENSOR_X_DISPLACEMENT / cos(wallHeading);
double dOffset = DistanceSensorLocaliser.SENSOR_X_DISPLACEMENT_RIGHT / cos(wallHeading);

return d + dOffset;
}

public Double[] getPartialState(double frontSensor, double backSensor, DistanceSensors.Wall wall) {
public Double[] getPartialState(double frontSensor, double backSensor, Wall wall) {
var wallHeading = getWallHeading(frontSensor, backSensor);
var wallDistance = getWallDistance(frontSensor, backSensor);

Expand All @@ -57,22 +63,22 @@ public Double[] getPartialState(double frontSensor, double backSensor, DistanceS
switch (wall) {
case FRONT:
wallDistance = FULL_FIELD - wallDistance;
if (this == LEFT) {
if (this == RIGHT) {
heading = PI / 2 + wallHeading;
} else {
heading = -PI / 2 - wallHeading;
}
break;
case RIGHT:
wallDistance = FULL_FIELD - wallDistance;
if (this == LEFT) {
heading = PI + wallHeading;
} else {
if (this == RIGHT) {
heading = -wallHeading;
} else {
heading = PI + wallHeading;
}
break;
case BACK:
if (this == LEFT) {
if (this == RIGHT) {
heading = -PI / 2 + wallHeading;
} else {
heading = PI / 2 - wallHeading;
Expand All @@ -88,41 +94,49 @@ public Double[] getPartialState(double frontSensor, double backSensor, DistanceS
}

Double[] pose;
if (wall == DistanceSensors.Wall.FRONT || wall == DistanceSensors.Wall.BACK)
if (wall == Wall.FRONT || wall == Wall.BACK)
pose = new Double[]{null, wallDistance, heading};
else
pose = new Double[]{wallDistance, null, heading};

return pose;
}

public DistanceSensors.Wall bestWall(Pose robotPose) {
public Wall bestWall(Pose covariance, Pose robotPose) {
var sensor1 = c(robotPose).add(rotate(offset1, robotPose.heading));
var sensor2 = c(robotPose).add(rotate(offset2, robotPose.heading));

var sensorHeading = this == RIGHT ? PI - robotPose.heading : -robotPose.heading;

var sensor1Line = Lines.fromPointAndAngle(sensor1, sensorHeading, DistanceSensors.p);
var sensor1Segment = sensor1Line.segment(
sensor1.add(sensor1Line.getDirection().multiply(40 * INCH_PER_MM)),
sensor1.add(sensor1Line.getDirection().multiply(300 * INCH_PER_MM))
);

var sensor2Line = Lines.fromPointAndAngle(sensor2, sensorHeading, DistanceSensors.p);
var sensor2Segment = sensor2Line.segment(
sensor2.add(sensor2Line.getDirection().multiply(40 * INCH_PER_MM)),
sensor2.add(sensor2Line.getDirection().multiply(300 * INCH_PER_MM))
);

if (sensor1Segment.intersection(DistanceSensors.Wall.LEFT.getSegment()) != null && sensor2Segment.intersection(DistanceSensors.Wall.LEFT.getSegment()) != null)
return DistanceSensors.Wall.LEFT;
else if (sensor1Segment.intersection(DistanceSensors.Wall.RIGHT.getSegment()) != null && sensor2Segment.intersection(DistanceSensors.Wall.RIGHT.getSegment()) != null)
return DistanceSensors.Wall.RIGHT;
else if (sensor1Segment.intersection(DistanceSensors.Wall.FRONT.getSegment()) != null && sensor2Segment.intersection(DistanceSensors.Wall.FRONT.getSegment()) != null)
return DistanceSensors.Wall.FRONT;
else if (sensor1Segment.intersection(DistanceSensors.Wall.BACK.getSegment()) != null && sensor2Segment.intersection(DistanceSensors.Wall.BACK.getSegment()) != null)
return DistanceSensors.Wall.BACK;
var sensorHeading = PI/2 + (this == RIGHT ? -(robotPose.heading + PI/2) : -(robotPose.heading - PI/2));

var sensor1Line = Lines.fromPointAndAngle(sensor1, sensorHeading, DistanceSensorLocaliser.p);
var sensor1Segment = sensor1Line.rayFrom(sensor1);

var sensor2Line = Lines.fromPointAndAngle(sensor2, sensorHeading, DistanceSensorLocaliser.p);
var sensor2Segment = sensor2Line.rayFrom(sensor2);

if (wallInRangeOfRays(covariance, robotPose.heading, Wall.LEFT, sensor1Segment, sensor2Segment))
return Wall.LEFT;
else if (wallInRangeOfRays(covariance, robotPose.heading, Wall.RIGHT, sensor1Segment, sensor2Segment))
return Wall.RIGHT;
else if (wallInRangeOfRays(covariance, robotPose.heading, Wall.FRONT, sensor1Segment, sensor2Segment))
return Wall.FRONT;
else if (wallInRangeOfRays(covariance, robotPose.heading, Wall.BACK, sensor1Segment, sensor2Segment))
return Wall.BACK;
else
return null;
}

private boolean wallInRangeOfRays(Pose variance, double heading, Wall wall, Ray... rays) {
for (var sensorRay : rays) {
var wallIntersection = sensorRay.intersection(wall.getSegment());
if (wallIntersection == null)
return false;

var wallDistance = sensorRay.getStartPoint().distance(wallIntersection);
if (!inRange(MIN_SENSOR_DISTANCE - variance.norm(), MAX_SENSOR_DISTANCE + variance.norm(), wallDistance))
return false;
}

return true;
}
}
Loading

0 comments on commit 67c3b0e

Please sign in to comment.