Changeset 379 in flair-src for trunk/demos/TwoWheelRobotCircleFollower


Ignore:
Timestamp:
Dec 17, 2020, 3:30:33 PM (4 years ago)
Author:
Sanahuja Guillaume
Message:

modifs ugv

Location:
trunk/demos/TwoWheelRobotCircleFollower
Files:
5 edited

Legend:

Unmodified
Added
Removed

  • trunk/demos/TwoWheelRobotCircleFollower/simulator/resources/core2-64/simulator.xml

    r378 r379  
    55      <TabWidget name="tabs">
    66        <Tab name="sampling">
    7           <DoubleSpinBox name="Tech (s):" value="0.010"/>
     7          <DoubleSpinBox name="Tech (s):" value="0.01"/>
    88        </Tab>
    99        <Tab name="optitrack">
    … …  
    1111        </Tab>
    1212        <Tab name="init">
    13           <Vector3DSpinBox name="position" value_x="1" value_y="0" value_z="-1"/>
    14           <SpinBox name="yaw (deg):" value="0"/>
     13          <Vector3DSpinBox name="position" value_x="2" value_y="0" value_z="-1"/>
     14          <SpinBox name="yaw (deg):" value="90"/>
    1515        </Tab>
    1616        <Tab name="model">
    1717          <DoubleSpinBox name="mass (kg):" value="0.2"/>
    1818          <DoubleSpinBox name="size (m):" value="0.2"/>
    19         <DoubleSpinBox name="translational speed (m/s):" value="1"/><DoubleSpinBox name="rotational speed (deg/s):" value="2"/></Tab>
     19        <DoubleSpinBox name="translational speed (m/s):" value="2"/><DoubleSpinBox name="rotational speed (deg/s):" value="4"/></Tab>
    2020        <Tab name="visual">
    2121          <SpinBox name="arm color (R):" value="255"/>
    … …  
    3838        </Tab>
    3939        <Tab name="model">
    40           <DoubleSpinBox name="translational speed (m/s):" value="0"/>
    41           <DoubleSpinBox name="rotational speed (deg/s):" value="0"/>
     40          <DoubleSpinBox name="translational speed (m/s):" value="1.30"/>
     41          <DoubleSpinBox name="rotational speed (deg/s):" value="60.00"/>
    4242        </Tab>
    4343      </TabWidget>

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

    r375 r379  
    9696  gui = new Parser(960, 480, 960, 480, media_path, scene_file);
    9797#endif
    98 
    9998 
    10099  robot = new TwoWheelRobot(name, 0);
    101100
    102 
    103101#ifdef GL
    104   //man = new Man("target",1);
     102  man = new Man("target",1);
    105103#endif
    106104

  • trunk/demos/TwoWheelRobotCircleFollower/ugv/resources/core2-64/setup_simu.xml

    r378 r379  
    4040          <GroupBox name="circle">
    4141            <DoubleSpinBox name="period, 0 for auto" value="0"/>
    42             <DoubleSpinBox name="R" value="0"/>
    43             <DoubleSpinBox name="velocity" value="0"/>
    44             <DoubleSpinBox name="acceleration (absolute)" value="0"/>
     42            <DoubleSpinBox name="R" value="2"/>
     43            <DoubleSpinBox name="velocity" value="1"/>
     44            <DoubleSpinBox name="acceleration (absolute)" value="0.2"/>
    4545          </GroupBox>
    4646        </Tab>
    4747        <Tab name="Mesures ugv_0">
    4848          <DataPlot1D name="x" period="100" enabled="1"/>
    49           <DataPlot1D name="y" period="100" enabled="0"/>
     49          <DataPlot1D name="y" period="100" enabled="1"/>
    5050          <DataPlot1D name="z" period="100" enabled="0"/>
    51           <DataPlot1D name="vx" period="100" enabled="0"/>
    52           <DataPlot1D name="vy" period="100" enabled="0"/>
     51          <DataPlot1D name="vx" period="100" enabled="1"/>
     52          <DataPlot1D name="vy" period="100" enabled="1"/>
    5353          <DataPlot1D name="vz" period="100" enabled="0"/>
    5454        </Tab>
    … …  
    7272      <GridLayout name="buttons">
    7373        <PushButton name="kill"/>
    74         <PushButton name="start_circle"/>
    75         <PushButton name="stop_circle"/>
     74        <PushButton name="start_circle" value="1"/>
     75        <PushButton name="stop_circle" value="1"/>
    7676      </GridLayout>
    7777    </Tab>
    78   </TabWidget>
     78  <Tab name="control laws"><TabWidget name="laws"><Tab name="Setup"><GroupBox name="u_x"><DoubleSpinBox name="period, 0 for auto" value="0"/><DoubleSpinBox name="kp:" value="0.8"/><DoubleSpinBox name="ki:" value="0.1"/><DoubleSpinBox name="sat i:" value="0"/><DoubleSpinBox name="kd:" value="0.3"/><DoubleSpinBox name="sat:" value="1"/></GroupBox><GroupBox name="u_y"><DoubleSpinBox name="period, 0 for auto" value="0"/><DoubleSpinBox name="kp:" value="0.8"/><DoubleSpinBox name="ki:" value="0.1"/><DoubleSpinBox name="sat i:" value="0"/><DoubleSpinBox name="kd:" value="0.3"/><DoubleSpinBox name="sat:" value="1"/></GroupBox><DoubleSpinBox name="L" value="1"/></Tab><Tab name="Graphes"><DataPlot1D name="u_x" period="100" enabled="0"/><DataPlot1D name="u_y" period="100" enabled="0"/></Tab></TabWidget></Tab></TabWidget>
    7979  <PushButton name="save config on target (ugv_0)" value="1"/>
    8080</root>

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

    r377 r379  
    2424#include <Matrix.h>
    2525#include <Tab.h>
     26#include <TabWidget.h>
     27#include <DoubleSpinBox.h>
    2628#include <Pid.h>
     29#include <Quaternion.h>
     30#include <Euler.h>
    2731#include <Ugv.h>
    2832#include <UgvControls.h>
     33#include <math.h>
    2934
    3035using namespace std;
    … …  
    3641using namespace flair::actuator;
    3742
    38 CircleFollower::CircleFollower(string name,TargetController *controller): Thread(getFrameworkManager(),"CircleFollower",50), behaviourMode(BehaviourMode_t::Default), vrpnLost(false) {
    39    this->controller=controller;
     43CircleFollower::CircleFollower(string name,TargetController *controller): Thread(getFrameworkManager(),"CircleFollower",50), behaviourMode(BehaviourMode_t::Manual), vrpnLost(false) {
     44    this->controller=controller;
    4045    controller->Start();
    4146   
    … …  
    5762    vrpnclient->Start();
    5863       
    59         Tab *ugvTab = new Tab(getFrameworkManager()->GetTabWidget(), "ugv", 0);
     64    Tab *ugvTab = new Tab(getFrameworkManager()->GetTabWidget(), "ugv", 0);
    6065    GridLayout* buttonslayout = new GridLayout(ugvTab->NewRow(), "buttons");
    61         button_kill = new PushButton(buttonslayout->NewRow(), "kill");
     66    button_kill = new PushButton(buttonslayout->NewRow(), "kill");
    6267    startCircle=new PushButton(buttonslayout->NewRow(),"start_circle");
    6368    stopCircle=new PushButton(buttonslayout->LastRowLastCol(),"stop_circle");
    … …  
    6974    ugvVrpn->VyPlot()->AddCurve(circle->GetMatrix()->Element(1,1),DataPlot::Blue);
    7075    ugvVrpn->XyPlot()->AddCurve(circle->GetMatrix()->Element(0,1),circle->GetMatrix()->Element(0,0),DataPlot::Blue,"circle");
    71 /*
     76
     77    Tab *lawTab = new Tab(getFrameworkManager()->GetTabWidget(), "control laws");
     78    TabWidget *tabWidget = new TabWidget(lawTab->NewRow(), "laws");
     79    Tab *setupLawTab = new Tab(tabWidget, "Setup");
     80    Tab *graphLawTab = new Tab(tabWidget, "Graphes");
    7281    uX=new Pid(setupLawTab->At(1,0),"u_x");
    7382    uX->UseDefaultPlot(graphLawTab->NewRow());
    7483    uY=new Pid(setupLawTab->At(1,1),"u_y");
    75     uY->UseDefaultPlot(graphLawTab->LastRowLastCol());*/
     84    uY->UseDefaultPlot(graphLawTab->LastRowLastCol());
     85   
     86    l=new DoubleSpinBox(setupLawTab->NewRow(),"L", " m", 0, 10, 0.1, 1,1);
    7687}
    7788
    … …  
    8899
    89100    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);
     101        if(behaviourMode==BehaviourMode_t::Manual) ComputeManualControls();
     102        if(behaviourMode==BehaviourMode_t::Circle) ComputeCircleControls();
     103       
     104        if (startCircle->Clicked() == true)
     105            StartCircle();
     106       
     107        if (stopCircle->Clicked() == true)
     108            StopCircle();
     109       
     110         //R1 and Circle
     111        if(controller->IsButtonPressed(9) && controller->IsButtonPressed(4) && (behaviourMode!=BehaviourMode_t::Circle)) {
     112            StartCircle();
     113        }
     114
     115        //R1 and Cross
     116        if(controller->IsButtonPressed(9) && controller->IsButtonPressed(5) && (behaviourMode==BehaviourMode_t::Circle)) {
     117            StopCircle();
     118        }
     119   
    95120        if (button_kill->Clicked() == true)
    96121            SafeStop();
    … …  
    100125}
    101126
    102 void CircleFollower::StartCircle(void) {/*
    103     if (SetOrientationMode(OrientationMode_t::Custom)) {
    104         Thread::Info("CircleFollower: start circle\n");
    105     } else {
    106         Thread::Warn("CircleFollower: could not start circle\n");
    107         return;
    108     }
    109     Vector3Df uav_pos,target_pos;
    110     Vector2Df uav_2Dpos,target_2Dpos;
     127void CircleFollower::ComputeManualControls(void) {
     128  float speed=-controller->GetAxisValue(3);
     129  float turn=controller->GetAxisValue(0);
     130  GetUgv()->GetUgvControls()->SetControls(speed,turn);
     131}
     132
     133void CircleFollower::ComputeCircleControls(void) {
     134
     135  Vector3Df ugv_pos,ugv_vel,target_pos; // in VRPN coordinate system
     136  Vector2Df ugv_2Dpos,ugv_2Dvel,target_2Dpos; // in VRPN coordinate system
     137  Vector2Df pos_error,vel_error;
     138  Vector2Df circle_pos,circle_vel;
     139   
     140  ugvVrpn->GetPosition(ugv_pos);
     141  ugvVrpn->GetSpeed(ugv_vel);
     142
     143  ugv_pos.To2Dxy(ugv_2Dpos);
     144  ugv_vel.To2Dxy(ugv_2Dvel);
     145 
     146  targetVrpn->GetPosition(target_pos);
     147  target_pos.To2Dxy(target_2Dpos);
     148  circle->SetCenter(target_2Dpos);
     149
     150  //circle reference
     151  circle->Update(GetTime());
     152  circle->GetPosition(circle_pos);
     153  circle->GetSpeed(circle_vel);
     154
     155  //error in optitrack frame
     156  pos_error=ugv_2Dpos-circle_pos;
     157  vel_error=ugv_2Dvel-circle_vel;
     158   
     159  uX->SetValues(pos_error.x, vel_error.x);
     160  uX->Update(GetTime());
     161  uY->SetValues(pos_error.y, vel_error.y);
     162  uY->Update(GetTime());
     163 
     164  //get yaw from vrpn
     165  Quaternion vrpnQuaternion;
     166  ugvVrpn->GetQuaternion(vrpnQuaternion);
     167  float yaw=vrpnQuaternion.ToEuler().yaw;
     168  float L=1;
     169  float v= cosf(yaw)*uX->Output() + sinf(yaw)*uY->Output();
     170  float w = -sinf(yaw)/l->Value()*uX->Output() + cosf(yaw)/l->Value()*uY->Output();
     171  GetUgv()->GetUgvControls()->SetControls(-v,-w);
     172}
     173
     174
     175void CircleFollower::StartCircle(void) {
     176    Vector3Df ugv_pos,target_pos;
     177    Vector2Df ugv_2Dpos,target_2Dpos;
    111178
    112179    targetVrpn->GetPosition(target_pos);
    … …  
    114181    circle->SetCenter(target_2Dpos);
    115182
    116     uavVrpn->GetPosition(uav_pos);
    117     uav_pos.To2Dxy(uav_2Dpos);
    118     circle->StartTraj(uav_2Dpos);
     183    ugvVrpn->GetPosition(ugv_pos);
     184    ugv_pos.To2Dxy(ugv_2Dpos);
     185    circle->StartTraj(ugv_2Dpos);
    119186
    120187    uX->Reset();
    121188    uY->Reset();
    122     behaviourMode=BehaviourMode_t::Circle;*/
     189    behaviourMode=BehaviourMode_t::Circle;
     190    Thread::Info("CircleFollower: start circle\n");
    123191}
    124192
    125193void CircleFollower::StopCircle(void) {
    126         /*
     194       
    127195    circle->FinishTraj();
    128196    //GetJoystick()->Rumble(0x70);
    129     Thread::Info("CircleFollower: finishing circle\n");*/
    130 }
    131 
     197    behaviourMode=BehaviourMode_t::Manual;
     198    Thread::Info("CircleFollower: finishing circle\n");
     199}
     200

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

    r377 r379  
    2020    namespace gui {
    2121        class PushButton;
     22        class DoubleSpinBox;
    2223    }
    2324    namespace filter {
    … …  
    4142
    4243        enum class BehaviourMode_t {
    43             Default,
     44            Manual,
    4445            Circle
    4546        };
    4647
     48        void Run(void);
     49        void StartCircle(void);
     50        void StopCircle(void);
     51        void ComputeManualControls(void);
     52        void ComputeCircleControls(void);
     53
     54        flair::filter::Pid *uX, *uY;
     55        flair::gui::PushButton *startCircle,*stopCircle,*button_kill;
     56        flair::gui::DoubleSpinBox *l;
     57        flair::meta::MetaVrpnObject *targetVrpn,*ugvVrpn;
     58        flair::filter::TrajectoryGenerator2DCircle *circle;
    4759        BehaviourMode_t behaviourMode;
    4860        bool vrpnLost;
    49                 flair::sensor::TargetController *controller;
    50 
    51                 void Run(void);
    52         void StartCircle(void);
    53         void StopCircle(void);
    54 
    55         flair::filter::Pid *uX, *uY;
    56 
    57         flair::gui::PushButton *startCircle,*stopCircle,*button_kill;
    58         flair::meta::MetaVrpnObject *targetVrpn,*ugvVrpn;
    59         flair::filter::TrajectoryGenerator2DCircle *circle;
    60 
     61        flair::sensor::TargetController *controller;
    6162};
    6263
Note: See TracChangeset for help on using the changeset viewer.