Clone per testare Mbed studio
Dependencies: PwmIn IONMcMotor MPU6050 Eigen ROVER
Fork of Hyfliers_Completo_testato by
Diff: Robots/Rover/Rover.cpp
- Revision:
- 4:3f22193053d0
- Parent:
- 3:fc26045926d9
- Child:
- 6:584653235830
diff -r fc26045926d9 -r 3f22193053d0 Robots/Rover/Rover.cpp --- a/Robots/Rover/Rover.cpp Wed Nov 06 10:57:51 2019 +0000 +++ b/Robots/Rover/Rover.cpp Wed Jan 08 11:05:36 2020 +0000 @@ -3,19 +3,19 @@ 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; + 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_meccanumWheel = 0.03; - r_omniWheel = 0.019; + r_frontWheel = 0.03; + r_retroWheel = 0.03; pipeCurve_I = 0.165; pipeCurve_E = 0.2925; @@ -23,18 +23,18 @@ pipeDir = 0; - omniMeccanumCentralDistance = 0.25; + retroFrontCentralDistance = 0.25; wheelsCntactPointDistanceFromPipeCenter = 0.08; eth_time_out = 2.0; - S = Eigen::Matrix3f::Zero(); + S = Eigen::Matrix<float, 4, 3>::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); + 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); + frontActuonix = new Servo(PE_11); + retroActuonix = new Servo(PE_9); mpu = new MPU6050(PF_15, PF_14); eth_tcp = new Eth_tcp(3154, 500); @@ -72,7 +72,6 @@ acquireTofs(frontDistance, retroDistance); - float frontVel = 0.05*frontDistance; float retroVel = -0.04*retroDistance; @@ -132,16 +131,16 @@ void Rover::setWheelsVelocity(float forward_speed, float stabilization_speed, float asset_correction_speed, float pipe_radius, float maxWheelAcceleration){ Eigen::Vector3f cartesianSpeed; - Eigen::Vector3f wheelsSpeed; + Eigen::Vector4f 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); - + ionMcFront->setSpeed(1, wheelsSpeed[0],maxWheelAcceleration); + ionMcFront->setSpeed(2, wheelsSpeed[1],maxWheelAcceleration); + ionMcRetro->setSpeed(1,wheelsSpeed[2],maxWheelAcceleration); + ionMcRetro->setSpeed(2,wheelsSpeed[3],maxWheelAcceleration); } @@ -179,17 +178,18 @@ void Rover::getRoverVelocity(float &forward_speed, float &stabilization_speed, float &asset_correction_speed, float pipe_radius){ Eigen::Vector3f cartesianSpeed; - Eigen::Vector3f wheelsSpeed; + 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); + getMotorsSpeed(speedM1, speedM2, speedM3, speedM3); - wheelsSpeed << speedM1, speedM2, speedM3; + wheelsSpeed << speedM1, speedM2, speedM3, speedM4; cartesianSpeed = S_inv*wheelsSpeed; @@ -199,28 +199,29 @@ } -void Rover::getRoverWheelsVelocity(float &speed_wheel_dx, float &speed_wheel_sx, float &speed_wheels_retro){ +void Rover::getRoverWheelsVelocity(float &front_dx, float &front_sx, float &retro_dx, float &retro_sx){ Eigen::Vector3f cartesianSpeed; - Eigen::Vector3f wheelsSpeed; + Eigen::Vector4f wheelsSpeed; float speedM1 = 0.0; float speedM2 = 0.0; float speedM3 = 0.0; - - getMotorsSpeed(speedM1, speedM2, speedM3); + float speedM4 = 0.0; + + getMotorsSpeed(speedM1, speedM2, speedM3, speedM4); - speed_wheel_dx = speedM1; - speed_wheel_sx = speedM2; - speed_wheels_retro = speedM3; - + front_dx = speedM1; + front_sx = speedM2; + retro_dx = speedM3; + retro_sx = speedM4; } void Rover::updateRoverKin(float pipe_radius, int pipeDir){ - float rm = r_meccanumWheel; - float ro = r_omniWheel; - float l = omniMeccanumCentralDistance; + float rf = r_frontWheel; + float rr = r_retroWheel; + float l = retroFrontCentralDistance; float L = wheelsCntactPointDistanceFromPipeCenter; float pr = pipe_radius; float R_dx = 0.0; @@ -240,30 +241,33 @@ 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; + //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_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); + + Eigen::FullPivLU<Matrix4f> Core_S(S*S.transpose()); + S_inv = S.transpose()*Core_S.inverse(); + } -void Rover::getMotorsTorque(float &torM1, float &torM2, float &torM3){ +void Rover::getMotorsTorque(float &torM1, float &torM2, float &torM3, float &torM4){ - torM1 = ionMcMeccanum->getMotorTorque(1); - torM2 = ionMcMeccanum->getMotorTorque(2); - torM3 = ionMcOmni->getMotorTorque(1); - + 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){ +void Rover::getMotorsSpeed(float &speedM1, float &speedM2, float &speedM3, float &speedM4){ - speedM1 = ionMcMeccanum->getMotorSpeed(1); - speedM2 = ionMcMeccanum->getMotorSpeed(2); - speedM3 = ionMcOmni->getMotorSpeed(1); + speedM1 = ionMcFront->getMotorSpeed(1); + speedM2 = ionMcFront->getMotorSpeed(2); + speedM3 = ionMcRetro->getMotorSpeed(1); + speedM4 = ionMcRetro->getMotorSpeed(2); } @@ -276,8 +280,8 @@ float LmFront = LmRestPosFront - centralJoint2actuonix(jointFront); float LmRetro = LmRestPosRetro - centralJoint2actuonix(jointRetro); - meccanumActuonix->write(1.0 - LmFront/maxLenght); - omniActuonix->write(1.0 - LmRetro/maxLenght); + frontActuonix->write(1.0 - LmFront/maxLenght); + retroActuonix->write(1.0 - LmRetro/maxLenght); if (jointFront > 3.14*5/180){ pipeDir = -1;