Revision 0:f757110a016c, committed 2021-10-04

Comitter:

marcodesilva

Date:

Mon Oct 04 12:06:02 2021 +0000

Commit message:

accorpati i due file in forma di libreria

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/systemControl.cpp	Mon Oct 04 12:06:02 2021 +0000
@@ -0,0 +1,833 @@
+#include "systemControl.h"
+
+#include <iostream>
+#include <iomanip>
+
+SystemControl::SystemControl(){
+    
+    dtPassedStab = 0.0;
+    dtPassedArm = 0.0;
+    dtPassedNavig = 0.0;
+    dtPassedEth = 0.0;
+    
+    wdTime = 0.2;
+    
+    t = 0.0;
+    
+    printSchedulerCounter = 0;
+    motor_enabled = false;
+        
+    actualDtControlCycle = 0.0;
+    
+    q_cmd.resize(8,1);
+    dq_cmd.resize(8,1);
+    ddq_cmd.resize(8,1);
+
+    q_cmd = Vector8f::Zero(); 
+    dq_cmd = Vector8f::Zero();
+    ddq_cmd = Vector8f::Zero();     
+    q_mis = Vector8f::Zero();
+    drone_orientation = Vector3f::Zero();
+
+    omega.resize(8,1);
+    zita.resize(8,1);
+    maxJerk.resize(8,1);
+    maxAcc.resize(8,1);
+    maxVel.resize(8,1);
+    maxPos.resize(8,1);
+    minPos.resize(8,1);
+    
+    dq_stab_int = 0.0;
+    dq_stab = 0.0;
+    q_stab = 0.0;
+
+    
+    
+    debugVector = Matrix<float, 24, 1> ::Zero();
+    
+    debug = false;
+    
+    parametersConfigurated = false;
+
+    ethDone = false;
+ 
+    printThread = new Thread(osPriorityNormal, 1 * 1024 /*32K stack size*/);
+    
+    ledEth = new DigitalOut(PG_3);
+    ledError = new DigitalOut(PG_2);
+
+}
+
+void SystemControl::initEthernet(){
+    printf("INIT ETHERNET!! \r\n");
+    eth_tcp = new Eth_tcp(3); // server timeout 1s
+    robotStatus = eth_tcp->connect();  
+    wdTimer.start(); 
+
+}
+
+void SystemControl::initArm() {  
+    printf("Creo il braccio \r\n");
+    arm = new ARAP180_WITH_ROVER();
+    
+    i2c_tool = new I2C_master(PC_9, PA_8, 0x54);
+    
+    K_firstOrder << 25,25,25,5,5,5;
+    K_secondOrder << 1500,1500,1500,500,500,500;
+
+    D_secondOrder << 20,20,20,15,15,15;
+    
+    invKin_n_max_iter = 1;
+    invKin_max_pError = 0.001; 
+    invKin_max_oError = 0.001;
+    
+    omega << 3.0,3.0,3.0,3.0,3.0,3.0,3.0,3.0;
+    zita << 0.8,0.8,0.8,0.8,0.8,0.8,0.8,0.8;
+    maxJerk << 3,3,3,3,3,3,3,3;
+    maxAcc << 20,100,100,100,100,100,100,100;
+    maxVel << 1,3,3,3,3,3,3,3;
+
+    minPos << -1000.0,-M_PI/4, -2*M_PI, -M_PI/2 + M_PI/8,        0.0,   -3.1, -M_PI, -M_PI;
+    maxPos << 1000.0, M_PI/4, 2*M_PI,   M_PI - M_PI/6, 2*M_PI/3, 0.85, M_PI,  M_PI; 
+
+    first_q.resize(8,1);
+    first_q <<   0.0,
+                 0.0,
+                 0.0,
+                -M_PI/2 + M_PI/6,
+                0.75,
+                -2.5,
+                M_PI/2,
+                0.0;
+
+    velAccSafetyFactor = 2;
+    forceTorqueSafetyFactor = 100;
+    
+    minCartPos << 0.1,0.1,0.1;
+    maxCartPos << 0.3,0.3,0.3;
+    
+    followFilter = new FollowFilter(omega, zita, maxJerk, maxAcc, maxVel, maxPos, minPos);
+    inverseKinematicsControl = new InverseKinematicsControl(arm, K_firstOrder, K_secondOrder, D_secondOrder, maxAcc, maxVel, maxPos, minPos, invKin_n_max_iter, invKin_max_pError, invKin_max_oError);
+    safeCheck = new SafeCheck(arm, maxPos, minPos, velAccSafetyFactor*maxVel, velAccSafetyFactor*maxAcc, maxCartPos, minCartPos);
+    followFilter->initFollowFilter(first_q);
+    inverseKinematicsControl->initInverseKinematicsControl(first_q);
+    safeCheck->initSafeCheck();
+
+    q_mis = first_q;
+    q_cmd = q_mis;
+    
+    T_e_b_m = arm->forwardKinematics(first_q);
+    
+    batteryServo = new Servo(PA_3);
+    batteryServo->write(0.0);
+
+}
+
+void SystemControl::initRover() {  
+    printf("Creo il rover \r\n");
+
+    rover = new Rover();
+    pitch_m = 0.0;
+    roll_m = 0.0;
+  
+    pitch_d = 0.0;
+    vels_d = 0.0; 
+    velf_d = 0.0;
+    vela_d = 0.0; 
+    velf_m = 0.0;
+    vels_m = 0.0;    
+    vela_m = 0.0;
+    front_vel_dx = 0.0;
+    front_vel_sx = 0.0;    
+    retro_vel_dx = 0.0;
+    retro_vel_sx = 0.0;
+    forward_vel = 0.0; 
+    jointFront_d = 0.0;
+    jointRetro_d = 0.0; 
+    roverLaserFrontDx  = 0.0; 
+    roverLaserFrontSx  = 0.0; 
+    roverLaserRetroDx  = 0.0; 
+    roverLaserRetroSx  = 0.0;  
+    roverFrontDistance = 0.0;
+    roverRetroDistance = 0.0;
+    
+    roverLongitudinalIntegratedPosition = 0.0;
+    
+    robotCmd.Data.roverForwardVel = 0.0;
+    robotCmd.Data.roverZVel = 0.0;
+    robotCmd.Data.roverFrontJointAngle = 0.0;
+    robotCmd.Data.roverRetroJointAngle = 0.0;
+    
+    pipeRadious = 0.125;
+    Kp_stabilization=1.2; //0.6  
+    wheelsAcceleration = 10000;
+    
+    rover->initializeImu();
+    //rover->initializeTofs();  //in radiants  (front, retro) ///////////////////////////////
+    rover->setRoverVelocity(0.0,0.0 , 0, pipeRadious, wheelsAcceleration); //velForward, velStabilization, velAsset, pipeRadious, acceleration
+    rover->setCentralJointsAngle(0.0,0.0);  //in radiants  (front, retro)
+
+    printf("Creato il rover \r\n");
+    
+
+
+}
+
+void SystemControl::startControlThread(){
+    
+    printf("\nControl thread started!\r\n");
+
+    //stabilizationThread->start(this,&SystemControl::stabilizationThreadFunction);    
+    //navigationThread->start(this,&SystemControl::navigationThreadFunction);    
+    //armThread->start(this,&SystemControl::armThreadFunction);    
+    //printThread->start(this,&SystemControl::printThreadFunction);    
+    //schedulerThread->start(this,&SystemControl::schedulerThreadFunction);
+    
+    
+}   
+
+
+void SystemControl::setConfigurationParameters() {
+    
+    Matrix<double, 7, 1> roverParameters;
+    Matrix<double, 40, 1> armParameters;
+    
+    wdTime = robotCnf.Data.wdTime;
+    debug = robotCnf.Data.debug;
+    
+    dtBoard = robotCnf.Data.dtBoard;
+    r_frontWheel = robotCnf.Data.r_frontWheel;
+    r_retroWheel = robotCnf.Data.r_retroWheel;
+    pipeCurve_I = robotCnf.Data.pipeCurve_I;
+    pipeCurve_E = robotCnf.Data.pipeCurve_E;
+    pipeCurve_M = robotCnf.Data.pipeCurve_M;
+    retroFrontCentralDistance = robotCnf.Data.retroFrontCentralDistance;
+    wheelsCntactPointDistanceFromPipeCenter = robotCnf.Data.wheelsCntactPointDistanceFromPipeCenter;
+    pipeRadious = robotCnf.Data.pipeRadious;
+    Kp_stabilization = robotCnf.Data.Kp_stabilization;
+    Kd_stabilization = robotCnf.Data.Kd_stabilization;
+    
+    Kp_zero_torque = robotCnf.Data.Kp_zero_torque;
+    Ki_zero_torque = robotCnf.Data.Ki_zero_torque;
+    
+    std::cout << "Kp_stab: " << Kp_stabilization << "   Kd_stab: " << Kd_stabilization << std::endl;
+    std::cout << "Kp_zeroTorque: " << Kp_zero_torque << "   Kd_zeroTorque: " << Ki_zero_torque << std::endl;
+
+    wheelsAcceleration = robotCnf.Data.wheelsAcceleration;
+    
+    velAccSafetyFactor = robotCnf.Data.velAccSafetyFactor;
+      
+    maxCartPos << robotCnf.Data.maxCartPos_x, robotCnf.Data.maxCartPos_y, robotCnf.Data.maxCartPos_z;
+    minCartPos << robotCnf.Data.minCartPos_x, robotCnf.Data.minCartPos_y, robotCnf.Data.minCartPos_z;
+    
+    
+    roverParameters << r_frontWheel, r_retroWheel, pipeCurve_I, pipeCurve_E, pipeCurve_M, retroFrontCentralDistance, wheelsCntactPointDistanceFromPipeCenter;
+    
+    rover->setParameters(roverParameters);
+        
+    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,
+    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;
+
+    arm->setParameters(armParameters);
+
+    float o = robotCnf.Data.jointsFilterOmega;
+    float z = robotCnf.Data.jointsFilterZita;
+    float ma = robotCnf.Data.jointsMaxAcc;
+    float mv = robotCnf.Data.jointsMaxVel;
+    omega << o,o,o,o,o,o,o,o;
+    zita << z,z,z,z,z,z,z,z;
+    maxJerk << 1000,1000,1000,1000,1000,1000,1000,1000;
+    maxAcc << 20,ma,ma,ma,ma,ma,ma,ma;
+    maxVel << 1,mv,mv,mv,mv,mv,mv,mv;
+    
+    invKin_n_max_iter = 1;
+    invKin_max_pError = 0.001; 
+    invKin_max_oError = 0.001;
+    
+    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;
+ 
+    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;
+    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;
+  
+    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;
+    
+   
+    followFilter->setFollowFilterParameters(omega, zita, maxJerk, maxAcc, maxVel, maxPos, minPos);
+    inverseKinematicsControl->setInverseKinematicsControlParameters(K_firstOrder, K_secondOrder, D_secondOrder, WeightVector, maxAcc, maxVel, maxPos, minPos, invKin_n_max_iter, invKin_max_pError, invKin_max_oError);
+
+
+    safeCheck->setSafeCheckParameters(maxPos,minPos, velAccSafetyFactor*maxVel, velAccSafetyFactor*maxAcc, maxCartPos, minCartPos);
+    safeCheck->initSafeCheck(); 
+    
+    std::cout << "K_firstOrder: " << K_firstOrder.transpose() << std::endl;
+    std::cout << "K_secondOrder: " << K_secondOrder.transpose() << std::endl;
+    std::cout << "D_secondOrder: " << D_secondOrder.transpose() << std::endl;
+    std::cout << "WeightVector: " << WeightVector.transpose() << std::endl;
+    
+    dq_stab_int = 0.0;
+    dq_stab = 0.0;
+    q_stab = 0.0;
+
+    if(!debug){    
+        arm_mutex.lock();
+        if(!parametersConfigurated){
+             if(!debug)arm->setOperatingMode(3,4);
+             std::cout << "set operating mode" << std::endl;
+             }
+        if(!debug)q_mis << roverLongitudinalIntegratedPosition,utilities::deg2rad(pitch_m),arm->getJointPos(true);
+        arm_mutex.unlock();
+    }else{
+         q_mis = first_q;
+    }
+
+    q_cmd = q_mis;
+    
+
+ 
+}
+
+
+void SystemControl::armEnableMotors() {    
+    
+    if(!motor_enabled && parametersConfigurated) {
+        
+        arm_mutex.lock();
+            if(!debug)arm->enableMotors(1);
+        arm_mutex.unlock();
+        motor_enabled = true;
+
+    } 
+    
+}
+
+void SystemControl::armDisableMotors() {    
+    arm_mutex.lock();
+        if(!debug)arm->enableMotors(0);
+    arm_mutex.unlock();
+    motor_enabled = false;
+}
+
+
+void SystemControl::setResponse() {
+    data_mutex.lock();
+    
+    robotRsp.Data.estRoverJointPos1 = q_mis(0);
+    robotRsp.Data.estRoverJointPos2 = q_mis(1);
+    robotRsp.Data.misArmJointPos1 = q_mis(2);
+    robotRsp.Data.misArmJointPos2 = q_mis(3);
+    robotRsp.Data.misArmJointPos3 = q_mis(4);
+    robotRsp.Data.misArmJointPos4 = q_mis(5);
+    robotRsp.Data.misArmJointPos5 = q_mis(6);
+    robotRsp.Data.misArmJointPos6 = q_mis(7);
+    
+    Vector4f quaternion_mis = utilities::rot2quat(T_e_b_m.block(0,0,3,3));
+    
+    robotRsp.Data.misTebX = T_e_b_m(0,3);
+    robotRsp.Data.misTebY = T_e_b_m(1,3);
+    robotRsp.Data.misTebZ = T_e_b_m(2,3);
+    
+    robotRsp.Data.misTebQ0 = quaternion_mis(0);
+    robotRsp.Data.misTebQ1 = quaternion_mis(1);
+    robotRsp.Data.misTebQ2 = quaternion_mis(2);
+    robotRsp.Data.misTebQ3 = quaternion_mis(3);
+    
+
+    robotRsp.Data.droneFx = wrench_system(0);
+    robotRsp.Data.droneFy = wrench_system(1);
+    robotRsp.Data.droneFz = wrench_system(2);
+    robotRsp.Data.droneTx = wrench_system(3);
+    robotRsp.Data.droneTy = wrench_system(4);
+    robotRsp.Data.droneTz = wrench_system(5);
+
+    robotRsp.Data.roverPitchAngle = utilities::deg2rad(pitch_m); 
+    robotRsp.Data.roverStabVel = vels_d;
+    robotRsp.Data.roverLaserFrontDx = roverLaserFrontDx;
+    robotRsp.Data.roverLaserFrontSx = roverLaserFrontSx;
+    robotRsp.Data.roverLaserRetroDx = roverLaserRetroDx;
+    robotRsp.Data.roverLaserRetroSx = roverLaserRetroSx;
+                      
+    robotRsp.Data.roverForwardVel = dq_cmd(0);
+    robotRsp.Data.roverZVel = vela_m;    
+    
+    robotRsp.StatusData = robotStatus;
+
+    data_mutex.unlock();
+    
+}
+
+
+
+void SystemControl::updateEthCommunication(){
+
+    setResponse();    
+      
+    data_mutex.lock();  
+    robotStatus = eth_tcp->updateEthCommunication(robotCnf, robotCmd, robotRsp);
+    data_mutex.unlock();
+    
+    switch(robotStatus){
+        
+    case ETH_LOST_CONNECTION:
+        //azioni da fare se la connessione si interrompe
+        //armDisableMotors();
+        //robotCmd.Cmd = DISABLE_MOTORS;
+        rover->setRoverVelocity(0.0,0.0 , 0, pipeRadious, wheelsAcceleration); //velForward, velStabilization, velAsset, pipeRadious, acceleration
+        dq_cmd(5) = 0.0; 
+        ematClosed = false;
+        ledEth->write(0);
+
+    break;
+    
+    case ETH_ERROR_SENDING_DATA:
+        //azioni da fare se non riesco ad inviare la risposta
+    break;
+    
+    case ETH_WRONG_RECV_SIZE:
+    case ETH_CHECKSUM_ERROR:
+        //azioni da fare se  il pacchetto arrivato non è corretto
+
+    break;
+    
+    case ETH_NEW_CMD:
+        wdTimer.reset();
+    break;
+        
+    case ETH_NEW_CMD_CURRENT_VAL_REQUEST:
+        //azioni da fare se è arrivato un comando
+
+        wdTimer.reset();
+    break;
+    
+    case ETH_NEW_CNF:
+        //azioni da fare se è arrivata una configurazione
+        printf("E arrivata una configurazione!!!\n");
+        setConfigurationParameters();
+        parametersConfigurated = true;
+        ledEth->write(1);
+       
+
+    break;
+    
+    default:
+        //istruzioni di default
+    break;
+    }
+    
+    ethDone = true;  
+
+    
+
+}
+
+
+void SystemControl::stabilizationFunction() {
+   
+    if(parametersConfigurated){
+        ComandMsg cmd_;
+    
+        data_mutex.lock();
+        cmd_ = robotCmd;        
+        data_mutex.unlock();
+        
+
+        float torqueError = 0.0-wrench_system(3);
+        dq_stab_int += Ki_zero_torque*torqueError*dtBoard;
+        
+        dq_stab = Kp_zero_torque*torqueError + dq_stab_int;
+
+        rover->calcImuAngles(pitch_m, pitch_vel_m, roll_m, dtBoard);
+        //std::cout  << "pd: " << pitch_d << "  pm: " << pitch_m << std::endl;
+        vels_d = rover->computeStabilizationVel(pitch_d, pitch_m, pitch_vel_m, Kp_stabilization, Kd_stabilization);
+    
+        rover->setRoverVelocity(velf_d, vels_d , 0.0, pipeRadious, wheelsAcceleration); //velForward, velStabilization, velAsset, pipeRadious, acceleration
+    }else{
+        
+        rover->setRoverVelocity(0.0,0.0 , 0, pipeRadious, wheelsAcceleration); //velForward, velStabilization, velAsset, pipeRadious, acceleration
+        ledError->write(1);
+    }
+
+}
+
+void SystemControl::navigationFunction() {
+
+    if(parametersConfigurated && wdTimer.read() < wdTime){
+        ComandMsg cmd_;
+        
+        data_mutex.lock();
+        cmd_ = robotCmd;        
+        data_mutex.unlock();
+        //rover->computeCentralJointsFromTofs(roverLaserFrontDx, roverLaserFrontSx, roverLaserRetroDx, roverLaserRetroSx);
+        
+        
+        //rover->acquireTofs(roverLaserFrontDx, roverLaserFrontSx, roverLaserRetroDx, roverLaserRetroSx, roverFrontDistance, roverRetroDistance);;   
+        //if(!debug)rover->setCentralJointsAngle(cmd_.Data.roverFrontJointAngle,cmd_.Data.roverRetroJointAngle);  //in radiants  (front, retro)
+        rover->setCentralJointsAngle(0.04,0.0);  //in radiants  (front, retro)
+
+        rover->getRoverVelocity(velf_m, vels_m, vela_m, pipeRadious); 
+        roverLongitudinalIntegratedPosition -= velf_m*dtBoard;
+        
+
+    }else{
+        rover->setRoverVelocity(0.0,0.0 , 0, pipeRadious, wheelsAcceleration); //velForward, velStabilization, velAsset, pipeRadious, acceleration
+        ledError->write(0);
+
+    }
+
+}
+
+Vector8f SystemControl::calcNullSpaceVelocity(){
+    Vector8f dq_null_space;
+   
+    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;
+    return dq_null_space;
+}
+
+
+void SystemControl::armRoverKinematicControlFunction() {
+    
+    Matrix4f T_e_b = Matrix4f::Identity();
+    Vector8f dq_null_space;
+
+
+    Vector4f quaternion = Vector4f::Zero();
+    data_mutex.lock();
+        ComandMsg cmd_ = robotCmd;  
+    data_mutex.unlock();
+        
+    if(parametersConfigurated && wdTimer.read() < wdTime){   
+        switch (cmd_.Cmd)
+        {
+            
+        case IDLE_STATE:
+            robotControlMode = IDLE_MODE;
+
+        break;    
+            
+        case DISABLE_MOTORS:
+            armDisableMotors();
+            q_stab = 0.0;
+            pitch_d = 0.0;
+            velf_d = 0.0;
+        break;
+        
+        case ENABLE_ARM:
+            armEnableMotors();
+            safeCheck->initSafeCheck();
+            q_stab = 0.0;
+            pitch_d = 0.0;
+            velf_d = 0.0;
+            //roverLongitudinalIntegratedPosition = 0.0; ///////////////////////////ricorda di togliere
+
+            //roverLongitudinalIntegratedPosition = 0.0; ///////////////////////////ricorda di togliere
+            //std::cout << "enable arm cmd" << std::endl;
+        break;
+        
+        case GET_CURRENT_ROBOT_STATE:
+            //std::cout << "Get robot state" << std::endl;
+
+            if(debug){
+                q_mis = q_cmd;
+            }else{
+                arm_mutex.lock();
+                    if(!debug)q_mis << roverLongitudinalIntegratedPosition,utilities::deg2rad(pitch_m),arm->getJointPos(true);
+                arm_mutex.unlock();
+            }
+            T_e_b_m = arm->forwardKinematics(q_mis); //Dyrect kinematics
+            robotControlMode = GET_ROBOT_STATE_MODE;
+
+        break;
+        
+        case ARM_JOINT_CONTROL:
+            if(precCmd != ARM_JOINT_CONTROL){
+                std::cout << "Joint cntrl" << std::endl;
+                if(debug){
+                    q_mis = q_cmd;
+                }else{
+                    arm_mutex.lock();
+                        std::cout << "get joint pos" << std::endl;
+                        //roverLongitudinalIntegratedPosition = 0.0;
+                        if(!debug)q_mis << roverLongitudinalIntegratedPosition,utilities::deg2rad(pitch_m),arm->getJointPos(true);
+                    arm_mutex.unlock();
+                }
+                followFilter->initFollowFilter(q_mis);  
+                        
+            }
+            robotControlMode = JOINT_CONTROL_MODE;
+
+            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;
+            followFilter->updateFollowFilter(q_cmd, dtBoard);
+            followFilter->getJointCmd(q_cmd, dq_cmd, ddq_cmd);
+            //dq_cmd(0) = dq_stab;
+            if(safeCheck->check(q_cmd,dq_cmd,ddq_cmd, safeCheck->JOINT_MODE)){
+                arm_mutex.lock();
+                    if(!debug)arm->setJointPos(q_cmd); 
+                    //q_stab += dq_stab*dtBoard;
+                    float torqueError = 0.0-wrench_system(3);
+                    q_cmd(1) = 0.04*torqueError;
+                    pitch_d = utilities::rad2deg(q_cmd(1));
+                    velf_d = -dq_cmd(0);
+
+                arm_mutex.unlock();
+            }else{
+                printf("arm locked!\t");
+                data_mutex.lock();
+                    robotStatus = ROBOT_SAFETY_MODE;
+                    pitch_d = 0.0;
+                    velf_d = 0.0;
+                    q_stab = 0.0;
+
+                data_mutex.unlock();
+            } 
+    
+        break;
+        
+        case ARM_KINEMATIC_CONTROL_FIRST_ORDER:
+    
+            if(precCmd != ARM_KINEMATIC_CONTROL_FIRST_ORDER){
+                std::cout << "Kinematic first order" << std::endl;
+
+                if(debug){
+                    q_mis = q_cmd;
+                }else{
+                    arm_mutex.lock();
+                        if(!debug)q_mis << roverLongitudinalIntegratedPosition,utilities::deg2rad(pitch_m),arm->getJointPos(true);
+                    arm_mutex.unlock();
+                }
+                followFilter->initFollowFilter(q_mis);  
+                inverseKinematicsControl->initInverseKinematicsControl(q_mis);   
+            }
+            robotControlMode = KINEMATIC_CONTROL_FIRST_ORDER_MODE;
+
+            T_e_b(0,3) = cmd_.Data.armX; T_e_b(1,3) = cmd_.Data.armY; T_e_b(2,3) = cmd_.Data.armZ;            
+            quaternion <<  cmd_.Data.armQ0, cmd_.Data.armQ1, cmd_.Data.armQ2, cmd_.Data.armQ3;
+            
+            T_e_b.block(0,0,3,3) = utilities::matrixOrthonormalization( utilities::QuatToMat(quaternion) );
+            dq_null_space = calcNullSpaceVelocity();
+            inverseKinematicsControl->updateInverseKinematicsControl_firstOrder(T_e_b, Vector6f::Zero(),dq_null_space, dtBoard);
+            inverseKinematicsControl->getJointCmd(q_cmd, dq_cmd, ddq_cmd);
+            inverseKinematicsControl->getDebugVector(debugVector);
+            if(safeCheck->check(q_cmd,dq_cmd,ddq_cmd,safeCheck->CARTESIAN_MODE)){
+                arm_mutex.lock();
+                     if(!debug)arm->setJointPos(q_cmd); 
+                     pitch_d = utilities::rad2deg(q_cmd(1));
+                     velf_d = -dq_cmd(0);
+                arm_mutex.unlock();
+            }else{
+                printf("arm locked!\t");
+                pitch_d = 0.0;
+                velf_d = 0.0;
+
+                data_mutex.lock();
+                robotStatus = ROBOT_SAFETY_MODE;
+                data_mutex.unlock();
+            } 
+            
+        break;
+        
+        case ARM_KINEMATIC_CONTROL_SECOND_ORDER:
+    
+            if(precCmd != ARM_KINEMATIC_CONTROL_SECOND_ORDER){
+                std::cout << "Kinematic second order" << std::endl;
+
+                if(debug){
+                    q_mis = q_cmd;
+                }else{
+                    arm_mutex.lock();
+                        if(!debug)q_mis << roverLongitudinalIntegratedPosition,utilities::deg2rad(pitch_m),arm->getJointPos(true);
+                    arm_mutex.unlock();
+                }
+                inverseKinematicsControl->initInverseKinematicsControl(q_mis);           
+            }
+            robotControlMode = KINEMATIC_CONTROL_SECOND_ORDER_MODE;
+
+    
+            T_e_b(0,3) = cmd_.Data.armX; T_e_b(1,3) = cmd_.Data.armY; T_e_b(2,3) = cmd_.Data.armZ;            
+            quaternion <<  cmd_.Data.armQ0, cmd_.Data.armQ1, cmd_.Data.armQ2, cmd_.Data.armQ3;
+            
+            T_e_b.block(0,0,3,3) = utilities::matrixOrthonormalization( utilities::QuatToMat(quaternion) );
+    
+            inverseKinematicsControl->updateInverseKinematicsControl_secondOrder(T_e_b, Vector6f::Zero(), Vector6f::Zero(), dtBoard);
+            inverseKinematicsControl->getJointCmd(q_cmd, dq_cmd, ddq_cmd);
+            inverseKinematicsControl->getDebugVector(debugVector);
+    
+            if(safeCheck->check(q_cmd,dq_cmd,ddq_cmd,safeCheck->CARTESIAN_MODE)){
+                arm_mutex.lock();
+                     if(!debug)arm->setJointPos(q_cmd); 
+                     pitch_d = utilities::rad2deg(q_cmd(1));
+                     velf_d = -dq_cmd(0);
+
+                arm_mutex.unlock();
+            }else{
+                printf("arm locked!\t");
+                data_mutex.lock();
+                robotStatus = ROBOT_SAFETY_MODE;
+                pitch_d = 0.0;
+                velf_d = 0.0;
+                data_mutex.unlock();
+            }
+        break;
+        
+        default:
+        
+        break;
+        }
+        
+        precCmd = cmd_.Cmd;
+        drone_orientation << cmd_.Data.drone_roll, cmd_.Data.drone_pitch, 0.0;
+        ematClosed = cmd_.Data.toolEmatClose;
+        
+    }else{
+        precCmd = DISABLE_MOTORS;
+    }
+}
+
+
+float SystemControl::calcDroneDynamics(){
+    Vector8f q_forDyn;
+    if(parametersConfigurated){  
+    
+        q_forDyn << roverLongitudinalIntegratedPosition, utilities::deg2rad(pitch_m), q_cmd.block(2,0,6,1);
+
+        //batteryServo->write(battery_position/0.14);
+        drone_orientation << 0,0,0;
+
+        wrench_armAndRover = arm->wrenchGr(q_cmd,drone_orientation);
+        
+        battery_pos = arm->batteryPos_from_wrench(-wrench_armAndRover, drone_orientation);  
+        //battery_pos = 0.0;
+        
+        batteryServo->write(battery_pos/0.14);
+
+        
+        wrench_system = arm->wrenchBattery(battery_pos, drone_orientation) + wrench_armAndRover; //
+    
+        float battery_vel = (battery_pos - battery_pos_prec)/dtBoard;
+        float max_motor_vel = 0.03;
+        float max_battery_pos = 0.14;
+        
+        if(battery_vel > max_motor_vel) battery_vel = max_motor_vel;
+        if(battery_vel < -max_motor_vel) battery_vel = -max_motor_vel;
+        
+        battery_pos_saturated = battery_pos_prec + battery_vel*dtBoard;
+        if(battery_pos_saturated < 0.0) battery_pos_saturated = 0.0;
+        if(battery_pos_saturated > max_battery_pos) battery_pos_saturated = max_battery_pos;
+        battery_pos_prec = battery_pos_saturated;
+
+    }
+    return battery_pos_saturated;
+
+
+}
+
+void SystemControl::toolControlFunction(){
+    
+    float _toolWheelsPos_dx_mis, _toolWheelsPos_sx_mis, _toolWheelsSpeed_dx_mis, _toolWheelsSpeed_sx_mis;    
+    
+    toolWheelsVel_d = dq_cmd(7); //(3.14/8.128)*sin(0.02*2*3.14*t_temp);//q_cmd(7); //dq_cmd(7);
+    
+    if(ematClosed){
+        toolClampPos_d = 9.0;
+    }else{
+        toolClampPos_d = -13.0;
+    }
+
+
+    
+    if(i2c_tool->updateI2CCommunication(Cmd1,  toolClampPos_d, toolWheelsVel_d, _toolWheelsPos_dx_mis, _toolWheelsPos_sx_mis, _toolWheelsSpeed_dx_mis, _toolWheelsSpeed_sx_mis)){
+        toolWheelsPos_dx_mis = _toolWheelsPos_dx_mis;
+        toolWheelsPos_sx_mis = _toolWheelsPos_sx_mis;
+        toolWheelsSpeed_dx_mis = _toolWheelsSpeed_dx_mis;
+        toolWheelsSpeed_sx_mis = _toolWheelsSpeed_sx_mis;
+    }
+    
+    //t_temp = t_temp + actualDtControlCycle;
+
+}
+
+/*void SystemControl::schedulerThreadFunction() {
+    
+    double timeStabPrec = 0.0;
+    double timeNavigPrec = 0.0;   
+    double timeEthPrec = 0.0;    
+    double timeArmPrec = 0.0;    
+
+    float time = 0.0;
+
+    Timer schedulerTimer;
+        
+    schedulerTimer.start();  
+    
+    timeStabPrec = schedulerTimer.read();
+    timeEthPrec = schedulerTimer.read();    
+    timeNavigPrec = schedulerTimer.read();  
+    timeArmPrec = schedulerTimer.read();  
+
+    while (1){
+        
+         time = schedulerTimer.read(); 
+         //if(ethDone){
+         if(time - timeStabPrec > dtStab){  
+            //dtPassedStab = time - timeStabPrec; 
+            timeStabPrec = time;
+     
+            stabilizationThread->flags_set(STAB_ENABLE_FLAG);
+             
+         }
+         
+          if(NAVIG && time - timeNavigPrec > dtNavig){
+             //dtPassedNavig = time - timeNavigPrec; 
+             timeNavigPrec = time; 
+                    
+             navigationThread->flags_set(NAVIG_ENABLE_FLAG);
+
+         }
+         
+          if(time - timeArmPrec > dtArm){
+
+            //dtPassedArm = time - timeArmPrec;             
+            timeArmPrec = time; 
+            
+            armThread->flags_set(ARM_ENABLE_FLAG);
+
+        }
+                
+         ethDone = false;
+         //}
+
+
+         ThisThread::sleep_for(1);
+
+    }       
+
+}*/
+
+
+void SystemControl::printThreadFunction() {
+    
+    //while (1){
+        if(printSchedulerCounter == 100){
+            std::cout << std::fixed; 
+            std::cout << std::setprecision(4);
+             
+            std::cout 
+            << "Cmd: " << robotCmd.Cmd 
+            //<< "  State: " << robotRsp.StatusData 
+            << "  q_cmd: " << q_cmd.transpose() 
+            << "  dq_cmd: " << dq_cmd.transpose() 
+            << "  toolWheelsVel_d: " << toolWheelsVel_d           
+
+            //<< "  wrench: " << wrench_armAndRover.transpose() 
+            << "  velf_d: " << velf_d
+            << "  vels_d: " << vels_d
+            << "  battery_pos: " << battery_pos
+            << "  pitch_m: " << utilities::deg2rad(pitch_m)
+            << "  forw_pos: " << roverLongitudinalIntegratedPosition
+            << "  Ts: " << actualDtControlCycle*1000 
+            << std::endl;  
+            printSchedulerCounter = 0;
+        }
+        printSchedulerCounter++;
+        //ThisThread::sleep_for(100);
+    //}       
+
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/systemControl.h	Mon Oct 04 12:06:02 2021 +0000
@@ -0,0 +1,236 @@
+#ifndef SYSTEM_CONTROL_H
+#define SYSTEM_CONTROL_H
+
+#include "mbed.h"
+#include "Servo.h"
+#include "Rover.h"
+#include "ARAP180_with_rover.h"
+
+#include "math.h"
+#include "TOFs.h"
+#include "Eth_tcp.h"
+#include "mbed_mem_trace.h"
+
+#include "controlDefineVariables.h"
+
+#include "FollowFilter.h"
+
+#include "InverseKinematicsControl.h"
+
+#include "SafeCheck.h"
+
+#include "I2C_master.h"
+
+#define PRINT_DEBUG true
+#define NAVIG true 
+
+#define STAB_ENABLE_FLAG 1
+#define NAVIG_ENABLE_FLAG 1
+#define ARM_ENABLE_FLAG 1
+
+
+class SystemControl
+{
+public:
+    SystemControl();
+    void initArm();  
+    void initRover();  
+
+    void initEthernet();  
+
+    void startControlThread();  
+    void updateEthCommunication();
+    
+    void stabilizationFunction();
+    void navigationFunction();
+    void armRoverKinematicControlFunction();
+    //void schedulerThreadFunction();
+    void printThreadFunction();  
+    
+    float calcDroneDynamics();
+    void toolControlFunction();
+
+    
+    Vector8f calcNullSpaceVelocity();
+
+
+    
+   
+    double actualDtControlCycle;
+    
+    double t;
+
+    bool ethDone;
+    bool stabDone;
+    bool navigDone;
+    bool armDone;
+    float dtPassedStab;
+    float dtPassedArm;
+    float dtPassedNavig;
+    float dtPassedEth;
+    
+    float dtBoard;
+    float t_temp = 0.0;
+    bool ematClosed = false;
+    float toolWheelsVel_d = 0.0;
+
+
+   
+private:
+
+    /*ead *stabilizationThread;
+    Thread *navigationThread;
+    Thread *armThread;
+    Thread *schedulerThread;*/
+    Thread *printThread;
+    
+    Timer wdTimer;
+    
+    Eth_tcp *eth_tcp;
+
+    Rover *rover;
+    ARAP180_WITH_ROVER *arm;
+    
+    I2C_master *i2c_tool;
+    
+    Servo *batteryServo;
+
+    
+    DigitalOut *ledEth;
+    DigitalOut *ledError;
+    
+    Status robotStatus;
+    ControlMode robotControlMode;
+
+    Mutex data_mutex;
+    Mutex arm_mutex;
+
+    ConfigMsg robotCnf;
+    ComandMsg robotCmd;
+    ResponseMsg robotRsp; 
+        
+    FollowFilter *followFilter;
+        
+    InverseKinematicsControl *inverseKinematicsControl;
+    
+    SafeCheck *safeCheck;
+    
+    float velAccSafetyFactor;
+    float forceTorqueSafetyFactor;
+    int precCmd;
+    
+    int invKin_n_max_iter;
+    float invKin_max_pError; 
+    float invKin_max_oError;
+       
+    bool parametersConfigurated;
+    bool motor_enabled;
+    
+    float wdTime;
+    bool debug;
+    
+    float r_frontWheel;
+    float r_retroWheel;
+    float pipeCurve_I;
+    float pipeCurve_E;
+    float pipeCurve_M;
+    float retroFrontCentralDistance;
+    float wheelsCntactPointDistanceFromPipeCenter;
+    float pipeRadious;
+    float Kp_stabilization;
+    float Kd_stabilization;
+    float wheelsAcceleration;
+    
+    float dq_stab_int;
+    float dq_stab;
+    float q_stab;
+
+    float Kp_zero_torque;
+    float Ki_zero_torque;
+    
+    float pitch_m;
+    float pitch_vel_m;
+    float roll_m;
+    float pitch_d;
+    
+    float vels_d;   
+    float velf_d;
+    float vela_d; 
+    float velf_m;
+    float vels_m;    
+    float vela_m;
+    
+    float roverLongitudinalIntegratedPosition;
+    
+    float front_vel_dx;
+    float front_vel_sx;    
+    float retro_vel_dx;
+    float retro_vel_sx;
+    float forward_vel; 
+    
+    float jointFront_d;
+    float jointRetro_d; 
+    float roverLaserFrontDx; 
+    float roverLaserFrontSx; 
+    float roverLaserRetroDx; 
+    float roverLaserRetroSx;  
+    float roverFrontDistance;
+    float roverRetroDistance;
+    
+    
+
+    VectorXf q_cmd;  
+    VectorXf dq_cmd;
+    VectorXf ddq_cmd; 
+    VectorXf first_q;  
+    Vector8f q_mis;
+    
+    Matrix4f T_e_b_m;
+    
+    VectorXf omega;
+    VectorXf zita;
+    VectorXf maxJerk;
+    VectorXf maxAcc;
+    VectorXf maxVel;
+    VectorXf maxPos;
+    VectorXf minPos;
+    
+    Vector3f maxCartPos;
+    Vector3f minCartPos;
+    
+    float battery_pos;
+    float battery_pos_saturated;
+    float battery_pos_prec;  
+    
+    Vector6f K_firstOrder;
+    Vector6f K_secondOrder;
+
+    Vector6f D_secondOrder;
+    
+    Vector8f WeightVector;
+    
+    Vector6f wrench_armAndRover;
+    Vector6f wrench_system;
+
+    
+    Matrix<float, 24, 1>  debugVector;
+
+    Vector3f drone_orientation;
+
+    void armEnableMotors();
+    void armDisableMotors();
+    void setResponse();
+    
+    void setConfigurationParameters();
+    
+    int printSchedulerCounter;
+    
+    float toolClampPos_d = -13.0;
+    
+
+    float toolWheelsPos_dx_mis, toolWheelsPos_sx_mis, toolWheelsSpeed_dx_mis, toolWheelsSpeed_sx_mis;    
+
+
+};
+
+#endif
\ No newline at end of file