Changeset 377 in flair-src
- Timestamp:
- Dec 16, 2020, 9:40:44 AM (4 years ago)
- Location:
- trunk
- Files:
-
- 6 added
- 12 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/demos/TwoWheelRobotCircleFollower/ugv/resources/core2-64/ugv.sh
r376 r377 4 4 EXEC=./TwoWheelRobotCircleFollower_ugv_rt 5 5 else 6 EXEC=. TwoWheelRobotCircleFollower_ugv_nrt6 EXEC=./TwoWheelRobotCircleFollower_ugv_nrt 7 7 fi 8 8 -
trunk/demos/TwoWheelRobotCircleFollower/ugv/src/CircleFollower.cpp
r376 r377 25 25 #include <Tab.h> 26 26 #include <Pid.h> 27 #include <Ugv.h> 28 #include <UgvControls.h> 27 29 28 30 using namespace std; … … 32 34 using namespace flair::filter; 33 35 using namespace flair::meta; 36 using namespace flair::actuator; 34 37 35 38 CircleFollower::CircleFollower(string name,TargetController *controller): Thread(getFrameworkManager(),"CircleFollower",50), behaviourMode(BehaviourMode_t::Default), vrpnLost(false) { 36 39 this->controller=controller; 37 40 controller->Start(); 38 39 VrpnClient* vrpnclient=new VrpnClient("vrpn", "127.0.0.1:3883",80); 41 42 Ugv* ugv=GetUgv(); 43 ugv->UseDefaultPlot(); 44 45 VrpnClient* vrpnclient=new VrpnClient("vrpn", ugv->GetDefaultVrpnAddress(),80); 40 46 41 47 if(vrpnclient->ConnectionType()==VrpnClient::Xbee) { 42 u avVrpn = new MetaVrpnObject(name,(uint8_t)0);48 ugvVrpn = new MetaVrpnObject(name,(uint8_t)0); 43 49 targetVrpn=new MetaVrpnObject("target",1); 44 50 } else if (vrpnclient->ConnectionType()==VrpnClient::Vrpn) { 45 u avVrpn = new MetaVrpnObject(name);51 ugvVrpn = new MetaVrpnObject(name); 46 52 targetVrpn=new MetaVrpnObject("target"); 47 53 } 48 54 49 getFrameworkManager()->AddDeviceToLog(u avVrpn);55 getFrameworkManager()->AddDeviceToLog(ugvVrpn); 50 56 getFrameworkManager()->AddDeviceToLog(targetVrpn); 51 57 vrpnclient->Start(); … … 58 64 59 65 circle=new TrajectoryGenerator2DCircle(vrpnclient->GetLayout()->NewRow(),"circle"); 60 u avVrpn->xPlot()->AddCurve(circle->GetMatrix()->Element(0,0),DataPlot::Blue);61 u avVrpn->yPlot()->AddCurve(circle->GetMatrix()->Element(0,1),DataPlot::Blue);62 u avVrpn->VxPlot()->AddCurve(circle->GetMatrix()->Element(1,0),DataPlot::Blue);63 u avVrpn->VyPlot()->AddCurve(circle->GetMatrix()->Element(1,1),DataPlot::Blue);64 u avVrpn->XyPlot()->AddCurve(circle->GetMatrix()->Element(0,1),circle->GetMatrix()->Element(0,0),DataPlot::Blue,"circle");66 ugvVrpn->xPlot()->AddCurve(circle->GetMatrix()->Element(0,0),DataPlot::Blue); 67 ugvVrpn->yPlot()->AddCurve(circle->GetMatrix()->Element(0,1),DataPlot::Blue); 68 ugvVrpn->VxPlot()->AddCurve(circle->GetMatrix()->Element(1,0),DataPlot::Blue); 69 ugvVrpn->VyPlot()->AddCurve(circle->GetMatrix()->Element(1,1),DataPlot::Blue); 70 ugvVrpn->XyPlot()->AddCurve(circle->GetMatrix()->Element(0,1),circle->GetMatrix()->Element(0,0),DataPlot::Blue,"circle"); 65 71 /* 66 72 uX=new Pid(setupLawTab->At(1,0),"u_x"); … … 81 87 } 82 88 83 while (!ToBeStopped()) { /*84 float x=-controller->GetAxisValue(3)*forwardCoef->Value();85 float rot=controller->GetAxisValue(0)*turnCoef->Value();86 87 sumoControl->move(x, rot);88 89 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); 89 95 if (button_kill->Clicked() == true) 90 96 SafeStop(); 91 */97 92 98 WaitPeriod(); 93 99 } -
trunk/demos/TwoWheelRobotCircleFollower/ugv/src/CircleFollower.h
r376 r377 23 23 namespace filter { 24 24 class TrajectoryGenerator2DCircle; 25 25 class Pid; 26 26 } 27 27 namespace meta { … … 56 56 57 57 flair::gui::PushButton *startCircle,*stopCircle,*button_kill; 58 flair::meta::MetaVrpnObject *targetVrpn,*u avVrpn;58 flair::meta::MetaVrpnObject *targetVrpn,*ugvVrpn; 59 59 flair::filter::TrajectoryGenerator2DCircle *circle; 60 60 61 }; 61 62 -
trunk/demos/TwoWheelRobotCircleFollower/ugv/src/main.cpp
r376 r377 14 14 #include "CircleFollower.h" 15 15 #include <FrameworkManager.h> 16 #include <UgvFactory.h> 16 17 #include <stdio.h> 17 18 #include <tclap/CmdLine.h> … … 24 25 using namespace flair::sensor; 25 26 27 string ugv_type; 26 28 string log_path; 27 29 int port; … … 43 45 manager->SetupLogger(log_path); 44 46 47 Ugv* ugv=CreateUgv(name,ugv_type); 45 48 TargetEthController *controller=new TargetEthController("Dualshock3",ds3port); 46 49 CircleFollower* demo=new CircleFollower(name,controller); … … 59 62 ValueArg<string> nameArg("n","name","uav name, also used for vrpn",true,"x4","string"); 60 63 cmd.add( nameArg ); 64 65 ValueArg<string> typeArg("t","type","ugv type: sumo, ugv_simu or ugv_simux (with x the number of the simulated ugv)",true,"ugv_sumo","string"); 66 cmd.add( typeArg ); 61 67 62 68 ValueArg<string> xmlArg("x","xml","fichier xml",true,"./reglages.xml","string"); … … 84 90 name=nameArg.getValue(); 85 91 address=addressArg.getValue(); 92 ugv_type=typeArg.getValue(); 86 93 87 94 } catch (ArgException &e) { // catch any exceptions -
trunk/lib/FlairMeta/src/Uav.cpp
r215 r377 21 21 #include <GridLayout.h> 22 22 #include <DataPlot1D.h> 23 #include <VrpnClient.h>24 23 #include <Ahrs.h> 25 24 #include <Imu.h> … … 31 30 #include <Matrix.h> 32 31 #include "MetaUsRangeFinder.h" 33 #include "MetaVrpnObject.h" 32 34 33 35 34 using std::string; -
trunk/lib/FlairMeta/src/Uav.h
r215 r377 11 11 */ 12 12 13 #ifndef HDSUAV_H14 #define HDSUAV_H13 #ifndef UAV_H 14 #define UAV_H 15 15 16 16 #include <Object.h> 17 #include <UsRangeFinder.h>18 17 19 18 namespace flair { … … 103 102 } // end namespace meta 104 103 } // end namespace flair 105 #endif // HDSUAV_H104 #endif // UAV_H -
trunk/lib/FlairSensorActuator/src/SimulatedUgvControls.cpp
r376 r377 31 31 namespace actuator { 32 32 33 SimulatedUgvControls::SimulatedUgvControls( const IODevice *parent,string name, uint32_t modelId,uint32_t deviceId)34 : UgvControls( parent,name) {33 SimulatedUgvControls::SimulatedUgvControls(string name, uint32_t modelId,uint32_t deviceId) 34 : UgvControls(name) { 35 35 36 shmem = new SharedMem( (Thread *)this, ShMemName(modelId, deviceId),2 * sizeof(float)+sizeof(Time));36 shmem = new SharedMem(this, ShMemName(modelId, deviceId),2 * sizeof(float)+sizeof(Time)); 37 37 38 38 buf=(char*)malloc(2 * sizeof(float)+sizeof(Time)); … … 57 57 void SimulatedUgvControls::SetControls(float speed,float turn) { 58 58 float *values=(float*)buf; 59 buf[0]=speed;60 buf[1]=turn;59 values[0]=speed; 60 values[1]=turn; 61 61 Time time=GetTime(); 62 62 memcpy(buf+2 * sizeof(float),&time,sizeof(Time)); … … 67 67 output->GetMutex(); 68 68 for (int i = 0; i < 2; i++) { 69 output->SetValueNoMutex(i, 0, buf[i]);69 output->SetValueNoMutex(i, 0, values[i]); 70 70 } 71 71 output->ReleaseMutex(); -
trunk/lib/FlairSensorActuator/src/SimulatedUgvControls.h
r376 r377 36 36 * Construct a SimulatedUgvControls. 37 37 * 38 * \param parent parent39 38 * \param name name 40 39 * \param modelId Model id 41 40 * \param deviceId Imu id of the Model 42 41 */ 43 SimulatedUgvControls( const core::IODevice *parent,std::string name,42 SimulatedUgvControls(std::string name, 44 43 uint32_t modelId,uint32_t deviceId); 45 44 … … 50 49 ~SimulatedUgvControls(); 51 50 52 private:53 54 51 /*! 55 52 * \brief Set controls values … … 61 58 */ 62 59 void SetControls(float speed,float turn); 60 private: 61 63 62 64 63 char *buf; -
trunk/lib/FlairSensorActuator/src/UgvControls.cpp
r376 r377 18 18 #include "UgvControls.h" 19 19 #include <Matrix.h> 20 #include <FrameworkManager.h> 21 #include <Tab.h> 22 #include <DataPlot1D.h> 20 23 21 24 using namespace flair::core; 25 using namespace flair::gui; 22 26 23 27 using std::string; … … 26 30 namespace actuator { 27 31 28 UgvControls::UgvControls(const IODevice *parent, string name) 29 : IODevice(parent, name) { 32 UgvControls::UgvControls(string name) 33 : IODevice(getFrameworkManager(), name) { 34 mainTab = new Tab(getFrameworkManager()->GetTabWidget(), name); 30 35 31 36 MatrixDescriptor *desc = new MatrixDescriptor(2, 1); … … 42 47 43 48 void UgvControls::UseDefaultPlot(void) { 44 49 DataPlot1D *speedPlot = new DataPlot1D(mainTab->NewRow(), "speed", -1, 1); 50 speedPlot->AddCurve(output->Element(0)); 51 DataPlot1D *turnPlot = new DataPlot1D(mainTab->LastRowLastCol(), "turn", -1, 1); 52 turnPlot->AddCurve(output->Element(1)); 45 53 } 46 54 -
trunk/lib/FlairSensorActuator/src/UgvControls.h
r376 r377 18 18 namespace flair { 19 19 namespace core { 20 class Matrix; 20 class Matrix; 21 } 22 namespace gui { 23 class Tab; 21 24 } 22 25 } … … 35 38 * \brief Constructor 36 39 * 37 * Construct a UgvControls. 40 * Construct a UgvControls. It will be child of the FrameworkManager. 38 41 * 39 * \param parent parent40 42 * \param name name 41 43 */ 42 UgvControls( const core::IODevice *parent,std::string name);44 UgvControls(std::string name); 43 45 44 46 /*! … … 63 65 core::Matrix *Output(void) const; 64 66 67 /*! 68 * \brief Set controls values 69 * 70 * \param speed speed value 71 * \param turn turn value 72 */ 73 virtual void SetControls(float speed,float turn)=0; 74 65 75 protected: 66 76 core::Matrix *output; … … 75 85 */ 76 86 void UpdateFrom(const core::io_data *data){}; 77 78 /*!79 * \brief Set controls values80 *81 * \param speed speed value82 * \param turn turn value83 */84 virtual void SetControls(float speed,float turn)=0;85 87 86 88 gui::Tab *mainTab; 87 89 }; 88 90 } // end namespace actuator -
trunk/lib/FlairSimulator/src/TwoWheelRobot.cpp
r376 r377 50 50 m = new DoubleSpinBox(setup_tab->NewRow(), "mass (kg):", 0, 20, 0.1,1,0.2); 51 51 size = new DoubleSpinBox(setup_tab->NewRow(), "size (m):", 0, 20, 0.1,1,0.1); 52 t_speed = new DoubleSpinBox(setup_tab->NewRow(), "translational speed (m/s):", 53 0, 5, 0.1); 54 r_speed = new DoubleSpinBox(setup_tab->NewRow(), "rotational speed (deg/s):", 55 0, 180, 10); 52 56 53 57 … … 56 60 bodyColorG = new SpinBox(visual_tab->LastRowLastCol(), "arm color (G):", 0, 255, 1,0); 57 61 bodyColorB = new SpinBox(visual_tab->LastRowLastCol(), "arm color (B):", 0, 255, 1,0); 62 63 58 64 59 65 controls = new SimuUgvControls(this, name, modelId,0); … … 155 161 156 162 controls->GetControls(&speed,&turn,&motorTime); 157 163 158 164 // compute quaternion from W 159 165 // Quaternion derivative: dQ = 0.5*( Q*Qw) 160 166 state[0].W.x=0; 161 167 state[0].W.y=0; 162 state[0].W.z=turn ;168 state[0].W.z=turn*r_speed->Value(); 163 169 Quaternion dQ = state[-1].Quat.GetDerivative(state[0].W); 164 170 … … 167 173 state[0].Quat.Normalize(); 168 174 169 Vector3D<double> dir = Vector3D<double>(speed ,0,0);175 Vector3D<double> dir = Vector3D<double>(speed*t_speed->Value(),0,0); 170 176 dir.Rotate(state[0].Quat); 171 177 state[0].Pos = state[-1].Pos + dT() * dir; -
trunk/lib/FlairSimulator/src/TwoWheelRobot.h
r375 r377 64 64 #endif 65 65 gui::SpinBox *bodyColorR,*bodyColorG,*bodyColorB; 66 gui::DoubleSpinBox *size,*m ;66 gui::DoubleSpinBox *size,*m,*t_speed,*r_speed; 67 67 actuator::SimuUgvControls *controls; 68 68
Note:
See TracChangeset
for help on using the changeset viewer.