Skip to content

Wireling Kickstarter Premium Robot Kit Tutorial

If you backed the Wireling Kickstarter for the Premium Robot Kit, there is ALREADY code loaded onto the RobotZero processor. The program loaded has a few separate options:

  • Tiny Robot Car Line-Follower: The robot will follow a line using Color Sensor Wirelings, and display data
  • Servo Calibration: This is used during the assembly of the Tiny Arm
  • Tiny Robot Arm Pick-and-Toss: This program will use the combination of the assembled Car and Arm in order to make the robot move around and pick things up using the TOF Distance Sensor positioned below the claw. The program here includes the ability to use either the Line Follower program or the Pick-and-Toss program - you just need to plug in the correct Wirelings following the port tables below

All three of these different options are configured based on what Wirelings are plugged into the RobotZero processor board, so depending on what you plug in, you have a few options!


Tiny Robot Car Line-Follower

Download Tiny Car Assembly Steps

For most, the assembly takes an hour, so make sure you have enough time to spend building the car as well as a sufficient work area with good lighting to keep track of all of the components.

After the assembly is complete, use the following table to plug the Wirelings into the correct ports. The tutorials and libraries for each of these Wirelings are also included:

Wireling Learn Library
Port 0 0.96" OLED 0.96" OLED TinierScreen and GraphicsBuffer
Port 1 Color Sensor(*Left) Color Sensor TCS34725 Library
Port 2 Color Sensor(*Right) Color Sensor TCS34725 Library
Port 3 Time-of-Flight Distance Sensor Vl53L0X Library

*From the point of view of the below picture

Plug in the rechargeable battery to the battery port on the RobotZero. (You may need to charge the battery, to do so just plug the battery into the RobotZero and plug the RobotZero into a power source using a MicroUSB cable)

Once everything is assembled, charged, and plugged in correctly, you should have a functional line-following robot.

  • The Color Sensor Wirelings are used to detect a dark line on a light background - the on-board LEDs assist with this.
  • The Time-of-Flight Distance Wireling tracks the distance of the car to obstacles in front of it. This also partially applies to the car robot being able to detect distances that correlate with an edge. For this behavior, the sensor needs to be installed securely without an angle. In addition, there must be nothing in front of the car when it is by the ledge, as this can trigger a false distance. Do not leave the robot unsupervised near any ledges as the acrylic of the car can break when dropped.
  • The 0.96" OLED Screen Wireling will display 3 graphs. The center graph displays a bar that tracks the distance read by the TOF Sensor, while the right and left graphs correlate with the colors read by the Color Sensors.

To see the Car follow a line, get a blank piece of paper and a black marker. Draw a thick black loop on the paper to see the robot follow the tiny course!


Upload Program

The program used for the Basic Car Robots can be downloaded here:

Download Line Follower Arduino Sketch

Wireling Program
/**********************************************************************
 * Tiny Car Robot Line Follower
 * This program uses 2 Color Sensors, a Time-of-Flight Distance
 * Sensor, a 0.96" Screen, and 2 stepper motors with the RobotZero 
 * processor to create a car robot that follows a line with the ability
 * to stop quickly when obstacles are in the path.
 * 
 * Hardware by: TinyCircuits
 * Written by: Ben Rose for TinyCircuits
 * 
 * Initialized: Dec 2019
 * Last modified: Jan 2020
 **********************************************************************/
#include <Wire.h>
#include <Wireling.h>
#include <MotorDriver.h>
#include <SPI.h>
#include "Adafruit_TCS34725.h"  // The library used for the Color Sensor
#include "VL53L0X.h"    // For interfacing with the Time-of-Flight Distance sensor

#include <TinierScreen.h>
#include <GraphicsBuffer.h>

TinierScreen display = TinierScreen(TinierScreen096);
GraphicsBuffer screenBuffer = GraphicsBuffer(128, 64, colorDepth1BPP);

/* Initialise with specific int time and gain values */
Adafruit_TCS34725 tcs0 = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_24MS, TCS34725_GAIN_60X);
Adafruit_TCS34725 tcs1 = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_24MS, TCS34725_GAIN_60X);

