lemons silly hat controller

#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();
}