From 72773939f598e40e55014f3653e6c7ea9f0afe20 Mon Sep 17 00:00:00 2001
From: Arnaud Cadot <arnaud.cadot@telecom-bretagne.eu>
Date: Thu, 17 Mar 2016 12:42:34 +0100
Subject: [PATCH] Finished setting up the beacons system in the simulator

---
 stm32/include/hardware/tourelle.h |  7 +++--
 stm32/src/hardware/tourelle.cpp   | 49 +++++++++++++++++++------------
 stm32/src/initialisation.cpp      |  2 +-
 stm32/src/simul/robot.cpp         |  2 ++
 stm32/src/simul/table.cpp         | 14 ++++++++-
 5 files changed, 52 insertions(+), 22 deletions(-)

diff --git a/stm32/include/hardware/tourelle.h b/stm32/include/hardware/tourelle.h
index e33ea991..c445d242 100644
--- a/stm32/include/hardware/tourelle.h
+++ b/stm32/include/hardware/tourelle.h
@@ -24,8 +24,8 @@
 
     // Maximum angular error (in degree)
     #define TURRET_TEST_MAX_ANGLE_DEV  2/2
-    // Maximum linear error (in cm)
-    #define TURRET_TEST_MAX_LIN_DEV  10/2
+    // Maximum linear error (in mm)
+    #define TURRET_TEST_MAX_LIN_DEV  100/2
 
 #endif
 
@@ -54,6 +54,9 @@ class PositionsList
 		// Returns the size of the data set
 		unsigned int size() const;
 
+        // Returns true if the list is empty
+        bool isEmpty() const;
+
 		// Access to i-ist element in the set (O(1)). Warning: there are no bound checks
 		PositionData& operator[](unsigned int i);
 		const PositionData& operator[](unsigned int i) const;
diff --git a/stm32/src/hardware/tourelle.cpp b/stm32/src/hardware/tourelle.cpp
index eba3b790..f0523c0c 100644
--- a/stm32/src/hardware/tourelle.cpp
+++ b/stm32/src/hardware/tourelle.cpp
@@ -21,13 +21,21 @@
  // PositionsList //
 ///////////////////
 
-PositionsList::PositionsList():m_usedSize(0),m_allocatedSize(0),m_array(0)
+PositionsList::PositionsList()
 {
+    m_usedSize      = 0;
+    m_allocatedSize = 0;
+    m_array         = 0;
+
 	reserve(5);
 }
 
-PositionsList::PositionsList(const PositionsList& l):m_usedSize(0),m_allocatedSize(0),m_array(0)
+PositionsList::PositionsList(const PositionsList& l)
 {
+    m_usedSize      = 0;
+    m_allocatedSize = 0;
+    m_array         = 0;
+
 	reserve(l.m_allocatedSize);
 	m_usedSize=l.m_usedSize;
 	memcpy(m_array,l.m_array,m_usedSize);
@@ -40,12 +48,12 @@ void PositionsList::reserve(unsigned int i)
 	memset(ptr, 0, i*sizeof(PositionData));
 
 	m_usedSize = min(m_usedSize, i);
-	m_allocatedSize=i;
-
-	if(m_array != 0)
-	{
+	m_allocatedSize=i;
+
+	if(m_array != 0)
+	{
         memcpy(ptr, m_array, m_usedSize*sizeof(PositionData));
-        delete m_array;
+        delete m_array;
 	}
 	m_array = ptr;
 }
@@ -63,6 +71,11 @@ unsigned int PositionsList::size() const
 	return m_usedSize;
 }
 
+bool PositionsList::isEmpty() const
+{
+    return size() == 0;
+}
+
 void PositionsList::clear()
 {
 	m_usedSize=0;
@@ -99,16 +112,16 @@ const PositionData& PositionsList::operator[](unsigned int i) const
 {
 	return (*this)[i];
 }
-
+
 #ifndef ROBOTHW
 PositionsList PositionsList::fromQList(const QList<PositionData>& list)
 {
     PositionsList l;
 
-    for(size_t i=0; i<list.size();++i)
+    for(int i=0; i<list.size();++i)
         l.append(list[i]);
     return l;
-}
+}
 #endif
 
   ////////////////////
