Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 20 additions & 0 deletions TeamCode/src/main/java/suitbots/math/Stats.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,20 @@
package suitbots.math;

public class Stats {
public static double mean(final double[] xs) {
double sum = 0.0;
for (final double x : xs) {
sum += x;
}
return sum / xs.length;
}

public static double stddev(final double[] xs) {
final double mean = mean(xs);
double sum = 0.0;
for (final double x : xs) {
sum += Math.pow(x - mean, 2.0);
}
return Math.sqrt(sum / xs.length);
}
}
37 changes: 37 additions & 0 deletions TeamCode/src/main/java/suitbots/opmode/FlexSensorTest.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,37 @@
package suitbots.opmode;

import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

import suitbots.sensor.FlexSensor;

@TeleOp(name = "Flex")
public class FlexSensorTest extends OpMode {
private FlexSensor flex;
private int flexCount;

@Override
public void init() {
flex = new FlexSensor(hardwareMap.analogInput.get("flex"), .3);
}

@Override
public void init_loop() {
super.init_loop();
flex.init_loop();
telemetry.addData("Flex sensor ready", flex.hasBaseline() ? "YES" : "no.");
telemetry.update();
}

@Override
public void loop() {
flex.loop();
if (flex.triggeredOnce()) {
++flexCount;
}
telemetry.addData("Flex Sensor", flex.voltage());
telemetry.addData("Flex Count", flexCount);
flex.dump(telemetry);
telemetry.update();
}
}
80 changes: 80 additions & 0 deletions TeamCode/src/main/java/suitbots/sensor/FlexSensor.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,80 @@
package suitbots.sensor;


import com.qualcomm.robotcore.hardware.AnalogInput;

import org.firstinspires.ftc.robotcore.external.Telemetry;

import suitbots.math.Stats;

public class FlexSensor {
private static final int BASELINE_BUFFER_SIZE = 128;
private static final int CURRENT_BUFFER_SIZE = 32;

private final AnalogInput inputDevice;
private double[] baseline;
private double baselineMemo = Double.NaN;
private double baselineStddev;
private double[] current;
private int bi, ci;
private double changeThreshold;

private int triggeredCount = 0;


public FlexSensor(AnalogInput _input, final double _changeThreshold) {
inputDevice = _input;
changeThreshold = _changeThreshold;
baseline = new double[BASELINE_BUFFER_SIZE];
current = new double[CURRENT_BUFFER_SIZE];
}

public boolean hasBaseline() {
return bi > baseline.length;
}

private boolean hasCurrent() {
return ci > current.length;
}

private void setupAfterInit() {
if (Double.isNaN(baselineMemo)) {
baselineMemo = Stats.mean(baseline);
}
}

public double voltage() {
return inputDevice.getVoltage();
}

public void init_loop() {
baseline[bi++ % baseline.length] = voltage();
}

public void loop() {
setupAfterInit();
current[ci++ % current.length] = voltage();
if (hasCurrent()) {
final double c = Stats.mean(current);
if (changeThreshold < Math.abs(1.0 - c / baselineMemo)) {
++triggeredCount;
} else {
triggeredCount = 0;
}
}
}

public boolean triggered() {
return 0 < triggeredCount;
}

public boolean triggeredOnce() {
return 1 == triggeredCount;
}

public void dump(Telemetry t) {
t.addData("Baseline", baselineMemo);
t.addData("Current Ratio", Stats.mean(current) / baselineMemo);
t.addData("Current Noise", Stats.stddev(current));
}
}