Cours:SaéSBC robotAutonome

L'objectif général est de concevoir la partie logicielle (POO/C++/Qt) d'une voiture autonome.

Le projet sera décomposé en deux parties :

1. Conception logicielle d'une voiture simulée
2. Conception logicielle d'une voiture réelle=

Partie 1 : voiture simulée

• À partir du lundi matin jusqu'au mardi fin de matinée
• Objectifs progressifs

1) Tours de circuits sans sortir des limites, au minimum sans la gestion des collisions entre voitures

2) Ajout d'une interface déportée :

• Un bouton Start
• Affichage d'informations sur l'état de la voiture
• Communiquant avec la voiture par TCP

3) Tours sur un circuit plus complexe, comportant des virages serrés et nécessitant une gestion de la marche arrière

Partie 2 : voiture réelle

Objectifs progressifs

1) Déplacement en évitant les obstacles

2) Ajout d'une interface délocalisée

3) Gestion de la marche arrière

4) Déplacement sur un trajet connu à l'avance, comportant des croisements (par ex. départ en GEII, arrivée dans la hall central de l'IUT).

Tests unitaires

Ajouter dans le fichier de projet :

QT += testlib

testcorrecteurkp.h #ifndef TESTCORRECTEURKP_H #define TESTCORRECTEURKP_H #include <QtTest/QTest> #include <QObject> class TestCorrecteurKp : public QObject { Q_OBJECT public: explicit TestCorrecteurKp(QObject *parent = nullptr); private slots: void process_data(); void process(); signals: }; #endif // TESTCORRECTEURKP_H

testcorrecteurkp.cpp #include "testcorrecteurkp.h" #include "correcteurkp.h" TestCorrecteurKp::TestCorrecteurKp(QObject *parent) : QObject{parent} { } void TestCorrecteurKp::process_data() { QTest::addColumn<double>("consigne"); QTest::addColumn<double>("Kp"); QTest::addColumn<double>("input"); QTest::addColumn<double>("output"); QTest::newRow("") << 100.0 << 1.0 << 100.0 << 0.0 ; QTest::newRow("") << 100.0 << 1.0 << 80.0 << 20.0 ; QTest::newRow("") << 100.0 << 1.0 << 120.0 << -20.0 ; QTest::newRow("") << 80.0 << 1.0 << 80.0 << 0.0 ; QTest::newRow("") << 80.0 << 1.0 << 70.0 << 10.0 ; QTest::newRow("") << 80.0 << 1.0 << 90.0 << -10.0 ; } void TestCorrecteurKp::process() { QFETCH(double, consigne); QFETCH(double, Kp); QFETCH(double, input); QFETCH(double, output); QCOMPARE(CorrecteurKp(consigne,Kp).process(input), output); }

//main.cpp
#include "testcorrecteurkp.h"

QTEST_MAIN(TestCorrecteurKp)

testtcpclient.h #ifndef TESTTCPCLIENT_H #define TESTTCPCLIENT_H #include <QtTest/QTest> #include <QObject> #include <tcpclient.h> class TestTcpClient : public QObject { Q_OBJECT public: explicit TestTcpClient(QObject *parent = nullptr); private : QString data; QDataStream dataIn; private slots: void readyRead_data(); void readyRead(); void readClientValue(QString message); void readServeurValue(); QString getTcpClientData(QTcpSocket * serveurSocket,QString message); QString sendTcpClientData(TcpClient &monClient,QString message); signals: }; #endif // TESTTCPCLIENT_H

