Changeset 377 in flair-src


Ignore:
Timestamp:
Dec 16, 2020, 9:40:44 AM (4 years ago)
Author:
Sanahuja Guillaume
Message:

ugv update

Location:
trunk
Files:
6 added
12 edited

Legend:

Unmodified
Added
Removed

  • trunk/demos/TwoWheelRobotCircleFollower/ugv/resources/core2-64/ugv.sh

    r376 r377  
    44        EXEC=./TwoWheelRobotCircleFollower_ugv_rt
    55else
    6         EXEC=.TwoWheelRobotCircleFollower_ugv_nrt
     6        EXEC=./TwoWheelRobotCircleFollower_ugv_nrt
    77fi
    88

  • trunk/demos/TwoWheelRobotCircleFollower/ugv/src/CircleFollower.cpp

    r376 r377  
    2525#include <Tab.h>
    2626#include <Pid.h>
     27#include <Ugv.h>
     28#include <UgvControls.h>
    2729
    2830using namespace std;
    … …  
    3234using namespace flair::filter;
    3335using namespace flair::meta;
     36using namespace flair::actuator;
    3437
    3538CircleFollower::CircleFollower(string name,TargetController *controller): Thread(getFrameworkManager(),"CircleFollower",50), behaviourMode(BehaviourMode_t::Default), vrpnLost(false) {
    3639   this->controller=controller;
    3740    controller->Start();
    38        
    39     VrpnClient* vrpnclient=new VrpnClient("vrpn", "127.0.0.1:3883",80);
     41   
     42    Ugv* ugv=GetUgv();
     43    ugv->UseDefaultPlot();
     44   
     45    VrpnClient* vrpnclient=new VrpnClient("vrpn", ugv->GetDefaultVrpnAddress(),80);
    4046   
    4147    if(vrpnclient->ConnectionType()==VrpnClient::Xbee) {
    42         uavVrpn = new MetaVrpnObject(name,(uint8_t)0);
     48        ugvVrpn = new MetaVrpnObject(name,(uint8_t)0);
    4349        targetVrpn=new MetaVrpnObject("target",1);
    4450    } else if (vrpnclient->ConnectionType()==VrpnClient::Vrpn) {
    45         uavVrpn = new MetaVrpnObject(name);
     51        ugvVrpn = new MetaVrpnObject(name);
    4652        targetVrpn=new MetaVrpnObject("target");
    4753    }
    4854   
    49     getFrameworkManager()->AddDeviceToLog(uavVrpn);
     55    getFrameworkManager()->AddDeviceToLog(ugvVrpn);
    5056    getFrameworkManager()->AddDeviceToLog(targetVrpn);
    5157    vrpnclient->Start();
    … …  
    5864   
    5965    circle=new TrajectoryGenerator2DCircle(vrpnclient->GetLayout()->NewRow(),"circle");
    60     uavVrpn->xPlot()->AddCurve(circle->GetMatrix()->Element(0,0),DataPlot::Blue);
    61     uavVrpn->yPlot()->AddCurve(circle->GetMatrix()->Element(0,1),DataPlot::Blue);
    62     uavVrpn->VxPlot()->AddCurve(circle->GetMatrix()->Element(1,0),DataPlot::Blue);
    63     uavVrpn->VyPlot()->AddCurve(circle->GetMatrix()->Element(1,1),DataPlot::Blue);
    64     uavVrpn->XyPlot()->AddCurve(circle->GetMatrix()->Element(0,1),circle->GetMatrix()->Element(0,0),DataPlot::Blue,"circle");
     66    ugvVrpn->xPlot()->AddCurve(circle->GetMatrix()->Element(0,0),DataPlot::Blue);
     67    ugvVrpn->yPlot()->AddCurve(circle->GetMatrix()->Element(0,1),DataPlot::Blue);
     68    ugvVrpn->VxPlot()->AddCurve(circle->GetMatrix()->Element(1,0),DataPlot::Blue);
     69    ugvVrpn->VyPlot()->AddCurve(circle->GetMatrix()->Element(1,1),DataPlot::Blue);
     70    ugvVrpn->XyPlot()->AddCurve(circle->GetMatrix()->Element(0,1),circle->GetMatrix()->Element(0,0),DataPlot::Blue,"circle");
    6571/*
    6672    uX=new Pid(setupLawTab->At(1,0),"u_x");
    … …  
    8187    }
    8288
    83     while (!ToBeStopped()) {/*
    84         float x=-controller->GetAxisValue(3)*forwardCoef->Value();
    85         float rot=controller->GetAxisValue(0)*turnCoef->Value();
    86         
    87         sumoControl->move(x, rot);
    88        
     89    while (!ToBeStopped()) {
     90        float speed=-controller->GetAxisValue(3);
     91        float turn=controller->GetAxisValue(0);
     92       
     93        //sumoControl->move(x, rot);
     94        GetUgv()->GetUgvControls()->SetControls(speed,turn);
    8995        if (button_kill->Clicked() == true)
    9096            SafeStop();
    91         */
     97       
    9298        WaitPeriod();
    9399    }

  • trunk/demos/TwoWheelRobotCircleFollower/ugv/src/CircleFollower.h

    r376 r377  
    2323    namespace filter {
    2424        class TrajectoryGenerator2DCircle;
    25                 class Pid;
     25        class Pid;
    2626    }
    2727    namespace meta {
    … …  
    5656
    5757        flair::gui::PushButton *startCircle,*stopCircle,*button_kill;
    58         flair::meta::MetaVrpnObject *targetVrpn,*uavVrpn;
     58        flair::meta::MetaVrpnObject *targetVrpn,*ugvVrpn;
    5959        flair::filter::TrajectoryGenerator2DCircle *circle;
     60
    6061};
    6162

  • trunk/demos/TwoWheelRobotCircleFollower/ugv/src/main.cpp

    r376 r377  
    1414#include "CircleFollower.h"
    1515#include <FrameworkManager.h>
     16#include <UgvFactory.h>
    1617#include <stdio.h>
    1718#include <tclap/CmdLine.h>
    … …  
    2425using namespace flair::sensor;
    2526
     27string ugv_type;
    2628string log_path;
    2729int port;
    … …  
    4345    manager->SetupLogger(log_path);
    4446
     47    Ugv* ugv=CreateUgv(name,ugv_type);
    4548    TargetEthController *controller=new TargetEthController("Dualshock3",ds3port);
    4649    CircleFollower* demo=new CircleFollower(name,controller);
    … …  
    5962        ValueArg<string> nameArg("n","name","uav name, also used for vrpn",true,"x4","string");
    6063        cmd.add( nameArg );
     64       
     65        ValueArg<string> typeArg("t","type","ugv type: sumo, ugv_simu or ugv_simux (with x the number of the simulated ugv)",true,"ugv_sumo","string");
     66        cmd.add( typeArg );
    6167
    6268        ValueArg<string> xmlArg("x","xml","fichier xml",true,"./reglages.xml","string");
    … …  
    8490        name=nameArg.getValue();
    8591        address=addressArg.getValue();
     92        ugv_type=typeArg.getValue();
    8693
    8794        } catch (ArgException &e) { // catch any exceptions

  • trunk/lib/FlairMeta/src/Uav.cpp

    r215 r377  
    2121#include <GridLayout.h>
    2222#include <DataPlot1D.h>
    23 #include <VrpnClient.h>
    2423#include <Ahrs.h>
    2524#include <Imu.h>
    … …  
    3130#include <Matrix.h>
    3231#include "MetaUsRangeFinder.h"
    33 #include "MetaVrpnObject.h"
     32
    3433
    3534using std::string;

  • trunk/lib/FlairMeta/src/Uav.h

    r215 r377  
    1111 */
    1212
    13 #ifndef HDSUAV_H
    14 #define HDSUAV_H
     13#ifndef UAV_H
     14#define UAV_H
    1515
    1616#include <Object.h>
    17 #include <UsRangeFinder.h>
    1817
    1918namespace flair {
    … …  
    103102} // end namespace meta
    104103} // end namespace flair
    105 #endif // HDSUAV_H
     104#endif // UAV_H

  • trunk/lib/FlairSensorActuator/src/SimulatedUgvControls.cpp

    r376 r377  
    3131namespace actuator {
    3232
    33 SimulatedUgvControls::SimulatedUgvControls(const IODevice *parent,string name, uint32_t modelId,uint32_t deviceId)
    34     : UgvControls(parent,name) {
     33SimulatedUgvControls::SimulatedUgvControls(string name, uint32_t modelId,uint32_t deviceId)
     34    : UgvControls(name) {
    3535 
    36   shmem = new SharedMem((Thread *)this, ShMemName(modelId, deviceId),2 * sizeof(float)+sizeof(Time));
     36  shmem = new SharedMem(this, ShMemName(modelId, deviceId),2 * sizeof(float)+sizeof(Time));
    3737 
    3838  buf=(char*)malloc(2 * sizeof(float)+sizeof(Time));
    … …  
    5757void SimulatedUgvControls::SetControls(float speed,float turn) {
    5858  float *values=(float*)buf;
    59   buf[0]=speed;
    60   buf[1]=turn;
     59  values[0]=speed;
     60  values[1]=turn;
    6161  Time time=GetTime();
    6262  memcpy(buf+2 * sizeof(float),&time,sizeof(Time));
    … …  
    6767  output->GetMutex();
    6868  for (int i = 0; i < 2; i++) {
    69     output->SetValueNoMutex(i, 0, buf[i]);
     69    output->SetValueNoMutex(i, 0, values[i]);
    7070  }
    7171  output->ReleaseMutex();

  • trunk/lib/FlairSensorActuator/src/SimulatedUgvControls.h

    r376 r377  
    3636  * Construct a SimulatedUgvControls.
    3737  *
    38   * \param parent parent
    3938  * \param name name
    4039  * \param modelId Model id
    4140  * \param deviceId Imu id of the Model
    4241  */
    43   SimulatedUgvControls(const core::IODevice *parent,std::string name,
     42  SimulatedUgvControls(std::string name,
    4443          uint32_t modelId,uint32_t deviceId);
    4544
    … …  
    5049  ~SimulatedUgvControls();
    5150
    52 private:
    53  
    5451  /*!
    5552  * \brief Set controls values
    … …  
    6158  */
    6259  void SetControls(float speed,float turn);
     60private:
     61 
    6362 
    6463  char *buf;

  • trunk/lib/FlairSensorActuator/src/UgvControls.cpp

    r376 r377  
    1818#include "UgvControls.h"
    1919#include <Matrix.h>
     20#include <FrameworkManager.h>
     21#include <Tab.h>
     22#include <DataPlot1D.h>
    2023
    2124using namespace flair::core;
     25using namespace flair::gui;
    2226
    2327using std::string;
    … …  
    2630namespace actuator {
    2731
    28 UgvControls::UgvControls(const IODevice *parent, string name)
    29     : IODevice(parent, name) {
     32UgvControls::UgvControls(string name)
     33    : IODevice(getFrameworkManager(), name) {
     34  mainTab = new Tab(getFrameworkManager()->GetTabWidget(), name);
    3035 
    3136  MatrixDescriptor *desc = new MatrixDescriptor(2, 1);
    … …  
    4247
    4348void UgvControls::UseDefaultPlot(void) {
    44  
     49  DataPlot1D *speedPlot = new DataPlot1D(mainTab->NewRow(), "speed", -1, 1);
     50  speedPlot->AddCurve(output->Element(0));
     51  DataPlot1D *turnPlot = new DataPlot1D(mainTab->LastRowLastCol(), "turn", -1, 1);
     52  turnPlot->AddCurve(output->Element(1));
    4553}
    4654

  • trunk/lib/FlairSensorActuator/src/UgvControls.h

    r376 r377  
    1818namespace flair {
    1919namespace core {
    20 class Matrix;
     20  class Matrix;
     21}
     22namespace gui {
     23  class Tab;
    2124}
    2225}
    … …  
    3538  * \brief Constructor
    3639  *
    37   * Construct a UgvControls.
     40  * Construct a UgvControls. It will be child of the FrameworkManager.
    3841  *
    39   * \param parent parent
    4042  * \param name name
    4143  */
    42   UgvControls(const core::IODevice *parent, std::string name);
     44  UgvControls(std::string name);
    4345
    4446  /*!
    … …  
    6365  core::Matrix *Output(void) const;
    6466 
     67  /*!
     68  * \brief Set controls values
     69  *
     70  * \param speed speed value
     71  * \param turn turn value
     72  */
     73  virtual void SetControls(float speed,float turn)=0;
     74 
    6575protected:
    6676  core::Matrix *output;
    … …  
    7585  */
    7686  void UpdateFrom(const core::io_data *data){};
    77 
    78   /*!
    79   * \brief Set controls values
    80   *
    81   * \param speed speed value
    82   * \param turn turn value
    83   */
    84   virtual void SetControls(float speed,float turn)=0;
    8587 
    86 
     88  gui::Tab *mainTab;
    8789};
    8890} // end namespace actuator

  • trunk/lib/FlairSimulator/src/TwoWheelRobot.cpp

    r376 r377  
    5050  m = new DoubleSpinBox(setup_tab->NewRow(), "mass (kg):", 0, 20, 0.1,1,0.2);
    5151  size = new DoubleSpinBox(setup_tab->NewRow(), "size (m):", 0, 20, 0.1,1,0.1);
     52  t_speed = new DoubleSpinBox(setup_tab->NewRow(), "translational speed (m/s):",
     53                              0, 5, 0.1);
     54  r_speed = new DoubleSpinBox(setup_tab->NewRow(), "rotational speed (deg/s):",
     55                              0, 180, 10);
    5256 
    5357 
    … …  
    5660  bodyColorG = new SpinBox(visual_tab->LastRowLastCol(), "arm color (G):", 0, 255, 1,0);
    5761  bodyColorB = new SpinBox(visual_tab->LastRowLastCol(), "arm color (B):", 0, 255, 1,0);
     62 
     63 
    5864 
    5965  controls = new SimuUgvControls(this, name, modelId,0);
    … …  
    155161
    156162  controls->GetControls(&speed,&turn,&motorTime);
    157           
     163         
    158164  // compute quaternion from W
    159165  // Quaternion derivative: dQ = 0.5*(  Q*Qw)
    160166  state[0].W.x=0;
    161167  state[0].W.y=0;
    162   state[0].W.z=turn;
     168  state[0].W.z=turn*r_speed->Value();
    163169  Quaternion dQ = state[-1].Quat.GetDerivative(state[0].W);
    164170
    … …  
    167173  state[0].Quat.Normalize();
    168174
    169   Vector3D<double> dir = Vector3D<double>(speed,0,0);
     175  Vector3D<double> dir = Vector3D<double>(speed*t_speed->Value(),0,0);
    170176  dir.Rotate(state[0].Quat);
    171177  state[0].Pos = state[-1].Pos + dT() * dir;

  • trunk/lib/FlairSimulator/src/TwoWheelRobot.h

    r375 r377  
    6464#endif
    6565  gui::SpinBox *bodyColorR,*bodyColorG,*bodyColorB;
    66   gui::DoubleSpinBox *size,*m;
     66  gui::DoubleSpinBox *size,*m,*t_speed,*r_speed;
    6767  actuator::SimuUgvControls *controls;
    6868
Note: See TracChangeset for help on using the changeset viewer.