MotorDriver servo(15); //value passed is the address- remove resistor R1 for 1, R2 for 2, R1 and R2 for 3

VL53L0X distanceSensor; // Name of sensor
const int tofPort = 3;  // Port # of sensor (Found on Wireling Adapter Board)

#if defined (ARDUINO_ARCH_AVR)
#define SerialMonitorInterface Serial
#elif defined(ARDUINO_ARCH_SAMD)
#define SerialMonitorInterface SerialUSB
#endif

int leftSensorPort = 1;
int rightSensorPort = 2;

int displayPort = 0;
int resetPin = A0 + displayPort;


void setup() {
  delay(100);

  SerialMonitorInterface.begin(115200);

  //while (!SerialMonitorInterface);

  Wire.begin();
  delay(100);
  Wireling.begin();

  stepperInit();
  delay(100);

  //Reset servo driver
  pinMode(9, OUTPUT);
  digitalWrite(9, HIGH);
  delay(5);
  digitalWrite(9, LOW);
  delay(5);
  digitalWrite(9, HIGH);
  delay(100);
  // Set the period to 20000us or 20ms, correct for driving most servos
  if (servo.begin(20000)) {
    SerialMonitorInterface.println("Motor driver not detected!");
  }

  Wireling.selectPort(leftSensorPort);
  if (tcs0.begin()) {
    SerialMonitorInterface.println("Found sensor 0");
    // Enable the color sensor LED
    tcs0.setInterrupt(true);
  } else {
    SerialMonitorInterface.println("No TCS34725 found ... check your connections");
    //while (1);
  }

  delay(100);

  Wireling.selectPort(rightSensorPort);
  if (tcs1.begin()) {
    SerialMonitorInterface.println("Found sensor 1");
    // Enable the color sensor LED
    tcs1.setInterrupt(true);
  } else {
    SerialMonitorInterface.println("No TCS34725 found ... check your connections");
    //while (1);
  }


  Wireling.selectPort(displayPort);
  display.begin(resetPin);
  display.setFlip(true);
  if (screenBuffer.begin()) {
    //memory allocation error- buffer too big!
  }
  screenBuffer.setFont(thinPixel7_10ptFontInfo);

  Wireling.selectPort(tofPort);

  // Initialize the distance sensor and set a timeout
  distanceSensor.init();
  distanceSensor.setTimeout(5);
  distanceSensor.setMeasurementTimingBudget(200000);
  distanceSensor.startContinuous();

  // Set the current for the stepper motors
  setMotorCurrent(80);
}


int lastSetSpeedLeft = 0;
int lastSetSpeedRight = 0;

unsigned long motorUpdateInterval = 100;
unsigned long lastMotorUpdate = 0;

// Variables to hold the values the sensor reads
uint16_t r0, g0, b0, c0, luxLeft;
uint16_t r1, g1, b1, c1, luxRight;
int luxMin = 500;
int luxMax = 3000;

const int amtLeftSensorSamples = 32;
int leftSensorSampleBuff[amtLeftSensorSamples];
int leftSensorSampleBuffPos = 0;

const int amtRightSensorSamples = 32;
int rightSensorSampleBuff[amtRightSensorSamples];
int rightSensorSampleBuffPos = 0;

const int amtTOFsensorSamples = 32;
int TOFsensorSampleBuff[amtTOFsensorSamples];
int TOFsensorSampleBuffPos = 0;

int TOFlastGoodMeasurement = 0;

