a
Rover.h
- Committer:
- marcodesilva
- Date:
- 2021-10-04
- Revision:
- 0:65943c77d1dc
File content as of revision 0:65943c77d1dc:
#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