BeerBot.ino

#include <NewPing.h>
#include <Servo.h>
#include "i2c_t3.h"
#include "teensy3_Melexis90620.h"
#include "BeerBotLib.h"
#include "ThermalCam.h"
 
 
ThermalCam thermCam;
int thermCamServoPin = 14;
int temperatureBorder = 24;
 
ContactSensor frontSensor;
int frontLeftPushButtonPin = 8;
int frontRightPushButtonPin = 15;
 
ContactSensor capSensor;
int capRightPushButtonPin = 7;
int capLeftPushButtonPin = 6;
int capConductorPin = 9;
 
Pusher pusher;
int pusherPin = 21;
 
Barrier lowBarrier;
int lowBarrierServoPin = 12;
int lowBarrierClosedPos = 150;
int lowBarrierOpenPos = 60;
 
Barrier highBarrier;
int highBarrierServoPin = 13;
int highBarrierClosedPos = 130;
int highBarrierOpenPos = 83;
 
Gear myGear;
float wheelDiameter = 9.0;
float axisDistance = 24.0;
int leftMotorStepPin = 5;
int leftMotorDirPin = 4;
int rightMotorStepPin = 3;
int rightMotorDirPin = 2;
 
int phase = 0;
boolean bottleFound = false;
int lastFoundPos = 90;
 
void setup() {
  Serial.begin(9600); // for debugging purposes only
 
  thermCam.setup(thermCamServoPin, temperatureBorder);
  myGear.setup(wheelDiameter, axisDistance, leftMotorStepPin, leftMotorDirPin, rightMotorStepPin, rightMotorDirPin);
  frontSensor.setup(frontRightPushButtonPin, frontLeftPushButtonPin, -1);
  capSensor.setup(capRightPushButtonPin, capLeftPushButtonPin, capConductorPin);
  pusher.setup(pusherPin);
  lowBarrier.setup(lowBarrierServoPin, lowBarrierClosedPos, lowBarrierOpenPos);
  highBarrier.setup(highBarrierServoPin, highBarrierClosedPos, highBarrierOpenPos);    
}
 
void loop() {
 
	main1();
 
}
 
/**
* test functions for the different components of the robot.
* for debugging and presentation purposes.
*/
 
void testOpen() {
  lowBarrier.closeBarrier();
  delay(500);
  highBarrier.closeBarrier();
  delay(1000);
  pusher.push(300);
  delay(2000);
}
 
void testPusher() {
  pusher.push(300);
  delay(3000);
}
 
void testBarriers() {
  highBarrier.closeBarrier();
  delay(15);
  lowBarrier.closeBarrier();
  delay(1000);
  lowBarrier.openBarrier();
  delay(15);
  highBarrier.openBarrier();
  delay(1000);
}
 
void testDrive() {
  myGear.drive(20, 12, true);
  myGear.turnAngle(90, true, 12);
}
 
/**
* the main function of the robots software.
* defines the different phases of the process
* it is not working pefectly in its current state due to some problems with e.g. the contact sensors.
* it is modified to work as good as possible under the given circumstances.
*/
 
