Things used in this project

Hardware components:
Ard nano
Arduino Arduino Nano R3
×1
Tens70
9V battery (generic)
×2
Hand tools and fabrication machines:
Lasercutter
Laser cutter (generic)
09507 01
Soldering iron (generic)

Custom parts and enclosures

Laser-cut parts
Parts 2

Schematics

Wiring schematic

No document.

Code

BalanceRobot.inoArduino
Main code to control the robot movement
#include <NewPing.h>

int internalLedPin = 13;
int externalLedPin = 12;
int potentiometerPin = A0;

int dc1EnablePin = 4;
int dc1Pin = 10;
int dc1ReversePin = 9;
int dc2EnablePin = 3;
int dc2Pin = 6;
int dc2ReversePin = 5;

int distanceTriggerPin = 8;
int distanceEchoPin = 7;
int maximumDistance = 5000;
int balancedDistanceOriginal = -1;
int balancedDistance = -1;

const int measuredDistancesCount = 10;
int measuredDistances[measuredDistancesCount];

int potentiometerValue = 512;

unsigned long timerLogLast = 0;
int timerLogDelta = 200;

unsigned long timerDistanceLast = 0;
int timerDistanceDelta = 10;

unsigned long timerDebugLast = 0;
int timerDebugDelta = 3000;

bool isTurningCW = true;

// magic numbers for power calculation
const float powerFactor0 = 0;
const float powerFactor2 = 0.00005;


NewPing sonar(distanceTriggerPin, distanceEchoPin, maximumDistance);

void setup() {
  digitalWrite(internalLedPin, HIGH);
  //pinMode(externalLedPin, OUTPUT);
  
  pinMode(dc1EnablePin, OUTPUT);
  pinMode(dc1Pin, OUTPUT);
  pinMode(dc1ReversePin, OUTPUT);
  pinMode(dc2EnablePin, OUTPUT);
  pinMode(dc2Pin, OUTPUT);
  pinMode(dc2ReversePin, OUTPUT);

  pinMode(distanceTriggerPin, OUTPUT);
  pinMode(distanceEchoPin, INPUT);
  Serial.begin(9600);
}

void loop() {
  unsigned long timerNow = millis();

  // check sonar value
  if (timerNow > timerDistanceLast + timerDistanceDelta) {
    sonar.ping_timer(sonarCallback);
    timerDistanceLast = timerNow;

    // balance
    double stableDistance = getAverageDistance(3);
    if (stableDistance > balancedDistance + 1) {
      //powerBothWheels(255);
    } else if (stableDistance < balancedDistance - 1) {
      //powerBothWheels(-255);
    }
  }

  // debug
  if (timerNow > timerDebugLast + timerDebugDelta) {
    
    // change direction
    
    if (isTurningCW) {
      //powerBothWheels(-255);
    } else {
      //powerBothWheels(255);
    }
    isTurningCW = !isTurningCW;
    Serial.println("Changing direction");
    

    int powerValue = digitalToAnalog(potentiometerValue);

    timerDebugLast = timerNow;
  }

  // log misc. stuff
  if (timerNow > timerLogLast + timerLogDelta) {
    // update potentiometer value

    //potentiometerValue = analogRead(potentiometerPin);
    potentiometerValue = 512;
    balancedDistance = balancedDistanceOriginal + (map(potentiometerValue - 512, 0, 512, 0, 10));

    Serial.print("Last distances: ");
    Serial.print(getMeasuredDistancesAsString());

    Serial.print(" Average distance: ");
    Serial.print(getAverageDistance(10));

    Serial.print(" Last power value: ");
    Serial.println(calculatePowerValue());

    timerLogLast = timerNow;
  }
}

void sonarCallback() {
  if (sonar.check_timer()) {
    int currentMeasuredDistance = (sonar.ping_result / 24);

    // shift all elemts of the array by -1
    for (char i = 0; i < measuredDistancesCount - 1; i++) {   
      measuredDistances[i] = measuredDistances[i + 1];
    }

    // store the new distance at the end of the array
    measuredDistances[measuredDistancesCount - 1] = currentMeasuredDistance;

    // if no balanced distance has been set, use the current one
    if (balancedDistanceOriginal < 0) {
      balancedDistanceOriginal = currentMeasuredDistance;
      balancedDistance = balancedDistanceOriginal;
    }

    // let LED indicate if balanced or not
    int deltaFromBalanced = currentMeasuredDistance - balancedDistance;

    if (deltaFromBalanced <= 1 && deltaFromBalanced >= -1) {
      digitalWrite(internalLedPin, HIGH);
    } else {
      digitalWrite(internalLedPin, LOW);
    }

    int powerValue = calculatePowerValue();
    powerBothWheels(powerValue);

  } else {
    // invalid distance measured
  }  
}

