Changeset 389 in flair-src


Ignore:
Timestamp:
Dec 21, 2020, 2:50:44 PM (11 months ago)
Author:
Sanahuja Guillaume
Message:

update demo code

Location:
trunk/demos/TwoWheelRobotCircleFollower/ugv/src
Files:
2 edited

Legend:

Unmodified
Added
Removed
  • trunk/demos/TwoWheelRobotCircleFollower/ugv/src/CircleFollower.cpp

    r379 r389  
    4949   
    5050    VrpnClient* vrpnclient=new VrpnClient("vrpn", ugv->GetDefaultVrpnAddress(),80);
    51    
    52     if(vrpnclient->ConnectionType()==VrpnClient::Xbee) {
    53         ugvVrpn = new MetaVrpnObject(name,(uint8_t)0);
    54         targetVrpn=new MetaVrpnObject("target",1);
    55     } else if (vrpnclient->ConnectionType()==VrpnClient::Vrpn) {
    56         ugvVrpn = new MetaVrpnObject(name);
    57         targetVrpn=new MetaVrpnObject("target");
    58     }
     51    ugvVrpn = new MetaVrpnObject(name);
     52    targetVrpn=new MetaVrpnObject("target");
    5953   
    6054    getFrameworkManager()->AddDeviceToLog(ugvVrpn);
     
    9286void CircleFollower::Run(void) {
    9387    WarnUponSwitches(true);
    94     SetPeriodMS(100);
     88    SetPeriodMS(20);
    9589   
    9690    if (getFrameworkManager()->ErrorOccured() == true) {
     
    9993
    10094    while (!ToBeStopped()) {
     95        SecurityCheck();
     96        CheckJoystick();
     97        CheckPushButton();
     98       
    10199        if(behaviourMode==BehaviourMode_t::Manual) ComputeManualControls();
    102100        if(behaviourMode==BehaviourMode_t::Circle) ComputeCircleControls();
     101        WaitPeriod();
     102    }
     103}
     104
     105void CircleFollower::CheckPushButton(void) {
     106  if (startCircle->Clicked() == true)
     107      StartCircle();
    103108       
    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)) {
     109  if (stopCircle->Clicked() == true)
     110      StopCircle();
     111
     112  if (button_kill->Clicked() == true)
     113      SafeStop();
     114}
     115
     116void CircleFollower::CheckJoystick(void) {
     117  //R1 and Circle
     118  if(controller->IsButtonPressed(9) && controller->IsButtonPressed(4)) {
     119      StartCircle();
     120  }
     121
     122  //R1 and Cross
     123  if(controller->IsButtonPressed(9) && controller->IsButtonPressed(5)) {
     124      StopCircle();
     125  }
     126}
     127
     128void CircleFollower::SecurityCheck(void) {
     129    if ((!vrpnLost) && (behaviourMode==BehaviourMode_t::Circle)) {
     130        if (!targetVrpn->IsTracked(500)) {
     131            Thread::Err("VRPN, target lost\n");
     132            vrpnLost=true;
    117133            StopCircle();
    118134        }
    119    
    120         if (button_kill->Clicked() == true)
    121             SafeStop();
    122        
    123         WaitPeriod();
     135        if (!ugvVrpn->IsTracked(500)) {
     136            Thread::Err("VRPN, ugv lost\n");
     137            vrpnLost=true;
     138            StopCircle();
     139        }
    124140    }
    125141}
     
    174190
    175191void CircleFollower::StartCircle(void) {
     192  if(behaviourMode!=BehaviourMode_t::Circle) {
    176193    Vector3Df ugv_pos,target_pos;
    177194    Vector2Df ugv_2Dpos,target_2Dpos;
     
    189206    behaviourMode=BehaviourMode_t::Circle;
    190207    Thread::Info("CircleFollower: start circle\n");
     208  }
    191209}
    192210
    193211void CircleFollower::StopCircle(void) {
    194        
     212        if(behaviourMode==BehaviourMode_t::Circle) {
    195213    circle->FinishTraj();
    196214    //GetJoystick()->Rumble(0x70);
    197215    behaviourMode=BehaviourMode_t::Manual;
    198216    Thread::Info("CircleFollower: finishing circle\n");
    199 }
    200 
     217  }
     218}
     219
  • trunk/demos/TwoWheelRobotCircleFollower/ugv/src/CircleFollower.h

    r379 r389  
    5151        void ComputeManualControls(void);
    5252        void ComputeCircleControls(void);
     53        void SecurityCheck(void);
     54        void CheckJoystick(void);
     55        void CheckPushButton(void);
    5356
    5457        flair::filter::Pid *uX, *uY;
Note: See TracChangeset for help on using the changeset viewer.