a
systemControl.h
- Committer:
- marcodesilva
- Date:
- 2021-10-04
- Revision:
- 0:f757110a016c
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