void loop() {
  if (millis() - lastMotorUpdate > motorUpdateInterval) {
    lastMotorUpdate = millis();

    Wireling.selectPort(leftSensorPort);
    tcs0.getRawData(&r0, &g0, &b0, &c0);
    luxLeft = constrain(tcs0.calculateLux(r0, g0, b0), luxMin, luxMax);

    Wireling.selectPort(rightSensorPort);
    tcs1.getRawData(&r1, &g1, &b1, &c1);
    luxRight = constrain(tcs1.calculateLux(r1, g1, b1), luxMin, luxMax);

    luxLeft = map(luxLeft, luxMin, luxMax, 0, 100);
    luxRight = map(luxRight, luxMin, luxMax, 0, 100);
    float Motor1Speed = 8, Motor2Speed = 8;
    float steeringBias = (float)(100.0 - luxLeft) - (float)(100.0 - luxRight);
    if (steeringBias > 0) {
      Motor1Speed -= steeringBias / 7.0;
      // if(abs(steeringBias)>50){
      Motor2Speed += steeringBias / 50.0;
      //}
    } else {
      // if(abs(steeringBias)>50){
      Motor1Speed -= steeringBias / 50.0;
      //}
      Motor2Speed += steeringBias / 7.0;
    }

    Wireling.selectPort(tofPort);
    unsigned int TOFdistance = distanceSensor.readRangeContinuousMillimeters();
    if (TOFdistance != 65535) {
      TOFlastGoodMeasurement = TOFdistance;
    }

    if (TOFlastGoodMeasurement < 50) {
      Motor2Speed = 0;
      Motor1Speed = 0;
    }

    if (Motor2Speed != lastSetSpeedLeft) {
      setMotor(1, Motor2Speed);
      lastSetSpeedLeft = Motor2Speed;
    }
    if (Motor1Speed != lastSetSpeedRight) {
      setMotor(2, Motor1Speed);
      lastSetSpeedRight = Motor1Speed;
    }

    /*
        SerialMonitorInterface.print(Motor2Speed);
        SerialMonitorInterface.print('\t');
        SerialMonitorInterface.print(Motor1Speed);
        SerialMonitorInterface.print('\t');
        SerialMonitorInterface.print(luxLeft);
        SerialMonitorInterface.print('\t');
        SerialMonitorInterface.print(luxRight);
        SerialMonitorInterface.print('\t');
        SerialMonitorInterface.print(steeringBias);
        SerialMonitorInterface.print('\t');
        SerialMonitorInterface.println(TOFlastGoodMeasurement);
    */

    Wireling.selectPort(displayPort);
    screenBuffer.clear();
    screenBuffer.setCursor(0, 43);
    screenBuffer.print("M2: ");
    screenBuffer.print(Motor2Speed);
    screenBuffer.setCursor(90, 43);
    screenBuffer.print("M1: ");
    screenBuffer.print(Motor1Speed);
    screenBuffer.setCursor(0, 53);
    screenBuffer.print("Steering bias: ");
    screenBuffer.print(steeringBias);

    screenBuffer.setCursor(0, 0);
    screenBuffer.print("Lux:");
    screenBuffer.setCursor(127 - 32, 0);
    screenBuffer.print("Lux:");
    screenBuffer.setCursor(64 - 16, 0);
    screenBuffer.print("Dist:");

    if ((int)Motor2Speed == 0 && (int)Motor1Speed == 0) {
      screenBuffer.setCursor(44, 43);
      screenBuffer.print("Stopped!");
    }

    leftSensorSampleBuff[leftSensorSampleBuffPos] = luxLeft;
    rightSensorSampleBuff[rightSensorSampleBuffPos] = luxRight;
    TOFsensorSampleBuff[TOFsensorSampleBuffPos] = constrain(TOFlastGoodMeasurement, 0, 1000);

    leftSensorSampleBuffPos++;
    if (leftSensorSampleBuffPos >= amtLeftSensorSamples) leftSensorSampleBuffPos = 0;
    rightSensorSampleBuffPos++;
    if (rightSensorSampleBuffPos >= amtRightSensorSamples) rightSensorSampleBuffPos = 0;
    TOFsensorSampleBuffPos++;
    if (TOFsensorSampleBuffPos >= amtTOFsensorSamples) TOFsensorSampleBuffPos = 0;
    int graphY = 10;
    displayGraph(0, graphY, 32, 32, 0, 100, rightSensorSampleBuff, rightSensorSampleBuffPos);
    displayGraph(127 - 32, graphY, 32, 32, 0, 100, leftSensorSampleBuff, leftSensorSampleBuffPos);
    displayGraph(64 - 16, graphY, 32, 32, 0, 1000, TOFsensorSampleBuff, TOFsensorSampleBuffPos);

    Wire.setClock(1000000);
    display.writeBuffer(screenBuffer.getBuffer(), screenBuffer.getBufferSize());
    Wire.setClock(100000);
  }
}


