Commit 72773939 authored by Arnaud Cadot's avatar Arnaud Cadot

Finished setting up the beacons system in the simulator

parent 8b7c4a82
......@@ -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;
......
......@@ -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
......
......@@ -27,7 +27,7 @@ extern "C" void SysTick_Handler()
Odometrie::odometrie->update();
//sStrategieV2::update();
//StrategieV2::update();
Tourelle::getSingleton()->update();
......
......@@ -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();
......
......@@ -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);
}
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment