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

modifs ugv

File:
1 edited

Legend:

Unmodified
Added
Removed
  • 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
Note: See TracChangeset for help on using the changeset viewer.