void displayGraph(int xDispPos, int yDispPos, int width, int height, int dataMin, int dataMax, int * buffToDisplay, int buffToDisplayPos) {

  screenBuffer.drawLine(xDispPos, yDispPos, xDispPos + width, yDispPos, 0xFFFF);
  screenBuffer.drawLine(xDispPos, yDispPos, xDispPos, yDispPos + height, 0xFFFF);
  screenBuffer.drawLine(xDispPos + width, yDispPos, xDispPos + width, yDispPos + height, 0xFFFF);
  screenBuffer.drawLine(xDispPos, yDispPos + height, xDispPos + height, yDispPos + height, 0xFFFF);

  for (uint8_t i = 1; i < width; i++) {
    int sample = map(buffToDisplay[(buffToDisplayPos + (i)) % width], dataMin, dataMax, 0, height);
    int sample0 = map(buffToDisplay[(buffToDisplayPos + (i - 1)) % width], dataMin, dataMax, 0, height);
    screenBuffer.drawLine(xDispPos + i - 1, yDispPos + height - sample0, xDispPos + i, yDispPos + height - sample, 0xFFFF);
  }
}

If you want to change any of the port number assignments to best fit your car, there is a block at the top of the program before the setup() loop you can use.


Servo Calibration

Note: Had logic errors in original program. The most updated version is from Feb 24th.

To enable servo calibration, all you need to do is plug in a battery to the RobotZero (charged), and the servos will move to a calibrated position. No Wirelings or anything other than the servos and battery should be plugged in for the Servo Calibration.

This step is necessary when constructing the arm so that the mechanical structure of the arm is correctly positioned for the max range of movement, and for the uploaded program.


Tiny Robot Arm Pick-and-Toss

robot arm

Before assembling the Robot Arm - be sure to calibrate the servos, and find the servo with the longest wire to use for the claw part of the arm (servo wire lengths may vary slightly)

Download Tiny Arm Assembly Steps

Once assembled, use the following table with included tutorial (Learn) pages to plug Wirelings into the ports on the RobotZero:

Wireling Learn Library
Port 0 Time-of-Flight Distance Sensor Vl53L0X Library
Port 1 0.96" OLED 0.96" OLED TinierScreen and GraphicsBuffer

The Car / Arm Robot when first turned on will scan around for an object like a tube of chapstick. The Time-of-Flight Distance Sensor is best able to see an object when it is a few inches away. The 0.96" Screen mounted on the car will display useful distance data for any debugging.

After the robot has found an object, it will hone in on it, pick it up, and then give it a toss. What more could you want in a robot?

Disclaimer: The uploaded program was designed for a tube of chapstick and several different surfaces, but not every object on every surface will be detectable.

Note on power consumption: To charge the battery after it has been installed to the arm, you will need to unplug all the servos when charging the battery so that no current is drawn


Download Libraries

This program requires the ATtiny841 Library in order to use the Servo Motors. This library replaces the TinyCircuits Motor Driver library - so the MotorDriver library must be removed from the Arduino/libraries folder so that there are no conflicts between the libraries.


Upload Program

This program is not rated for beginners:

Download Robot Arm Arduino Sketch

With this program uploaded, you can use the Car Line-Follower program, Servo Calibration sketch, or the Arm Pick-and-Toss program. All you have to do once the program is uploaded is plug in the correct Wirelings to the correct ports following the tables above. The Servo Calibration program can be activated by plugging in just the servos and a battery.

Robot Arm Wireling Program
 //-------------------------------------------------------------------------------
//  TinyCircuits Robot Kit Examples
//  Last Updated 24 Feb 2020
//
//  This example includes line following and object detecting code for the
//  RobotZero processor board in the robot car and arm kits. The sensors detected
//  at startup determine which example runs. The line following code attemps to
//  follow a 5 to 10mm wide dark line drawn on a light sheet. The object detecting
//  code attempts to find a small object like a pen cap to pick up and throw.
//
//  Written by Ben Rose for TinyCircuits, https://tinycircuits.com
//
//-------------------------------------------------------------------------------