int calculatePowerValue() {
  int powerValue = 0;
  double lastMeasuredDistance = measuredDistances[measuredDistancesCount - 1];
  //double lastMeasuredDistance = getAverageDistance(5);
  
  double deltaFromBalanced = lastMeasuredDistance - balancedDistance;

  // modify power value using pedometer (centered pedometer --> factor = 1)
  //double factor = map(potentiometerValue, 0, 1023, 0, 200) / (double) 100;
  double factor = 1;

  if (lastMeasuredDistance - balancedDistance < 0) {
    deltaFromBalanced = deltaFromBalanced * (-1);
  }

  // powerValue is this formular solved for x: 
  // deltaFromBalanced = powerFactor2 * x^2 + powerFactor1 * x + powerFactor0
  powerValue = factor * sqrt((deltaFromBalanced - powerFactor0) / powerFactor2);

  if (powerValue == 0) {
    double stableDistance = getAverageDistance(5);
    double stableDistanceDelta = stableDistance - balancedDistance;
    powerValue = 0.5 * sqrt((stableDistanceDelta) / powerFactor2);
    if (stableDistanceDelta > 0) {
      powerValue = powerValue * (-1);
    }
  } else {
    if (deltaFromBalanced < 0) {
      powerValue = powerValue * (-1);
    }
  }

  powerValue = min(255, powerValue);
  powerValue = max(-255, powerValue);

  if (lastMeasuredDistance - balancedDistance > 0) {
    powerValue = powerValue * (-1);
  }

  return powerValue;
}

double getAverageDistance(int count) {
  if (count > 10) {
    count = 10;
  }

  double average = 0;
  for (char i = measuredDistancesCount - 1; i >= 0 && i + count >= measuredDistancesCount; i--) {
    average += measuredDistances[i];
  }

  average = average / count;
  return average;
}

String getMeasuredDistancesAsString() {
  String distanceArray = "[";
  for (char i = 0; i < measuredDistancesCount; i++) {
    distanceArray = distanceArray + " " + measuredDistances[i] + " ";
  }
  distanceArray = distanceArray + "]";
  return distanceArray;
}

void powerBothWheels(int value) {
  powerLeftWheel(value);
  powerRightWheel(value);
}

// Left wheel
void powerLeftWheel(int value) {
  if (value > 0) {    
    powerLeftWheelForwards(value);
  } else if (value < 0) {
    powerLeftWheelBackwards(value * (-1));
  } else {
    stopLeftWheel();
  }
}

void powerLeftWheelForwards(int value) {
  digitalWrite(dc1EnablePin, HIGH);
  analogWrite(dc1Pin, value);
  analogWrite(dc1ReversePin, 0);
}

void powerLeftWheelBackwards(int value) {
  digitalWrite(dc1EnablePin, HIGH);
  analogWrite(dc1Pin, 0);
  analogWrite(dc1ReversePin, value);
}

void stopLeftWheel() {
  digitalWrite(dc1EnablePin, LOW);
  analogWrite(dc1Pin, 0);
  analogWrite(dc1ReversePin, 0);
}

// Right wheel
void powerRightWheel(int value) {
  if (value > 0) {    
    powerRightWheelForwards(value);
  } else if (value < 0) {
    powerRightWheelBackwards(value * (-1));
  } else {
    stopRightWheel();
  }
}

void powerRightWheelForwards(int value) {
  digitalWrite(dc2EnablePin, HIGH);
  analogWrite(dc2Pin, value);
  analogWrite(dc2ReversePin, 0);
}

void powerRightWheelBackwards(int value) {
  digitalWrite(dc2EnablePin, HIGH);
  analogWrite(dc2Pin, 0);
  analogWrite(dc2ReversePin, value);
}

void stopRightWheel() {
  digitalWrite(dc2EnablePin, LOW);
  analogWrite(dc2Pin, 0);
  analogWrite(dc2ReversePin, 0);
}

int digitalToAnalog(int digitalValue) {
  return digitalValue / 4;
}

Credits

Img 5193 3
Stephan Schultz

I develop apps: http://steppschuh.net

Contact

Replications

Did you replicate this project? Share it!

I made one

Love this project? Think it could be improved? Tell us what you think!

Give feedback

Comments

Similar projects you might like

Punch Activated Arm Flamethrowers (Real Firebending)
Advanced
  • 38,596
  • 294

Shoot fireballs from your fists when you throw a punch with these arm mounted smart flamethrowers!

Visual Capturing with OV7670 on Arduino
Advanced
  • 4,357
  • 23

Protip

This is an Arduino camera module, using the surveillance camera's digital image processing chip-OV0706.

Get Nostalgic and Use Nokia 84x48 LCD with Arduino
Advanced
  • 2,930
  • 14

Protip

These 84 by 48 pixel LCDs are what you might have found in Nokia 3310. And what about using them in the Arduino projects?

Smart Energy Monitor Based on Arduino
Advanced
  • 223
  • 3

Full instructions

Open source Smart energy monitor with some new features like "Terminal Commands " , "Readings Logger", "Data Plotting ".

Magic VR Hat
Advanced
  • 198
  • 3

Wear the hat, get transported to different 360 VR experience.

Air Quality License Plate Holder
Advanced
  • 1,691
  • 4

Full instructions

Air Quality License Plate Holder uses air quality sensor attaching to the vehicle to monitor the air quality through out the city.

Sign up / LoginProjectsPlatformsTopicsContestsLiveAppsBetaFree StoreBlog