void main1() {
 
  switch (phase) {
 
    case 0:
      if (bottleLoaded()) {
        phase = 4;
      }
      else if (thermCam.searchColdSpot()) {
        phase = 2;
      }
      else if (bottleClose()) {
        phase = 3;
      }
      else {
        phase = 1;
      }
 
      break;
 
    case 1:
      easySearch(100, 5);
      if (bottleFound) {
        phase = 2;
      }
 
      break;
 
    case 2:
      myGear.turnToServoPos(thermCam.getPos(), 12, 10, 10);
      delay(500);
      myGear.drive(5, 12, true);
      delay(500);
 
      if (bottleClose()) {
        phase = 3;
      }
      if (bottleLoaded()) {
        phase = 4;
      }
      else {
        bottleFound = thermCam.searchColdSpot();
        if (thermCam.getPos() != 90) {
          lastFoundPos = thermCam.getPos();
        }
        Serial.println(lastFoundPos);
 
        if (!bottleFound) {
          phase = 3;
        }
      }
 
      break;
 
    case 3:
     // adjustPosition();
     myGear.turnToServoPos(lastFoundPos, 12, 0, 5);
     myGear.drive(3, 12, true);
     delay(1000);
 
      if (bottleLoaded()) {
        phase = 4;
      }
 
      break;
 
   case 4:
     lowBarrier.closeBarrier();
     for (int i = 0; i < 4; i++) {
       highBarrier.openBarrier();
       delay(500);
       highBarrier.closeBarrier();
       delay(500);
     }
 
     phase = 5;
 
     break;
 
   case 5:
     if (capSensor.isConducting()) {
       pusher.push(300);
       delay(2000); 
     }
 
     break;
  }
}
 
/**
* helper function to check if the bottle is loaded by the robot.
*/
 
boolean bottleLoaded() {
  return (capSensor.leftButtonPushed() || capSensor.rightButtonPushed());
}
 
/**
* helper function to check if the robot touched the bottle.
*/
 
boolean bottleClose() {
  return (frontSensor.leftButtonPushed() || frontSensor.rightButtonPushed());
}
 
/**
* an easy algorithm that makes the robot drive forward and scan for cold bottles in steps.
*/
 
boolean easySearch(float totalDistance, float partDistance) {
  for (float driven = 0; driven < totalDistance; driven += partDistance) {
    myGear.drive(partDistance, 12, true);    
 
    if (thermCam.searchColdSpot()) {
      bottleFound = true;
      break;
    }
 
  }
}
 
/**
* a more complex function to scan the room for cold bottles.
* has not been tested.
*/
 
boolean complexSearch(float totalDistance, float partDistance) {
  for (float driven = 0; driven < totalDistance; driven += partDistance) {
    myGear.drive(partDistance, 12, true);
    if (thermCam.searchColdSpot()) {
      return true;
    }
  }
 
  myGear.turnAngle(90, false, 12);
  myGear.drive(partDistance, 12, true);
  myGear.turnAngle(90, false, 12);
 
  for (float driven = 0; driven < totalDistance; driven += partDistance) {
    myGear.drive(partDistance, 12, true);
    if (thermCam.searchColdSpot()) {
      return true;
    }
  }
 
  myGear.turnAngle(90, true, 12);
  myGear.drive(partDistance, 12, true);
  myGear.turnAngle(90, true, 12);
 
  return false; 
}
 
/**
* a function to adjust the postion of the robot as it touches the bottle.
*/
 
void adjustPosition() {
  if (frontSensor.leftButtonPushed()) {
    myGear.turnAngle(2, true, 12);
  }
  else if (frontSensor.rightButtonPushed()) {
    myGear.turnAngle(2, false, 12);
  }
  myGear.drive(1, 12, true);
}

This library defines and implements all classes that are needed to control the different components of the robot, except for the thermal camera.

BeerBotLib.h

#ifndef BeerBotLib_h
#define BeerBotLib_h
 
#include "Arduino.h"
#include "Servo.h"
#include "NewPing.h"
 
/**
* class to add some additional features for the servo.
*/
 
class NewServo {
	public:
		void setup(int servoPin, int pos, boolean newServo);
		int getPos();
		void setPos(int pos);
 		void sweep(int servoSpeed, int servoRange, int servoDelay);
	private:
		Servo myServo;
		int position;
		int direction;
};
 
/**
* class for working with an ultrasonic distance sensor.
* is not used for the roboter in its current state. 
*/
 
class DistanceSensor {
	public:
		void setup(int inputPin, int outputPin, int servoPin);
		int getPos();
		int getDistance();
 
	private:
		NewPing sensor;
		NewServo myServo;	 
};
 