#include <Wire.h>
#include <Wireling.h>
#include <ATtiny841Lib.h>
#include <ServoDriver.h>
#include <MotorDriver.h>
#include <SPI.h>
#include <TinierScreen.h>
#include <GraphicsBuffer.h>
#include "Adafruit_TCS34725.h"
#include "VL53L0X.h"

#if defined (ARDUINO_ARCH_AVR)
#define SerialMonitorInterface Serial
#elif defined(ARDUINO_ARCH_SAMD)
#define SerialMonitorInterface SerialUSB
#endif


TinierScreen display = TinierScreen(TinierScreen096);
GraphicsBuffer screenBuffer = GraphicsBuffer(128, 64, colorDepth1BPP);

ServoDriver servo(ROBOTZERO_SERVO_ADDR); //value passed is the address- remove resistor R1 for 1, R2 for 2, R1 and R2 for 3

VL53L0X distanceSensor; // Name of sensor
int tofPort = 0;  // Port # of sensor (Found on Wireling Adapter Board)

int displayPort = 1;
int resetPin = A0 + displayPort;


unsigned int getTOFMeasurement() {
  Wireling.selectPort(tofPort);
  unsigned int TOFdistance = distanceSensor.readRangeContinuousMillimeters();
  while (TOFdistance == 65535) {
    TOFdistance = distanceSensor.readRangeContinuousMillimeters();
  }
  return TOFdistance;
}


/* Initialise with specific int time and gain values */
Adafruit_TCS34725 tcs0 = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_24MS, TCS34725_GAIN_60X);
Adafruit_TCS34725 tcs1 = Adafruit_TCS34725(TCS34725_INTEGRATIONTIME_24MS, TCS34725_GAIN_60X);

int leftSensorPort = 1;
int rightSensorPort = 2;


//1000=open
const int clawServo = 1;
//1000 = right
const int rotationServo = 2;
//1000 = back
const int frontBackServo = 3;
//1000 = down
const int upDownServo = 4;

int clawClosePosition = 2200;
int clawOpenPosition = 1000;

int rotationServoN45Pos = 1000;
int rotationServoP45Pos = 2050;

int scanningHeightServoPosition = 1000;


void setup() {
  SerialMonitorInterface.begin(115200);
  Wire.begin();
  delay(10);
  Wireling.begin();

  stepperInit();
  delay(100);

  servo.useResetPin();
  if (servo.begin(10000)) {
    while (1) {
      SerialMonitorInterface.println("Servo driver not detected!");
      delay(1000);
    }
  }
  servo.setServo(1, 1500);
  servo.setServo(2, 1500);
  servo.setServo(3, 1500);
  servo.setServo(4, 1500);

  delay(1000);
  setMotorCurrent(80);

  //try to init color sensors, change screen port to 0 for line follower example
  bool noColorSensor = false;
  Wireling.selectPort(leftSensorPort);
  if (tcs0.begin()) {
    tcs0.setInterrupt(true);
    Wireling.selectPort(rightSensorPort);
    if (tcs1.begin()) {
      tcs1.setInterrupt(true);
      displayPort = 0;
      resetPin = A0 + displayPort;
      tofPort = 3;
    } else {
      while (1) {
        SerialMonitorInterface.println("Unexpected Wireling configuration! 1");
        delay(1000);
      }
    }
  } else {
    noColorSensor = true;
  }

  Wireling.selectPort(displayPort);
  if (noColorSensor) {
    Wire.beginTransmission(SSD1306_DEFAULT_ADDRESS);
    if (Wire.endTransmission()) {
      while (1) {
        SerialMonitorInterface.println("Unexpected Wireling configuration! 2");
        delay(1000);
      }
    }
  }
  display.begin(resetPin);
  display.setFlip(true);
  if (screenBuffer.begin()) {
    //memory allocation error- buffer too big!
  }
  screenBuffer.setFont(thinPixel7_10ptFontInfo);


  Wireling.selectPort(tofPort);
  // Initialize the distance sensor and set a timeout
  if (distanceSensor.init()) {
    distanceSensor.setTimeout(5);
    distanceSensor.setMeasurementTimingBudget(100000);
    distanceSensor.startContinuous();
  }

  closeClaw(200);
  servo.setServo(frontBackServo, 2200);
  servo.setServo(upDownServo, scanningHeightServoPosition);
}



