Marco De Silva / SystemControl

a

systemControl.cpp@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 #include "systemControl.h"
marcodesilva 0:f757110a016c 2
marcodesilva 0:f757110a016c 3 #include <iostream>
marcodesilva 0:f757110a016c 4 #include <iomanip>
marcodesilva 0:f757110a016c 5
marcodesilva 0:f757110a016c 6 SystemControl::SystemControl(){
marcodesilva 0:f757110a016c 7
marcodesilva 0:f757110a016c 8 dtPassedStab = 0.0;
marcodesilva 0:f757110a016c 9 dtPassedArm = 0.0;
marcodesilva 0:f757110a016c 10 dtPassedNavig = 0.0;
marcodesilva 0:f757110a016c 11 dtPassedEth = 0.0;
marcodesilva 0:f757110a016c 12
marcodesilva 0:f757110a016c 13 wdTime = 0.2;
marcodesilva 0:f757110a016c 14
marcodesilva 0:f757110a016c 15 t = 0.0;
marcodesilva 0:f757110a016c 16
marcodesilva 0:f757110a016c 17 printSchedulerCounter = 0;
marcodesilva 0:f757110a016c 18 motor_enabled = false;
marcodesilva 0:f757110a016c 19
marcodesilva 0:f757110a016c 20 actualDtControlCycle = 0.0;
marcodesilva 0:f757110a016c 21
marcodesilva 0:f757110a016c 22 q_cmd.resize(8,1);
marcodesilva 0:f757110a016c 23 dq_cmd.resize(8,1);
marcodesilva 0:f757110a016c 24 ddq_cmd.resize(8,1);
marcodesilva 0:f757110a016c 25
marcodesilva 0:f757110a016c 26 q_cmd = Vector8f::Zero();
marcodesilva 0:f757110a016c 27 dq_cmd = Vector8f::Zero();
marcodesilva 0:f757110a016c 28 ddq_cmd = Vector8f::Zero();
marcodesilva 0:f757110a016c 29 q_mis = Vector8f::Zero();
marcodesilva 0:f757110a016c 30 drone_orientation = Vector3f::Zero();
marcodesilva 0:f757110a016c 31
marcodesilva 0:f757110a016c 32 omega.resize(8,1);
marcodesilva 0:f757110a016c 33 zita.resize(8,1);
marcodesilva 0:f757110a016c 34 maxJerk.resize(8,1);
marcodesilva 0:f757110a016c 35 maxAcc.resize(8,1);
marcodesilva 0:f757110a016c 36 maxVel.resize(8,1);
marcodesilva 0:f757110a016c 37 maxPos.resize(8,1);
marcodesilva 0:f757110a016c 38 minPos.resize(8,1);
marcodesilva 0:f757110a016c 39
marcodesilva 0:f757110a016c 40 dq_stab_int = 0.0;
marcodesilva 0:f757110a016c 41 dq_stab = 0.0;
marcodesilva 0:f757110a016c 42 q_stab = 0.0;
marcodesilva 0:f757110a016c 43
marcodesilva 0:f757110a016c 44
marcodesilva 0:f757110a016c 45
marcodesilva 0:f757110a016c 46 debugVector = Matrix<float, 24, 1> ::Zero();
marcodesilva 0:f757110a016c 47
marcodesilva 0:f757110a016c 48 debug = false;
marcodesilva 0:f757110a016c 49
marcodesilva 0:f757110a016c 50 parametersConfigurated = false;
marcodesilva 0:f757110a016c 51
marcodesilva 0:f757110a016c 52 ethDone = false;
marcodesilva 0:f757110a016c 53
marcodesilva 0:f757110a016c 54 printThread = new Thread(osPriorityNormal, 1 * 1024 /*32K stack size*/);
marcodesilva 0:f757110a016c 55
marcodesilva 0:f757110a016c 56 ledEth = new DigitalOut(PG_3);
marcodesilva 0:f757110a016c 57 ledError = new DigitalOut(PG_2);
marcodesilva 0:f757110a016c 58
marcodesilva 0:f757110a016c 59 }
marcodesilva 0:f757110a016c 60
marcodesilva 0:f757110a016c 61 void SystemControl::initEthernet(){
marcodesilva 0:f757110a016c 62 printf("INIT ETHERNET!! \r\n");
marcodesilva 0:f757110a016c 63 eth_tcp = new Eth_tcp(3); // server timeout 1s
marcodesilva 0:f757110a016c 64 robotStatus = eth_tcp->connect();
marcodesilva 0:f757110a016c 65 wdTimer.start();
marcodesilva 0:f757110a016c 66
marcodesilva 0:f757110a016c 67 }
marcodesilva 0:f757110a016c 68
marcodesilva 0:f757110a016c 69 void SystemControl::initArm() {
marcodesilva 0:f757110a016c 70 printf("Creo il braccio \r\n");
marcodesilva 0:f757110a016c 71 arm = new ARAP180_WITH_ROVER();
marcodesilva 0:f757110a016c 72
marcodesilva 0:f757110a016c 73 i2c_tool = new I2C_master(PC_9, PA_8, 0x54);
marcodesilva 0:f757110a016c 74
marcodesilva 0:f757110a016c 75 K_firstOrder << 25,25,25,5,5,5;
marcodesilva 0:f757110a016c 76 K_secondOrder << 1500,1500,1500,500,500,500;
marcodesilva 0:f757110a016c 77
marcodesilva 0:f757110a016c 78 D_secondOrder << 20,20,20,15,15,15;
marcodesilva 0:f757110a016c 79
marcodesilva 0:f757110a016c 80 invKin_n_max_iter = 1;
marcodesilva 0:f757110a016c 81 invKin_max_pError = 0.001;
marcodesilva 0:f757110a016c 82 invKin_max_oError = 0.001;
marcodesilva 0:f757110a016c 83
marcodesilva 0:f757110a016c 84 omega << 3.0,3.0,3.0,3.0,3.0,3.0,3.0,3.0;
marcodesilva 0:f757110a016c 85 zita << 0.8,0.8,0.8,0.8,0.8,0.8,0.8,0.8;
marcodesilva 0:f757110a016c 86 maxJerk << 3,3,3,3,3,3,3,3;
marcodesilva 0:f757110a016c 87 maxAcc << 20,100,100,100,100,100,100,100;
marcodesilva 0:f757110a016c 88 maxVel << 1,3,3,3,3,3,3,3;
marcodesilva 0:f757110a016c 89
marcodesilva 0:f757110a016c 90 minPos << -1000.0,-M_PI/4, -2*M_PI, -M_PI/2 + M_PI/8, 0.0, -3.1, -M_PI, -M_PI;
marcodesilva 0:f757110a016c 91 maxPos << 1000.0, M_PI/4, 2*M_PI, M_PI - M_PI/6, 2*M_PI/3, 0.85, M_PI, M_PI;
marcodesilva 0:f757110a016c 92
marcodesilva 0:f757110a016c 93 first_q.resize(8,1);
marcodesilva 0:f757110a016c 94 first_q << 0.0,
marcodesilva 0:f757110a016c 95 0.0,
marcodesilva 0:f757110a016c 96 0.0,
marcodesilva 0:f757110a016c 97 -M_PI/2 + M_PI/6,
marcodesilva 0:f757110a016c 98 0.75,
marcodesilva 0:f757110a016c 99 -2.5,
marcodesilva 0:f757110a016c 100 M_PI/2,
marcodesilva 0:f757110a016c 101 0.0;
marcodesilva 0:f757110a016c 102
marcodesilva 0:f757110a016c 103 velAccSafetyFactor = 2;
marcodesilva 0:f757110a016c 104 forceTorqueSafetyFactor = 100;
marcodesilva 0:f757110a016c 105
marcodesilva 0:f757110a016c 106 minCartPos << 0.1,0.1,0.1;
marcodesilva 0:f757110a016c 107 maxCartPos << 0.3,0.3,0.3;
marcodesilva 0:f757110a016c 108
marcodesilva 0:f757110a016c 109 followFilter = new FollowFilter(omega, zita, maxJerk, maxAcc, maxVel, maxPos, minPos);
marcodesilva 0:f757110a016c 110 inverseKinematicsControl = new InverseKinematicsControl(arm, K_firstOrder, K_secondOrder, D_secondOrder, maxAcc, maxVel, maxPos, minPos, invKin_n_max_iter, invKin_max_pError, invKin_max_oError);
marcodesilva 0:f757110a016c 111 safeCheck = new SafeCheck(arm, maxPos, minPos, velAccSafetyFactor*maxVel, velAccSafetyFactor*maxAcc, maxCartPos, minCartPos);
marcodesilva 0:f757110a016c 112 followFilter->initFollowFilter(first_q);
marcodesilva 0:f757110a016c 113 inverseKinematicsControl->initInverseKinematicsControl(first_q);
marcodesilva 0:f757110a016c 114 safeCheck->initSafeCheck();
marcodesilva 0:f757110a016c 115
marcodesilva 0:f757110a016c 116 q_mis = first_q;
marcodesilva 0:f757110a016c 117 q_cmd = q_mis;
marcodesilva 0:f757110a016c 118
marcodesilva 0:f757110a016c 119 T_e_b_m = arm->forwardKinematics(first_q);
marcodesilva 0:f757110a016c 120
marcodesilva 0:f757110a016c 121 batteryServo = new Servo(PA_3);
marcodesilva 0:f757110a016c 122 batteryServo->write(0.0);
marcodesilva 0:f757110a016c 123
marcodesilva 0:f757110a016c 124 }
marcodesilva 0:f757110a016c 125
marcodesilva 0:f757110a016c 126 void SystemControl::initRover() {
marcodesilva 0:f757110a016c 127 printf("Creo il rover \r\n");
marcodesilva 0:f757110a016c 128
marcodesilva 0:f757110a016c 129 rover = new Rover();
marcodesilva 0:f757110a016c 130 pitch_m = 0.0;
marcodesilva 0:f757110a016c 131 roll_m = 0.0;
marcodesilva 0:f757110a016c 132
marcodesilva 0:f757110a016c 133 pitch_d = 0.0;
marcodesilva 0:f757110a016c 134 vels_d = 0.0;
marcodesilva 0:f757110a016c 135 velf_d = 0.0;
marcodesilva 0:f757110a016c 136 vela_d = 0.0;
marcodesilva 0:f757110a016c 137 velf_m = 0.0;
marcodesilva 0:f757110a016c 138 vels_m = 0.0;
marcodesilva 0:f757110a016c 139 vela_m = 0.0;
marcodesilva 0:f757110a016c 140 front_vel_dx = 0.0;
marcodesilva 0:f757110a016c 141 front_vel_sx = 0.0;
marcodesilva 0:f757110a016c 142 retro_vel_dx = 0.0;
marcodesilva 0:f757110a016c 143 retro_vel_sx = 0.0;
marcodesilva 0:f757110a016c 144 forward_vel = 0.0;
marcodesilva 0:f757110a016c 145 jointFront_d = 0.0;
marcodesilva 0:f757110a016c 146 jointRetro_d = 0.0;
marcodesilva 0:f757110a016c 147 roverLaserFrontDx = 0.0;
marcodesilva 0:f757110a016c 148 roverLaserFrontSx = 0.0;
marcodesilva 0:f757110a016c 149 roverLaserRetroDx = 0.0;
marcodesilva 0:f757110a016c 150 roverLaserRetroSx = 0.0;
marcodesilva 0:f757110a016c 151 roverFrontDistance = 0.0;
marcodesilva 0:f757110a016c 152 roverRetroDistance = 0.0;
marcodesilva 0:f757110a016c 153
marcodesilva 0:f757110a016c 154 roverLongitudinalIntegratedPosition = 0.0;
marcodesilva 0:f757110a016c 155
marcodesilva 0:f757110a016c 156 robotCmd.Data.roverForwardVel = 0.0;
marcodesilva 0:f757110a016c 157 robotCmd.Data.roverZVel = 0.0;
marcodesilva 0:f757110a016c 158 robotCmd.Data.roverFrontJointAngle = 0.0;
marcodesilva 0:f757110a016c 159 robotCmd.Data.roverRetroJointAngle = 0.0;
marcodesilva 0:f757110a016c 160
marcodesilva 0:f757110a016c 161 pipeRadious = 0.125;
marcodesilva 0:f757110a016c 162 Kp_stabilization=1.2; //0.6
marcodesilva 0:f757110a016c 163 wheelsAcceleration = 10000;
marcodesilva 0:f757110a016c 164
marcodesilva 0:f757110a016c 165 rover->initializeImu();
marcodesilva 0:f757110a016c 166 //rover->initializeTofs(); //in radiants (front, retro) ///////////////////////////////
marcodesilva 0:f757110a016c 167 rover->setRoverVelocity(0.0,0.0 , 0, pipeRadious, wheelsAcceleration); //velForward, velStabilization, velAsset, pipeRadious, acceleration
marcodesilva 0:f757110a016c 168 rover->setCentralJointsAngle(0.0,0.0); //in radiants (front, retro)
marcodesilva 0:f757110a016c 169
marcodesilva 0:f757110a016c 170 printf("Creato il rover \r\n");
marcodesilva 0:f757110a016c 171
marcodesilva 0:f757110a016c 172
marcodesilva 0:f757110a016c 173
marcodesilva 0:f757110a016c 174 }
marcodesilva 0:f757110a016c 175
marcodesilva 0:f757110a016c 176 void SystemControl::startControlThread(){
marcodesilva 0:f757110a016c 177
marcodesilva 0:f757110a016c 178 printf("\nControl thread started!\r\n");
marcodesilva 0:f757110a016c 179
marcodesilva 0:f757110a016c 180 //stabilizationThread->start(this,&SystemControl::stabilizationThreadFunction);
marcodesilva 0:f757110a016c 181 //navigationThread->start(this,&SystemControl::navigationThreadFunction);
marcodesilva 0:f757110a016c 182 //armThread->start(this,&SystemControl::armThreadFunction);
marcodesilva 0:f757110a016c 183 //printThread->start(this,&SystemControl::printThreadFunction);
marcodesilva 0:f757110a016c 184 //schedulerThread->start(this,&SystemControl::schedulerThreadFunction);
marcodesilva 0:f757110a016c 185
marcodesilva 0:f757110a016c 186
marcodesilva 0:f757110a016c 187 }
marcodesilva 0:f757110a016c 188
marcodesilva 0:f757110a016c 189
marcodesilva 0:f757110a016c 190 void SystemControl::setConfigurationParameters() {
marcodesilva 0:f757110a016c 191
marcodesilva 0:f757110a016c 192 Matrix<double, 7, 1> roverParameters;
marcodesilva 0:f757110a016c 193 Matrix<double, 40, 1> armParameters;
marcodesilva 0:f757110a016c 194
marcodesilva 0:f757110a016c 195 wdTime = robotCnf.Data.wdTime;
marcodesilva 0:f757110a016c 196 debug = robotCnf.Data.debug;
marcodesilva 0:f757110a016c 197
marcodesilva 0:f757110a016c 198 dtBoard = robotCnf.Data.dtBoard;
marcodesilva 0:f757110a016c 199 r_frontWheel = robotCnf.Data.r_frontWheel;
marcodesilva 0:f757110a016c 200 r_retroWheel = robotCnf.Data.r_retroWheel;
marcodesilva 0:f757110a016c 201 pipeCurve_I = robotCnf.Data.pipeCurve_I;
marcodesilva 0:f757110a016c 202 pipeCurve_E = robotCnf.Data.pipeCurve_E;
marcodesilva 0:f757110a016c 203 pipeCurve_M = robotCnf.Data.pipeCurve_M;
marcodesilva 0:f757110a016c 204 retroFrontCentralDistance = robotCnf.Data.retroFrontCentralDistance;
marcodesilva 0:f757110a016c 205 wheelsCntactPointDistanceFromPipeCenter = robotCnf.Data.wheelsCntactPointDistanceFromPipeCenter;
marcodesilva 0:f757110a016c 206 pipeRadious = robotCnf.Data.pipeRadious;
marcodesilva 0:f757110a016c 207 Kp_stabilization = robotCnf.Data.Kp_stabilization;
marcodesilva 0:f757110a016c 208 Kd_stabilization = robotCnf.Data.Kd_stabilization;
marcodesilva 0:f757110a016c 209
marcodesilva 0:f757110a016c 210 Kp_zero_torque = robotCnf.Data.Kp_zero_torque;
marcodesilva 0:f757110a016c 211 Ki_zero_torque = robotCnf.Data.Ki_zero_torque;
marcodesilva 0:f757110a016c 212
marcodesilva 0:f757110a016c 213 std::cout << "Kp_stab: " << Kp_stabilization << " Kd_stab: " << Kd_stabilization << std::endl;
marcodesilva 0:f757110a016c 214 std::cout << "Kp_zeroTorque: " << Kp_zero_torque << " Kd_zeroTorque: " << Ki_zero_torque << std::endl;
marcodesilva 0:f757110a016c 215
marcodesilva 0:f757110a016c 216 wheelsAcceleration = robotCnf.Data.wheelsAcceleration;
marcodesilva 0:f757110a016c 217
marcodesilva 0:f757110a016c 218 velAccSafetyFactor = robotCnf.Data.velAccSafetyFactor;
marcodesilva 0:f757110a016c 219
marcodesilva 0:f757110a016c 220 maxCartPos << robotCnf.Data.maxCartPos_x, robotCnf.Data.maxCartPos_y, robotCnf.Data.maxCartPos_z;
marcodesilva 0:f757110a016c 221 minCartPos << robotCnf.Data.minCartPos_x, robotCnf.Data.minCartPos_y, robotCnf.Data.minCartPos_z;
marcodesilva 0:f757110a016c 222
marcodesilva 0:f757110a016c 223
marcodesilva 0:f757110a016c 224 roverParameters << r_frontWheel, r_retroWheel, pipeCurve_I, pipeCurve_E, pipeCurve_M, retroFrontCentralDistance, wheelsCntactPointDistanceFromPipeCenter;
marcodesilva 0:f757110a016c 225
marcodesilva 0:f757110a016c 226 rover->setParameters(roverParameters);
marcodesilva 0:f757110a016c 227
marcodesilva 0:f757110a016c 228 armParameters << robotCnf.Data.a3, robotCnf.Data.a4 , robotCnf.Data.a5, robotCnf.Data.a5i, robotCnf.Data.a6, robotCnf.Data.d7, robotCnf.Data.x_0_r2, robotCnf.Data.y_0_r2, robotCnf.Data.z_0_r2,
marcodesilva 0:f757110a016c 229 robotCnf.Data.massr ,robotCnf.Data.mpxr ,robotCnf.Data.mpyr ,robotCnf.Data.mpzr ,robotCnf.Data.mass1, robotCnf.Data.mpx1 ,robotCnf.Data.mpy1 ,robotCnf.Data.mpz1 ,robotCnf.Data.mass2,robotCnf.Data.mpx2 ,robotCnf.Data.mpy2 ,robotCnf.Data.mpz2 ,robotCnf.Data.mass3,robotCnf.Data.mpx3 ,robotCnf.Data.mpy3 ,robotCnf.Data.mpz3 ,robotCnf.Data.mass4,robotCnf.Data.mpx4 ,robotCnf.Data.mpy4 ,robotCnf.Data.mpz4 ,robotCnf.Data.mass5,robotCnf.Data.mpx5 ,robotCnf.Data.mpy5 ,robotCnf.Data.mpz5 ,robotCnf.Data.mass6,robotCnf.Data.mpx6 ,robotCnf.Data.mpy6 ,robotCnf.Data.mpz6,robotCnf.Data.mass_battery ,robotCnf.Data.mpx_battery ,robotCnf.Data.mpz_battery;
marcodesilva 0:f757110a016c 230
marcodesilva 0:f757110a016c 231 arm->setParameters(armParameters);
marcodesilva 0:f757110a016c 232
marcodesilva 0:f757110a016c 233 float o = robotCnf.Data.jointsFilterOmega;
marcodesilva 0:f757110a016c 234 float z = robotCnf.Data.jointsFilterZita;
marcodesilva 0:f757110a016c 235 float ma = robotCnf.Data.jointsMaxAcc;
marcodesilva 0:f757110a016c 236 float mv = robotCnf.Data.jointsMaxVel;
marcodesilva 0:f757110a016c 237 omega << o,o,o,o,o,o,o,o;
marcodesilva 0:f757110a016c 238 zita << z,z,z,z,z,z,z,z;
marcodesilva 0:f757110a016c 239 maxJerk << 1000,1000,1000,1000,1000,1000,1000,1000;
marcodesilva 0:f757110a016c 240 maxAcc << 20,ma,ma,ma,ma,ma,ma,ma;
marcodesilva 0:f757110a016c 241 maxVel << 1,mv,mv,mv,mv,mv,mv,mv;
marcodesilva 0:f757110a016c 242
marcodesilva 0:f757110a016c 243 invKin_n_max_iter = 1;
marcodesilva 0:f757110a016c 244 invKin_max_pError = 0.001;
marcodesilva 0:f757110a016c 245 invKin_max_oError = 0.001;
marcodesilva 0:f757110a016c 246
marcodesilva 0:f757110a016c 247 WeightVector << robotCnf.Data.jacobianInversionWeight1,robotCnf.Data.jacobianInversionWeight2,robotCnf.Data.jacobianInversionWeight3,robotCnf.Data.jacobianInversionWeight4,robotCnf.Data.jacobianInversionWeight5,robotCnf.Data.jacobianInversionWeight6,robotCnf.Data.jacobianInversionWeight7,robotCnf.Data.jacobianInversionWeight8;
marcodesilva 0:f757110a016c 248
marcodesilva 0:f757110a016c 249 K_firstOrder << robotCnf.Data.kp_first_order,robotCnf.Data.kp_first_order,robotCnf.Data.kp_first_order,robotCnf.Data.ko_first_order,robotCnf.Data.ko_first_order,robotCnf.Data.ko_first_order;
marcodesilva 0:f757110a016c 250 K_secondOrder << robotCnf.Data.kp_second_order,robotCnf.Data.kp_second_order,robotCnf.Data.kp_second_order,robotCnf.Data.ko_second_order,robotCnf.Data.ko_second_order,robotCnf.Data.ko_second_order;
marcodesilva 0:f757110a016c 251
marcodesilva 0:f757110a016c 252 D_secondOrder << robotCnf.Data.dp_second_order,robotCnf.Data.dp_second_order,robotCnf.Data.dp_second_order,robotCnf.Data.do_second_order,robotCnf.Data.do_second_order,robotCnf.Data.do_second_order;
marcodesilva 0:f757110a016c 253
marcodesilva 0:f757110a016c 254
marcodesilva 0:f757110a016c 255 followFilter->setFollowFilterParameters(omega, zita, maxJerk, maxAcc, maxVel, maxPos, minPos);
marcodesilva 0:f757110a016c 256 inverseKinematicsControl->setInverseKinematicsControlParameters(K_firstOrder, K_secondOrder, D_secondOrder, WeightVector, maxAcc, maxVel, maxPos, minPos, invKin_n_max_iter, invKin_max_pError, invKin_max_oError);
marcodesilva 0:f757110a016c 257
marcodesilva 0:f757110a016c 258
marcodesilva 0:f757110a016c 259 safeCheck->setSafeCheckParameters(maxPos,minPos, velAccSafetyFactor*maxVel, velAccSafetyFactor*maxAcc, maxCartPos, minCartPos);
marcodesilva 0:f757110a016c 260 safeCheck->initSafeCheck();
marcodesilva 0:f757110a016c 261
marcodesilva 0:f757110a016c 262 std::cout << "K_firstOrder: " << K_firstOrder.transpose() << std::endl;
marcodesilva 0:f757110a016c 263 std::cout << "K_secondOrder: " << K_secondOrder.transpose() << std::endl;
marcodesilva 0:f757110a016c 264 std::cout << "D_secondOrder: " << D_secondOrder.transpose() << std::endl;
marcodesilva 0:f757110a016c 265 std::cout << "WeightVector: " << WeightVector.transpose() << std::endl;
marcodesilva 0:f757110a016c 266
marcodesilva 0:f757110a016c 267 dq_stab_int = 0.0;
marcodesilva 0:f757110a016c 268 dq_stab = 0.0;
marcodesilva 0:f757110a016c 269 q_stab = 0.0;
marcodesilva 0:f757110a016c 270
marcodesilva 0:f757110a016c 271 if(!debug){
marcodesilva 0:f757110a016c 272 arm_mutex.lock();
marcodesilva 0:f757110a016c 273 if(!parametersConfigurated){
marcodesilva 0:f757110a016c 274 if(!debug)arm->setOperatingMode(3,4);
marcodesilva 0:f757110a016c 275 std::cout << "set operating mode" << std::endl;
marcodesilva 0:f757110a016c 276 }
marcodesilva 0:f757110a016c 277 if(!debug)q_mis << roverLongitudinalIntegratedPosition,utilities::deg2rad(pitch_m),arm->getJointPos(true);
marcodesilva 0:f757110a016c 278 arm_mutex.unlock();
marcodesilva 0:f757110a016c 279 }else{
marcodesilva 0:f757110a016c 280 q_mis = first_q;
marcodesilva 0:f757110a016c 281 }
marcodesilva 0:f757110a016c 282
marcodesilva 0:f757110a016c 283 q_cmd = q_mis;
marcodesilva 0:f757110a016c 284
marcodesilva 0:f757110a016c 285
marcodesilva 0:f757110a016c 286
marcodesilva 0:f757110a016c 287 }
marcodesilva 0:f757110a016c 288
marcodesilva 0:f757110a016c 289
marcodesilva 0:f757110a016c 290 void SystemControl::armEnableMotors() {
marcodesilva 0:f757110a016c 291
marcodesilva 0:f757110a016c 292 if(!motor_enabled && parametersConfigurated) {
marcodesilva 0:f757110a016c 293
marcodesilva 0:f757110a016c 294 arm_mutex.lock();
marcodesilva 0:f757110a016c 295 if(!debug)arm->enableMotors(1);
marcodesilva 0:f757110a016c 296 arm_mutex.unlock();
marcodesilva 0:f757110a016c 297 motor_enabled = true;
marcodesilva 0:f757110a016c 298
marcodesilva 0:f757110a016c 299 }
marcodesilva 0:f757110a016c 300
marcodesilva 0:f757110a016c 301 }
marcodesilva 0:f757110a016c 302
marcodesilva 0:f757110a016c 303 void SystemControl::armDisableMotors() {
marcodesilva 0:f757110a016c 304 arm_mutex.lock();
marcodesilva 0:f757110a016c 305 if(!debug)arm->enableMotors(0);
marcodesilva 0:f757110a016c 306 arm_mutex.unlock();
marcodesilva 0:f757110a016c 307 motor_enabled = false;
marcodesilva 0:f757110a016c 308 }
marcodesilva 0:f757110a016c 309
marcodesilva 0:f757110a016c 310
marcodesilva 0:f757110a016c 311 void SystemControl::setResponse() {
marcodesilva 0:f757110a016c 312 data_mutex.lock();
marcodesilva 0:f757110a016c 313
marcodesilva 0:f757110a016c 314 robotRsp.Data.estRoverJointPos1 = q_mis(0);
marcodesilva 0:f757110a016c 315 robotRsp.Data.estRoverJointPos2 = q_mis(1);
marcodesilva 0:f757110a016c 316 robotRsp.Data.misArmJointPos1 = q_mis(2);
marcodesilva 0:f757110a016c 317 robotRsp.Data.misArmJointPos2 = q_mis(3);
marcodesilva 0:f757110a016c 318 robotRsp.Data.misArmJointPos3 = q_mis(4);
marcodesilva 0:f757110a016c 319 robotRsp.Data.misArmJointPos4 = q_mis(5);
marcodesilva 0:f757110a016c 320 robotRsp.Data.misArmJointPos5 = q_mis(6);
marcodesilva 0:f757110a016c 321 robotRsp.Data.misArmJointPos6 = q_mis(7);
marcodesilva 0:f757110a016c 322
marcodesilva 0:f757110a016c 323 Vector4f quaternion_mis = utilities::rot2quat(T_e_b_m.block(0,0,3,3));
marcodesilva 0:f757110a016c 324
marcodesilva 0:f757110a016c 325 robotRsp.Data.misTebX = T_e_b_m(0,3);
marcodesilva 0:f757110a016c 326 robotRsp.Data.misTebY = T_e_b_m(1,3);
marcodesilva 0:f757110a016c 327 robotRsp.Data.misTebZ = T_e_b_m(2,3);
marcodesilva 0:f757110a016c 328
marcodesilva 0:f757110a016c 329 robotRsp.Data.misTebQ0 = quaternion_mis(0);
marcodesilva 0:f757110a016c 330 robotRsp.Data.misTebQ1 = quaternion_mis(1);
marcodesilva 0:f757110a016c 331 robotRsp.Data.misTebQ2 = quaternion_mis(2);
marcodesilva 0:f757110a016c 332 robotRsp.Data.misTebQ3 = quaternion_mis(3);
marcodesilva 0:f757110a016c 333
marcodesilva 0:f757110a016c 334
marcodesilva 0:f757110a016c 335 robotRsp.Data.droneFx = wrench_system(0);
marcodesilva 0:f757110a016c 336 robotRsp.Data.droneFy = wrench_system(1);
marcodesilva 0:f757110a016c 337 robotRsp.Data.droneFz = wrench_system(2);
marcodesilva 0:f757110a016c 338 robotRsp.Data.droneTx = wrench_system(3);
marcodesilva 0:f757110a016c 339 robotRsp.Data.droneTy = wrench_system(4);
marcodesilva 0:f757110a016c 340 robotRsp.Data.droneTz = wrench_system(5);
marcodesilva 0:f757110a016c 341
marcodesilva 0:f757110a016c 342 robotRsp.Data.roverPitchAngle = utilities::deg2rad(pitch_m);
marcodesilva 0:f757110a016c 343 robotRsp.Data.roverStabVel = vels_d;
marcodesilva 0:f757110a016c 344 robotRsp.Data.roverLaserFrontDx = roverLaserFrontDx;
marcodesilva 0:f757110a016c 345 robotRsp.Data.roverLaserFrontSx = roverLaserFrontSx;
marcodesilva 0:f757110a016c 346 robotRsp.Data.roverLaserRetroDx = roverLaserRetroDx;
marcodesilva 0:f757110a016c 347 robotRsp.Data.roverLaserRetroSx = roverLaserRetroSx;
marcodesilva 0:f757110a016c 348
marcodesilva 0:f757110a016c 349 robotRsp.Data.roverForwardVel = dq_cmd(0);
marcodesilva 0:f757110a016c 350 robotRsp.Data.roverZVel = vela_m;
marcodesilva 0:f757110a016c 351
marcodesilva 0:f757110a016c 352 robotRsp.StatusData = robotStatus;
marcodesilva 0:f757110a016c 353
marcodesilva 0:f757110a016c 354 data_mutex.unlock();
marcodesilva 0:f757110a016c 355
marcodesilva 0:f757110a016c 356 }
marcodesilva 0:f757110a016c 357
marcodesilva 0:f757110a016c 358
marcodesilva 0:f757110a016c 359
marcodesilva 0:f757110a016c 360 void SystemControl::updateEthCommunication(){
marcodesilva 0:f757110a016c 361
marcodesilva 0:f757110a016c 362 setResponse();
marcodesilva 0:f757110a016c 363
marcodesilva 0:f757110a016c 364 data_mutex.lock();
marcodesilva 0:f757110a016c 365 robotStatus = eth_tcp->updateEthCommunication(robotCnf, robotCmd, robotRsp);
marcodesilva 0:f757110a016c 366 data_mutex.unlock();
marcodesilva 0:f757110a016c 367
marcodesilva 0:f757110a016c 368 switch(robotStatus){
marcodesilva 0:f757110a016c 369
marcodesilva 0:f757110a016c 370 case ETH_LOST_CONNECTION:
marcodesilva 0:f757110a016c 371 //azioni da fare se la connessione si interrompe
marcodesilva 0:f757110a016c 372 //armDisableMotors();
marcodesilva 0:f757110a016c 373 //robotCmd.Cmd = DISABLE_MOTORS;
marcodesilva 0:f757110a016c 374 rover->setRoverVelocity(0.0,0.0 , 0, pipeRadious, wheelsAcceleration); //velForward, velStabilization, velAsset, pipeRadious, acceleration
marcodesilva 0:f757110a016c 375 dq_cmd(5) = 0.0;
marcodesilva 0:f757110a016c 376 ematClosed = false;
marcodesilva 0:f757110a016c 377 ledEth->write(0);
marcodesilva 0:f757110a016c 378
marcodesilva 0:f757110a016c 379 break;
marcodesilva 0:f757110a016c 380
marcodesilva 0:f757110a016c 381 case ETH_ERROR_SENDING_DATA:
marcodesilva 0:f757110a016c 382 //azioni da fare se non riesco ad inviare la risposta
marcodesilva 0:f757110a016c 383 break;
marcodesilva 0:f757110a016c 384
marcodesilva 0:f757110a016c 385 case ETH_WRONG_RECV_SIZE:
marcodesilva 0:f757110a016c 386 case ETH_CHECKSUM_ERROR:
marcodesilva 0:f757110a016c 387 //azioni da fare se il pacchetto arrivato non è corretto
marcodesilva 0:f757110a016c 388
marcodesilva 0:f757110a016c 389 break;
marcodesilva 0:f757110a016c 390
marcodesilva 0:f757110a016c 391 case ETH_NEW_CMD:
marcodesilva 0:f757110a016c 392 wdTimer.reset();
marcodesilva 0:f757110a016c 393 break;
marcodesilva 0:f757110a016c 394
marcodesilva 0:f757110a016c 395 case ETH_NEW_CMD_CURRENT_VAL_REQUEST:
marcodesilva 0:f757110a016c 396 //azioni da fare se è arrivato un comando
marcodesilva 0:f757110a016c 397
marcodesilva 0:f757110a016c 398 wdTimer.reset();
marcodesilva 0:f757110a016c 399 break;
marcodesilva 0:f757110a016c 400
marcodesilva 0:f757110a016c 401 case ETH_NEW_CNF:
marcodesilva 0:f757110a016c 402 //azioni da fare se è arrivata una configurazione
marcodesilva 0:f757110a016c 403 printf("E arrivata una configurazione!!!\n");
marcodesilva 0:f757110a016c 404 setConfigurationParameters();
marcodesilva 0:f757110a016c 405 parametersConfigurated = true;
marcodesilva 0:f757110a016c 406 ledEth->write(1);
marcodesilva 0:f757110a016c 407
marcodesilva 0:f757110a016c 408
marcodesilva 0:f757110a016c 409 break;
marcodesilva 0:f757110a016c 410
marcodesilva 0:f757110a016c 411 default:
marcodesilva 0:f757110a016c 412 //istruzioni di default
marcodesilva 0:f757110a016c 413 break;
marcodesilva 0:f757110a016c 414 }
marcodesilva 0:f757110a016c 415
marcodesilva 0:f757110a016c 416 ethDone = true;
marcodesilva 0:f757110a016c 417
marcodesilva 0:f757110a016c 418
marcodesilva 0:f757110a016c 419
marcodesilva 0:f757110a016c 420 }
marcodesilva 0:f757110a016c 421
marcodesilva 0:f757110a016c 422
marcodesilva 0:f757110a016c 423 void SystemControl::stabilizationFunction() {
marcodesilva 0:f757110a016c 424
marcodesilva 0:f757110a016c 425 if(parametersConfigurated){
marcodesilva 0:f757110a016c 426 ComandMsg cmd_;
marcodesilva 0:f757110a016c 427
marcodesilva 0:f757110a016c 428 data_mutex.lock();
marcodesilva 0:f757110a016c 429 cmd_ = robotCmd;
marcodesilva 0:f757110a016c 430 data_mutex.unlock();
marcodesilva 0:f757110a016c 431
marcodesilva 0:f757110a016c 432
marcodesilva 0:f757110a016c 433 float torqueError = 0.0-wrench_system(3);
marcodesilva 0:f757110a016c 434 dq_stab_int += Ki_zero_torque*torqueError*dtBoard;
marcodesilva 0:f757110a016c 435
marcodesilva 0:f757110a016c 436 dq_stab = Kp_zero_torque*torqueError + dq_stab_int;
marcodesilva 0:f757110a016c 437
marcodesilva 0:f757110a016c 438 rover->calcImuAngles(pitch_m, pitch_vel_m, roll_m, dtBoard);
marcodesilva 0:f757110a016c 439 //std::cout << "pd: " << pitch_d << " pm: " << pitch_m << std::endl;
marcodesilva 0:f757110a016c 440 vels_d = rover->computeStabilizationVel(pitch_d, pitch_m, pitch_vel_m, Kp_stabilization, Kd_stabilization);
marcodesilva 0:f757110a016c 441
marcodesilva 0:f757110a016c 442 rover->setRoverVelocity(velf_d, vels_d , 0.0, pipeRadious, wheelsAcceleration); //velForward, velStabilization, velAsset, pipeRadious, acceleration
marcodesilva 0:f757110a016c 443 }else{
marcodesilva 0:f757110a016c 444
marcodesilva 0:f757110a016c 445 rover->setRoverVelocity(0.0,0.0 , 0, pipeRadious, wheelsAcceleration); //velForward, velStabilization, velAsset, pipeRadious, acceleration
marcodesilva 0:f757110a016c 446 ledError->write(1);
marcodesilva 0:f757110a016c 447 }
marcodesilva 0:f757110a016c 448
marcodesilva 0:f757110a016c 449 }
marcodesilva 0:f757110a016c 450
marcodesilva 0:f757110a016c 451 void SystemControl::navigationFunction() {
marcodesilva 0:f757110a016c 452
marcodesilva 0:f757110a016c 453 if(parametersConfigurated && wdTimer.read() < wdTime){
marcodesilva 0:f757110a016c 454 ComandMsg cmd_;
marcodesilva 0:f757110a016c 455
marcodesilva 0:f757110a016c 456 data_mutex.lock();
marcodesilva 0:f757110a016c 457 cmd_ = robotCmd;
marcodesilva 0:f757110a016c 458 data_mutex.unlock();
marcodesilva 0:f757110a016c 459 //rover->computeCentralJointsFromTofs(roverLaserFrontDx, roverLaserFrontSx, roverLaserRetroDx, roverLaserRetroSx);
marcodesilva 0:f757110a016c 460
marcodesilva 0:f757110a016c 461
marcodesilva 0:f757110a016c 462 //rover->acquireTofs(roverLaserFrontDx, roverLaserFrontSx, roverLaserRetroDx, roverLaserRetroSx, roverFrontDistance, roverRetroDistance);;
marcodesilva 0:f757110a016c 463 //if(!debug)rover->setCentralJointsAngle(cmd_.Data.roverFrontJointAngle,cmd_.Data.roverRetroJointAngle); //in radiants (front, retro)
marcodesilva 0:f757110a016c 464 rover->setCentralJointsAngle(0.04,0.0); //in radiants (front, retro)
marcodesilva 0:f757110a016c 465
marcodesilva 0:f757110a016c 466 rover->getRoverVelocity(velf_m, vels_m, vela_m, pipeRadious);
marcodesilva 0:f757110a016c 467 roverLongitudinalIntegratedPosition -= velf_m*dtBoard;
marcodesilva 0:f757110a016c 468
marcodesilva 0:f757110a016c 469
marcodesilva 0:f757110a016c 470 }else{
marcodesilva 0:f757110a016c 471 rover->setRoverVelocity(0.0,0.0 , 0, pipeRadious, wheelsAcceleration); //velForward, velStabilization, velAsset, pipeRadious, acceleration
marcodesilva 0:f757110a016c 472 ledError->write(0);
marcodesilva 0:f757110a016c 473
marcodesilva 0:f757110a016c 474 }
marcodesilva 0:f757110a016c 475
marcodesilva 0:f757110a016c 476 }
marcodesilva 0:f757110a016c 477
marcodesilva 0:f757110a016c 478 Vector8f SystemControl::calcNullSpaceVelocity(){
marcodesilva 0:f757110a016c 479 Vector8f dq_null_space;
marcodesilva 0:f757110a016c 480
marcodesilva 0:f757110a016c 481 dq_null_space << 0.0,dq_stab,0.0,0.0,2.0*(M_PI/6-q_cmd(4)),0.0,0.0,0.0;
marcodesilva 0:f757110a016c 482 return dq_null_space;
marcodesilva 0:f757110a016c 483 }
marcodesilva 0:f757110a016c 484
marcodesilva 0:f757110a016c 485
marcodesilva 0:f757110a016c 486 void SystemControl::armRoverKinematicControlFunction() {
marcodesilva 0:f757110a016c 487
marcodesilva 0:f757110a016c 488 Matrix4f T_e_b = Matrix4f::Identity();
marcodesilva 0:f757110a016c 489 Vector8f dq_null_space;
marcodesilva 0:f757110a016c 490
marcodesilva 0:f757110a016c 491
marcodesilva 0:f757110a016c 492 Vector4f quaternion = Vector4f::Zero();
marcodesilva 0:f757110a016c 493 data_mutex.lock();
marcodesilva 0:f757110a016c 494 ComandMsg cmd_ = robotCmd;
marcodesilva 0:f757110a016c 495 data_mutex.unlock();
marcodesilva 0:f757110a016c 496
marcodesilva 0:f757110a016c 497 if(parametersConfigurated && wdTimer.read() < wdTime){
marcodesilva 0:f757110a016c 498 switch (cmd_.Cmd)
marcodesilva 0:f757110a016c 499 {
marcodesilva 0:f757110a016c 500
marcodesilva 0:f757110a016c 501 case IDLE_STATE:
marcodesilva 0:f757110a016c 502 robotControlMode = IDLE_MODE;
marcodesilva 0:f757110a016c 503
marcodesilva 0:f757110a016c 504 break;
marcodesilva 0:f757110a016c 505
marcodesilva 0:f757110a016c 506 case DISABLE_MOTORS:
marcodesilva 0:f757110a016c 507 armDisableMotors();
marcodesilva 0:f757110a016c 508 q_stab = 0.0;
marcodesilva 0:f757110a016c 509 pitch_d = 0.0;
marcodesilva 0:f757110a016c 510 velf_d = 0.0;
marcodesilva 0:f757110a016c 511 break;
marcodesilva 0:f757110a016c 512
marcodesilva 0:f757110a016c 513 case ENABLE_ARM:
marcodesilva 0:f757110a016c 514 armEnableMotors();
marcodesilva 0:f757110a016c 515 safeCheck->initSafeCheck();
marcodesilva 0:f757110a016c 516 q_stab = 0.0;
marcodesilva 0:f757110a016c 517 pitch_d = 0.0;
marcodesilva 0:f757110a016c 518 velf_d = 0.0;
marcodesilva 0:f757110a016c 519 //roverLongitudinalIntegratedPosition = 0.0; ///////////////////////////ricorda di togliere
marcodesilva 0:f757110a016c 520
marcodesilva 0:f757110a016c 521 //roverLongitudinalIntegratedPosition = 0.0; ///////////////////////////ricorda di togliere
marcodesilva 0:f757110a016c 522 //std::cout << "enable arm cmd" << std::endl;
marcodesilva 0:f757110a016c 523 break;
marcodesilva 0:f757110a016c 524
marcodesilva 0:f757110a016c 525 case GET_CURRENT_ROBOT_STATE:
marcodesilva 0:f757110a016c 526 //std::cout << "Get robot state" << std::endl;
marcodesilva 0:f757110a016c 527
marcodesilva 0:f757110a016c 528 if(debug){
marcodesilva 0:f757110a016c 529 q_mis = q_cmd;
marcodesilva 0:f757110a016c 530 }else{
marcodesilva 0:f757110a016c 531 arm_mutex.lock();
marcodesilva 0:f757110a016c 532 if(!debug)q_mis << roverLongitudinalIntegratedPosition,utilities::deg2rad(pitch_m),arm->getJointPos(true);
marcodesilva 0:f757110a016c 533 arm_mutex.unlock();
marcodesilva 0:f757110a016c 534 }
marcodesilva 0:f757110a016c 535 T_e_b_m = arm->forwardKinematics(q_mis); //Dyrect kinematics
marcodesilva 0:f757110a016c 536 robotControlMode = GET_ROBOT_STATE_MODE;
marcodesilva 0:f757110a016c 537
marcodesilva 0:f757110a016c 538 break;
marcodesilva 0:f757110a016c 539
marcodesilva 0:f757110a016c 540 case ARM_JOINT_CONTROL:
marcodesilva 0:f757110a016c 541 if(precCmd != ARM_JOINT_CONTROL){
marcodesilva 0:f757110a016c 542 std::cout << "Joint cntrl" << std::endl;
marcodesilva 0:f757110a016c 543 if(debug){
marcodesilva 0:f757110a016c 544 q_mis = q_cmd;
marcodesilva 0:f757110a016c 545 }else{
marcodesilva 0:f757110a016c 546 arm_mutex.lock();
marcodesilva 0:f757110a016c 547 std::cout << "get joint pos" << std::endl;
marcodesilva 0:f757110a016c 548 //roverLongitudinalIntegratedPosition = 0.0;
marcodesilva 0:f757110a016c 549 if(!debug)q_mis << roverLongitudinalIntegratedPosition,utilities::deg2rad(pitch_m),arm->getJointPos(true);
marcodesilva 0:f757110a016c 550 arm_mutex.unlock();
marcodesilva 0:f757110a016c 551 }
marcodesilva 0:f757110a016c 552 followFilter->initFollowFilter(q_mis);
marcodesilva 0:f757110a016c 553
marcodesilva 0:f757110a016c 554 }
marcodesilva 0:f757110a016c 555 robotControlMode = JOINT_CONTROL_MODE;
marcodesilva 0:f757110a016c 556
marcodesilva 0:f757110a016c 557 q_cmd << cmd_.Data.roverLongitudinalPosition,0.0,cmd_.Data.jointPos1, cmd_.Data.jointPos2, cmd_.Data.jointPos3, cmd_.Data.jointPos4, cmd_.Data.jointPos5, cmd_.Data.toolOrientAroundPipeAxis;
marcodesilva 0:f757110a016c 558 followFilter->updateFollowFilter(q_cmd, dtBoard);
marcodesilva 0:f757110a016c 559 followFilter->getJointCmd(q_cmd, dq_cmd, ddq_cmd);
marcodesilva 0:f757110a016c 560 //dq_cmd(0) = dq_stab;
marcodesilva 0:f757110a016c 561 if(safeCheck->check(q_cmd,dq_cmd,ddq_cmd, safeCheck->JOINT_MODE)){
marcodesilva 0:f757110a016c 562 arm_mutex.lock();
marcodesilva 0:f757110a016c 563 if(!debug)arm->setJointPos(q_cmd);
marcodesilva 0:f757110a016c 564 //q_stab += dq_stab*dtBoard;
marcodesilva 0:f757110a016c 565 float torqueError = 0.0-wrench_system(3);
marcodesilva 0:f757110a016c 566 q_cmd(1) = 0.04*torqueError;
marcodesilva 0:f757110a016c 567 pitch_d = utilities::rad2deg(q_cmd(1));
marcodesilva 0:f757110a016c 568 velf_d = -dq_cmd(0);
marcodesilva 0:f757110a016c 569
marcodesilva 0:f757110a016c 570 arm_mutex.unlock();
marcodesilva 0:f757110a016c 571 }else{
marcodesilva 0:f757110a016c 572 printf("arm locked!\t");
marcodesilva 0:f757110a016c 573 data_mutex.lock();
marcodesilva 0:f757110a016c 574 robotStatus = ROBOT_SAFETY_MODE;
marcodesilva 0:f757110a016c 575 pitch_d = 0.0;
marcodesilva 0:f757110a016c 576 velf_d = 0.0;
marcodesilva 0:f757110a016c 577 q_stab = 0.0;
marcodesilva 0:f757110a016c 578
marcodesilva 0:f757110a016c 579 data_mutex.unlock();
marcodesilva 0:f757110a016c 580 }
marcodesilva 0:f757110a016c 581
marcodesilva 0:f757110a016c 582 break;
marcodesilva 0:f757110a016c 583
marcodesilva 0:f757110a016c 584 case ARM_KINEMATIC_CONTROL_FIRST_ORDER:
marcodesilva 0:f757110a016c 585
marcodesilva 0:f757110a016c 586 if(precCmd != ARM_KINEMATIC_CONTROL_FIRST_ORDER){
marcodesilva 0:f757110a016c 587 std::cout << "Kinematic first order" << std::endl;
marcodesilva 0:f757110a016c 588
marcodesilva 0:f757110a016c 589 if(debug){
marcodesilva 0:f757110a016c 590 q_mis = q_cmd;
marcodesilva 0:f757110a016c 591 }else{
marcodesilva 0:f757110a016c 592 arm_mutex.lock();
marcodesilva 0:f757110a016c 593 if(!debug)q_mis << roverLongitudinalIntegratedPosition,utilities::deg2rad(pitch_m),arm->getJointPos(true);
marcodesilva 0:f757110a016c 594 arm_mutex.unlock();
marcodesilva 0:f757110a016c 595 }
marcodesilva 0:f757110a016c 596 followFilter->initFollowFilter(q_mis);
marcodesilva 0:f757110a016c 597 inverseKinematicsControl->initInverseKinematicsControl(q_mis);
marcodesilva 0:f757110a016c 598 }
marcodesilva 0:f757110a016c 599 robotControlMode = KINEMATIC_CONTROL_FIRST_ORDER_MODE;
marcodesilva 0:f757110a016c 600
marcodesilva 0:f757110a016c 601 T_e_b(0,3) = cmd_.Data.armX; T_e_b(1,3) = cmd_.Data.armY; T_e_b(2,3) = cmd_.Data.armZ;
marcodesilva 0:f757110a016c 602 quaternion << cmd_.Data.armQ0, cmd_.Data.armQ1, cmd_.Data.armQ2, cmd_.Data.armQ3;
marcodesilva 0:f757110a016c 603
marcodesilva 0:f757110a016c 604 T_e_b.block(0,0,3,3) = utilities::matrixOrthonormalization( utilities::QuatToMat(quaternion) );
marcodesilva 0:f757110a016c 605 dq_null_space = calcNullSpaceVelocity();
marcodesilva 0:f757110a016c 606 inverseKinematicsControl->updateInverseKinematicsControl_firstOrder(T_e_b, Vector6f::Zero(),dq_null_space, dtBoard);
marcodesilva 0:f757110a016c 607 inverseKinematicsControl->getJointCmd(q_cmd, dq_cmd, ddq_cmd);
marcodesilva 0:f757110a016c 608 inverseKinematicsControl->getDebugVector(debugVector);
marcodesilva 0:f757110a016c 609 if(safeCheck->check(q_cmd,dq_cmd,ddq_cmd,safeCheck->CARTESIAN_MODE)){
marcodesilva 0:f757110a016c 610 arm_mutex.lock();
marcodesilva 0:f757110a016c 611 if(!debug)arm->setJointPos(q_cmd);
marcodesilva 0:f757110a016c 612 pitch_d = utilities::rad2deg(q_cmd(1));
marcodesilva 0:f757110a016c 613 velf_d = -dq_cmd(0);
marcodesilva 0:f757110a016c 614 arm_mutex.unlock();
marcodesilva 0:f757110a016c 615 }else{
marcodesilva 0:f757110a016c 616 printf("arm locked!\t");
marcodesilva 0:f757110a016c 617 pitch_d = 0.0;
marcodesilva 0:f757110a016c 618 velf_d = 0.0;
marcodesilva 0:f757110a016c 619
marcodesilva 0:f757110a016c 620 data_mutex.lock();
marcodesilva 0:f757110a016c 621 robotStatus = ROBOT_SAFETY_MODE;
marcodesilva 0:f757110a016c 622 data_mutex.unlock();
marcodesilva 0:f757110a016c 623 }
marcodesilva 0:f757110a016c 624
marcodesilva 0:f757110a016c 625 break;
marcodesilva 0:f757110a016c 626
marcodesilva 0:f757110a016c 627 case ARM_KINEMATIC_CONTROL_SECOND_ORDER:
marcodesilva 0:f757110a016c 628
marcodesilva 0:f757110a016c 629 if(precCmd != ARM_KINEMATIC_CONTROL_SECOND_ORDER){
marcodesilva 0:f757110a016c 630 std::cout << "Kinematic second order" << std::endl;
marcodesilva 0:f757110a016c 631
marcodesilva 0:f757110a016c 632 if(debug){
marcodesilva 0:f757110a016c 633 q_mis = q_cmd;
marcodesilva 0:f757110a016c 634 }else{
marcodesilva 0:f757110a016c 635 arm_mutex.lock();
marcodesilva 0:f757110a016c 636 if(!debug)q_mis << roverLongitudinalIntegratedPosition,utilities::deg2rad(pitch_m),arm->getJointPos(true);
marcodesilva 0:f757110a016c 637 arm_mutex.unlock();
marcodesilva 0:f757110a016c 638 }
marcodesilva 0:f757110a016c 639 inverseKinematicsControl->initInverseKinematicsControl(q_mis);
marcodesilva 0:f757110a016c 640 }
marcodesilva 0:f757110a016c 641 robotControlMode = KINEMATIC_CONTROL_SECOND_ORDER_MODE;
marcodesilva 0:f757110a016c 642
marcodesilva 0:f757110a016c 643
marcodesilva 0:f757110a016c 644 T_e_b(0,3) = cmd_.Data.armX; T_e_b(1,3) = cmd_.Data.armY; T_e_b(2,3) = cmd_.Data.armZ;
marcodesilva 0:f757110a016c 645 quaternion << cmd_.Data.armQ0, cmd_.Data.armQ1, cmd_.Data.armQ2, cmd_.Data.armQ3;
marcodesilva 0:f757110a016c 646
marcodesilva 0:f757110a016c 647 T_e_b.block(0,0,3,3) = utilities::matrixOrthonormalization( utilities::QuatToMat(quaternion) );
marcodesilva 0:f757110a016c 648
marcodesilva 0:f757110a016c 649 inverseKinematicsControl->updateInverseKinematicsControl_secondOrder(T_e_b, Vector6f::Zero(), Vector6f::Zero(), dtBoard);
marcodesilva 0:f757110a016c 650 inverseKinematicsControl->getJointCmd(q_cmd, dq_cmd, ddq_cmd);
marcodesilva 0:f757110a016c 651 inverseKinematicsControl->getDebugVector(debugVector);
marcodesilva 0:f757110a016c 652
marcodesilva 0:f757110a016c 653 if(safeCheck->check(q_cmd,dq_cmd,ddq_cmd,safeCheck->CARTESIAN_MODE)){
marcodesilva 0:f757110a016c 654 arm_mutex.lock();
marcodesilva 0:f757110a016c 655 if(!debug)arm->setJointPos(q_cmd);
marcodesilva 0:f757110a016c 656 pitch_d = utilities::rad2deg(q_cmd(1));
marcodesilva 0:f757110a016c 657 velf_d = -dq_cmd(0);
marcodesilva 0:f757110a016c 658
marcodesilva 0:f757110a016c 659 arm_mutex.unlock();
marcodesilva 0:f757110a016c 660 }else{
marcodesilva 0:f757110a016c 661 printf("arm locked!\t");
marcodesilva 0:f757110a016c 662 data_mutex.lock();
marcodesilva 0:f757110a016c 663 robotStatus = ROBOT_SAFETY_MODE;
marcodesilva 0:f757110a016c 664 pitch_d = 0.0;
marcodesilva 0:f757110a016c 665 velf_d = 0.0;
marcodesilva 0:f757110a016c 666 data_mutex.unlock();
marcodesilva 0:f757110a016c 667 }
marcodesilva 0:f757110a016c 668 break;
marcodesilva 0:f757110a016c 669
marcodesilva 0:f757110a016c 670 default:
marcodesilva 0:f757110a016c 671
marcodesilva 0:f757110a016c 672 break;
marcodesilva 0:f757110a016c 673 }
marcodesilva 0:f757110a016c 674
marcodesilva 0:f757110a016c 675 precCmd = cmd_.Cmd;
marcodesilva 0:f757110a016c 676 drone_orientation << cmd_.Data.drone_roll, cmd_.Data.drone_pitch, 0.0;
marcodesilva 0:f757110a016c 677 ematClosed = cmd_.Data.toolEmatClose;
marcodesilva 0:f757110a016c 678
marcodesilva 0:f757110a016c 679 }else{
marcodesilva 0:f757110a016c 680 precCmd = DISABLE_MOTORS;
marcodesilva 0:f757110a016c 681 }
marcodesilva 0:f757110a016c 682 }
marcodesilva 0:f757110a016c 683
marcodesilva 0:f757110a016c 684
marcodesilva 0:f757110a016c 685 float SystemControl::calcDroneDynamics(){
marcodesilva 0:f757110a016c 686 Vector8f q_forDyn;
marcodesilva 0:f757110a016c 687 if(parametersConfigurated){
marcodesilva 0:f757110a016c 688
marcodesilva 0:f757110a016c 689 q_forDyn << roverLongitudinalIntegratedPosition, utilities::deg2rad(pitch_m), q_cmd.block(2,0,6,1);
marcodesilva 0:f757110a016c 690
marcodesilva 0:f757110a016c 691 //batteryServo->write(battery_position/0.14);
marcodesilva 0:f757110a016c 692 drone_orientation << 0,0,0;
marcodesilva 0:f757110a016c 693
marcodesilva 0:f757110a016c 694 wrench_armAndRover = arm->wrenchGr(q_cmd,drone_orientation);
marcodesilva 0:f757110a016c 695
marcodesilva 0:f757110a016c 696 battery_pos = arm->batteryPos_from_wrench(-wrench_armAndRover, drone_orientation);
marcodesilva 0:f757110a016c 697 //battery_pos = 0.0;
marcodesilva 0:f757110a016c 698
marcodesilva 0:f757110a016c 699 batteryServo->write(battery_pos/0.14);
marcodesilva 0:f757110a016c 700
marcodesilva 0:f757110a016c 701
marcodesilva 0:f757110a016c 702 wrench_system = arm->wrenchBattery(battery_pos, drone_orientation) + wrench_armAndRover; //
marcodesilva 0:f757110a016c 703
marcodesilva 0:f757110a016c 704 float battery_vel = (battery_pos - battery_pos_prec)/dtBoard;
marcodesilva 0:f757110a016c 705 float max_motor_vel = 0.03;
marcodesilva 0:f757110a016c 706 float max_battery_pos = 0.14;
marcodesilva 0:f757110a016c 707
marcodesilva 0:f757110a016c 708 if(battery_vel > max_motor_vel) battery_vel = max_motor_vel;
marcodesilva 0:f757110a016c 709 if(battery_vel < -max_motor_vel) battery_vel = -max_motor_vel;
marcodesilva 0:f757110a016c 710
marcodesilva 0:f757110a016c 711 battery_pos_saturated = battery_pos_prec + battery_vel*dtBoard;
marcodesilva 0:f757110a016c 712 if(battery_pos_saturated < 0.0) battery_pos_saturated = 0.0;
marcodesilva 0:f757110a016c 713 if(battery_pos_saturated > max_battery_pos) battery_pos_saturated = max_battery_pos;
marcodesilva 0:f757110a016c 714 battery_pos_prec = battery_pos_saturated;
marcodesilva 0:f757110a016c 715
marcodesilva 0:f757110a016c 716 }
marcodesilva 0:f757110a016c 717 return battery_pos_saturated;
marcodesilva 0:f757110a016c 718
marcodesilva 0:f757110a016c 719
marcodesilva 0:f757110a016c 720 }
marcodesilva 0:f757110a016c 721
marcodesilva 0:f757110a016c 722 void SystemControl::toolControlFunction(){
marcodesilva 0:f757110a016c 723
marcodesilva 0:f757110a016c 724 float _toolWheelsPos_dx_mis, _toolWheelsPos_sx_mis, _toolWheelsSpeed_dx_mis, _toolWheelsSpeed_sx_mis;
marcodesilva 0:f757110a016c 725
marcodesilva 0:f757110a016c 726 toolWheelsVel_d = dq_cmd(7); //(3.14/8.128)*sin(0.02*2*3.14*t_temp);//q_cmd(7); //dq_cmd(7);
marcodesilva 0:f757110a016c 727
marcodesilva 0:f757110a016c 728 if(ematClosed){
marcodesilva 0:f757110a016c 729 toolClampPos_d = 9.0;
marcodesilva 0:f757110a016c 730 }else{
marcodesilva 0:f757110a016c 731 toolClampPos_d = -13.0;
marcodesilva 0:f757110a016c 732 }
marcodesilva 0:f757110a016c 733
marcodesilva 0:f757110a016c 734
marcodesilva 0:f757110a016c 735
marcodesilva 0:f757110a016c 736 if(i2c_tool->updateI2CCommunication(Cmd1, toolClampPos_d, toolWheelsVel_d, _toolWheelsPos_dx_mis, _toolWheelsPos_sx_mis, _toolWheelsSpeed_dx_mis, _toolWheelsSpeed_sx_mis)){
marcodesilva 0:f757110a016c 737 toolWheelsPos_dx_mis = _toolWheelsPos_dx_mis;
marcodesilva 0:f757110a016c 738 toolWheelsPos_sx_mis = _toolWheelsPos_sx_mis;
marcodesilva 0:f757110a016c 739 toolWheelsSpeed_dx_mis = _toolWheelsSpeed_dx_mis;
marcodesilva 0:f757110a016c 740 toolWheelsSpeed_sx_mis = _toolWheelsSpeed_sx_mis;
marcodesilva 0:f757110a016c 741 }
marcodesilva 0:f757110a016c 742
marcodesilva 0:f757110a016c 743 //t_temp = t_temp + actualDtControlCycle;
marcodesilva 0:f757110a016c 744
marcodesilva 0:f757110a016c 745 }
marcodesilva 0:f757110a016c 746
marcodesilva 0:f757110a016c 747 /*void SystemControl::schedulerThreadFunction() {
marcodesilva 0:f757110a016c 748
marcodesilva 0:f757110a016c 749 double timeStabPrec = 0.0;
marcodesilva 0:f757110a016c 750 double timeNavigPrec = 0.0;
marcodesilva 0:f757110a016c 751 double timeEthPrec = 0.0;
marcodesilva 0:f757110a016c 752 double timeArmPrec = 0.0;
marcodesilva 0:f757110a016c 753
marcodesilva 0:f757110a016c 754 float time = 0.0;
marcodesilva 0:f757110a016c 755
marcodesilva 0:f757110a016c 756 Timer schedulerTimer;
marcodesilva 0:f757110a016c 757
marcodesilva 0:f757110a016c 758 schedulerTimer.start();
marcodesilva 0:f757110a016c 759
marcodesilva 0:f757110a016c 760 timeStabPrec = schedulerTimer.read();
marcodesilva 0:f757110a016c 761 timeEthPrec = schedulerTimer.read();
marcodesilva 0:f757110a016c 762 timeNavigPrec = schedulerTimer.read();
marcodesilva 0:f757110a016c 763 timeArmPrec = schedulerTimer.read();
marcodesilva 0:f757110a016c 764
marcodesilva 0:f757110a016c 765 while (1){
marcodesilva 0:f757110a016c 766
marcodesilva 0:f757110a016c 767 time = schedulerTimer.read();
marcodesilva 0:f757110a016c 768 //if(ethDone){
marcodesilva 0:f757110a016c 769 if(time - timeStabPrec > dtStab){
marcodesilva 0:f757110a016c 770 //dtPassedStab = time - timeStabPrec;
marcodesilva 0:f757110a016c 771 timeStabPrec = time;
marcodesilva 0:f757110a016c 772
marcodesilva 0:f757110a016c 773 stabilizationThread->flags_set(STAB_ENABLE_FLAG);
marcodesilva 0:f757110a016c 774
marcodesilva 0:f757110a016c 775 }
marcodesilva 0:f757110a016c 776
marcodesilva 0:f757110a016c 777 if(NAVIG && time - timeNavigPrec > dtNavig){
marcodesilva 0:f757110a016c 778 //dtPassedNavig = time - timeNavigPrec;
marcodesilva 0:f757110a016c 779 timeNavigPrec = time;
marcodesilva 0:f757110a016c 780
marcodesilva 0:f757110a016c 781 navigationThread->flags_set(NAVIG_ENABLE_FLAG);
marcodesilva 0:f757110a016c 782
marcodesilva 0:f757110a016c 783 }
marcodesilva 0:f757110a016c 784
marcodesilva 0:f757110a016c 785 if(time - timeArmPrec > dtArm){
marcodesilva 0:f757110a016c 786
marcodesilva 0:f757110a016c 787 //dtPassedArm = time - timeArmPrec;
marcodesilva 0:f757110a016c 788 timeArmPrec = time;
marcodesilva 0:f757110a016c 789
marcodesilva 0:f757110a016c 790 armThread->flags_set(ARM_ENABLE_FLAG);
marcodesilva 0:f757110a016c 791
marcodesilva 0:f757110a016c 792 }
marcodesilva 0:f757110a016c 793
marcodesilva 0:f757110a016c 794 ethDone = false;
marcodesilva 0:f757110a016c 795 //}
marcodesilva 0:f757110a016c 796
marcodesilva 0:f757110a016c 797
marcodesilva 0:f757110a016c 798 ThisThread::sleep_for(1);
marcodesilva 0:f757110a016c 799
marcodesilva 0:f757110a016c 800 }
marcodesilva 0:f757110a016c 801
marcodesilva 0:f757110a016c 802 }*/
marcodesilva 0:f757110a016c 803
marcodesilva 0:f757110a016c 804
marcodesilva 0:f757110a016c 805 void SystemControl::printThreadFunction() {
marcodesilva 0:f757110a016c 806
marcodesilva 0:f757110a016c 807 //while (1){
marcodesilva 0:f757110a016c 808 if(printSchedulerCounter == 100){
marcodesilva 0:f757110a016c 809 std::cout << std::fixed;
marcodesilva 0:f757110a016c 810 std::cout << std::setprecision(4);
marcodesilva 0:f757110a016c 811
marcodesilva 0:f757110a016c 812 std::cout
marcodesilva 0:f757110a016c 813 << "Cmd: " << robotCmd.Cmd
marcodesilva 0:f757110a016c 814 //<< " State: " << robotRsp.StatusData
marcodesilva 0:f757110a016c 815 << " q_cmd: " << q_cmd.transpose()
marcodesilva 0:f757110a016c 816 << " dq_cmd: " << dq_cmd.transpose()
marcodesilva 0:f757110a016c 817 << " toolWheelsVel_d: " << toolWheelsVel_d
marcodesilva 0:f757110a016c 818
marcodesilva 0:f757110a016c 819 //<< " wrench: " << wrench_armAndRover.transpose()
marcodesilva 0:f757110a016c 820 << " velf_d: " << velf_d
marcodesilva 0:f757110a016c 821 << " vels_d: " << vels_d
marcodesilva 0:f757110a016c 822 << " battery_pos: " << battery_pos
marcodesilva 0:f757110a016c 823 << " pitch_m: " << utilities::deg2rad(pitch_m)
marcodesilva 0:f757110a016c 824 << " forw_pos: " << roverLongitudinalIntegratedPosition
marcodesilva 0:f757110a016c 825 << " Ts: " << actualDtControlCycle*1000
marcodesilva 0:f757110a016c 826 << std::endl;
marcodesilva 0:f757110a016c 827 printSchedulerCounter = 0;
marcodesilva 0:f757110a016c 828 }
marcodesilva 0:f757110a016c 829 printSchedulerCounter++;
marcodesilva 0:f757110a016c 830 //ThisThread::sleep_for(100);
marcodesilva 0:f757110a016c 831 //}
marcodesilva 0:f757110a016c 832
marcodesilva 0:f757110a016c 833 }