/**
* class to control the stepper motors.
*/
 
class StepperMotor {
	public:
		void setup(int stepPin, int directionPin);
		void makeStep(boolean direction);
	private:
		int stepPin;
		int directionPin;
};
 
/**
* class to control the whole gear of the servo.
*/
 
class Gear {
	public:
		void setup(float wheelDiameter, float axisDistance, int leftMotorStepPin, int leftMotorDirPin, int rightMotorStepPin, int rightMotorDirPin);
		void turnToServoPos(int servPos, int stepSpeed, int tolerance, int partAngle);
		void turnAngle (int angle, boolean turnDirection, int stepSpeed); 
		void drive(float distance, int stepSpeed, boolean driveDirection);
	private:
		float wheelDiameter;
		float axisDistance;
		StepperMotor rightMotor;
		StepperMotor leftMotor;
};
 
/**
* class to trigger the pusher(s).
*/
 
class Pusher {
	public:
		void setup(int pusherPin);
		void push(int duration);
	private:
		int pusherPin;
};
 
/**
* class to control the barriers.
*/
 
class Barrier {
	public:
		void setup(int servoPin, int closedPos, int openPos);
		void openBarrier();
		void closeBarrier();
	private:
		NewServo myServo;
		boolean closed;
		int closedPos;
		int openPos;	
};
 
/**
* class to control the push button contact sensors.
*/
 
class ContactSensor {
	public:
		void setup(int rightButtonPin, int leftButtonPin, int conductorPin);
		boolean rightButtonPushed();
		boolean leftButtonPushed();
		boolean isConducting();
	private:
		int rightButtonPin;
		int leftButtonPin;
		int conductorPin;
};
 
 
#endif

BeerBotLib.cpp

#include "BeerBotLib.h"
 
////////////////////////////////////////
// NewServo class
////////////////////////////////////////
 
/**
* setup method for servo class.
* the setup mehod has to be called, before the implicit constructed objects can be used.
* 
* @param servoPin: pin the servo is attached to,
*		 pos: initial position of the servo,
*		 newServo: specifies if the servo is one of the new models.
*/
 
void NewServo::setup(int servoPin, int pos, boolean newServo) {
	if (newServo) {
		myServo.attach(servoPin, 800, 2100);
	}
	else {
		myServo.attach(servoPin);
	}
	setPos(pos);
	direction = 0;
}
 
/**
* getter method for servo position.
* 
* @return the last position to which the servo was set. 
*/
 
int NewServo::getPos() {
	return position;
}
 
/**
* setter method for the servo method.
* servo position is set to new position and servo is moved to new position.
* 
* @param pos: new servo position. 
*/
 
void NewServo::setPos(int pos) {
	myServo.write(pos);
	this-> position = pos;
}
 
/**
* method to let the servo sweep.
* sweeps only one step on each method call
* 
* @param servoSpeed: specifies the lenght of each step,
*	  	 servoRange: specifies the range in wich the servo should sweep
*		 (e.g. servoRange = 50 --> servo sweeps from 65 to 115),
*		 servoDelay: delay after each step.
*/
 
void NewServo::sweep(int servoSpeed, int servoRange, int servoDelay) {
	if (direction == 0) {
		direction = 1;
	}
	if (direction == 1) {
		moveRight(servoSpeed, servoDelay);
		if (position >= 90 + servoRange/2) {
			direction = -1;
		}
	}
	if (direction == -1) {
		moveLeft(servoSpeed, servoDelay);
		if (position <= 90 - servoRange/2) {
			direction = 1;
		}
	}
}
 
////////////////////////////////////////
// DistanceSensor class
////////////////////////////////////////
 
/**
* setup method for the distance sensor.
* initializes the distance sensor using third-party library NewPing.
* has to be called before the object can be used.
*
* @param inputPin: the echo pin of the sensor,
	 	 outputPin: the trigger pin of the sensor,
		 servoPin: the pin of the servo on which the sensor is mounted (if that is the case).
*/
 