void loop() {
  if (displayPort == 0) {
    lineFollowerLoop();
  } else {
    robotArmLoop();
  }
}


int minAngle = 0;
int minDistance = 0;

void scanForObject() {
  minDistance = 1000;
  int degreesScanned = 0;
  int degreeStep = 1;
  bool isDecreasing = false;
  int decreasingCount = 0;
  bool isIncreasing = false;
  int increasingCount = 0;
  while (1) {
    while (isMotorSpinning());
    turnRobotDegree(degreeStep, 4, false);
    Wireling.selectPort(tofPort);
    unsigned int TOFdistance = getTOFMeasurement();

    int val = constrain(TOFdistance, 0, 1000);
    val = TOFdistance * 64 / 300;
    screenBuffer.clear();
    screenBuffer.setCursor(0, 0);
    screenBuffer.print("Scanning for object! ");
    screenBuffer.setCursor(0, 10);
    screenBuffer.print("Distance reading: ");
    screenBuffer.print(TOFdistance);

    Wireling.selectPort(displayPort);
    Wire.setClock(1000000);
    display.writeBuffer(screenBuffer.getBuffer(), screenBuffer.getBufferSize());
    Wire.setClock(100000);

    if (TOFdistance > 80 && TOFdistance < 300) {
      if (TOFdistance < minDistance) {
        if (minAngle == degreesScanned - degreeStep) {
          decreasingCount++;
        } else {
          decreasingCount = 1;
        }
        minDistance = TOFdistance;
        minAngle = degreesScanned;
      } else {
        if (decreasingCount > 3) {
          return;
        } else {
          decreasingCount = 0;
          minDistance = 1000;
        }
      }
    } else {
      decreasingCount = 0;
      minDistance = 1000;
    }
    degreesScanned += degreeStep;
  }
}

int leftRightScan(int degrees) {
  turnRobotDegree(-degrees, 7, true);

  minDistance = 1000;
  screenBuffer.clear();


  for (int i = -degrees; i < degrees; i += 1) {
    while (isMotorSpinning());
    turnRobotDegree(1, 4, false);
    Wireling.selectPort(tofPort);
    unsigned int TOFdistance = getTOFMeasurement();
    int val = constrain(TOFdistance, 0, 1000);
    val = TOFdistance * 64 / 300;
    screenBuffer.drawLine(64 + (degrees * 2) - (degrees + i), 63, 64 + (degrees * 2) - (degrees + i), val, 0xFFFF);
    screenBuffer.drawLine(64 + (degrees * 2) - (degrees + i), val + 1, 64 + (degrees * 2) - (degrees + i), 0, 0);
    screenBuffer.setCursor(0, 0);
    screenBuffer.print("Scanning.. ");
    Wireling.selectPort(displayPort);
    Wire.setClock(1000000);
    display.writeBuffer(screenBuffer.getBuffer(), screenBuffer.getBufferSize());
    Wire.setClock(100000);
    if (TOFdistance < minDistance) {
      minDistance = TOFdistance;
      minAngle = i;
    }
  }
}

