Marco De Silva
a
Diff: systemControl.cpp
- Revision:
- 0:f757110a016c
--- /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);
+ //}
+
+}