Changeset 389 in flair-src for trunk/demos/TwoWheelRobotCircleFollower/ugv
- Timestamp:
- Dec 21, 2020, 2:50:44 PM (4 years ago)
- Location:
- trunk/demos/TwoWheelRobotCircleFollower/ugv/src
- Files:
-
- 2 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/demos/TwoWheelRobotCircleFollower/ugv/src/CircleFollower.cpp
r379 r389 49 49 50 50 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"); 59 53 60 54 getFrameworkManager()->AddDeviceToLog(ugvVrpn); … … 92 86 void CircleFollower::Run(void) { 93 87 WarnUponSwitches(true); 94 SetPeriodMS( 100);88 SetPeriodMS(20); 95 89 96 90 if (getFrameworkManager()->ErrorOccured() == true) { … … 99 93 100 94 while (!ToBeStopped()) { 95 SecurityCheck(); 96 CheckJoystick(); 97 CheckPushButton(); 98 101 99 if(behaviourMode==BehaviourMode_t::Manual) ComputeManualControls(); 102 100 if(behaviourMode==BehaviourMode_t::Circle) ComputeCircleControls(); 101 WaitPeriod(); 102 } 103 } 104 105 void CircleFollower::CheckPushButton(void) { 106 if (startCircle->Clicked() == true) 107 StartCircle(); 103 108 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 116 void 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 128 void 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; 117 133 StopCircle(); 118 134 } 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 } 124 140 } 125 141 } … … 174 190 175 191 void CircleFollower::StartCircle(void) { 192 if(behaviourMode!=BehaviourMode_t::Circle) { 176 193 Vector3Df ugv_pos,target_pos; 177 194 Vector2Df ugv_2Dpos,target_2Dpos; … … 189 206 behaviourMode=BehaviourMode_t::Circle; 190 207 Thread::Info("CircleFollower: start circle\n"); 208 } 191 209 } 192 210 193 211 void CircleFollower::StopCircle(void) { 194 212 if(behaviourMode==BehaviourMode_t::Circle) { 195 213 circle->FinishTraj(); 196 214 //GetJoystick()->Rumble(0x70); 197 215 behaviourMode=BehaviourMode_t::Manual; 198 216 Thread::Info("CircleFollower: finishing circle\n"); 199 } 200 217 } 218 } 219 -
trunk/demos/TwoWheelRobotCircleFollower/ugv/src/CircleFollower.h
r379 r389 51 51 void ComputeManualControls(void); 52 52 void ComputeCircleControls(void); 53 void SecurityCheck(void); 54 void CheckJoystick(void); 55 void CheckPushButton(void); 53 56 54 57 flair::filter::Pid *uX, *uY;
Note:
See TracChangeset
for help on using the changeset viewer.