a
systemControl.cpp
- Committer:
- marcodesilva
- Date:
- 2021-10-04
- Revision:
- 0:f757110a016c
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); //} }