void DistanceSensor::setup(int inputPin, int outputPin, int servoPin) {
	sensor.setup(outputPin, inputPin);
	myServo.setup(servoPin, 90, false);
}
 
/**
* getter method for the servo on which the sensor is mounted.
*
* @return the getPos method of the servo.
*/
 
int DistanceSensor::getPos() {
	return myServo.getPos();
}
 
/**
* method to measure the distance with the sensor.
* uses the third-party library NewPing.
* 
* @return the currently measured distance in cm.
*/
 
int DistanceSensor::getDistance() {
	return sensor.ping_cm();
}
 
////////////////////////////////////////
// Pusher class
////////////////////////////////////////
 
/**
* setup method for the pusher.
* sets the pin to which the relay controller of the pusher is connected
* and sets the pin to HIGH as default because that is needed for the relay.
*
* @param pusherPin: the pin to which the relay controller is connected.
*/
 
void Pusher::setup(int pusherPin) {
	this-> pusherPin = pusherPin;
	pinMode(pusherPin, OUTPUT);
	digitalWrite(pusherPin, HIGH);
}
 
/**
* method to trigger the pusher.
* pusher should not be triggerd for longer than 500ms continously.
*
* @param duration: the duration for which the pusher should be triggerd in ms.
*/
 
void Pusher::push(int duration) {
	digitalWrite(pusherPin, LOW);
	delay(duration);
	digitalWrite(pusherPin, HIGH);
}
 
////////////////////////////////////////
// Barrier class
////////////////////////////////////////
 
/**
* setup method for the barrier class. 
* the setup mehod has to be called, before the implicit constructed objects can be used.
*
* @param servoPin: the pin to which the servo of the barrier is connected,
*		 closedPos: the servo position in which the barrier is closed,
*		 openPos: the servo position in which the barrier is opened.
*/
 
void Barrier::setup(int servoPin, int closedPos, int openPos) {
	this-> closedPos = closedPos;
	this-> openPos = openPos;
	myServo.setup(servoPin, openPos, false);
	closed = false;
}
 
/**
* method that open the barrier.
* sets the servo to the specified open position.
*/
 
void Barrier::openBarrier() {
	if (closed) {
		myServo.setPos(openPos);
		closed = false;
		delay(1000);
	}
}
 
/**
* method that closes the barrier.
* sets the servo to the specified closed position.
*/
 
void Barrier::closeBarrier() {
	if (!closed) {
		myServo.setPos(closedPos);
		closed = true;
		delay(1000);
	}
}
 
 
////////////////////////////////////////
// ContactSensor class
////////////////////////////////////////
 
/**
* setup method for the contact sensor class.
* the setup mehod has to be called, before the implicit constructed objects can be used.
*
* @param leftButtonPin: pin to which the left button is connected,
*		 rightButtonPin: pin to which the right button is connected,
*		 conductorPin: pin to which the conduction signal is connected.
*/
 
void ContactSensor::setup(int leftButtonPin, int rightButtonPin, int conductorPin) {
	this-> leftButtonPin = leftButtonPin;
	this-> rightButtonPin = rightButtonPin;
	this-> conductorPin = conductorPin;
 
	pinMode(leftButtonPin, INPUT);
	pinMode(rightButtonPin, INPUT);
	pinMode(conductorPin, INPUT);
}
 
/**
* method to check if the left button is pushed.
*
* @return true if button is pushed, false if not.
*/
 
boolean ContactSensor::leftButtonPushed() {
	return (digitalRead(leftButtonPin) == HIGH);
}
 
/**
* method to check if the right button is pushed.
*
* @return true if button is pushed, false if not.
*/
 
boolean ContactSensor::rightButtonPushed() {
	return (digitalRead(leftButtonPin) == HIGH);
}
 