void robotArmLoop() {
  scanForObject();

  if (minDistance > 60 && minDistance < 300) {
    screenBuffer.clear();
    screenBuffer.setCursor(0, 0);
    screenBuffer.print("Object found ");
    screenBuffer.print(minDistance);
    screenBuffer.print(" mm away!");

    Wireling.selectPort(displayPort);
    Wire.setClock(1000000);
    display.writeBuffer(screenBuffer.getBuffer(), screenBuffer.getBufferSize());
    Wire.setClock(100000);

    turnRobotDegree(-3, 5, true);

    moveRobotForwardMM(max(0, (float)minDistance * 0.75 - 55.0));
    delay(100);
    servo.setServo(upDownServo, scanningHeightServoPosition - 100);
    leftRightScan(10);
    if (minDistance > 50 && minDistance < 100) {
      turnRobotDegree(-10 + minAngle + 2, 7, true);
      if (minDistance > 55)
        moveRobotForwardMM(max(0, minDistance - 55));
      delay(100);
      unsigned int TOFdistance = getTOFMeasurement();
      if (TOFdistance > 45 && TOFdistance < 100) {
        int distanceMoved = 0;
        while (TOFdistance > 45 && distanceMoved < 50) {
          while (isMotorSpinning());
          moveRobotForward(1, 5, false);
          TOFdistance = getTOFMeasurement();
          distanceMoved++;
        }
        closeClaw(1000);
        delay(100);
        servo.setServo(upDownServo, scanningHeightServoPosition);
        delay(500);
        rotateArmSlowly(0, -45);
        delay(1000);

        rotateArm(45);
        delay(50);
        servo.setServo(upDownServo, 1600);
        delay(100);
        servo.setServo(clawServo, 1000);
        delay(1000);
        rotateArmSlowly(45, 0);
        servo.setServo(upDownServo, scanningHeightServoPosition);
      }

    } else {
      //object too close/far to pick up
    }
  }
}





int lastSetSpeedLeft = 0;
int lastSetSpeedRight = 0;

unsigned long motorUpdateInterval = 100;
unsigned long lastMotorUpdate = 0;

// Variables to hold the values the sensor reads
uint16_t r0, g0, b0, c0, luxLeft;
uint16_t r1, g1, b1, c1, luxRight;
int luxMin = 500;
int luxMax = 3000;

const int amtLeftSensorSamples = 32;
int leftSensorSampleBuff[amtLeftSensorSamples];
int leftSensorSampleBuffPos = 0;

const int amtRightSensorSamples = 32;
int rightSensorSampleBuff[amtRightSensorSamples];
int rightSensorSampleBuffPos = 0;

const int amtTOFsensorSamples = 32;
int TOFsensorSampleBuff[amtTOFsensorSamples];
int TOFsensorSampleBuffPos = 0;

int TOFlastGoodMeasurement = 0;

