Marco De Silva / SystemControl

a

systemControl.h@0:f757110a016c, 2021-10-04 (annotated)

Committer:
marcodesilva
Date:
Mon Oct 04 12:06:02 2021 +0000
Revision:
0:f757110a016c
accorpati i due file in forma di libreria

Who changed what in which revision?

UserRevisionLine numberNew contents of line
marcodesilva 0:f757110a016c 1 #ifndef SYSTEM_CONTROL_H
marcodesilva 0:f757110a016c 2 #define SYSTEM_CONTROL_H
marcodesilva 0:f757110a016c 3
marcodesilva 0:f757110a016c 4 #include "mbed.h"
marcodesilva 0:f757110a016c 5 #include "Servo.h"
marcodesilva 0:f757110a016c 6 #include "Rover.h"
marcodesilva 0:f757110a016c 7 #include "ARAP180_with_rover.h"
marcodesilva 0:f757110a016c 8
marcodesilva 0:f757110a016c 9 #include "math.h"
marcodesilva 0:f757110a016c 10 #include "TOFs.h"
marcodesilva 0:f757110a016c 11 #include "Eth_tcp.h"
marcodesilva 0:f757110a016c 12 #include "mbed_mem_trace.h"
marcodesilva 0:f757110a016c 13
marcodesilva 0:f757110a016c 14 #include "controlDefineVariables.h"
marcodesilva 0:f757110a016c 15
marcodesilva 0:f757110a016c 16 #include "FollowFilter.h"
marcodesilva 0:f757110a016c 17
marcodesilva 0:f757110a016c 18 #include "InverseKinematicsControl.h"
marcodesilva 0:f757110a016c 19
marcodesilva 0:f757110a016c 20 #include "SafeCheck.h"
marcodesilva 0:f757110a016c 21
marcodesilva 0:f757110a016c 22 #include "I2C_master.h"
marcodesilva 0:f757110a016c 23
marcodesilva 0:f757110a016c 24 #define PRINT_DEBUG true
marcodesilva 0:f757110a016c 25 #define NAVIG true
marcodesilva 0:f757110a016c 26
marcodesilva 0:f757110a016c 27 #define STAB_ENABLE_FLAG 1
marcodesilva 0:f757110a016c 28 #define NAVIG_ENABLE_FLAG 1
marcodesilva 0:f757110a016c 29 #define ARM_ENABLE_FLAG 1
marcodesilva 0:f757110a016c 30
marcodesilva 0:f757110a016c 31
marcodesilva 0:f757110a016c 32 class SystemControl
marcodesilva 0:f757110a016c 33 {
marcodesilva 0:f757110a016c 34 public:
marcodesilva 0:f757110a016c 35 SystemControl();
marcodesilva 0:f757110a016c 36 void initArm();
marcodesilva 0:f757110a016c 37 void initRover();
marcodesilva 0:f757110a016c 38
marcodesilva 0:f757110a016c 39 void initEthernet();
marcodesilva 0:f757110a016c 40
marcodesilva 0:f757110a016c 41 void startControlThread();
marcodesilva 0:f757110a016c 42 void updateEthCommunication();
marcodesilva 0:f757110a016c 43
marcodesilva 0:f757110a016c 44 void stabilizationFunction();
marcodesilva 0:f757110a016c 45 void navigationFunction();
marcodesilva 0:f757110a016c 46 void armRoverKinematicControlFunction();
marcodesilva 0:f757110a016c 47 //void schedulerThreadFunction();
marcodesilva 0:f757110a016c 48 void printThreadFunction();
marcodesilva 0:f757110a016c 49
marcodesilva 0:f757110a016c 50 float calcDroneDynamics();
marcodesilva 0:f757110a016c 51 void toolControlFunction();
marcodesilva 0:f757110a016c 52
marcodesilva 0:f757110a016c 53
marcodesilva 0:f757110a016c 54 Vector8f calcNullSpaceVelocity();
marcodesilva 0:f757110a016c 55
marcodesilva 0:f757110a016c 56
marcodesilva 0:f757110a016c 57
marcodesilva 0:f757110a016c 58
marcodesilva 0:f757110a016c 59 double actualDtControlCycle;
marcodesilva 0:f757110a016c 60
marcodesilva 0:f757110a016c 61 double t;
marcodesilva 0:f757110a016c 62
marcodesilva 0:f757110a016c 63 bool ethDone;
marcodesilva 0:f757110a016c 64 bool stabDone;
marcodesilva 0:f757110a016c 65 bool navigDone;
marcodesilva 0:f757110a016c 66 bool armDone;
marcodesilva 0:f757110a016c 67 float dtPassedStab;
marcodesilva 0:f757110a016c 68 float dtPassedArm;
marcodesilva 0:f757110a016c 69 float dtPassedNavig;
marcodesilva 0:f757110a016c 70 float dtPassedEth;
marcodesilva 0:f757110a016c 71
marcodesilva 0:f757110a016c 72 float dtBoard;
marcodesilva 0:f757110a016c 73 float t_temp = 0.0;
marcodesilva 0:f757110a016c 74 bool ematClosed = false;
marcodesilva 0:f757110a016c 75 float toolWheelsVel_d = 0.0;
marcodesilva 0:f757110a016c 76
marcodesilva 0:f757110a016c 77
marcodesilva 0:f757110a016c 78
marcodesilva 0:f757110a016c 79 private:
marcodesilva 0:f757110a016c 80
marcodesilva 0:f757110a016c 81 /*ead *stabilizationThread;
marcodesilva 0:f757110a016c 82 Thread *navigationThread;
marcodesilva 0:f757110a016c 83 Thread *armThread;
marcodesilva 0:f757110a016c 84 Thread *schedulerThread;*/
marcodesilva 0:f757110a016c 85 Thread *printThread;
marcodesilva 0:f757110a016c 86
marcodesilva 0:f757110a016c 87 Timer wdTimer;
marcodesilva 0:f757110a016c 88
marcodesilva 0:f757110a016c 89 Eth_tcp *eth_tcp;
marcodesilva 0:f757110a016c 90
marcodesilva 0:f757110a016c 91 Rover *rover;
marcodesilva 0:f757110a016c 92 ARAP180_WITH_ROVER *arm;
marcodesilva 0:f757110a016c 93
marcodesilva 0:f757110a016c 94 I2C_master *i2c_tool;
marcodesilva 0:f757110a016c 95
marcodesilva 0:f757110a016c 96 Servo *batteryServo;
marcodesilva 0:f757110a016c 97
marcodesilva 0:f757110a016c 98
marcodesilva 0:f757110a016c 99 DigitalOut *ledEth;
marcodesilva 0:f757110a016c 100 DigitalOut *ledError;
marcodesilva 0:f757110a016c 101
marcodesilva 0:f757110a016c 102 Status robotStatus;
marcodesilva 0:f757110a016c 103 ControlMode robotControlMode;
marcodesilva 0:f757110a016c 104
marcodesilva 0:f757110a016c 105 Mutex data_mutex;
marcodesilva 0:f757110a016c 106 Mutex arm_mutex;
marcodesilva 0:f757110a016c 107
marcodesilva 0:f757110a016c 108 ConfigMsg robotCnf;
marcodesilva 0:f757110a016c 109 ComandMsg robotCmd;
marcodesilva 0:f757110a016c 110 ResponseMsg robotRsp;
marcodesilva 0:f757110a016c 111
marcodesilva 0:f757110a016c 112 FollowFilter *followFilter;
marcodesilva 0:f757110a016c 113
marcodesilva 0:f757110a016c 114 InverseKinematicsControl *inverseKinematicsControl;
marcodesilva 0:f757110a016c 115
marcodesilva 0:f757110a016c 116 SafeCheck *safeCheck;
marcodesilva 0:f757110a016c 117
marcodesilva 0:f757110a016c 118 float velAccSafetyFactor;
marcodesilva 0:f757110a016c 119 float forceTorqueSafetyFactor;
marcodesilva 0:f757110a016c 120 int precCmd;
marcodesilva 0:f757110a016c 121
marcodesilva 0:f757110a016c 122 int invKin_n_max_iter;
marcodesilva 0:f757110a016c 123 float invKin_max_pError;
marcodesilva 0:f757110a016c 124 float invKin_max_oError;
marcodesilva 0:f757110a016c 125
marcodesilva 0:f757110a016c 126 bool parametersConfigurated;
marcodesilva 0:f757110a016c 127 bool motor_enabled;
marcodesilva 0:f757110a016c 128
marcodesilva 0:f757110a016c 129 float wdTime;
marcodesilva 0:f757110a016c 130 bool debug;
marcodesilva 0:f757110a016c 131
marcodesilva 0:f757110a016c 132 float r_frontWheel;
marcodesilva 0:f757110a016c 133 float r_retroWheel;
marcodesilva 0:f757110a016c 134 float pipeCurve_I;
marcodesilva 0:f757110a016c 135 float pipeCurve_E;
marcodesilva 0:f757110a016c 136 float pipeCurve_M;
marcodesilva 0:f757110a016c 137 float retroFrontCentralDistance;
marcodesilva 0:f757110a016c 138 float wheelsCntactPointDistanceFromPipeCenter;
marcodesilva 0:f757110a016c 139 float pipeRadious;
marcodesilva 0:f757110a016c 140 float Kp_stabilization;
marcodesilva 0:f757110a016c 141 float Kd_stabilization;
marcodesilva 0:f757110a016c 142 float wheelsAcceleration;
marcodesilva 0:f757110a016c 143
marcodesilva 0:f757110a016c 144 float dq_stab_int;
marcodesilva 0:f757110a016c 145 float dq_stab;
marcodesilva 0:f757110a016c 146 float q_stab;
marcodesilva 0:f757110a016c 147
marcodesilva 0:f757110a016c 148 float Kp_zero_torque;
marcodesilva 0:f757110a016c 149 float Ki_zero_torque;
marcodesilva 0:f757110a016c 150
marcodesilva 0:f757110a016c 151 float pitch_m;
marcodesilva 0:f757110a016c 152 float pitch_vel_m;
marcodesilva 0:f757110a016c 153 float roll_m;
marcodesilva 0:f757110a016c 154 float pitch_d;
marcodesilva 0:f757110a016c 155
marcodesilva 0:f757110a016c 156 float vels_d;
marcodesilva 0:f757110a016c 157 float velf_d;
marcodesilva 0:f757110a016c 158 float vela_d;
marcodesilva 0:f757110a016c 159 float velf_m;
marcodesilva 0:f757110a016c 160 float vels_m;
marcodesilva 0:f757110a016c 161 float vela_m;
marcodesilva 0:f757110a016c 162
marcodesilva 0:f757110a016c 163 float roverLongitudinalIntegratedPosition;
marcodesilva 0:f757110a016c 164
marcodesilva 0:f757110a016c 165 float front_vel_dx;
marcodesilva 0:f757110a016c 166 float front_vel_sx;
marcodesilva 0:f757110a016c 167 float retro_vel_dx;
marcodesilva 0:f757110a016c 168 float retro_vel_sx;
marcodesilva 0:f757110a016c 169 float forward_vel;
marcodesilva 0:f757110a016c 170
marcodesilva 0:f757110a016c 171 float jointFront_d;
marcodesilva 0:f757110a016c 172 float jointRetro_d;
marcodesilva 0:f757110a016c 173 float roverLaserFrontDx;
marcodesilva 0:f757110a016c 174 float roverLaserFrontSx;
marcodesilva 0:f757110a016c 175 float roverLaserRetroDx;
marcodesilva 0:f757110a016c 176 float roverLaserRetroSx;
marcodesilva 0:f757110a016c 177 float roverFrontDistance;
marcodesilva 0:f757110a016c 178 float roverRetroDistance;
marcodesilva 0:f757110a016c 179
marcodesilva 0:f757110a016c 180
marcodesilva 0:f757110a016c 181
marcodesilva 0:f757110a016c 182 VectorXf q_cmd;
marcodesilva 0:f757110a016c 183 VectorXf dq_cmd;
marcodesilva 0:f757110a016c 184 VectorXf ddq_cmd;
marcodesilva 0:f757110a016c 185 VectorXf first_q;
marcodesilva 0:f757110a016c 186 Vector8f q_mis;
marcodesilva 0:f757110a016c 187
marcodesilva 0:f757110a016c 188 Matrix4f T_e_b_m;
marcodesilva 0:f757110a016c 189
marcodesilva 0:f757110a016c 190 VectorXf omega;
marcodesilva 0:f757110a016c 191 VectorXf zita;
marcodesilva 0:f757110a016c 192 VectorXf maxJerk;
marcodesilva 0:f757110a016c 193 VectorXf maxAcc;
marcodesilva 0:f757110a016c 194 VectorXf maxVel;
marcodesilva 0:f757110a016c 195 VectorXf maxPos;
marcodesilva 0:f757110a016c 196 VectorXf minPos;
marcodesilva 0:f757110a016c 197
marcodesilva 0:f757110a016c 198 Vector3f maxCartPos;
marcodesilva 0:f757110a016c 199 Vector3f minCartPos;
marcodesilva 0:f757110a016c 200
marcodesilva 0:f757110a016c 201 float battery_pos;
marcodesilva 0:f757110a016c 202 float battery_pos_saturated;
marcodesilva 0:f757110a016c 203 float battery_pos_prec;
marcodesilva 0:f757110a016c 204
marcodesilva 0:f757110a016c 205 Vector6f K_firstOrder;
marcodesilva 0:f757110a016c 206 Vector6f K_secondOrder;
marcodesilva 0:f757110a016c 207
marcodesilva 0:f757110a016c 208 Vector6f D_secondOrder;
marcodesilva 0:f757110a016c 209
marcodesilva 0:f757110a016c 210 Vector8f WeightVector;
marcodesilva 0:f757110a016c 211
marcodesilva 0:f757110a016c 212 Vector6f wrench_armAndRover;
marcodesilva 0:f757110a016c 213 Vector6f wrench_system;
marcodesilva 0:f757110a016c 214
marcodesilva 0:f757110a016c 215
marcodesilva 0:f757110a016c 216 Matrix<float, 24, 1> debugVector;
marcodesilva 0:f757110a016c 217
marcodesilva 0:f757110a016c 218 Vector3f drone_orientation;
marcodesilva 0:f757110a016c 219
marcodesilva 0:f757110a016c 220 void armEnableMotors();
marcodesilva 0:f757110a016c 221 void armDisableMotors();
marcodesilva 0:f757110a016c 222 void setResponse();
marcodesilva 0:f757110a016c 223
marcodesilva 0:f757110a016c 224 void setConfigurationParameters();
marcodesilva 0:f757110a016c 225
marcodesilva 0:f757110a016c 226 int printSchedulerCounter;
marcodesilva 0:f757110a016c 227
marcodesilva 0:f757110a016c 228 float toolClampPos_d = -13.0;
marcodesilva 0:f757110a016c 229
marcodesilva 0:f757110a016c 230
marcodesilva 0:f757110a016c 231 float toolWheelsPos_dx_mis, toolWheelsPos_sx_mis, toolWheelsSpeed_dx_mis, toolWheelsSpeed_sx_mis;
marcodesilva 0:f757110a016c 232
marcodesilva 0:f757110a016c 233
marcodesilva 0:f757110a016c 234 };
marcodesilva 0:f757110a016c 235
marcodesilva 0:f757110a016c 236 #endif