File content as of revision 0:f757110a016c:

#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);
    //}       

}