a
Diff: systemControl.cpp
- Revision:
- 0:f757110a016c
diff -r 000000000000 -r f757110a016c systemControl.cpp --- /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); + //} + +}