testtcpclient.cpp #include "testtcpclient.h" #include "tcpclient.h" #include <QTcpServer> TestTcpClient::TestTcpClient(QObject *parent) : QObject{parent} {} void TestTcpClient::readyRead_data() { QTest::addColumn<QString>("message"); QTest::newRow("") << "hello"; QTest::newRow("") << "couocu"; QTest::newRow("") << "100"; QTest::newRow("") << "10.4"; QTest::newRow("") << "0.5;-0.3"; } void TestTcpClient::readyRead() { QTcpServer monServeur; int port=rand()%1000+9000; monServeur.listen(QHostAddress("127.0.0.1"),port); TcpClient monClient{"127.0.0.1",port}; QTest::qWaitFor([&monServeur]() { return monServeur.hasPendingConnections(); },1000); QVERIFY(monServeur.hasPendingConnections()); QTcpSocket * serveurSocket=monServeur.nextPendingConnection(); dataIn.setDevice(serveurSocket); QFETCH(QString, message); connect(&monClient,&TcpClient::newDatas, this,&TestTcpClient::readClientValue); QCOMPARE(getTcpClientData(serveurSocket,message),message); disconnect(&monClient,&TcpClient::newDatas, this,&TestTcpClient::readClientValue); connect(serveurSocket,&QTcpSocket::readyRead, this,&TestTcpClient::readServeurValue); QCOMPARE(sendTcpClientData(monClient,message),message); disconnect(serveurSocket,&QTcpSocket::readyRead, this,&TestTcpClient::readServeurValue); dataIn.setDevice(nullptr); } void TestTcpClient::readClientValue(QString message) { data=message; } void TestTcpClient::readServeurValue() { dataIn.startTransaction(); if (!dataIn.commitTransaction()) return; dataIn >> data; } QString TestTcpClient::getTcpClientData(QTcpSocket *serveurSocket, QString message) { QByteArray block; QDataStream out(&block, QIODevice::WriteOnly); out.setVersion(QDataStream::Qt_5_0); out << message; serveurSocket->write(block); QTest::qWait(100); return data; } QString TestTcpClient::sendTcpClientData(TcpClient &monClient, QString message) { monClient.sendData(message); QTest::qWait(100); return data; }

//main.cpp
#include "testtcpclient.h"
QTEST_MAIN(TestTcpClient)

testtcpserver.h #ifndef TESTTCPSERVER_H #define TESTTCPSERVER_H #include <QtTest/QTest> #include <QObject> #include <QDataStream> class TestTcpServer : public QObject { Q_OBJECT public: explicit TestTcpServer(QObject *parent = nullptr); private: QString data; QDataStream dataIn; public slots: void getData(QString _data); void readyRead(); private slots: void sendData_data(); void sendData(); signals: }; #endif // TESTTCPSERVER_H

testtcpserver.cpp #include "testtcpserver.h" #include "tcpserver.h" #include <QTcpSocket> TestTcpServer::TestTcpServer(QObject *parent) : QObject{parent} {} void TestTcpServer::sendData_data() { QTest::addColumn<QString>("message"); QTest::newRow("") << "hello"; QTest::newRow("") << "coucou"; QTest::newRow("") << "100"; QTest::newRow("") << "10.4"; QTest::newRow("") << "0.5;-0.3"; } void TestTcpServer::sendData() { QFETCH(QString, message); int port=rand()%1000+9000; TcpServer monServer(port); QTcpSocket monClient; monClient.connectToHost("127.0.0.1",port); QTest::qWait(100); QVERIFY(monClient.state()==QAbstractSocket::ConnectedState); // vérif client vers server QByteArray block; QDataStream out(&block, QIODevice::WriteOnly); out << message; connect(&monServer,&TcpServer::newDatas, this,&TestTcpServer::getData); monClient.write(block); QTest::qWait(100); QCOMPARE(data,message); disconnect(&monServer,&TcpServer::newDatas, this,&TestTcpServer::getData); // vérif server vers client dataIn.setDevice(&monClient); connect(&monClient,&QTcpSocket::readyRead, this,&TestTcpServer::readyRead); monServer.sendData(message); QTest::qWait(100); QCOMPARE(data,message); disconnect(&monClient,&QTcpSocket::readyRead, this,&TestTcpServer::readyRead); monClient.disconnect(); dataIn.setDevice(nullptr); QTest::qWait(100); } void TestTcpServer::getData(QString _data) { data=_data; } void TestTcpServer::readyRead() { dataIn.startTransaction(); if (!dataIn.commitTransaction()) return; dataIn >> data; }

//main.cpp
#include "testtcpserver.h"
QTEST_MAIN(TestTcpServer);

testservomoteur.h #ifndef TESTSERVOMOTEUR_H #define TESTSERVOMOTEUR_H #include <QtTest/QTest> #include <QObject> class TestServoMoteur : public QObject { Q_OBJECT public: explicit TestServoMoteur(QObject *parent = nullptr); private slots: void setPosition_data(); void setPosition(); signals: }; #endif // TESTSERVOMOTEUR_H

