====== Quellcode ======
===== Main Sketch =====
#include
#include
#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);
}
===== Libraries =====
==== BeerBotLib ====
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);
}
}
}
==== ThermalCam ====
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
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;
}
==== 3rd party libraries ====
=== i2c_t3 ====
I2C library for Teensy3
http://forum.pjrc.com/threads/21680-New-I2C-library-for-Teensy3
=== teensy3_Melexis90620 ===
Library to control the Melexis90620 thermal camera
https://github.com/Zapalot/teensy3_Melexis90620
=== NewPing ===
Library for improved usage of an ultrasonic distance sensor with a Teensy.
https://github.com/PaulStoffregen/NewPing