Marco De Silva / ROVER

a

Rover.h@0:65943c77d1dc, 2021-10-04 (annotated)

Committer:
marcodesilva
Date:
Mon Oct 04 12:52:17 2021 +0000
Revision:
0:65943c77d1dc
a;

Who changed what in which revision?

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