void lineFollowerLoop() {
  if (millis() - lastMotorUpdate > motorUpdateInterval) {
    lastMotorUpdate = millis();

    Wireling.selectPort(leftSensorPort);
    tcs0.getRawData(&r0, &g0, &b0, &c0);
    luxLeft = constrain(tcs0.calculateLux(r0, g0, b0), luxMin, luxMax);

    Wireling.selectPort(rightSensorPort);
    tcs1.getRawData(&r1, &g1, &b1, &c1);
    luxRight = constrain(tcs1.calculateLux(r1, g1, b1), luxMin, luxMax);

    luxLeft = map(luxLeft, luxMin, luxMax, 0, 100);
    luxRight = map(luxRight, luxMin, luxMax, 0, 100);
    float Motor1Speed = 8, Motor2Speed = 8;
    float steeringBias = (float)(100.0 - luxLeft) - (float)(100.0 - luxRight);
    if (steeringBias > 0) {
      Motor1Speed -= steeringBias / 7.0;
      // if(abs(steeringBias)>50){
      Motor2Speed += steeringBias / 50.0;
      //}
    } else {
      // if(abs(steeringBias)>50){
      Motor1Speed -= steeringBias / 50.0;
      //}
      Motor2Speed += steeringBias / 7.0;
    }


    Wireling.selectPort(tofPort);
    unsigned int TOFdistance = distanceSensor.readRangeContinuousMillimeters();
    if (TOFdistance != 65535) {
      TOFlastGoodMeasurement = TOFdistance;
    }

    if (TOFlastGoodMeasurement < 50) {
      Motor2Speed = 0;
      Motor1Speed = 0;
    }

    if (Motor2Speed != lastSetSpeedLeft) {
      setMotor(1, Motor2Speed);
      lastSetSpeedLeft = Motor2Speed;
    }
    if (Motor1Speed != lastSetSpeedRight) {
      setMotor(2, Motor1Speed);
      lastSetSpeedRight = Motor1Speed;
    }

    /*
        SerialMonitorInterface.print(Motor2Speed);
        SerialMonitorInterface.print('\t');
        SerialMonitorInterface.print(Motor1Speed);
        SerialMonitorInterface.print('\t');
        SerialMonitorInterface.print(luxLeft);
        SerialMonitorInterface.print('\t');
        SerialMonitorInterface.print(luxRight);
        SerialMonitorInterface.print('\t');
        SerialMonitorInterface.print(steeringBias);
        SerialMonitorInterface.print('\t');
        SerialMonitorInterface.println(TOFlastGoodMeasurement);
    */

    Wireling.selectPort(displayPort);
    screenBuffer.clear();
    screenBuffer.setCursor(0, 43);
    screenBuffer.print("M2: ");
    screenBuffer.print(Motor2Speed);
    screenBuffer.setCursor(90, 43);
    screenBuffer.print("M1: ");
    screenBuffer.print(Motor1Speed);
    screenBuffer.setCursor(0, 53);
    screenBuffer.print("Steering bias: ");
    screenBuffer.print(steeringBias);

    screenBuffer.setCursor(0, 0);
    screenBuffer.print("Lux:");
    screenBuffer.setCursor(127 - 32, 0);
    screenBuffer.print("Lux:");
    screenBuffer.setCursor(64 - 16, 0);
    screenBuffer.print("Dist:");

    if ((int)Motor2Speed == 0 && (int)Motor1Speed == 0) {
      screenBuffer.setCursor(44, 43);
      screenBuffer.print("Stopped!");

    }
    leftSensorSampleBuff[leftSensorSampleBuffPos] = luxLeft;
    rightSensorSampleBuff[rightSensorSampleBuffPos] = luxRight;
    TOFsensorSampleBuff[TOFsensorSampleBuffPos] = constrain(TOFlastGoodMeasurement, 0, 1000);


    leftSensorSampleBuffPos++;
    if (leftSensorSampleBuffPos >= amtLeftSensorSamples) leftSensorSampleBuffPos = 0;
    rightSensorSampleBuffPos++;
    if (rightSensorSampleBuffPos >= amtRightSensorSamples) rightSensorSampleBuffPos = 0;
    TOFsensorSampleBuffPos++;
    if (TOFsensorSampleBuffPos >= amtTOFsensorSamples) TOFsensorSampleBuffPos = 0;
    int graphY = 10;
    displayGraph(0, graphY, 32, 32, 0, 100, rightSensorSampleBuff, rightSensorSampleBuffPos);
    displayGraph(127 - 32, graphY, 32, 32, 0, 100, leftSensorSampleBuff, leftSensorSampleBuffPos);
    displayGraph(64 - 16, graphY, 32, 32, 0, 1000, TOFsensorSampleBuff, TOFsensorSampleBuffPos);


    Wire.setClock(1000000);
    display.writeBuffer(screenBuffer.getBuffer(), screenBuffer.getBufferSize());
    Wire.setClock(100000);
  }
}

void displayGraph(int xDispPos, int yDispPos, int width, int height, int dataMin, int dataMax, int * buffToDisplay, int buffToDisplayPos) {

  screenBuffer.drawLine(xDispPos, yDispPos, xDispPos + width, yDispPos, 0xFFFF);
  screenBuffer.drawLine(xDispPos, yDispPos, xDispPos, yDispPos + height, 0xFFFF);
  screenBuffer.drawLine(xDispPos + width, yDispPos, xDispPos + width, yDispPos + height, 0xFFFF);
  screenBuffer.drawLine(xDispPos, yDispPos + height, xDispPos + height, yDispPos + height, 0xFFFF);

  for (uint8_t i = 1; i < width; i++) {
    int sample = map(buffToDisplay[(buffToDisplayPos + (i)) % width], dataMin, dataMax, 0, height);
    int sample0 = map(buffToDisplay[(buffToDisplayPos + (i - 1)) % width], dataMin, dataMax, 0, height);
    screenBuffer.drawLine(xDispPos + i - 1, yDispPos + height - sample0, xDispPos + i, yDispPos + height - sample, 0xFFFF);
  }
}

You can change the port definitions for different Wirelings at the beginning of the program.


Contact Us

s

If you have any questions or feedback, feel free to email us or make a post on our forum. Show us what you make by tagging @TinyCircuits on Instagram, Twitter, or Facebook so we can feature it.

Thanks for making with us!