/**
* method to check if an electric current is flowing.
*
* @return true if button is pushed, false if not.
*/
 
boolean ContactSensor::isConducting() {
	if (conductorPin > 0) {
		return (digitalRead(leftButtonPin) == HIGH);
	}
	return false;
}
 
////////////////////////////////////////
// StepperMotor class
////////////////////////////////////////
 
/**
* setup method for the stepper motor class.
* the setup mehod has to be called, before the implicit constructed objects can be used.
*
* @param stepPin: the pin which is connected to the stepper pin of the motor driver,
		 directionPin: the pin which is connected to the direction pin of the motor driver.
*/
 
void StepperMotor::setup(int stepPin, int directionPin) {
	this-> stepPin = stepPin;
	this-> directionPin = directionPin;
 
	pinMode(stepPin, OUTPUT);
	pinMode(directionPin, OUTPUT);
 
	Serial.println("motor");
}
 
/**
* method to make one step.
* one step is an 1.8 degree turn of the axis.
*
* @param stepDirection: defines the direction in which the step is made.
*/
 
void StepperMotor::makeStep(boolean stepDirection){    
  if (stepDirection){
    digitalWrite(directionPin, HIGH);
  }
  else {
    digitalWrite(directionPin,LOW);
  }
  digitalWrite(stepPin, HIGH);
  delayMicroseconds(1);
  digitalWrite(stepPin, LOW);  
}
 
////////////////////////////////////////
// Gear class
////////////////////////////////////////
 
/**
* setup method for the gear class.
* the setup mehod has to be called, before the implicit constructed objects can be used.
*
* @param wheelDiameter: diameter of the wheel,
* 		 axisDistance: length of the axis (distance between the weels),
*		 leftMotorStepPin: stepper pin of the left stepper motor,
*		 leftMotorDirPin: direction pin of the left stepper motor,
		 rightMotorStepPin: stepper pin of the right stepper motor,
		 rightMotorDirPin: stepper pin of the right stepper motor.
*/
 
void Gear::setup(float wheelDiameter, float axisDistance, int leftMotorStepPin, int leftMotorDirPin, int rightMotorStepPin, int rightMotorDirPin) {
	this-> wheelDiameter = wheelDiameter;
	this-> axisDistance = axisDistance;	
	leftMotor.setup(leftMotorStepPin, leftMotorDirPin);
	rightMotor.setup(rightMotorStepPin, rightMotorDirPin);
}
 
/**
* method to drive in a straight line.
* 
* @param distance: the distance to drive in cm,
*		 stepSpeed: delay between to steps in ms,
*		 driveDirection: true to drive forward, false to drive backwards.
*/
 
void Gear::drive(float distance, int stepSpeed, boolean driveDirection) {
	//stepsValue ist die Anzahl der Schritte die jeder Motor machen muss
	float stepsValue = round(distance/((wheelDiameter*PI)/200));
	//Beide Motoren fahren in die gleiche Richtung. Da der eine aber umgedreht ist, bekommen sie unterschiedliche direction Anweisungen.
	for (int countSteps = 0; countSteps < stepsValue; countSteps++) {
		rightMotor.makeStep(driveDirection);                                                      
		leftMotor.makeStep(!driveDirection);
		delay(stepSpeed);                                                    
	}	
}
 
/**
* method to turn the robot to a servo position.
* if the servo is not in the tolarance range (90 - tolerance/2 < servoPos > 90 + tolerance/2),
* the robot is turned in the direction which the servo, respectivly the mounted sensor, is facing.
*
* @param servoPos: the position of the servo,
*		 stepSpeed: the delay between the stepper motor steps,
*		 tolerance: the tolerance range (90 - tolerance/2 < servoPos > 90 + tolerance/2);
*		 partAngle: the angle for the turning of the robot.
*/
 
