Clone per testare Mbed studio
Dependencies: PwmIn IONMcMotor MPU6050 Eigen ROVER
Fork of Hyfliers_Completo_testato by
Diff: Robots/Rover/Rover.cpp
- Revision:
- 6:584653235830
- Parent:
- 4:3f22193053d0
--- a/Robots/Rover/Rover.cpp Wed Jan 08 11:12:51 2020 +0000 +++ b/Robots/Rover/Rover.cpp Tue Sep 14 11:18:35 2021 +0000 @@ -1,84 +1,84 @@ #include "Rover.h" +#include <iostream> Rover::Rover(){ - ionMcBoudRate = 460800; - frontBoardAddress = 128; - frontMotorGearBoxRatio = 103; - frontEncoderPulse = 512; - frontKt = 0.00667; - frontTransmissionRatio = 1.0; - retroTransmissionRatio = 1.0; - retroBoardAddress = 129; - retroMotorGearBoxRatio = 103; - retroEncoderPulse = 512; - retroKt = 0.00667; - - r_frontWheel = 0.03; - r_retroWheel = 0.03; - - pipeCurve_I = 0.165; - pipeCurve_E = 0.2925; - pipeCurve_M = 0.228; - pipeDir = 0; - - retroFrontCentralDistance = 0.25; - wheelsCntactPointDistanceFromPipeCenter = 0.08; - - eth_time_out = 2.0; - + S = Eigen::Matrix<float, 4, 3>::Zero(); - - ionMcFront = new IONMcMotors(frontBoardAddress,ionMcBoudRate, PG_14, PG_9, frontMotorGearBoxRatio, frontEncoderPulse, frontKt, frontKt); - ionMcRetro = new IONMcMotors(retroBoardAddress,ionMcBoudRate, PB_9, PB_8, retroMotorGearBoxRatio, retroEncoderPulse, retroKt, retroKt); + 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); - eth_tcp = new Eth_tcp(3154, 500); - distance_sensors = new distanceSensors(); - - - eth_state = -1; - - eth_status = false; - + tofs = new TOFs(); + + enableCurv = 1; + g_x_old = 0.0; } void Rover::initializeTofs(){ - distance_sensors->TOFs_init(); - distance_sensors->TOFs_offset(); -} + + tofs->TOFs_init(); + ThisThread::sleep_for(1000); + //tofs->TOFs_offset(); + -void Rover::acquireTofs(float &frontDistance, float &retroDistance){ - - distance_sensors->TOFs_acquireFiltr(); - frontDistance = distance_sensors->getFrontDistance(); - retroDistance = distance_sensors->getRetroDistance(); - +} - +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); + } -void Rover::computeCentralJointsFromTofs(){ +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; + float retroDistance; - acquireTofs(frontDistance, 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.005; - retroPos = retroPos + retroVel*0.005; + frontPos = frontPos + frontVel*0.02; + retroPos = retroPos + retroVel*0.02; }else{ frontPos = 0.0; retroPos = 0.0; @@ -98,24 +98,14 @@ 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) { @@ -128,7 +118,7 @@ calibrateImu(); } -void Rover::setWheelsVelocity(float forward_speed, float stabilization_speed, float asset_correction_speed, float pipe_radius, float maxWheelAcceleration){ +void Rover::setRoverVelocity(float forward_speed, float stabilization_speed, float asset_correction_speed, float pipe_radius, float maxWheelAcceleration){ Eigen::Vector3f cartesianSpeed; Eigen::Vector4f wheelsSpeed; @@ -136,44 +126,19 @@ updateRoverKin(pipe_radius, pipeDir); cartesianSpeed << forward_speed, stabilization_speed, asset_correction_speed; - wheelsSpeed = S*cartesianSpeed; + 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); + } -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){ @@ -187,10 +152,13 @@ updateRoverKin(pipe_radius, pipeDir); - getMotorsSpeed(speedM1, speedM2, speedM3, speedM3); + 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]; @@ -242,14 +210,14 @@ } //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; + 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<Matrix4f> Core_S(S*S.transpose()); - S_inv = S.transpose()*Core_S.inverse(); + Eigen::FullPivLU<Matrix3f> Core_S(S.transpose()*S); + S_inv = Core_S.inverse()*S.transpose(); } @@ -273,7 +241,7 @@ void Rover::setCentralJointsAngle(float jointFront, float jointRetro){ - float LmRestPosFront = (9.0+0.5)/1000; //9 + float LmRestPosFront = (9.0)/1000; //9 float LmRestPosRetro = (9.0-0.5)/1000; //9 float maxLenght = 0.02; @@ -290,6 +258,8 @@ }else{ pipeDir = 0; } + + pipeDir = 0;///////////////////////////////////////////////////////////// } @@ -316,57 +286,87 @@ -void Rover::calcImuAngles(float& pitch, float& roll, float dtImu) +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; - 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); - } + + //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"); - + 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 + 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; @@ -379,113 +379,19 @@ 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); + 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::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(); - -} - -