Unterschiede

Hier werden die Unterschiede zwischen zwei Versionen gezeigt.

--- projekte2014:beerbot:dokumentation:software [2014/07/29 13:32]
wuxmax
+++ projekte2014:beerbot:dokumentation:software [2016/01/21 12:45] (aktuell)
@@ Zeile 1: / Zeile 1: @@
-====== Softwaredokumentation ======
+====== Quellcode ======
+===== Main Sketch =====
+<code c++ 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);
+}
+</code>
 ===== 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 ===
@@ Zeile 512: / Zeile 791: @@
 ==== 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 ===
@@ Zeile 531: / Zeile 812: @@
 		int sweepSpeed;
-		int servoSpeed;
-		float tolerance;
 		double* pixelTemps;
-		double pixelCompressedTo16[16];
-		double midPixels;
-		double leftSide;
-		double rightSide;
-		double pixelSum;
-		double coldestPixel;
 		double temperatureBorder;
-		double pixelCompressedTo8[8];
-		int dire;
 		int servoRange;
-		float searchTolerance;
 		boolean foundColdPixel();
 		void getThermalData();
-		void summarizeVerticalPixels();
-		void summarizeMidPixels();
-		boolean checkMidPixelsForCold();
-		void summarizeSides();
-		void moveToColderSide();
-		void moveToCold();
-		void searchColdestPixel();
-		void compressPixelTo8();
 	public:
 		void setup(int servoPin, int temperatureBorder);
 		boolean searchColdSpot();
-		void followColdSpot();
 		int getPos();
 };
@@ Zeile 577: / Zeile 839: @@
 	this-> sweepSpeed = 2;
-	this-> servoSpeed = 1;
-	this-> tolerance = 1.5;
-	this-> midPixels = 0.0;
-	this-> leftSide = 0.0;
-	this-> rightSide = 0.0;
-	this-> pixelSum = 0.0;
-	this-> coldestPixel = 0.0;
 	this-> temperatureBorder = temperatureBorder;
-	this-> dire = 0.0;
 	this-> servoRange = 90;
-	this-> searchTolerance = 1.5;
 }
+/*
+    Function for getting the thermaldata.
+*/
 void ThermalCam::getThermalData() {
-  sensor.readTemperatures();
+    sensor.readTemperatures();
-  pixelTemps = sensor.getPixelTemperatures();
+    pixelTemps = sensor.getPixelTemperatures();
 }
-void ThermalCam::summarizeVerticalPixels() {
+/*
-  for( int x = 0; x < 16; x++ ) {
+    Function sets thermalcam-servo to 90 degrees.
-    for( int y = 0; y < 4; y++ ) {
+    Servo does a complete sweep and every degree the thermaldata gets checked for a cold source.
-      pixelSum += pixelTemps[ y + x * 4 ];
+    If there is a cold source, "true" is returned back, else "false".
-    }
+*/
-    pixelCompressedTo16[x] = pixelSum / 4.0;
-    pixelSum = 0.0;
-  }
-}
-void ThermalCam::summarizeMidPixels() {
-  midPixels = ( pixelCompressedTo16[8] + pixelCompressedTo16[9] ) / 2;
-}
-boolean ThermalCam::checkMidPixelsForCold() {
-	return (midPixels < temperatureBorder);
-}
-void ThermalCam::summarizeSides() {
-    for( int x = 3; x < 8; x++ ) {
-    rightSide += pixelCompressedTo16[x];
-  }
-  rightSide /= 7;
-    for( int x = 8; x < 13; x++ ) {
-    leftSide += pixelCompressedTo16[x];
-  }
-  leftSide /= 7;
-}
-void ThermalCam::moveToColderSide() {
-  if( leftSide < rightSide && ( rightSide - leftSide ) > tolerance ) myServo.moveLeft((int)servoSpeed, 15);
-  else if( rightSide < leftSide && ( leftSide - rightSide ) > tolerance ) myServo.moveRight((int)servoSpeed, 15);
-  else;
-}
 boolean ThermalCam::searchColdSpot() {
-	myServo.setPos(90);
+    myServo.setPos(90);
-	delay(100);
+    delay(100);
-	for (int i = 0; i < servoRange; i++) {
+    for (int i = 0; i < servoRange; i++) {
-		myServo.sweep((int)sweepSpeed, servoRange, 15);
+        myServo.sweep((int)sweepSpeed, servoRange, 15);
-		getThermalData();
+        getThermalData();
-		for (int i = 28; i < 36; i++) {
+	if (foundColdPixel()) {
-			Serial.println(pixelTemps[i]);
+	    return true;
-		}
-		// summarizeVerticalPixels();
-		// summarizeMidPixels();
-		if (foundColdPixel()) {
-			return true;
-		}
 	}
-	return false;
+    }
+    return false;
 }
-void ThermalCam::followColdSpot() {
+/*
-	getThermalData();
+    Function, which gives back the current position (angle) of the thermalcam-servo.
-	//summarizeVerticalPixels();
+*/
-    //compressPixelTo8();
-    searchColdestPixel();
-    moveToCold();
-}
 int ThermalCam::getPos() {
-  return myServo.getPos();
+    return myServo.getPos();
 }
-void ThermalCam::moveToCold() {
+/*
-  if(dire < 28 && coldestPixel < temperatureBorder && getPos() < 90 + servoRange/2) {
+    Function, which checks the two rows in the middle of the thermalcam-pixelarray for a cold source.
-	  myServo.setPos(getPos() + (int)servoSpeed);
+*/
-	  delay(10);
-  }
-  else if(dire > 27 && coldestPixel < temperatureBorder && getPos() > 90 - servoRange/2) {
-	  myServo.setPos(getPos() - (int)servoSpeed);
-	  delay(10);
-  }
-}
 boolean ThermalCam::foundColdPixel() {
-  for( int i = 28; i < 35; i++ )
+    for( int i = 28; i < 35; i++ ) {
-  {
+    if( pixelTemps[i] < temperatureBorder ) return true;
-    if( pixelTemps[i] < temperatureBorder )
-    {
-		return true;
     }
-  }
+    return false;
-  return false;
 }
-void ThermalCam::searchColdestPixel() {
+</code>
-  coldestPixel = pixelTemps[4];
-  for( int i = 5; i < 60; i++ )
+==== 3rd party libraries ====
-  {
-    if( pixelTemps[i] < coldestPixel && ( coldestPixel - pixelTemps[i] ) > searchTolerance )
+=== i2c_t3 ====
-    {
-    coldestPixel = pixelTemps[i];
+I2C library for Teensy3
-    dire = i;
-    }
+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
-void ThermalCam::compressPixelTo8() {
-  summarizeVerticalPixels();
-  int j = 0;
-  for( int i = 0; i < 8; i++, j += 2 )
-  {
-    pixelCompressedTo8[i] = pixelCompressedTo16[j] + pixelCompressedTo16[j + 1];
-    pixelCompressedTo8[i] /= 2;
-  }
-}
-</code>

Projektlabor Robotik MINTgrün

Benutzer-Werkzeuge

Webseiten-Werkzeuge

Unterschiede

Seiten-Werkzeuge