Changeset 379 in flair-src for trunk/demos/TwoWheelRobotCircleFollower/ugv
- Timestamp:
- Dec 17, 2020, 3:30:33 PM (4 years ago)
- Location:
- trunk/demos/TwoWheelRobotCircleFollower/ugv
- Files:
-
- 3 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/demos/TwoWheelRobotCircleFollower/ugv/resources/core2-64/setup_simu.xml
r378 r379 40 40 <GroupBox name="circle"> 41 41 <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"/> 45 45 </GroupBox> 46 46 </Tab> 47 47 <Tab name="Mesures ugv_0"> 48 48 <DataPlot1D name="x" period="100" enabled="1"/> 49 <DataPlot1D name="y" period="100" enabled=" 0"/>49 <DataPlot1D name="y" period="100" enabled="1"/> 50 50 <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"/> 53 53 <DataPlot1D name="vz" period="100" enabled="0"/> 54 54 </Tab> … … 72 72 <GridLayout name="buttons"> 73 73 <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"/> 76 76 </GridLayout> 77 77 </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> 79 79 <PushButton name="save config on target (ugv_0)" value="1"/> 80 80 </root> -
trunk/demos/TwoWheelRobotCircleFollower/ugv/src/CircleFollower.cpp
r377 r379 24 24 #include <Matrix.h> 25 25 #include <Tab.h> 26 #include <TabWidget.h> 27 #include <DoubleSpinBox.h> 26 28 #include <Pid.h> 29 #include <Quaternion.h> 30 #include <Euler.h> 27 31 #include <Ugv.h> 28 32 #include <UgvControls.h> 33 #include <math.h> 29 34 30 35 using namespace std; … … 36 41 using namespace flair::actuator; 37 42 38 CircleFollower::CircleFollower(string name,TargetController *controller): Thread(getFrameworkManager(),"CircleFollower",50), behaviourMode(BehaviourMode_t:: Default), vrpnLost(false) {39 this->controller=controller;43 CircleFollower::CircleFollower(string name,TargetController *controller): Thread(getFrameworkManager(),"CircleFollower",50), behaviourMode(BehaviourMode_t::Manual), vrpnLost(false) { 44 this->controller=controller; 40 45 controller->Start(); 41 46 … … 57 62 vrpnclient->Start(); 58 63 59 64 Tab *ugvTab = new Tab(getFrameworkManager()->GetTabWidget(), "ugv", 0); 60 65 GridLayout* buttonslayout = new GridLayout(ugvTab->NewRow(), "buttons"); 61 66 button_kill = new PushButton(buttonslayout->NewRow(), "kill"); 62 67 startCircle=new PushButton(buttonslayout->NewRow(),"start_circle"); 63 68 stopCircle=new PushButton(buttonslayout->LastRowLastCol(),"stop_circle"); … … 69 74 ugvVrpn->VyPlot()->AddCurve(circle->GetMatrix()->Element(1,1),DataPlot::Blue); 70 75 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"); 72 81 uX=new Pid(setupLawTab->At(1,0),"u_x"); 73 82 uX->UseDefaultPlot(graphLawTab->NewRow()); 74 83 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); 76 87 } 77 88 … … 88 99 89 100 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 95 120 if (button_kill->Clicked() == true) 96 121 SafeStop(); … … 100 125 } 101 126 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; 127 void CircleFollower::ComputeManualControls(void) { 128 float speed=-controller->GetAxisValue(3); 129 float turn=controller->GetAxisValue(0); 130 GetUgv()->GetUgvControls()->SetControls(speed,turn); 131 } 132 133 void 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 175 void CircleFollower::StartCircle(void) { 176 Vector3Df ugv_pos,target_pos; 177 Vector2Df ugv_2Dpos,target_2Dpos; 111 178 112 179 targetVrpn->GetPosition(target_pos); … … 114 181 circle->SetCenter(target_2Dpos); 115 182 116 u avVrpn->GetPosition(uav_pos);117 u av_pos.To2Dxy(uav_2Dpos);118 circle->StartTraj(u av_2Dpos);183 ugvVrpn->GetPosition(ugv_pos); 184 ugv_pos.To2Dxy(ugv_2Dpos); 185 circle->StartTraj(ugv_2Dpos); 119 186 120 187 uX->Reset(); 121 188 uY->Reset(); 122 behaviourMode=BehaviourMode_t::Circle;*/ 189 behaviourMode=BehaviourMode_t::Circle; 190 Thread::Info("CircleFollower: start circle\n"); 123 191 } 124 192 125 193 void CircleFollower::StopCircle(void) { 126 /*194 127 195 circle->FinishTraj(); 128 196 //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 20 20 namespace gui { 21 21 class PushButton; 22 class DoubleSpinBox; 22 23 } 23 24 namespace filter { … … 41 42 42 43 enum class BehaviourMode_t { 43 Default,44 Manual, 44 45 Circle 45 46 }; 46 47 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; 47 59 BehaviourMode_t behaviourMode; 48 60 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; 61 62 }; 62 63
Note:
See TracChangeset
for help on using the changeset viewer.