gistfile1.txt

#include <Arduino.h>
#include <stdint.h>
#include "SCMD.h"
#include "SCMD_config.h" //Contains #defines for common SCMD register names and values
#include "Wire.h"
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1327.h>
#include "Adafruit_VL53L1X.h"
SCMD driver;

int motorSpeed = 250;
int turnSpeed = 75;

#define TCAADDR 0x70

Adafruit_VL53L1X distance_right;  // Distance #1
Adafruit_VL53L1X distance_front;  // Distance #2

int16_t d1 = -1;
int16_t d2 = -1;
int16_t front_sensor_avg = 0;
int16_t right_sensor_avg = 0;

int continue_distance = 



void setup() {
  delay(15000);
  Serial.begin(115200);
  while (!Serial) delay(10);
  Wire.begin();
  
  tcaselect(3);      // TCA channel for bme1
  distance_right.begin();      // use the default address of 0x77
  Serial.println(F("distance 1 sensor OK!"));
  if (! distance_right.startRanging()) {
    Serial.print(F("Couldn't start ranging: "));
    Serial.println(distance_right.vl_status);
    while (1)       delay(10);
  }
  Serial.println(F("Ranging started"));
  distance_right.setTimingBudget(100);

  tcaselect(5);      // TCA channel for 2
  distance_front.begin();      // use the default address of 0x77
  Serial.println(F("distance 2 sensor OK!"));
  Serial.println(F("distance 1 sensor OK!"));
  if (! distance_front.startRanging()) {
    Serial.print(F("Couldn't start ranging: "));
    Serial.println(distance_front.vl_status);
    while (1)       delay(10);
  }
  Serial.println(F("Ranging started"));
  distance_front.setTimingBudget(100);

  driver.settings.commInterface = I2C_MODE;
  
  driver.settings.I2CAddress = 0x5D;

  while ( driver.begin() != 0xA9 )
  {
    Serial.println( "ID mismatch, trying again" );
    delay(500);
  }
  Serial.println( "ID matches 0xA9" );

  Serial.print("Waiting for enumeration...");
  while ( driver.ready() == false );
  Serial.println("Done.");
  Serial.println();

  while ( driver.busy() );
  driver.enable();

  delay(25);


  /*turnClockwise();
  delay(30000);
  stopMove();*/
}

// Helper functioxn for changing TCA output channel
void tcaselect(uint8_t channel) {
  if (channel > 7) return;
  Wire.beginTransmission(TCAADDR);
  Wire.write(1 << channel);
  Wire.endTransmission();  
}

void leftSpeed(int speed) {
  driver.setDrive(1, 0, speed);
}

void rightSpeed(int speed) {
  driver.setDrive(0, 1, speed);
}

void leftReverse() {
  driver.inversionMode(0, 1);
}

void rightReverse() {
  driver.inversionMode(1, 1);
}

void leftForward() {
  driver.inversionMode(0, 0);
}

void rightForward() {
  driver.inversionMode(1, 0);
}

void stopMove() {
  rightSpeed(0);
  leftSpeed(0);
}

/*
void moveForDuration(int time_in_ms) {
  Serial.println("in routine 1");
  rightSpeed(255);
  leftSpeed(255);
  leftFrontSpeed(255);
  rightSpeed(255);
  delay(time_in_ms);
  stopMove();
}*/

void loop() {
  tcaselect(3);
  // loop for time of flight sensor 1
  if (distance_right.dataReady()) {
    //   // new measurement for the taking!
    int16_t newd1 = distance_right.distance();
    if (newd1 != -1) {
      d1 = newd1;
      right_sensor_avg = (d1 + newd1) / 2;
    }
    Serial.print(F("Distance Right: "));
    Serial.print(right_sensor_avg);
    Serial.println();
    // data is read out, time for another reading!
  }
  distance_right.clearInterrupt();

  tcaselect(5);
  // loop for time of flight sensor 2
  if (distance_front.dataReady()) {
    //   // new measurement for the taking!
    int16_t newd2 = distance_front.distance();
    if (newd2 != -1) {
      d2 = newd2;
      front_sensor_avg = (d2 + newd2) / 2;
    }
    Serial.print(F("Distance Front: "));
    Serial.print(front_sensor_avg);
    Serial.println();
  }
  distance_front.clearInterrupt();
}

void turnCounterclockwise() {
  leftSpeed(0);
  rightSpeed(motorSpeed);
}

void turnClockwise() {
  rightSpeed(0);
  leftSpeed(motorSpeed);
}