a
old/Rover_omni_cpp.txt
- Committer:
- marcodesilva
- Date:
- 2021-10-04
- Revision:
- 0:65943c77d1dc
File content as of revision 0:65943c77d1dc:
#include "Rover.h" Rover::Rover(){ ionMcBoudRate = 460800; meccanumBoardAddress = 128; meccanumMotorGearBoxRatio = 103; meccanumEncoderPulse = 512; meccanumKt = 0.00667; meccanumTransmissionRatio = 1.0; omniTransmissionRatio = 1.0; omniBoardAddress = 129; omniMotorGearBoxRatio = 103; omniEncoderPulse = 512; omniKt = 0.00667; r_meccanumWheel = 0.03; r_omniWheel = 0.019; pipeCurve_I = 0.165; pipeCurve_E = 0.2925; pipeCurve_M = 0.228; pipeDir = 0; omniMeccanumCentralDistance = 0.25; wheelsCntactPointDistanceFromPipeCenter = 0.08; eth_time_out = 2.0; S = Eigen::Matrix3f::Zero(); ionMcMeccanum = new IONMcMotors(meccanumBoardAddress,ionMcBoudRate, PG_14, PG_9, meccanumMotorGearBoxRatio, meccanumEncoderPulse, meccanumKt, meccanumKt); ionMcOmni = new IONMcMotors(omniBoardAddress,ionMcBoudRate, PB_9, PB_8, omniMotorGearBoxRatio, omniEncoderPulse, omniKt, omniKt); meccanumActuonix = new Servo(PE_11); omniActuonix = new Servo(PE_9); mpu = new MPU6050(PF_15, PF_14); eth_tcp = new Eth_tcp(3154, 500); distance_sensors = new distanceSensors(); eth_state = -1; eth_status = false; } void Rover::initializeTofs(){ distance_sensors->TOFs_init(); distance_sensors->TOFs_offset(); } void Rover::acquireTofs(float &frontDistance, float &retroDistance){ distance_sensors->TOFs_acquireFiltr(); frontDistance = distance_sensors->getFrontDistance(); retroDistance = distance_sensors->getRetroDistance(); } void Rover::computeCentralJointsFromTofs(){ float frontDistance; float retroDistance; acquireTofs(frontDistance, retroDistance); float frontVel = 0.05*frontDistance; float retroVel = -0.04*retroDistance; if(enableCurv==1){ frontPos = frontPos + frontVel*0.005; retroPos = retroPos + retroVel*0.005; }else{ frontPos = 0.0; retroPos = 0.0; } if(frontPos > 0.5) frontPos=0.5; if(frontPos < -0.5) frontPos=-0.5; if(retroPos > 0.5) retroPos=0.5; if(retroPos < -0.5) retroPos=-0.5; setCentralJointsAngle(frontPos,retroPos); } void Rover::initializeImu(){ printf("Initialize IMU \r\n"); //IMU FS_a = 8192; //per avere letture in g, sarebbe 32768/4 perchè 4g è il fondo scala FS_g = 131.072; //per avere letture in gradi/s, sarebbe 32768/250 perchè 250 è il fondo scala accBias[0] = 0.0; accBias[1] = 0.0; accBias[2] = 0.0; gyroBias[0] = 0.0; gyroBias[1] = 0.0; gyroBias[2] = 0.0; pitchAcc = 0.0; rollAcc = 0.0; pitch_integrated = 0.0; roll_integrated = 0.0; mpu->initialize(); bool mpu6050TestResult = mpu->testConnection(); if(mpu6050TestResult) { printf("MPU6050 test passed \r\n"); } else { printf("MPU6050 test failed \r\n"); } calibrateImu(); } void Rover::setWheelsVelocity(float forward_speed, float stabilization_speed, float asset_correction_speed, float pipe_radius, float maxWheelAcceleration){ Eigen::Vector3f cartesianSpeed; Eigen::Vector3f wheelsSpeed; updateRoverKin(pipe_radius, pipeDir); cartesianSpeed << forward_speed, stabilization_speed, asset_correction_speed; wheelsSpeed = S*cartesianSpeed; ionMcMeccanum->setSpeed(1, wheelsSpeed[0],maxWheelAcceleration); ionMcMeccanum->setSpeed(2, wheelsSpeed[1],maxWheelAcceleration); ionMcOmni->setSpeed(1,wheelsSpeed[2],maxWheelAcceleration); } int Rover::get_forward_vel(){ int fv; eth_mutex.lock(); fv = forward_vel; eth_mutex.unlock(); return fv; } int Rover::get_pitch(){ int pit; eth_mutex.lock(); pit = pitch_d; eth_mutex.unlock(); return pit; } int Rover::get_jointFront(){ int jf; eth_mutex.lock(); jf = jointFront; eth_mutex.unlock(); return jf; } int Rover::get_jointRetro(){ int jr; eth_mutex.lock(); jr = jointRetro; eth_mutex.unlock(); return jr; } void Rover::getRoverVelocity(float &forward_speed, float &stabilization_speed, float &asset_correction_speed, float pipe_radius){ Eigen::Vector3f cartesianSpeed; Eigen::Vector3f wheelsSpeed; float speedM1 = 0.0; float speedM2 = 0.0; float speedM3 = 0.0; updateRoverKin(pipe_radius, pipeDir); getMotorsSpeed(speedM1, speedM2, speedM3); wheelsSpeed << speedM1, speedM2, speedM3; cartesianSpeed = S_inv*wheelsSpeed; forward_speed = cartesianSpeed[0]; stabilization_speed = cartesianSpeed[1]; asset_correction_speed = cartesianSpeed[2]; } void Rover::getRoverWheelsVelocity(float &speed_wheel_dx, float &speed_wheel_sx, float &speed_wheels_retro){ Eigen::Vector3f cartesianSpeed; Eigen::Vector3f wheelsSpeed; float speedM1 = 0.0; float speedM2 = 0.0; float speedM3 = 0.0; getMotorsSpeed(speedM1, speedM2, speedM3); speed_wheel_dx = speedM1; speed_wheel_sx = speedM2; speed_wheels_retro = speedM3; } void Rover::updateRoverKin(float pipe_radius, int pipeDir){ float rm = r_meccanumWheel; float ro = r_omniWheel; float l = omniMeccanumCentralDistance; float L = wheelsCntactPointDistanceFromPipeCenter; float pr = pipe_radius; float R_dx = 0.0; float R_sx = 0.0; float R_m = 0.0; if(pipeDir ==0){ R_dx = 1.0; R_sx = 1.0; R_m = 1.0; }else if(pipeDir == 1){ R_dx = pipeCurve_I; R_sx = pipeCurve_E; R_m = pipeCurve_M; }else if(pipeDir == -1){ R_dx = pipeCurve_E; R_sx = pipeCurve_I; R_m = pipeCurve_M; } S << -1.0*R_dx/(rm*R_m), pr/rm, -L/rm, 1.0*R_sx/(rm*R_m), pr/rm, -L/rm, 0, pr/ro, l/ro; S_inv << -rm/2, rm/2, 0, l*rm/(2*pr*(L+l)), l*rm/(2*pr*(L+l)), L*ro/(pr*(L+l)), -rm/(2*(L+l)), -rm/(2*(L+l)), ro/(L+l); } void Rover::getMotorsTorque(float &torM1, float &torM2, float &torM3){ torM1 = ionMcMeccanum->getMotorTorque(1); torM2 = ionMcMeccanum->getMotorTorque(2); torM3 = ionMcOmni->getMotorTorque(1); } void Rover::getMotorsSpeed(float &speedM1, float &speedM2, float &speedM3){ speedM1 = ionMcMeccanum->getMotorSpeed(1); speedM2 = ionMcMeccanum->getMotorSpeed(2); speedM3 = ionMcOmni->getMotorSpeed(1); } void Rover::setCentralJointsAngle(float jointFront, float jointRetro){ float LmRestPosFront = (9.0+0.5)/1000; //9 float LmRestPosRetro = (9.0-0.5)/1000; //9 float maxLenght = 0.02; float LmFront = LmRestPosFront - centralJoint2actuonix(jointFront); float LmRetro = LmRestPosRetro - centralJoint2actuonix(jointRetro); meccanumActuonix->write(1.0 - LmFront/maxLenght); omniActuonix->write(1.0 - LmRetro/maxLenght); if (jointFront > 3.14*5/180){ pipeDir = -1; }else if (jointFront < -3.14*5/180){ pipeDir = 1; }else{ pipeDir = 0; } } float Rover::centralJoint2actuonix(float jointAngle){ float alpha = deg2rad(34.4); float L1 = 23.022/1000; float L2 = 62.037/1000; float L3 = 51.013/1000; float L4 = 4.939/1000; float l1 = L1*sin(alpha + jointAngle); float l2 = L1*cos(alpha + jointAngle); float h = sqrt( (L2 - l1)*(L2 - l1) + (l2 - L4)*(l2 - L4) ); return L3 - h; } float Rover::deg2rad(float deg) { return deg * M_PI / 180.0; } void Rover::calcImuAngles(float& pitch, float& roll, float dtImu) { mpu->getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //acquisisco i dati dal sensore a_x=float(ax)/FS_a - accBias[0]; //li converto in float, (CALCOLI EVITABILE SE INSTANZIASSI LE VARIABILI SUBITO COME FLOAT) a_y=float(ay)/FS_a - accBias[1]; a_z=float(az)/FS_a; g_x=float(gx)/FS_g - gyroBias[0]; g_y=float(gy)/FS_g - gyroBias[1]; g_z=float(gz)/FS_g - gyroBias[2]; pitch_integrated = g_x * dtImu; // Angle around the X-axis // Integrate the gyro data(deg/s) over time to get angle, usato g_x perchè movimento attorno all'asse x roll_integrated = g_y * dtImu; // Angle around the Y-axis //uso g_y perchè ho il rollio ruotando atttorno all'asse y pitchAcc = atan2f(a_y, sqrt(a_z*a_z + a_x*a_x))*180/M_PI; //considerare formule paper, calcolo i contrubiti dovuti dall'accelerometro rollAcc = -atan2f(a_x, sqrt(a_y*a_y + a_z*a_z))*180/M_PI; // if ( abs( pitchAcc - pitchAcc_p ) > 10){ pitchAcc = pitch; } if ( abs( rollAcc - rollAcc_p ) > 10){ rollAcc = roll; } // Apply Complementary Filter pitch = 0.95f*( pitch + pitch_integrated) + 0.05f*pitchAcc; roll = 0.95f*( roll + roll_integrated) + 0.05f*(rollAcc); pitchAcc_p = pitchAcc; rollAcc_p = rollAcc; } void Rover::calibrateImu() { const int CALIBRATION = 500; for(int i=0;i<50;i++) { mpu->getMotion6(&ax, &ay, &az, &gx, &gy, &gz); } printf(" START CALIBRATION \r\n"); for(int i=0;i<CALIBRATION;i++) { mpu->getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //acquisisco i dati dal sensore a_x=(float)ax/FS_a; //li converto in float, (CALCOLI EVITABILE SE INSTANZIASSI LE VARIABILI SUBITO COME FLOAT) a_y=(float)ay/FS_a; //a_z=(float)az/FS_a; g_x=(float)gx/FS_g; g_y=(float)gy/FS_g;; g_z=(float)gz/FS_g;; accBias[0] += a_x; accBias[1] += a_y; gyroBias[0] += g_x; gyroBias[1] += g_y; gyroBias[2] += g_z; } //BIAS accBias[0] = accBias[0]/CALIBRATION; accBias[1] = accBias[1]/CALIBRATION; gyroBias[0] = gyroBias[0]/CALIBRATION; gyroBias[1] = gyroBias[1]/CALIBRATION; gyroBias[2] = gyroBias[2]/CALIBRATION; printf(" END CALIBRATION \r\n"); //printf("%f\t %f\t %f\t %f\t %f\t \r\n", accBias[0], accBias[1], gyroBias[0],gyroBias[1],gyroBias[2]); //stampo le misure } void Rover::startEthComunication(){ printf("Avvio la comunicazione ethernet \r\n"); comunicationTimer.start(); eth_tcp->connect(); setState(1); } void Rover::ethComunicationUpdate(float _time, float _pitch, float _vels_a, float _vels_m, float _velf_a, float _velf_m) { if( eth_tcp->is_connected()){ setState(2); printf("ETH: connected!\r\n"); sock_open = true; }else{ setState(1); } eth_status = eth_tcp->recv_pkt(cmd, recv_vec, n_of_int); eth_time = comunicationTimer.read(); //printf("%f\r\n", (double)(eth_time - eth_time_sample_received)); if(eth_status){ eth_time_sample_received = comunicationTimer.read(); if(cmd == 's'){ if(n_of_int == 4){ eth_mutex.lock(); forward_vel = recv_vec.get(); pitch_d = recv_vec.get(); //jointFront = recv_vec.get(); //jointRetro = recv_vec.get(); enableStab = recv_vec.get(); enableCurv= recv_vec.get(); eth_mutex.unlock(); } eth_mutex.lock(); vec_to_send.put(_time*1000); vec_to_send.put(_pitch*1000); vec_to_send.put(_vels_a*1000); vec_to_send.put(_vels_m*1000); vec_to_send.put(_velf_a*1000); vec_to_send.put(_velf_m*1000); eth_mutex.unlock(); eth_tcp->send_vec_of_int(vec_to_send); vec_to_send.clear(); cmd = ' '; } } if((double)(eth_time - eth_time_sample_received)>eth_time_out && sock_open == true){ eth_tcp->reset_connection(); sock_open = false; eth_mutex.lock(); forward_vel = 0.0; pitch_d = 0.0; //jointFront = 0.0; //jointRetro = 0.0; enableCurv = 0; eth_mutex.unlock(); } } int Rover::getEthState(){ int state; eth_mutex.lock(); state = eth_state; eth_mutex.unlock(); return state; } void Rover::setState(int state){ eth_mutex.lock(); eth_state = state; eth_mutex.unlock(); }