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