a
Rover.cpp
- Committer:
- marcodesilva
- Date:
- 2021-10-04
- Revision:
- 0:65943c77d1dc
File content as of revision 0:65943c77d1dc:
#include "Rover.h" #include <iostream> Rover::Rover(){ pipeDir = 0; S = Eigen::Matrix<float, 4, 3>::Zero(); ionMcFront = new IONMcMotors(frontBoardAddress,ionMcBoudRate, PB_9, PB_8, frontMotorGearBoxRatio, frontEncoderPulse, frontKt, frontKt); ionMcRetro = new IONMcMotors(retroBoardAddress,ionMcBoudRate, PG_14, PG_9, retroMotorGearBoxRatio, retroEncoderPulse, retroKt, retroKt); frontActuonix = new Servo(PE_11); retroActuonix = new Servo(PE_9); mpu = new MPU6050(PF_15, PF_14); tofs = new TOFs(); enableCurv = 1; g_x_old = 0.0; } void Rover::initializeTofs(){ tofs->TOFs_init(); ThisThread::sleep_for(1000); //tofs->TOFs_offset(); } void Rover::setParameters(Eigen::VectorXd roverParameters){ r_frontWheel = roverParameters(0); r_retroWheel = roverParameters(1); pipeCurve_I = roverParameters(2); pipeCurve_E = roverParameters(3); pipeCurve_M = roverParameters(4); retroFrontCentralDistance = roverParameters(5); wheelsCntactPointDistanceFromPipeCenter = roverParameters(6); } float Rover::computeStabilizationVel(float pitch_d, float pitch, float pitch_vel, float Kp, float Kd){ vels_a = -Kp*M_PI*(pitch_d-pitch)/180 -Kd*M_PI*(0-pitch_vel)/180; return vels_a; } void Rover::acquireTofs(float &roverLaserFrontDx, float &roverLaserFrontSx, float &roverLaserRetroDx, float &roverLaserRetroSx, float &roverFrontDistance, float &roverRetroDistance){ tofs->TOFs_acquireFiltr(); roverLaserFrontDx = tofs->getLaserFrontDx(); roverLaserFrontSx = tofs->getLaserFrontSx(); roverLaserRetroDx = tofs->getLaserRetroDx(); roverLaserRetroSx = tofs->getLaserRetroSx(); roverFrontDistance = tofs->getFrontDistance(); roverRetroDistance = tofs->getRetroDistance(); } void Rover::computeCentralJointsFromTofs(float &roverLaserFrontDx,float &roverLaserFrontSx,float &roverLaserRetroDx,float &roverLaserRetroSx){ float frontDistance; float retroDistance; acquireTofs(roverLaserFrontDx, roverLaserFrontSx, roverLaserRetroDx, roverLaserRetroSx, roverFrontDistance, roverRetroDistance); frontDistance = roverFrontDistance; retroDistance = roverRetroDistance; float frontVel = 0.05*frontDistance; float retroVel = -0.04*retroDistance; if(enableCurv==1){ frontPos = frontPos + frontVel*0.02; retroPos = retroPos + retroVel*0.02; }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 accBias[0] = 0.0; accBias[1] = 0.0; accBias[2] = 0.0; gyroBias[0] = 0.0; gyroBias[1] = 0.0; gyroBias[2] = 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::setRoverVelocity(float forward_speed, float stabilization_speed, float asset_correction_speed, float pipe_radius, float maxWheelAcceleration){ Eigen::Vector3f cartesianSpeed; Eigen::Vector4f wheelsSpeed; updateRoverKin(pipe_radius, pipeDir); cartesianSpeed << forward_speed, stabilization_speed, asset_correction_speed; wheelsSpeed = S*cartesianSpeed; //std::cout << "des: " << wheelsSpeed.transpose() << std::endl; ionMcFront->setSpeed(1, wheelsSpeed[0],maxWheelAcceleration); ionMcFront->setSpeed(2, wheelsSpeed[1],maxWheelAcceleration); ionMcRetro->setSpeed(1,wheelsSpeed[2],maxWheelAcceleration); ionMcRetro->setSpeed(2,wheelsSpeed[3],maxWheelAcceleration); } void Rover::getRoverVelocity(float &forward_speed, float &stabilization_speed, float &asset_correction_speed, float pipe_radius){ Eigen::Vector3f cartesianSpeed; Eigen::Vector4f wheelsSpeed; float speedM1 = 0.0; float speedM2 = 0.0; float speedM3 = 0.0; float speedM4 = 0.0; updateRoverKin(pipe_radius, pipeDir); getMotorsSpeed(speedM1, speedM2, speedM3, speedM4); wheelsSpeed << speedM1, speedM2, speedM3, speedM4; //td::cout << "mis: " << wheelsSpeed.transpose() << std::endl; cartesianSpeed = S_inv*wheelsSpeed; forward_speed = cartesianSpeed[0]; stabilization_speed = cartesianSpeed[1]; asset_correction_speed = cartesianSpeed[2]; } void Rover::getRoverWheelsVelocity(float &front_dx, float &front_sx, float &retro_dx, float &retro_sx){ Eigen::Vector3f cartesianSpeed; Eigen::Vector4f wheelsSpeed; float speedM1 = 0.0; float speedM2 = 0.0; float speedM3 = 0.0; float speedM4 = 0.0; getMotorsSpeed(speedM1, speedM2, speedM3, speedM4); front_dx = speedM1; front_sx = speedM2; retro_dx = speedM3; retro_sx = speedM4; } void Rover::updateRoverKin(float pipe_radius, int pipeDir){ float rf = r_frontWheel; float rr = r_retroWheel; float l = retroFrontCentralDistance; 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; } //AGGIORNARE la S S << 1.0*R_dx/(rf*R_m), pr/rf, -(L+l)/rf, -1.0*R_sx/(rf*R_m), pr/rf, -(L+l)/rf, 1.0*R_dx/(rr*R_m), -pr/rr, -(L+l)/rr, -1.0*R_sx/(rr*R_m), -pr/rr, -(L+l)/rr; Eigen::FullPivLU<Matrix3f> Core_S(S.transpose()*S); S_inv = Core_S.inverse()*S.transpose(); } void Rover::getMotorsTorque(float &torM1, float &torM2, float &torM3, float &torM4){ torM1 = ionMcFront->getMotorTorque(1); torM2 = ionMcFront->getMotorTorque(2); torM3 = ionMcRetro->getMotorTorque(1); torM4 = ionMcRetro->getMotorTorque(2); } void Rover::getMotorsSpeed(float &speedM1, float &speedM2, float &speedM3, float &speedM4){ speedM1 = ionMcFront->getMotorSpeed(1); speedM2 = ionMcFront->getMotorSpeed(2); speedM3 = ionMcRetro->getMotorSpeed(1); speedM4 = ionMcRetro->getMotorSpeed(2); } void Rover::setCentralJointsAngle(float jointFront, float jointRetro){ float LmRestPosFront = (9.0)/1000; //9 float LmRestPosRetro = (9.0-0.5)/1000; //9 float maxLenght = 0.02; float LmFront = LmRestPosFront - centralJoint2actuonix(jointFront); float LmRetro = LmRestPosRetro - centralJoint2actuonix(jointRetro); frontActuonix->write(1.0 - LmFront/maxLenght); retroActuonix->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; } 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& pitch_vel, float& roll, float dtImu) { int16_t ax = 0; int16_t ay = 0; int16_t az = 0; int16_t gx = 0; int16_t gy = 0; int16_t gz = 0; float pitchAcc = 0.0; float rollAcc = 0.0; float pitchAcc_p = 0.0; float rollAcc_p = 0.0; float pitch_integrated = 0.0; float roll_integrated = 0.0; mpu->getMotion6(ax, ay, az, gx, gy, gz); //acquisisco i dati dal sensore float a_x=float(ax)/FS_a - accBias[0]; //li converto in float, (CALCOLI EVITABILE SE INSTANZIASSI LE VARIABILI SUBITO COME FLOAT) float a_y=float(ay)/FS_a - accBias[1]; float a_z=float(az)/FS_a; float g_x=float(gx)/FS_g - gyroBias[0]; float g_y=float(gy)/FS_g - gyroBias[1]; float g_z=float(gz)/FS_g - gyroBias[2]; if(abs(g_x) > 100.0){ pitch_vel = 0; }else{ pitch_vel = g_x; } /*g_x_old = g_x;*/ //pitch_vel = g_x; 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() { //Imu variables int16_t ax = 0; int16_t ay = 0; int16_t az = 0; int16_t gx = 0; int16_t gy = 0; int16_t gz = 0; float a_x,a_y,a_z; float g_x, g_y, g_z; 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 }