a
Diff: Rover.h
- Revision:
- 0:65943c77d1dc
--- /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