Marco De Silva
a
Revision 0:65943c77d1dc, committed 2021-10-04
- Comitter:
- marcodesilva
- Date:
- Mon Oct 04 12:52:17 2021 +0000
- Commit message:
- a;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Rover.cpp Mon Oct 04 12:52:17 2021 +0000
@@ -0,0 +1,397 @@
+#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
+
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Rover.h Mon Oct 04 12:52:17 2021 +0000
@@ -0,0 +1,110 @@
+#ifndef ROVER_H
+#define ROVER_H
+#include "mbed.h"
+
+#include "controlDefineVariables.h"
+
+#include "IONMcMotors.h"
+#include "MPU6050.h"
+#include "TOFs.h"
+#include "Servo.h"
+#include "Core.h"
+#include <Eigen/Dense.h>
+#include "platform/PlatformMutex.h"
+
+typedef Eigen::Matrix<float, 4, 4> Matrix4f;
+typedef Eigen::Matrix<float, 3, 3> Matrix3f;
+
+class Rover
+{
+public:
+ Rover();
+
+ void setRoverVelocity(float forward_speed, float stabilization_speed, float asset_correction_speed, float pipe_radius, float maxWheelAcceleration);
+ void getRoverVelocity(float &forward_speed, float &stabilization_speed, float &asset_correction_speed, float pipe_radius);
+ void getMotorsTorque(float &torM1, float &torM2, float &torM3, float &torM4);
+ void getMotorsSpeed(float &speedM1, float &speedM2, float &speedM3, float &speedM4);
+
+ void setParameters(Eigen::VectorXd roverParameters);
+
+ void getRoverWheelsVelocity(float &front_dx, float &front_sx, float &retro_dx, float &retro_sx);
+
+ void setCentralJointsAngle(float act1, float act2);
+ void calcImuAngles(float& pitch, float& pitch_vel, float& roll, float dtImu);
+
+ void computeCentralJointsFromTofs(float &roverLaserFrontDx,float &roverLaserFrontSx,float &roverLaserRetroDx,float &roverLaserRetroSx);
+
+ float computeStabilizationVel(float pitch_d, float pitch, float pitch_vel, float Kp, float Kd);
+
+ void initializeImu();
+
+ void initializeTofs();
+
+ void acquireTofs(float &roverLaserFrontDx, float &roverLaserFrontSx, float &roverLaserRetroDx, float &roverLaserRetroSx, float &roverFrontDistance, float &roverRetroDistance);
+
+
+
+private:
+
+ IONMcMotors *ionMcFront;
+ IONMcMotors *ionMcRetro;
+
+ Servo *frontActuonix;
+ Servo *retroActuonix;
+
+ MPU6050 *mpu;
+
+ TOFs *tofs;
+
+ float r_frontWheel;
+ float r_retroWheel;
+ float pipeCurve_I;
+ float pipeCurve_E;
+ float pipeCurve_M;
+ float retroFrontCentralDistance;
+ float wheelsCntactPointDistanceFromPipeCenter;
+
+ //Imu variables
+
+ float accBias[3];
+ float gyroBias[3];
+ int pitch_d;
+ float pitch;
+ float g_x_old;
+
+ int forward_vel;
+
+ int jointFront;
+ int jointRetro;
+ int enableStab;
+ int enableCurv;
+
+ float time;
+ float vels_a;
+ float vels_m;
+
+ int pipeDir;
+
+ Eigen::Matrix<float, 4, 3> S;
+ Eigen::Matrix<float, 3, 4> S_inv;
+
+ float frontPos;
+ float retroPos;
+
+ //private functions
+ float deg2rad(float deg);
+ float centralJoint2actuonix(float jointAngle);
+ void calibrateImu();
+ void updateRoverKin(float pipe_radius, int pipeDir);
+
+ void setState(int state);
+
+ int get_forward_vel();
+ //PlatformMutex eth_mutex;
+
+ float roverFrontDistance;
+ float roverRetroDistance;
+
+};
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/old/Rover_omni_cpp.txt Mon Oct 04 12:52:17 2021 +0000
@@ -0,0 +1,487 @@
+#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();
+
+}
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/old/Rover_omni_h.txt Mon Oct 04 12:52:17 2021 +0000
@@ -0,0 +1,172 @@
+#ifndef ROVER_H
+#define ROVER_H
+#include "mbed.h"
+#include "IONMcMotors.h"
+#include "Servo.h"
+#include "Core.h"
+#include "MPU6050.h"
+#include "platform/PlatformMutex.h"
+
+#include "TOFs.h"
+
+#include "Eth_tcp.h"
+
+
+class Rover
+{
+public:
+ Rover();
+
+ void setWheelsVelocity(float forward_speed, float stabilization_speed, float asset_correction_speed, float pipe_radius, float maxWheelAcceleration);
+ void getRoverVelocity(float &forward_speed, float &stabilization_speed, float &asset_correction_speed, float pipe_radius);
+ void getMotorsTorque(float &torM1, float &torM2, float &torM3);
+ void getMotorsSpeed(float &speedM1, float &speedM2, float &speedM3);
+
+ void getRoverWheelsVelocity(float &speed_wheel_dx, float &speed_wheel_sx, float &speed_wheels_retro);
+
+
+ void setCentralJointsAngle(float act1, float act2);
+ void calcImuAngles(float& pitch, float& roll, float dtImu);
+
+ int get_forward_vel();
+ int get_pitch();
+ int get_jointFront();
+ int get_jointRetro();
+
+ void ethComunicationUpdate(float _time, float _pitch, float _vels_a, float _vels_m, float _velf_a, float _velf_m) ;
+
+ int getEthState(void);
+
+
+ void startEthComunication();
+
+ void initializeImu();
+
+ void initializeTofs();
+ void acquireTofs(float &frontDistance, float &retroDistance);
+
+ void computeCentralJointsFromTofs();
+
+
+
+private:
+
+ IONMcMotors *ionMcMeccanum;
+ IONMcMotors *ionMcOmni;
+
+ Servo *meccanumActuonix;
+ Servo *omniActuonix;
+
+ MPU6050 *mpu;
+
+ Eth_tcp *eth_tcp;
+
+ Thread EthThread;
+
+ distanceSensors *distance_sensors;
+
+ int ionMcBoudRate;
+
+ int meccanumMotorGearBoxRatio;
+ int omniMotorGearBoxRatio;
+
+ int meccanumBoardAddress;
+ int omniBoardAddress;
+
+ float meccanumTransmissionRatio;
+ float omniTransmissionRatio;
+
+ int meccanumEncoderPulse;
+ int omniEncoderPulse;
+
+ float meccanumKt;
+ float omniKt;
+
+ float r_meccanumWheel;
+ float r_omniWheel;
+
+ float wheelsCntactPointDistanceFromPipeCenter;
+ float omniMeccanumCentralDistance;
+
+ //Imu variables
+ int16_t ax, ay, az;
+ int16_t gx, gy, gz;
+
+ float a_x,a_y,a_z;
+ float g_x, g_y, g_z;
+
+ int16_t FS_a;
+ float FS_g;
+
+ PlatformMutex eth_mutex;
+
+
+ float accBias[3];
+ float gyroBias[3];
+
+
+
+ float pitchAcc;
+ float rollAcc;
+ float pitchAcc_p;
+ float rollAcc_p;
+ float pitch_integrated;
+ float roll_integrated;
+
+
+ int forward_vel;
+ int pitch_d;
+
+ int jointFront;
+ int jointRetro;
+ int enableStab;
+ int enableCurv;
+
+ float time;
+ float pitch;
+ float vels_a;
+ float vels_m;
+
+ float pipeCurve_I;
+ float pipeCurve_E;
+ float pipeCurve_M;
+ int pipeDir;
+
+ double eth_time_out; //ms
+
+ int eth_state;
+ double eth_time;
+ double eth_time_sample_received;
+
+ Eigen::Matrix3f S;
+ Eigen::Matrix3f S_inv;
+ //private functions
+ float deg2rad(float deg);
+ float centralJoint2actuonix(float jointAngle);
+ void calibrateImu();
+ void updateRoverKin(float pipe_radius, int pipeDir);
+
+ void setState(int state);
+
+
+ Timer comunicationTimer;
+
+ char cmd;
+ MyBuffer <int> vec_to_send;
+ MyBuffer <int> recv_vec;
+ double t_p;
+ double t;
+ int n_of_int;
+ bool eth_status;
+
+ bool sock_open;
+
+
+
+ float frontPos;
+ float retroPos;
+
+
+};
+
+#endif
\ No newline at end of file