a
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