PRISMA Lab / Mbed OS Hyfliers_Completo_testato

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   PwmIn IONMcMotor MPU6050 Eigen ROVER

Fork of Hyfliers_Completo_testato by Marco De Silva

Robots/Rover/Rover.h@6:584653235830, 2021-09-14 (annotated)

Committer:
anfontanelli
Date:
Tue Sep 14 11:18:35 2021 +0000
Revision:
6:584653235830
Parent:
4:3f22193053d0 
A

Who changed what in which revision?

UserRevisionLine numberNew contents of line
anfontanelli 3:fc26045926d9 1 #ifndef ROVER_H
anfontanelli 3:fc26045926d9 2 #define ROVER_H
anfontanelli 3:fc26045926d9 3 #include "mbed.h"
anfontanelli 6:584653235830 4
anfontanelli 6:584653235830 5 #include "controlDefineVariables.h"
anfontanelli 6:584653235830 6
anfontanelli 3:fc26045926d9 7 #include "IONMcMotors.h"
anfontanelli 6:584653235830 8 #include "MPU6050.h"
anfontanelli 6:584653235830 9 #include "TOFs.h"
anfontanelli 3:fc26045926d9 10 #include "Servo.h"
anfontanelli 3:fc26045926d9 11 #include "Core.h"
anfontanelli 4:3f22193053d0 12 #include <Eigen/Dense.h>
anfontanelli 3:fc26045926d9 13 #include "platform/PlatformMutex.h"
anfontanelli 3:fc26045926d9 14
anfontanelli 4:3f22193053d0 15 typedef Eigen::Matrix<float, 4, 4> Matrix4f;
anfontanelli 6:584653235830 16 typedef Eigen::Matrix<float, 3, 3> Matrix3f;
anfontanelli 3:fc26045926d9 17
anfontanelli 3:fc26045926d9 18 class Rover
anfontanelli 3:fc26045926d9 19 {
anfontanelli 3:fc26045926d9 20 public:
anfontanelli 3:fc26045926d9 21 Rover();
anfontanelli 3:fc26045926d9 22
anfontanelli 6:584653235830 23 void setRoverVelocity(float forward_speed, float stabilization_speed, float asset_correction_speed, float pipe_radius, float maxWheelAcceleration);
anfontanelli 3:fc26045926d9 24 void getRoverVelocity(float &forward_speed, float &stabilization_speed, float &asset_correction_speed, float pipe_radius);
anfontanelli 4:3f22193053d0 25 void getMotorsTorque(float &torM1, float &torM2, float &torM3, float &torM4);
anfontanelli 4:3f22193053d0 26 void getMotorsSpeed(float &speedM1, float &speedM2, float &speedM3, float &speedM4);
anfontanelli 3:fc26045926d9 27
anfontanelli 6:584653235830 28 void setParameters(Eigen::VectorXd roverParameters);
anfontanelli 6:584653235830 29
anfontanelli 4:3f22193053d0 30 void getRoverWheelsVelocity(float &front_dx, float &front_sx, float &retro_dx, float &retro_sx);
anfontanelli 3:fc26045926d9 31
anfontanelli 3:fc26045926d9 32 void setCentralJointsAngle(float act1, float act2);
anfontanelli 6:584653235830 33 void calcImuAngles(float& pitch, float& pitch_vel, float& roll, float dtImu);
anfontanelli 3:fc26045926d9 34
anfontanelli 6:584653235830 35 void computeCentralJointsFromTofs(float &roverLaserFrontDx,float &roverLaserFrontSx,float &roverLaserRetroDx,float &roverLaserRetroSx);
anfontanelli 3:fc26045926d9 36
anfontanelli 6:584653235830 37 float computeStabilizationVel(float pitch_d, float pitch, float pitch_vel, float Kp, float Kd);
anfontanelli 3:fc26045926d9 38
anfontanelli 3:fc26045926d9 39 void initializeImu();
anfontanelli 3:fc26045926d9 40
anfontanelli 3:fc26045926d9 41 void initializeTofs();
anfontanelli 3:fc26045926d9 42
anfontanelli 6:584653235830 43 void acquireTofs(float &roverLaserFrontDx, float &roverLaserFrontSx, float &roverLaserRetroDx, float &roverLaserRetroSx, float &roverFrontDistance, float &roverRetroDistance);
anfontanelli 3:fc26045926d9 44
anfontanelli 6:584653235830 45
anfontanelli 3:fc26045926d9 46
anfontanelli 3:fc26045926d9 47 private:
anfontanelli 3:fc26045926d9 48
anfontanelli 4:3f22193053d0 49 IONMcMotors *ionMcFront;
anfontanelli 4:3f22193053d0 50 IONMcMotors *ionMcRetro;
anfontanelli 3:fc26045926d9 51
anfontanelli 4:3f22193053d0 52 Servo *frontActuonix;
anfontanelli 4:3f22193053d0 53 Servo *retroActuonix;
anfontanelli 3:fc26045926d9 54
anfontanelli 3:fc26045926d9 55 MPU6050 *mpu;
anfontanelli 6:584653235830 56
anfontanelli 6:584653235830 57 TOFs *tofs;
anfontanelli 3:fc26045926d9 58
anfontanelli 4:3f22193053d0 59 float r_frontWheel;
anfontanelli 4:3f22193053d0 60 float r_retroWheel;
anfontanelli 6:584653235830 61 float pipeCurve_I;
anfontanelli 6:584653235830 62 float pipeCurve_E;
anfontanelli 6:584653235830 63 float pipeCurve_M;
anfontanelli 6:584653235830 64 float retroFrontCentralDistance;
anfontanelli 3:fc26045926d9 65 float wheelsCntactPointDistanceFromPipeCenter;
anfontanelli 6:584653235830 66
anfontanelli 3:fc26045926d9 67 //Imu variables
anfontanelli 6:584653235830 68
anfontanelli 3:fc26045926d9 69 float accBias[3];
anfontanelli 3:fc26045926d9 70 float gyroBias[3];
anfontanelli 6:584653235830 71 int pitch_d;
anfontanelli 6:584653235830 72 float pitch;
anfontanelli 6:584653235830 73 float g_x_old;
anfontanelli 4:3f22193053d0 74
anfontanelli 3:fc26045926d9 75 int forward_vel;
anfontanelli 3:fc26045926d9 76
anfontanelli 3:fc26045926d9 77 int jointFront;
anfontanelli 3:fc26045926d9 78 int jointRetro;
anfontanelli 3:fc26045926d9 79 int enableStab;
anfontanelli 3:fc26045926d9 80 int enableCurv;
anfontanelli 3:fc26045926d9 81
anfontanelli 3:fc26045926d9 82 float time;
anfontanelli 3:fc26045926d9 83 float vels_a;
anfontanelli 3:fc26045926d9 84 float vels_m;
anfontanelli 6:584653235830 85
anfontanelli 3:fc26045926d9 86 int pipeDir;
anfontanelli 6:584653235830 87
anfontanelli 4:3f22193053d0 88 Eigen::Matrix<float, 4, 3> S;
anfontanelli 4:3f22193053d0 89 Eigen::Matrix<float, 3, 4> S_inv;
anfontanelli 6:584653235830 90
anfontanelli 6:584653235830 91 float frontPos;
anfontanelli 6:584653235830 92 float retroPos;
anfontanelli 6:584653235830 93
anfontanelli 3:fc26045926d9 94 //private functions
anfontanelli 3:fc26045926d9 95 float deg2rad(float deg);
anfontanelli 3:fc26045926d9 96 float centralJoint2actuonix(float jointAngle);
anfontanelli 3:fc26045926d9 97 void calibrateImu();
anfontanelli 3:fc26045926d9 98 void updateRoverKin(float pipe_radius, int pipeDir);
anfontanelli 3:fc26045926d9 99
anfontanelli 3:fc26045926d9 100 void setState(int state);
anfontanelli 4:3f22193053d0 101
anfontanelli 6:584653235830 102 int get_forward_vel();
anfontanelli 6:584653235830 103 //PlatformMutex eth_mutex;
anfontanelli 3:fc26045926d9 104
anfontanelli 6:584653235830 105 float roverFrontDistance;
anfontanelli 6:584653235830 106 float roverRetroDistance;
anfontanelli 3:fc26045926d9 107
anfontanelli 3:fc26045926d9 108 };
anfontanelli 3:fc26045926d9 109
anfontanelli 3:fc26045926d9 110 #endif