testservomoteur.cpp #include "testservomoteur.h" #include "servomoteur.h" #include <QFile> TestServoMoteur::TestServoMoteur(QObject *parent) : QObject{parent} {} void TestServoMoteur::setPosition_data() { QTest::addColumn<double>("offset_ms"); QTest::addColumn<double>("amp_ms"); QTest::addColumn<double>("angle"); QTest::addColumn<int>("duty"); QTest::newRow("") << 1.5 << 0.5 << 0.0 << 1500000; QTest::newRow("") << 1.6 << 0.5 << 0.0 << 1600000; QTest::newRow("") << 1.4 << 0.5 << 0.0 << 1400000; QTest::newRow("") << 1.6 << 0.5 << 0.1 << 1650000; QTest::newRow("") << 1.4 << 0.5 << -0.1 << 1350000; QTest::newRow("") << 1.6 << 0.25 << 0.2 << 1650000; QTest::newRow("") << 1.4 << 0.25 << -0.2 << 1350000; } void TestServoMoteur::setPosition() { QFETCH(double, offset_ms); QFETCH(double, amp_ms); QFETCH(double, angle); QFETCH(int, duty); ServoMoteur servo{offset_ms,amp_ms}; QFile fileDuty{"/sys/class/pwm/pwmchip2/pwm2/duty_cycle"}; QVERIFY(fileDuty.open(QIODevice::ReadOnly)); servo.setPosition(angle); QCOMPARE(duty, fileDuty.readLine().toInt()); fileDuty.close(); }

//main.cpp
#include "testservomoteur.h"
QTEST_MAIN(TestServoMoteur)

testmotordrivermd04.h #ifndef TESTMOTORDRIVERMD04_H #define TESTMOTORDRIVERMD04_H #include <QtTest/QTest> #include <QObject> class TestMotorDriverMD04 : public QObject { Q_OBJECT public: explicit TestMotorDriverMD04(QObject *parent = nullptr); private slots: void setVitesse_data(); void setVitesse(); signals: }; #endif // TESTMOTORDRIVERMD04_H

testmotordrivermd04.cpp #include "testmotordrivermd04.h" #include "motordrivermd04.h" TestMotorDriverMD04::TestMotorDriverMD04(QObject *parent) : QObject{parent} { } void TestMotorDriverMD04::setVitesse_data() { QTest::addColumn<double>("vitesse"); QTest::addColumn<int>("regSensValue"); QTest::addColumn<int>("regSpeedValue"); QTest::newRow("") << 0.0 << 1 <<0; QTest::newRow("") << 0.1 << 1 << 25; QTest::newRow("") << -0.1 << 2 << 25; QTest::newRow("") << 0.0 << 1 << 0; } void TestMotorDriverMD04::setVitesse() { QFETCH(double, vitesse); QFETCH(int, regSensValue); QFETCH(int, regSpeedValue); MotorDriverMD04 moteur; QI2cDevice i2c{I2C1,0x58}; moteur.setVitesse(vitesse); unsigned char sensValue=i2c.readRegister(0); unsigned char speedValue=i2c.readRegister(2); QCOMPARE(regSpeedValue,speedValue); QCOMPARE(regSensValue,sensValue); }

//main.cpp
#include "testmotordrivermd04.h"
QTEST_MAIN(TestMotorDriverMD04);

Annexe : ressources

carte extension PI5

Media:PI5HAT_tamyia_v2.pdf

Dépôt fichier

Groupe 2 : https://drive.google.com/drive/folders/1LFSswM43qexz2vOMF9SCNCV1p5K9-Jxt?usp=drive_link

rplidar

Ajouter au fichier de projet :

unix:!macx: LIBS += -L/opt/electronique/pi4TpInfo/sysroot/rplidar/rplidar_sdk/output/Linux/Release/ -lsl_lidar_sdk

INCLUDEPATH += /opt/electronique/pi4TpInfo/sysroot/rplidar/rplidar_sdk/sdk/include
INCLUDEPATH += /opt/electronique/pi4TpInfo/sysroot/rplidar/rplidar_sdk/sdk/src
DEPENDPATH += /opt/electronique/pi4TpInfo/sysroot/rplidar/rplidar_sdk/sdk/include

