a
Diff: old/Rover_omni_h.txt
- Revision:
- 0:65943c77d1dc
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/old/Rover_omni_h.txt Mon Oct 04 12:52:17 2021 +0000 @@ -0,0 +1,172 @@ +#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 \ No newline at end of file