cporter · May 19, 2020 04:36 · May 19, 2020
diff --git a/lemons silly hat controller b/lemons silly hat controller
@@ -0,0 +1,179 @@
+#include <Wire.h>
+#include <Adafruit_VL53L0X.h>
+#include <Joystick.h>
+
+Joystick_ Joystick(JOYSTICK_DEFAULT_REPORT_ID, 
+  JOYSTICK_TYPE_MULTI_AXIS, 4, 0,
+  false, false, false, false, false, false,
+  false, false, true, true, true);
+
+const int TCAADDR = 0x70;
+
+void tcaselect(uint8_t i) {
+  if (i > 7) return;
+
+  Wire.beginTransmission(TCAADDR);
+  Wire.write(1 << i);
+  Wire.endTransmission();  
+}
+
+Adafruit_VL53L0X wheel_lox = Adafruit_VL53L0X();
+Adafruit_VL53L0X pedal_lox = Adafruit_VL53L0X();
+
+const int WHEEL_CHAN = 7;
+const int PEDAL_CHAN = 5;
+
+const int PEDAL_MAX = 260;
+const int WHEEL_MAX = 1000;
+
+void setup() {
+  Serial.begin(115200);
+
+  Serial.println("Setting up");
+
+  // wait until serial port opens for native USB devices
+  while (! Serial) {
+    delay(1);
+  }
+
+  Serial.println("Setting up lidars");
+
+  tcaselect(WHEEL_CHAN);
+  if (!wheel_lox.begin()) {
+    Serial.println("Could not initialize wheel");
+    while (1);
+  }
+
+  tcaselect(PEDAL_CHAN);
+  if (! pedal_lox.begin()) {
+    Serial.println("Could not initialize pedal");
+    while (1);
+  }
+
+  Joystick.setAcceleratorRange(0, PEDAL_MAX);
+  Joystick.setBrakeRange(0, PEDAL_MAX);
+  Joystick.setSteeringRange(0, WHEEL_MAX);
+
+  Joystick.begin(false);
+
+  center();
+  brake(1);
+}
+
+float clamp(float x) {
+  if (x < 0.0) {
+    return 0.0;
+  } else if (x > 1.0) {
+    return 1.0;
+  } else {
+    return x;
+  }
+}
+
+void brake(float x) {
+  Joystick.setAccelerator(0);
+  Joystick.setBrake(PEDAL_MAX * clamp(x));
+}
+
+void gas(float x) {
+  Joystick.setAccelerator(PEDAL_MAX * clamp(x));
+  Joystick.setBrake(0);
+}
+
+void coast() {
+  Joystick.setAccelerator(0);
+  Joystick.setBrake(0);
+
+}
+
+void left(float x) {
+  Joystick.setSteering((1 - clamp(x)) * (WHEEL_MAX / 2));
+}
+
+void right(float x) {
+  Joystick.setSteering((1 + clamp(x)) * (WHEEL_MAX / 2));
+}
+
+void center() {
+  Joystick.setSteering(WHEEL_MAX / 2);
+}
+
+const float BRAKE_THRESH = 250;
+const float GAS_THRESH = 150;
+const float ACCEL_MAX = 100;
+
+void pedal() {
+  VL53L0X_RangingMeasurementData_t measure;
+
+  tcaselect(PEDAL_CHAN);
+  pedal_lox.rangingTest(&measure, false); // pass in 'true' to get debug data printout!
+
+  if (measure.RangeStatus != 4) {  // phase failures have incorrect data
+
+    float range = measure.RangeMilliMeter;
+
+    if (range < GAS_THRESH) {
+      float bx = (GAS_THRESH - range) / ACCEL_MAX;
+      brake(bx);
+      Serial.print("GAS ");
+      Serial.println(bx);
+    } else if (range > BRAKE_THRESH) {
+      float gx = (range - BRAKE_THRESH)/ ACCEL_MAX;
+      gas(gx);
+      Serial.print("BRAKE ");
+      Serial.println(gx);
+    } else {
+      Serial.println("Coast (middle)");
+      coast();
+    }
+  } else {
+    coast();
+    Serial.println("Coast (out of range)");
+  }
+}
+
+const float STEER_RANGE = 150;
+const float LEFT_MAX = 210;
+const float LEFT_MIN = LEFT_MAX - STEER_RANGE;
+const float RIGHT_MIN = 250;
+const float RIGHT_MAX = RIGHT_MIN + STEER_RANGE;
+
+void wheel() {
+  VL53L0X_RangingMeasurementData_t measure;
+
+  tcaselect(WHEEL_CHAN);
+  wheel_lox.rangingTest(&measure, false); // pass in 'true' to get debug data printout!
+  float distance = measure.RangeMilliMeter;
+
+  if ((measure.RangeStatus == 4) || (2*RIGHT_MAX < distance)) {
+    Serial.println("Center");
+    return;
+  }
+
+  Serial.print(distance);
+  Serial.print("\t");
+
+  if (distance <= LEFT_MAX) {
+    // Allow you to go all the way to the sensor. (Clamped 0-1, anyway.)
+    float x = (LEFT_MAX - distance) / (LEFT_MAX - LEFT_MIN);
+    left(x);
+    Serial.print("left ");
+    Serial.println(x);
+  } else if (RIGHT_MIN <= distance && distance <= (2 * RIGHT_MAX)) {
+    // Allow a pretty good amount of extra range for going off to the right
+    // (we clamp everything between 0-1, anyway.)
+    float x = (distance - RIGHT_MIN) / (RIGHT_MAX - RIGHT_MIN);
+    right(x);
+    Serial.print("right ");
+    Serial.println(x);
+  } else {
+    Serial.println("Center (middle)");
+    center();
+  }
+}
+
+void loop() {
+  wheel();
+  pedal();
+  Joystick.sendState();
+}