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?
User | Revision | Line number | New 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 |