void Gear::turnToServoPos(int servoPos, int stepSpeed, int tolerance, int partAngle) {
	boolean turnDirection = true;
 
	if (servoPos > 90 + tolerance/2 || servoPos < 90 - tolerance/2) {
		if (servoPos > 90) {
			turnAngle(partAngle, !turnDirection, stepSpeed);
		}
		else {
			turnAngle(partAngle, turnDirection, stepSpeed);
		}	
	}
}
 
/**
* method to turn the robot by an specific angle.
*
* @param angle: amount of degrees (0 - 360) by which the robot should turn,
*		 turnDirection: true --> anti-clockwise turning, false --> clockwise turning,
*		 stepSpeed: the delay between two steps oft the stepper motors.
*/
 
void Gear::turnAngle (int angle, boolean turnDirection, int stepSpeed) {
	float stepsValue = round(angle/(360.0/((axisDistance*PI)/((wheelDiameter*PI)/200.0))));
 
	for (int countSteps = 0; countSteps < stepsValue; countSteps++) {
		if (turnDirection) {
			rightMotor.makeStep(false);
			leftMotor.makeStep(false);
			delay(stepSpeed);
		}
		else {
			rightMotor.makeStep(true);
			leftMotor.makeStep(true);
			delay(stepSpeed);
		}
	}
}

This library defines and implements only the ThermalCam class. We seperated the thermal camera library, because it is the largest and most complex part of the software.

ThermalCam.h

#ifndef ThermalCam_h
#define ThermalCam_h
 
 
#include "Arduino.h"
#include "i2c_t3.h"
#include "teensy3_Melexis90620.h"
#include "BeerBotLib.h"
 
class ThermalCam {
	private:
		Melexis90620 sensor;
		NewServo myServo;
 
		int sweepSpeed;
 
		double* pixelTemps;
		double temperatureBorder;
		int servoRange;
 
		boolean foundColdPixel();
		void getThermalData();
 
	public:
		void setup(int servoPin, int temperatureBorder);
		boolean searchColdSpot();
		int getPos();
};
 
#endif

ThermalCam.cpp

#include <ThermalCam.h>
 
void ThermalCam::setup(int servoPin, int temperatureBorder){
	myServo.setup(servoPin, 90, true);
	sensor.init(16);
 
	this-> sweepSpeed = 2;
	this-> temperatureBorder = temperatureBorder;
	this-> servoRange = 90;
}
 
/*
    Function for getting the thermaldata.
*/
 
void ThermalCam::getThermalData() {
    sensor.readTemperatures();
    pixelTemps = sensor.getPixelTemperatures();
}
 
/*
    Function sets thermalcam-servo to 90 degrees.
    Servo does a complete sweep and every degree the thermaldata gets checked for a cold source.
    If there is a cold source, "true" is returned back, else "false".
*/
 
boolean ThermalCam::searchColdSpot() {
    myServo.setPos(90);
    delay(100);
    for (int i = 0; i < servoRange; i++) {
        myServo.sweep((int)sweepSpeed, servoRange, 15);
        getThermalData();
	if (foundColdPixel()) {
	    return true;
	}
    }
    return false;
}
 
/*
    Function, which gives back the current position (angle) of the thermalcam-servo.
*/
 
int ThermalCam::getPos() {
    return myServo.getPos();
}
 
/*
    Function, which checks the two rows in the middle of the thermalcam-pixelarray for a cold source.
*/
 
boolean ThermalCam::foundColdPixel() {
    for( int i = 28; i < 35; i++ ) {
    if( pixelTemps[i] < temperatureBorder ) return true;
    }
    return false;
}

I2C library for Teensy3

http://forum.pjrc.com/threads/21680-New-I2C-library-for-Teensy3

Library to control the Melexis90620 thermal camera

https://github.com/Zapalot/teensy3_Melexis90620

Library for improved usage of an ultrasonic distance sensor with a Teensy.

https://github.com/PaulStoffregen/NewPing