@@ -197,14 +210,14 @@ Tourelle* Tourelle::getSingleton()
 
 Tourelle::Tourelle()
 {
-#ifdef ROBOTHW
+#ifdef ROBOTHW
 #ifdef STM32F10X_CL //h107
     initClocksAndPortsGPIO(RCC_APB1Periph_USART2, GPIO_Remap_USART2, GPIOD, GPIO_Pin_6, GPIOD, GPIO_Pin_5);
-    initUART(TURRET_USART_INDEX, USART_BAUDRATE);
-#endif
-#ifdef STM32F40_41xxx // For stm32 h405
-    // TODO
-#endif
+    initUART(TURRET_USART_INDEX, USART_BAUDRATE);
+#endif
+#ifdef STM32F40_41xxx // For stm32 h405
+    // TODO
+#endif
 
 #endif
 }
@@ -225,7 +238,7 @@ unsigned int Tourelle::beaconsDetected()
     return Table::getMainInstance()->getBeaconsRelativePosition().size();
 #endif
 }
-
+
 #ifndef ROBOTHW
 int alea(int mi, int ma)
 {
@@ -246,7 +259,7 @@ int alea(int mi, int ma)
         ini = true;
     }
     return rand()%(ma-mi+1)+mi;
-}
+}
 #endif
 
 
diff --git a/stm32/src/initialisation.cpp b/stm32/src/initialisation.cpp
index 063026de..a0a211dd 100644
--- a/stm32/src/initialisation.cpp
+++ b/stm32/src/initialisation.cpp
@@ -27,7 +27,7 @@ extern "C" void SysTick_Handler()
 
     Odometrie::odometrie->update();
 
-    //sStrategieV2::update();
+    //StrategieV2::update();
 
     Tourelle::getSingleton()->update();
 
diff --git a/stm32/src/simul/robot.cpp b/stm32/src/simul/robot.cpp
index 1b3c4a05..d2c99783 100644
--- a/stm32/src/simul/robot.cpp
+++ b/stm32/src/simul/robot.cpp
@@ -6,6 +6,7 @@
 #include "asservissement.h"
 #include "strategieV2.h"
 #include "sensors.h"
+#include "hardware/tourelle.h"
 #include <iostream>
 #include <QDebug>
 
@@ -348,6 +349,7 @@ void Robot::paint(QPainter &p, int dt)
         else
         {
             Odometrie::odometrie->update();
+            Tourelle::getSingleton()->update();
             StrategieV2::update();
             Asservissement::asservissement->update();
             deriv.position.x = asservissement->getLinearSpeed();
diff --git a/stm32/src/simul/table.cpp b/stm32/src/simul/table.cpp
index 1455070c..2e1e2610 100644
--- a/stm32/src/simul/table.cpp
+++ b/stm32/src/simul/table.cpp
@@ -479,6 +479,17 @@ void Table::update(int dt)
 
     debugText += "Sharps : \n " + sharpsChecked + "\n\n";
 
+    debugText += "Detected beacons: \n";
+    PositionsList l = Tourelle::getSingleton()->getPositionsList();
+    if(!l.isEmpty())
+    {
+        for(size_t i=0;i<l.size();++i)
+            debugText += ("Distance: " + QString::number(l[i].distance) + "mm Angle: " + QString::number(l[i].angle) + " degs\n");
+        debugText += "\n";
+    }
+    else
+        debugText += "None\n\n";
+
     DebugWindow::getInstance()->setText(debugText);
     if (!mRemoteMod)
     {
@@ -708,6 +719,7 @@ QList<PositionData> Table::getBeaconsRelativePosition(Robot* refBot)
     refBot = (!refBot)?getMainRobot():refBot;
 
     Position refPosition = refBot->getPos().getPosition();
+    float refAngle       = refBot->getPos().getAngle();
 
     QList<PositionData> positions;
 
@@ -722,7 +734,7 @@ QList<PositionData> Table::getBeaconsRelativePosition(Robot* refBot)
 
         PositionData polarPosition;
         polarPosition.distance = sqrt(x*x+y*y);
-        polarPosition.angle = static_cast<unsigned long>(atan2(y, x) * 180.f / 3.1415f + 180.f);
+        polarPosition.angle = static_cast<unsigned long>((atan2(y, x) + refAngle) * 180.f / 3.1415f)%360;
 
         positions.append(polarPosition);
     }
-- 
GitLab