unix:!macx: PRE_TARGETDEPS += /opt/electronique/pi4TpInfo/sysroot/rplidar/rplidar_sdk/output/Linux/Release/libsl_lidar_sdk.a

Initialisation :

// fichier .h

#include "sl_lidar_driver.h"
using namespace std;
using namespace sl;

ILidarDriver * drv;


// fichier .cpp
#include <QtMath>

#ifndef _countof
#define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0]))
#endif



    drv = *createLidarDriver();
    if (!drv) {
        qDebug()<<"erreur";
    }
    else
    {
        qDebug()<<"driver ok";
    }

    IChannel* _channel;
    _channel = (*createSerialPortChannel("/dev/ttyUSB0", 115200));

    sl_result     op_result;
    sl_lidar_response_device_info_t devinfo;
    bool connectSuccess = false;
    if (SL_IS_OK((drv)->connect(_channel))) {
        op_result = drv->getDeviceInfo(devinfo);
        if (SL_IS_OK(op_result))
        {
            connectSuccess = true;
            qDebug()<<"connecté sur le lidar";
        }
        else{
            delete drv;
            drv = NULL;
        }
        qDebug()<<"SLAMTEC LIDAR S/N: "<<devinfo.serialnum;
        qDebug()<<"Firmware Ver: "<<(devinfo.firmware_version>>8)<<"."<<(devinfo.firmware_version & 0xFF)
                 <<"Hardware Rev: "<<(int)devinfo.hardware_version;
    }
    drv->setMotorSpeed();
    drv->startScan(0,1);

Lecture des données (utiliser un QTimer, toutes les 50ms par ex):

    sl_lidar_response_measurement_node_hq_t nodes[8192];
    size_t   count = _countof(nodes);
    sl_result op_result = drv->grabScanDataHq(nodes, count);

    if (SL_IS_OK(op_result))
    {
        distances.fill(0);
        for (int i=0;i<(int)count;i++)
        {
            int angle=round((nodes[i].angle_z_q14 * 90.f) / 16384.f);
            if (angle>=180) angle=angle-360;
            if ((angle>=-180)and(angle<180))
            {
                int d=nodes[i].dist_mm_q2/4.0;
                if (d>0) distances.at(angle+180)=d;
            }
        }
        for (int i=0;i<360;i++)
        {
            if (distances.at(i)==0)
            {
                for (int j=1;j<10;j++)
                {
                    int pos=(i+j)%360;
                    if (distances.at(pos)!=0) distances.at(i)=distances.at(pos);
                }
            }
        }
    }

driver moteur MD04

• Media:Md04tech.pdf

pi5 et gpio

cat /sys/kernel/debug/gpio

utiliser libgpiod !!

pi5 et pwm

configuration de la pi5

• config : modifier le fichier /boot/firmware/config.txt

dtoverlay=pwm-2chan,pin=18,pin2=19

ressources

• https://gist.github.com/Gadgetoid/b92ad3db06ff8c264eef2abf0e09d569
• https://community.element14.com/products/raspberry-pi/m/files/148385
• https://forums.raspberrypi.com/viewtopic.php?t=359251

principe

on utilisera la PWM2 GPIO18 du pwmchip2 ( sinon PWM3 GPIO19 du pwmchip2)

• rendre la PWM accessible dans sysfs
  • écrire la valeur 2 dans le fichier /sys/class/pwm/pwmchip2/export
  • en console : echo 2 > /sys/class/pwm/pwmchip2/export
• dans le dossier /sys/class/pwm/pwmchip2/pwm2
  • le fichier duty_cyle permet de modifier la durée de l'état haut ( en ns)
  • le fichier period permet de modifier la période de la MLI ( en ns)
  • ATTENTION : pour pouvoir modifier period il convient d'avoir duty_cyle<period (mettre à 0 avant par ex)
• pour écrire dans des fichiers, utiliser QFILE, puis les méthodes
  • open
  • write
  • flush
  • close
• il convient d'attendre après avoir écrit dans le fichier export :
  • QThread::msleep(10);
• utiliser QByteArray::number

tfmini

• https://github.com/TFmini/TFmini-I2C-MasterExample_Arduino/blob/master/TFminiReadData/TFminiReadData.ino

simulateur

• https://github.com/bjacquot/simulateurVoitureAutonome

git clone https://github.com/bjacquot/simulateurVoitureAutonome.git