Controller.h

00001 /*
00002  * Controller.cpp
00003  * Copyright (c) 2018, ZHAW
00004  * All rights reserved.
00005  *
00006  *  Created on: 27.03.2018
00007  *      Author: BaBoRo Development Team
00008  */
00009 
00010 #ifndef CONTROLLER_H_
00011 #define CONTROLLER_H_
00012 
00013 #include <stdint.h>
00014 #include "mbed.h"
00015 #include "EncoderCounter.h"
00016 #include "IMU.h"
00017 #include "LowpassFilter.h"
00018 #include "Motion.h"
00019 #include "Signal.h"
00020 #include "SerialCom.h"
00021 
00022 /**
00023  * controller class
00024  */
00025 class Controller {
00026     
00027     public:
00028         
00029         //static const float      TRANSLATIONAL_PROFILE_VELOCITY;
00030         //static const float      ROTATIONAL_PROFILE_VELOCITY;
00031         static const float        ALPHA;
00032         static const float        RB;
00033         static const float        RW;
00034         static const float        PI;
00035         static const float        SQRT_3;
00036         static const float        LOWPASS_FILTER_FREQUENCY;
00037         static const float        COUNTS_PER_TURN;
00038         static const float        KI;
00039         static const float        MIN_DUTY_CYCLE;
00040         static const float        MAX_DUTY_CYCLE;
00041         static const float        MAX_ACC_M;
00042         static const float        COS_ALPHA;
00043         static const float        SIN_ALPHA;
00044         static const float        MB;
00045         
00046                     Controller(PwmOut& pwm0, PwmOut& pwm1, PwmOut& pwm2, EncoderCounter& counter1, EncoderCounter& counter2, EncoderCounter& counter3, IMU& imu);
00047         virtual     ~Controller();
00048         void        setGammaX(float gammaX);
00049         void        setGammaY(float gammaY);
00050         void        setGammaZ(float gammaZ);
00051         void        setPhiX(float phiX);
00052         void        setPhiY(float phiY);
00053         void        setX(float x);
00054         void        setY(float y);
00055         
00056         float       getPhiX();
00057         float       getPhiY();
00058         float       getX();
00059         float       getY();
00060         float       gainG;
00061         float       gain_dG;
00062         float       offsetX;
00063         float       offsetY;
00064         float       sign(float v);
00065 
00066         
00067     private:
00068         
00069         static const uint32_t   STACK_SIZE = 4096;  // stack size of thread, given in [bytes]
00070         static const float      PERIOD;             // the period of the timer interrupt, given in [s]
00071         
00072         PwmOut&             pwm0;
00073         PwmOut&             pwm1;
00074         PwmOut&             pwm2;
00075         
00076         EncoderCounter&     counter1;
00077         EncoderCounter&     counter2;
00078         EncoderCounter&     counter3;
00079         
00080         float               gammaXref;
00081         float               gammaYref;
00082         float               gammaZref;
00083         float               phiXref;
00084         float               phiYref;
00085         
00086         float               gammaX;
00087         float               gammaY;
00088         float               gammaZ;
00089         float               phiX;
00090         float               phiY;
00091         float               x;
00092         float               y;
00093         
00094         float               d_gammaX;
00095         float               d_gammaY;
00096         float               d_gammaZ;
00097         float               d_phiX;
00098         float               d_phiY;
00099         
00100         LowpassFilter       speedFilter1;
00101         LowpassFilter       speedFilter2;
00102         LowpassFilter       speedFilter3;
00103         
00104         LowpassFilter       d_phiXFilter;
00105         LowpassFilter       d_phiYFilter;
00106         
00107         LowpassFilter       gammaXFilter;
00108         LowpassFilter       gammaYFilter;
00109         LowpassFilter       d_gammaXFilter;
00110         LowpassFilter       d_gammaYFilter;
00111         
00112         LowpassFilter       M1Filter;
00113         LowpassFilter       M2Filter;
00114         LowpassFilter       M3Filter;
00115         
00116         float               previousValueCounter1;
00117         float               previousValueCounter2;
00118         float               previousValueCounter3;
00119         float               actualSpeed1;
00120         float               actualSpeed2;
00121         float               actualSpeed3;
00122         
00123         Motion              M1motion;
00124         Motion              M2motion;
00125         Motion              M3motion;
00126         Motion              d_phiXMotion;
00127         Motion              d_phiYMotion;
00128         
00129         IMU&                imu;
00130         //SerialCom           com;
00131         Signal              signal;
00132         Thread              thread;
00133         Ticker              ticker;
00134         Mutex               mutex;      // mutex to lock critical sections
00135         
00136         void        sendSignal();
00137         void        run();
00138 };
00139 
00140 #endif /* CONTROLLER_H_ */
00141 
00142