File content as of revision 0:f757110a016c:

#ifndef SYSTEM_CONTROL_H
#define SYSTEM_CONTROL_H

#include "mbed.h"
#include "Servo.h"
#include "Rover.h"
#include "ARAP180_with_rover.h"

#include "math.h"
#include "TOFs.h"
#include "Eth_tcp.h"
#include "mbed_mem_trace.h"

#include "controlDefineVariables.h"

#include "FollowFilter.h"

#include "InverseKinematicsControl.h"

#include "SafeCheck.h"

#include "I2C_master.h"

#define PRINT_DEBUG true
#define NAVIG true 

#define STAB_ENABLE_FLAG 1
#define NAVIG_ENABLE_FLAG 1
#define ARM_ENABLE_FLAG 1


class SystemControl
{
public:
    SystemControl();
    void initArm();  
    void initRover();  

    void initEthernet();  

    void startControlThread();  
    void updateEthCommunication();
    
    void stabilizationFunction();
    void navigationFunction();
    void armRoverKinematicControlFunction();
    //void schedulerThreadFunction();
    void printThreadFunction();  
    
    float calcDroneDynamics();
    void toolControlFunction();

    
    Vector8f calcNullSpaceVelocity();


    
   
    double actualDtControlCycle;
    
    double t;

    bool ethDone;
    bool stabDone;
    bool navigDone;
    bool armDone;
    float dtPassedStab;
    float dtPassedArm;
    float dtPassedNavig;
    float dtPassedEth;
    
    float dtBoard;
    float t_temp = 0.0;
    bool ematClosed = false;
    float toolWheelsVel_d = 0.0;


   
private:

    /*ead *stabilizationThread;
    Thread *navigationThread;
    Thread *armThread;
    Thread *schedulerThread;*/
    Thread *printThread;
    
    Timer wdTimer;
    
    Eth_tcp *eth_tcp;

    Rover *rover;
    ARAP180_WITH_ROVER *arm;
    
    I2C_master *i2c_tool;
    
    Servo *batteryServo;

    
    DigitalOut *ledEth;
    DigitalOut *ledError;
    
    Status robotStatus;
    ControlMode robotControlMode;

    Mutex data_mutex;
    Mutex arm_mutex;

    ConfigMsg robotCnf;
    ComandMsg robotCmd;
    ResponseMsg robotRsp; 
        
    FollowFilter *followFilter;
        
    InverseKinematicsControl *inverseKinematicsControl;
    
    SafeCheck *safeCheck;
    
    float velAccSafetyFactor;
    float forceTorqueSafetyFactor;
    int precCmd;
    
    int invKin_n_max_iter;
    float invKin_max_pError; 
    float invKin_max_oError;
       
    bool parametersConfigurated;
    bool motor_enabled;
    
    float wdTime;
    bool debug;
    
    float r_frontWheel;
    float r_retroWheel;
    float pipeCurve_I;
    float pipeCurve_E;
    float pipeCurve_M;
    float retroFrontCentralDistance;
    float wheelsCntactPointDistanceFromPipeCenter;
    float pipeRadious;
    float Kp_stabilization;
    float Kd_stabilization;
    float wheelsAcceleration;
    
    float dq_stab_int;
    float dq_stab;
    float q_stab;

    float Kp_zero_torque;
    float Ki_zero_torque;
    
    float pitch_m;
    float pitch_vel_m;
    float roll_m;
    float pitch_d;
    
    float vels_d;   
    float velf_d;
    float vela_d; 
    float velf_m;
    float vels_m;    
    float vela_m;
    
    float roverLongitudinalIntegratedPosition;
    
    float front_vel_dx;
    float front_vel_sx;    
    float retro_vel_dx;
    float retro_vel_sx;
    float forward_vel; 
    
    float jointFront_d;
    float jointRetro_d; 
    float roverLaserFrontDx; 
    float roverLaserFrontSx; 
    float roverLaserRetroDx; 
    float roverLaserRetroSx;  
    float roverFrontDistance;
    float roverRetroDistance;
    
    

    VectorXf q_cmd;  
    VectorXf dq_cmd;
    VectorXf ddq_cmd; 
    VectorXf first_q;  
    Vector8f q_mis;
    
    Matrix4f T_e_b_m;
    
    VectorXf omega;
    VectorXf zita;
    VectorXf maxJerk;
    VectorXf maxAcc;
    VectorXf maxVel;
    VectorXf maxPos;
    VectorXf minPos;
    
    Vector3f maxCartPos;
    Vector3f minCartPos;
    
    float battery_pos;
    float battery_pos_saturated;
    float battery_pos_prec;  
    
    Vector6f K_firstOrder;
    Vector6f K_secondOrder;

    Vector6f D_secondOrder;
    
    Vector8f WeightVector;
    
    Vector6f wrench_armAndRover;
    Vector6f wrench_system;

    
    Matrix<float, 24, 1>  debugVector;

    Vector3f drone_orientation;

    void armEnableMotors();
    void armDisableMotors();
    void setResponse();
    
    void setConfigurationParameters();
    
    int printSchedulerCounter;
    
    float toolClampPos_d = -13.0;
    

    float toolWheelsPos_dx_mis, toolWheelsPos_sx_mis, toolWheelsSpeed_dx_mis, toolWheelsSpeed_sx_mis;    


};

#endif