a
old/Rover_omni_h.txt
- 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 "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