Diff: systemControl.h

Revision:

0:f757110a016c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/systemControl.h	Mon Oct 04 12:06:02 2021 +0000
@@ -0,0 +1,236 @@
+#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
\ No newline at end of file