source: flair-src/trunk/demos/PidStandalone/uav/src/PidUav.cpp @ 44

Last change on this file since 44 was 44, checked in by Bayard Gildas, 5 years ago

Demo Pid et controller automatique scripté

File size: 3.9 KB
Line 
1//  created:    2016/06/01
2//  filename:   PidUav.cpp
3//
4//  author:     Gildas Bayard
5//              Copyright Heudiasyc UMR UTC/CNRS 7253
6//
7//  version:    $Id: $
8//
9//  purpose:    Uav class showing the incidence of pid parameters choice
10//
11//
12/*********************************************************************/
13
14#include "PidUav.h"
15#include <TargetController.h>
16#include <cvmatrix.h>
17#include <GridLayout.h>
18#include <DataPlot1D.h>
19#include <Tab.h>
20#include <TabWidget.h>
21#include <GroupBox.h>
22#include <DoubleSpinBox.h>
23#include <FrameworkManager.h>
24#include <MetaDualShock3.h>
25#include <Pid.h>
26#include <AhrsData.h>
27
28#include <stdio.h>
29
30using namespace std;
31using namespace framework::core;
32using namespace framework::sensor;
33using namespace framework::gui;
34using namespace framework::filter;
35using namespace framework::meta;
36
37/*
38** ===================================================================
39**    constructor and destructor
40**
41** ===================================================================
42*/
43PidUav::PidUav(Uav* uav,TargetController *controller): behaviourMode(BehaviourMode_t::Default),UavStateMachine(uav,controller) {
44    my_uRoll=new Pid(setupLawTab->At(1,1),"custom uRoll");
45    my_uRoll->UseDefaultPlot(graphLawTab->LastRowLastCol());
46    my_uPitch=new Pid(setupLawTab->At(1,0),"custom uPitch");
47    my_uPitch->UseDefaultPlot(graphLawTab->NewRow());
48}
49
50void PidUav::SignalEvent(Event_t event) {
51    UavStateMachine::SignalEvent(event);
52    switch(event) {
53    case Event_t::TakingOff:
54        //always take off in default mode
55        behaviourMode=BehaviourMode_t::Default;
56        break;
57    case Event_t::EnteringFailSafeMode:
58        //return to default mode
59        Thread::Info("Entering failSafe mode\n");
60        behaviourMode=BehaviourMode_t::Default;
61        break;
62    }
63}
64
65bool PidUav::StartCustomMode() {
66    //ask UavStateMachine to enter in custom torques
67    if (SetTorqueMode(TorqueMode_t::Custom)) {
68        Thread::Info("CustomTorques: start\n");
69        my_uPitch->Reset();
70        my_uRoll->Reset();
71        return true;
72    } else {
73        Thread::Warn("CustomTorques: could not start\n");
74        return false;
75    }
76}
77
78void PidUav::StartOscillatingMode() {
79    if (behaviourMode==BehaviourMode_t::Default) {
80        if (!StartCustomMode()) return;
81    }
82    behaviourMode=BehaviourMode_t::Oscillating;
83    Thread::Info("Entering oscillating mode\n");
84    //remove the 'D' effect with a strong 'P'
85
86}
87
88void PidUav::ExtraCheckJoystick(void) {
89    static bool wasOscillatingModeButtonPressed=false;
90    // controller button R1 enters optical flow mode
91    if(GetJoystick()->IsButtonPressed(9)) { // R1
92        if (!wasOscillatingModeButtonPressed) {
93            wasOscillatingModeButtonPressed=true;
94            if (behaviourMode!=BehaviourMode_t::Oscillating) StartOscillatingMode();
95        }
96    } else {
97        wasOscillatingModeButtonPressed=false;
98    }
99}
100
101void PidUav::ComputeCustomTorques(Euler &torques) {
102    ComputeDefaultTorques(torques);
103    //overload default torques calculation for pitch and roll
104    const AhrsData *refOrientation=GetDefaultReferenceOrientation();
105    Quaternion refQuaternion;
106    Vector3D refAngularRates;
107    refOrientation->GetQuaternionAndAngularRates(refQuaternion,refAngularRates);
108    Euler refAngles=refQuaternion.ToEuler();
109
110    const AhrsData *currentOrientation=GetDefaultOrientation();
111    Quaternion currentQuaternion;
112    Vector3D currentAngularRates;
113    currentOrientation->GetQuaternionAndAngularRates(currentQuaternion,currentAngularRates);
114    Euler currentAngles=currentQuaternion.ToEuler();
115
116    my_uRoll->SetValues(currentAngles.roll-refAngles.roll,currentAngularRates.x);
117    my_uRoll->Update(GetTime());
118    torques.roll=my_uRoll->Output();
119
120    my_uPitch->SetValues(currentAngles.pitch-refAngles.pitch,currentAngularRates.y);
121    my_uPitch->Update(GetTime());
122    torques.pitch=my_uPitch->Output();
123}
124
125PidUav::~PidUav() {
126}
Note: See TracBrowser for help on using the repository browser.