Calculate position and display it.

Author: dhebbeker

remote-website/robot-control.php

@@ -2,21 +2,29 @@
 $image = imagecreatetruecolor(1000, 1000);
 $default_color = imagecolorallocate($image, 0xFF, 0xFF, 0xFF);
 
+$data_server = "http://kps32.ddns.net/?positions";
+$positionString = file_get_contents($data_server);
+
+
 $positions = array();
-for ($i = 0; $i < 10; $i ++) {
-    $positions[] = rand(0, 1000);
+if($positionString === FALSE) {
+    echo "Could not read the data from ".$data_server.".";
+} else {
+    $positions = explode(";", $positionString);
 }
 
 $diameter = 10;
-$start_x = 0;
-$start_y = 0;
+$offset_x = 500;
+$offset_y = 500;
+$start_x = $offset_x;
+$start_y = $offset_y;
 
-for ($i = 0; $i < count($positions); $i += 2) {
-    $x = $positions[$i];
-    $y = $positions[$i + 1];
+for ($i = 0; $i+1 < count($positions); $i += 2) {
+    $x = ((int) $positions[$i])/10+$offset_x;
+    $y = ((int) $positions[$i + 1])/10+$offset_y;
 
     imagefilledellipse($image, $x, $y, $diameter, $diameter, $default_color);
-    imagestring($image, 3, $x + $diameter / 2, $y + $diameter / 2, sprintf("%u;%u", $x, $y), $default_color);
+    imagestring($image, 3, $x + $diameter / 2, $y + $diameter / 2, sprintf("%u", $i/2), $default_color);
     imageline($image, $start_x, $start_y, $x, $y, $default_color);
 
     $start_x = $x;

robot-control-src/Drives.cpp

@@ -8,6 +8,17 @@
 namespace drives
 {
 
+static constexpr float stepsPerDeg = 100/(4.8*360);
+static constexpr float stepsPerRad = stepsPerDeg / ((2*pi)/360);
+
+static enum class Action
+{
+	STRAIGHTFORWARD, TURN_LEFT, TURN_RIGHT
+} lastAction;
+
+Position lastKnownPosition = {0,0};
+static float orientation = 0; // in radians
+
 template<typename MOTORCONTROL, MOTORCONTROL &motorControlpin, typename DIRECTIONPIN, DIRECTIONPIN &directionPin, typename ODOPIN, ODOPIN &odoPin>
 Counter volatile Drive<MOTORCONTROL, motorControlpin, DIRECTIONPIN, directionPin, ODOPIN, odoPin>::counter = 0;
 template<typename MOTORCONTROL, MOTORCONTROL &motorControlpin, typename DIRECTIONPIN, DIRECTIONPIN &directionPin, typename ODOPIN, ODOPIN &odoPin>
@@ -49,19 +60,20 @@ static Amplitude calcRightSpeed(const Amplitude leftSpeed)
 
 void rotateCounter(const Counter steps, const Amplitude amplitude, bool const clockwise)
 {
+	lastAction = clockwise ? Action::TURN_RIGHT : Action::TURN_LEFT;
   LeftDrive::drive(steps, amplitude, !clockwise);
   RightDrive::drive(steps, calcRightSpeed(amplitude), clockwise);
 }
 
 void driveCounter(const Counter steps, const Amplitude amplitude, const bool backwards)
 {
+	lastAction = Action::STRAIGHTFORWARD;
   LeftDrive::drive(steps, amplitude, backwards);
   RightDrive::drive(steps, calcRightSpeed(amplitude), backwards);
 }
 
 void rotate(const float deg, const Amplitude amplitude, bool const clockwise)
 {
-  constexpr float stepsPerDeg = 100/(4.8*360);
   const Counter steps = std::round(deg * stepsPerDeg);
   rotateCounter(steps, amplitude, clockwise);
 }
@@ -73,7 +85,35 @@ void drive(const float distance, const Amplitude amplitude, const bool backwards
   driveCounter(steps, amplitude, backwards);
 }
 
-IRAM_ATTR void stopDrives() {
+Position flushCurrentPosition()
+{
+	switch (lastAction) {
+		case Action::STRAIGHTFORWARD:
+		{
+			lastKnownPosition.x += LeftDrive::counter * board::odoIntervalLength * std::cos(orientation);
+			lastKnownPosition.y += LeftDrive::counter * board::odoIntervalLength * std::sin(orientation);
+			break;
+		}
+		case Action::TURN_LEFT:
+		{
+			orientation -= LeftDrive::counter / stepsPerRad;
+			break;
+		}
+		case Action::TURN_RIGHT:
+		{
+			orientation += LeftDrive::counter / stepsPerRad;
+			break;
+		}
+		default:
+			break;
+	}
+	LeftDrive::counter = 0;
+	RightDrive::counter = 0;
+	return lastKnownPosition;
+}
+
+IRAM_ATTR void stopDrives()
+{
 	LeftDrive::stop();
 	RightDrive::stop();
 }

robot-control-src/Drives.hpp

@@ -6,6 +6,12 @@
 #include <cstdint>
 #include <Arduino.h>
 
+using Distance = std::int16_t; //!< in [mm]
+struct Position
+{
+	Distance x, y;
+};
+
 namespace drives
 {
 
@@ -73,6 +79,8 @@ void rotate(const float deg, const Amplitude amplitude, bool const clockwise);
 void driveCounter(const Counter distance, const Amplitude amplitude, const bool backwards);
 void drive(const float distance, const Amplitude amplitude, const bool backwards);
 
+Position flushCurrentPosition();
+
 void stopDrives();
 
 };

robot-control-src/robot-control-src.ino

@@ -1,6 +1,7 @@
 #include "board.hpp"
 #include "Drives.hpp"
 #include "wifi_ap.hpp"
+#include <assert.h>
 #include <ESP8266WebServer.h>
 
 static ESP8266WebServer server(80);
@@ -14,6 +15,10 @@ static struct
   bool clockwise = true;
 } newTarget;
 
+static constexpr std::size_t numberOfPositions = 50;
+static Position positions[numberOfPositions] = { {0,0} };
+static std::size_t positionIndex = 0;
+
 constexpr auto htmlSource = 
     "<!DOCTYPE html>\n"
     "<html lang=\"en\">\n"
@@ -68,7 +73,24 @@ static void handleRoot()
     newTarget.isTargetNew = true;
     Serial.printf("Got right by %u!\n", newTarget.newRotate);
   }
-  server.send(200, "text/html", htmlSource);
+  if(server.hasArg("positions"))
+  {
+	  constexpr std::size_t charPerEntry = (5+1)*2;
+	  constexpr std::size_t bufferLength = charPerEntry*numberOfPositions+1; 
+	  char positionStringBuffer[bufferLength] = { 0 };
+	  std::size_t charPos = 0;
+	  for(std::size_t i=0; i<positionIndex; i++)
+	  {
+		  const int writtenCharacters = snprintf(&(positionStringBuffer[charPos]), charPerEntry+1, "%i;%i;", positions[i].x, positions[i].y);
+		  assert(writtenCharacters>0);
+		  charPos += writtenCharacters;	  
+	  }
+	  server.send(200, "text/plain", positionStringBuffer);
+  }
+  else
+  {
+	  server.send(200, "text/html", htmlSource);
+  }
   digitalWrite(board::debugLed, HIGH);
 }
 
@@ -117,6 +139,8 @@ void loop()
   //Serial.printf("left: \t%3u, right: \t%3u\n", drives::LeftDrive::counter, drives::RightDrive::counter);
   if(drives::LeftDrive::isIdle && drives::RightDrive::isIdle && newTarget.isTargetNew)
   {
+	  positions[positionIndex++] = drives::flushCurrentPosition();
+	  positionIndex %= numberOfPositions;
     if(newTarget.newDrive!=0)
     {
       drives::driveCounter(newTarget.newDrive, drives::cruiseSpeed, !newTarget.forward);