![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Clone per testare Mbed studio
Dependencies: PwmIn IONMcMotor MPU6050 Eigen ROVER
Fork of Hyfliers_Completo_testato by
Robots/Rover/Rover_omni_cpp.txt@4:3f22193053d0, 2020-01-08 (annotated)
- Committer:
- anfontanelli
- Date:
- Wed Jan 08 11:05:36 2020 +0000
- Revision:
- 4:3f22193053d0
- Parent:
- Robots/Rover/Rover.cpp@3:fc26045926d9
release bad
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
anfontanelli | 3:fc26045926d9 | 1 | #include "Rover.h" |
anfontanelli | 3:fc26045926d9 | 2 | |
anfontanelli | 3:fc26045926d9 | 3 | Rover::Rover(){ |
anfontanelli | 3:fc26045926d9 | 4 | |
anfontanelli | 3:fc26045926d9 | 5 | ionMcBoudRate = 460800; |
anfontanelli | 3:fc26045926d9 | 6 | meccanumBoardAddress = 128; |
anfontanelli | 3:fc26045926d9 | 7 | meccanumMotorGearBoxRatio = 103; |
anfontanelli | 3:fc26045926d9 | 8 | meccanumEncoderPulse = 512; |
anfontanelli | 3:fc26045926d9 | 9 | meccanumKt = 0.00667; |
anfontanelli | 3:fc26045926d9 | 10 | meccanumTransmissionRatio = 1.0; |
anfontanelli | 3:fc26045926d9 | 11 | omniTransmissionRatio = 1.0; |
anfontanelli | 3:fc26045926d9 | 12 | omniBoardAddress = 129; |
anfontanelli | 3:fc26045926d9 | 13 | omniMotorGearBoxRatio = 103; |
anfontanelli | 3:fc26045926d9 | 14 | omniEncoderPulse = 512; |
anfontanelli | 3:fc26045926d9 | 15 | omniKt = 0.00667; |
anfontanelli | 3:fc26045926d9 | 16 | |
anfontanelli | 3:fc26045926d9 | 17 | r_meccanumWheel = 0.03; |
anfontanelli | 3:fc26045926d9 | 18 | r_omniWheel = 0.019; |
anfontanelli | 3:fc26045926d9 | 19 | |
anfontanelli | 3:fc26045926d9 | 20 | pipeCurve_I = 0.165; |
anfontanelli | 3:fc26045926d9 | 21 | pipeCurve_E = 0.2925; |
anfontanelli | 3:fc26045926d9 | 22 | pipeCurve_M = 0.228; |
anfontanelli | 3:fc26045926d9 | 23 | |
anfontanelli | 3:fc26045926d9 | 24 | pipeDir = 0; |
anfontanelli | 3:fc26045926d9 | 25 | |
anfontanelli | 3:fc26045926d9 | 26 | omniMeccanumCentralDistance = 0.25; |
anfontanelli | 3:fc26045926d9 | 27 | wheelsCntactPointDistanceFromPipeCenter = 0.08; |
anfontanelli | 3:fc26045926d9 | 28 | |
anfontanelli | 3:fc26045926d9 | 29 | eth_time_out = 2.0; |
anfontanelli | 3:fc26045926d9 | 30 | |
anfontanelli | 3:fc26045926d9 | 31 | S = Eigen::Matrix3f::Zero(); |
anfontanelli | 3:fc26045926d9 | 32 | |
anfontanelli | 3:fc26045926d9 | 33 | |
anfontanelli | 3:fc26045926d9 | 34 | ionMcMeccanum = new IONMcMotors(meccanumBoardAddress,ionMcBoudRate, PG_14, PG_9, meccanumMotorGearBoxRatio, meccanumEncoderPulse, meccanumKt, meccanumKt); |
anfontanelli | 3:fc26045926d9 | 35 | ionMcOmni = new IONMcMotors(omniBoardAddress,ionMcBoudRate, PB_9, PB_8, omniMotorGearBoxRatio, omniEncoderPulse, omniKt, omniKt); |
anfontanelli | 3:fc26045926d9 | 36 | meccanumActuonix = new Servo(PE_11); |
anfontanelli | 3:fc26045926d9 | 37 | omniActuonix = new Servo(PE_9); |
anfontanelli | 3:fc26045926d9 | 38 | mpu = new MPU6050(PF_15, PF_14); |
anfontanelli | 3:fc26045926d9 | 39 | eth_tcp = new Eth_tcp(3154, 500); |
anfontanelli | 3:fc26045926d9 | 40 | |
anfontanelli | 3:fc26045926d9 | 41 | distance_sensors = new distanceSensors(); |
anfontanelli | 3:fc26045926d9 | 42 | |
anfontanelli | 3:fc26045926d9 | 43 | |
anfontanelli | 3:fc26045926d9 | 44 | eth_state = -1; |
anfontanelli | 3:fc26045926d9 | 45 | |
anfontanelli | 3:fc26045926d9 | 46 | eth_status = false; |
anfontanelli | 3:fc26045926d9 | 47 | |
anfontanelli | 3:fc26045926d9 | 48 | |
anfontanelli | 3:fc26045926d9 | 49 | } |
anfontanelli | 3:fc26045926d9 | 50 | |
anfontanelli | 3:fc26045926d9 | 51 | void Rover::initializeTofs(){ |
anfontanelli | 3:fc26045926d9 | 52 | distance_sensors->TOFs_init(); |
anfontanelli | 3:fc26045926d9 | 53 | distance_sensors->TOFs_offset(); |
anfontanelli | 3:fc26045926d9 | 54 | } |
anfontanelli | 3:fc26045926d9 | 55 | |
anfontanelli | 3:fc26045926d9 | 56 | void Rover::acquireTofs(float &frontDistance, float &retroDistance){ |
anfontanelli | 3:fc26045926d9 | 57 | |
anfontanelli | 3:fc26045926d9 | 58 | distance_sensors->TOFs_acquireFiltr(); |
anfontanelli | 3:fc26045926d9 | 59 | frontDistance = distance_sensors->getFrontDistance(); |
anfontanelli | 3:fc26045926d9 | 60 | retroDistance = distance_sensors->getRetroDistance(); |
anfontanelli | 3:fc26045926d9 | 61 | |
anfontanelli | 3:fc26045926d9 | 62 | |
anfontanelli | 3:fc26045926d9 | 63 | |
anfontanelli | 3:fc26045926d9 | 64 | } |
anfontanelli | 3:fc26045926d9 | 65 | |
anfontanelli | 3:fc26045926d9 | 66 | |
anfontanelli | 3:fc26045926d9 | 67 | void Rover::computeCentralJointsFromTofs(){ |
anfontanelli | 3:fc26045926d9 | 68 | |
anfontanelli | 3:fc26045926d9 | 69 | float frontDistance; |
anfontanelli | 3:fc26045926d9 | 70 | float retroDistance; |
anfontanelli | 3:fc26045926d9 | 71 | |
anfontanelli | 3:fc26045926d9 | 72 | acquireTofs(frontDistance, retroDistance); |
anfontanelli | 3:fc26045926d9 | 73 | |
anfontanelli | 3:fc26045926d9 | 74 | |
anfontanelli | 3:fc26045926d9 | 75 | |
anfontanelli | 3:fc26045926d9 | 76 | float frontVel = 0.05*frontDistance; |
anfontanelli | 3:fc26045926d9 | 77 | float retroVel = -0.04*retroDistance; |
anfontanelli | 3:fc26045926d9 | 78 | |
anfontanelli | 3:fc26045926d9 | 79 | |
anfontanelli | 3:fc26045926d9 | 80 | if(enableCurv==1){ |
anfontanelli | 3:fc26045926d9 | 81 | frontPos = frontPos + frontVel*0.005; |
anfontanelli | 3:fc26045926d9 | 82 | retroPos = retroPos + retroVel*0.005; |
anfontanelli | 3:fc26045926d9 | 83 | }else{ |
anfontanelli | 3:fc26045926d9 | 84 | frontPos = 0.0; |
anfontanelli | 3:fc26045926d9 | 85 | retroPos = 0.0; |
anfontanelli | 3:fc26045926d9 | 86 | } |
anfontanelli | 3:fc26045926d9 | 87 | |
anfontanelli | 3:fc26045926d9 | 88 | if(frontPos > 0.5) frontPos=0.5; |
anfontanelli | 3:fc26045926d9 | 89 | if(frontPos < -0.5) frontPos=-0.5; |
anfontanelli | 3:fc26045926d9 | 90 | if(retroPos > 0.5) retroPos=0.5; |
anfontanelli | 3:fc26045926d9 | 91 | if(retroPos < -0.5) retroPos=-0.5; |
anfontanelli | 3:fc26045926d9 | 92 | |
anfontanelli | 3:fc26045926d9 | 93 | setCentralJointsAngle(frontPos,retroPos); |
anfontanelli | 3:fc26045926d9 | 94 | |
anfontanelli | 3:fc26045926d9 | 95 | } |
anfontanelli | 3:fc26045926d9 | 96 | |
anfontanelli | 3:fc26045926d9 | 97 | |
anfontanelli | 3:fc26045926d9 | 98 | void Rover::initializeImu(){ |
anfontanelli | 3:fc26045926d9 | 99 | |
anfontanelli | 3:fc26045926d9 | 100 | printf("Initialize IMU \r\n"); |
anfontanelli | 3:fc26045926d9 | 101 | //IMU |
anfontanelli | 3:fc26045926d9 | 102 | FS_a = 8192; //per avere letture in g, sarebbe 32768/4 perchè 4g è il fondo scala |
anfontanelli | 3:fc26045926d9 | 103 | FS_g = 131.072; //per avere letture in gradi/s, sarebbe 32768/250 perchè 250 è il fondo scala |
anfontanelli | 3:fc26045926d9 | 104 | |
anfontanelli | 3:fc26045926d9 | 105 | |
anfontanelli | 3:fc26045926d9 | 106 | accBias[0] = 0.0; |
anfontanelli | 3:fc26045926d9 | 107 | accBias[1] = 0.0; |
anfontanelli | 3:fc26045926d9 | 108 | accBias[2] = 0.0; |
anfontanelli | 3:fc26045926d9 | 109 | gyroBias[0] = 0.0; |
anfontanelli | 3:fc26045926d9 | 110 | gyroBias[1] = 0.0; |
anfontanelli | 3:fc26045926d9 | 111 | gyroBias[2] = 0.0; |
anfontanelli | 3:fc26045926d9 | 112 | |
anfontanelli | 3:fc26045926d9 | 113 | |
anfontanelli | 3:fc26045926d9 | 114 | pitchAcc = 0.0; |
anfontanelli | 3:fc26045926d9 | 115 | rollAcc = 0.0; |
anfontanelli | 3:fc26045926d9 | 116 | |
anfontanelli | 3:fc26045926d9 | 117 | pitch_integrated = 0.0; |
anfontanelli | 3:fc26045926d9 | 118 | roll_integrated = 0.0; |
anfontanelli | 3:fc26045926d9 | 119 | |
anfontanelli | 3:fc26045926d9 | 120 | mpu->initialize(); |
anfontanelli | 3:fc26045926d9 | 121 | bool mpu6050TestResult = mpu->testConnection(); |
anfontanelli | 3:fc26045926d9 | 122 | if(mpu6050TestResult) { |
anfontanelli | 3:fc26045926d9 | 123 | printf("MPU6050 test passed \r\n"); |
anfontanelli | 3:fc26045926d9 | 124 | } else { |
anfontanelli | 3:fc26045926d9 | 125 | printf("MPU6050 test failed \r\n"); |
anfontanelli | 3:fc26045926d9 | 126 | } |
anfontanelli | 3:fc26045926d9 | 127 | |
anfontanelli | 3:fc26045926d9 | 128 | |
anfontanelli | 3:fc26045926d9 | 129 | calibrateImu(); |
anfontanelli | 3:fc26045926d9 | 130 | } |
anfontanelli | 3:fc26045926d9 | 131 | |
anfontanelli | 3:fc26045926d9 | 132 | void Rover::setWheelsVelocity(float forward_speed, float stabilization_speed, float asset_correction_speed, float pipe_radius, float maxWheelAcceleration){ |
anfontanelli | 3:fc26045926d9 | 133 | |
anfontanelli | 3:fc26045926d9 | 134 | Eigen::Vector3f cartesianSpeed; |
anfontanelli | 3:fc26045926d9 | 135 | Eigen::Vector3f wheelsSpeed; |
anfontanelli | 3:fc26045926d9 | 136 | |
anfontanelli | 3:fc26045926d9 | 137 | updateRoverKin(pipe_radius, pipeDir); |
anfontanelli | 3:fc26045926d9 | 138 | |
anfontanelli | 3:fc26045926d9 | 139 | cartesianSpeed << forward_speed, stabilization_speed, asset_correction_speed; |
anfontanelli | 3:fc26045926d9 | 140 | wheelsSpeed = S*cartesianSpeed; |
anfontanelli | 3:fc26045926d9 | 141 | ionMcMeccanum->setSpeed(1, wheelsSpeed[0],maxWheelAcceleration); |
anfontanelli | 3:fc26045926d9 | 142 | ionMcMeccanum->setSpeed(2, wheelsSpeed[1],maxWheelAcceleration); |
anfontanelli | 3:fc26045926d9 | 143 | ionMcOmni->setSpeed(1,wheelsSpeed[2],maxWheelAcceleration); |
anfontanelli | 3:fc26045926d9 | 144 | |
anfontanelli | 3:fc26045926d9 | 145 | } |
anfontanelli | 3:fc26045926d9 | 146 | |
anfontanelli | 3:fc26045926d9 | 147 | |
anfontanelli | 3:fc26045926d9 | 148 | int Rover::get_forward_vel(){ |
anfontanelli | 3:fc26045926d9 | 149 | int fv; |
anfontanelli | 3:fc26045926d9 | 150 | eth_mutex.lock(); |
anfontanelli | 3:fc26045926d9 | 151 | fv = forward_vel; |
anfontanelli | 3:fc26045926d9 | 152 | eth_mutex.unlock(); |
anfontanelli | 3:fc26045926d9 | 153 | return fv; |
anfontanelli | 3:fc26045926d9 | 154 | |
anfontanelli | 3:fc26045926d9 | 155 | } |
anfontanelli | 3:fc26045926d9 | 156 | int Rover::get_pitch(){ |
anfontanelli | 3:fc26045926d9 | 157 | int pit; |
anfontanelli | 3:fc26045926d9 | 158 | eth_mutex.lock(); |
anfontanelli | 3:fc26045926d9 | 159 | pit = pitch_d; |
anfontanelli | 3:fc26045926d9 | 160 | eth_mutex.unlock(); |
anfontanelli | 3:fc26045926d9 | 161 | return pit; |
anfontanelli | 3:fc26045926d9 | 162 | } |
anfontanelli | 3:fc26045926d9 | 163 | |
anfontanelli | 3:fc26045926d9 | 164 | int Rover::get_jointFront(){ |
anfontanelli | 3:fc26045926d9 | 165 | int jf; |
anfontanelli | 3:fc26045926d9 | 166 | eth_mutex.lock(); |
anfontanelli | 3:fc26045926d9 | 167 | jf = jointFront; |
anfontanelli | 3:fc26045926d9 | 168 | eth_mutex.unlock(); |
anfontanelli | 3:fc26045926d9 | 169 | return jf; |
anfontanelli | 3:fc26045926d9 | 170 | } |
anfontanelli | 3:fc26045926d9 | 171 | int Rover::get_jointRetro(){ |
anfontanelli | 3:fc26045926d9 | 172 | int jr; |
anfontanelli | 3:fc26045926d9 | 173 | eth_mutex.lock(); |
anfontanelli | 3:fc26045926d9 | 174 | jr = jointRetro; |
anfontanelli | 3:fc26045926d9 | 175 | eth_mutex.unlock(); |
anfontanelli | 3:fc26045926d9 | 176 | return jr; |
anfontanelli | 3:fc26045926d9 | 177 | } |
anfontanelli | 3:fc26045926d9 | 178 | |
anfontanelli | 3:fc26045926d9 | 179 | void Rover::getRoverVelocity(float &forward_speed, float &stabilization_speed, float &asset_correction_speed, float pipe_radius){ |
anfontanelli | 3:fc26045926d9 | 180 | |
anfontanelli | 3:fc26045926d9 | 181 | Eigen::Vector3f cartesianSpeed; |
anfontanelli | 3:fc26045926d9 | 182 | Eigen::Vector3f wheelsSpeed; |
anfontanelli | 3:fc26045926d9 | 183 | |
anfontanelli | 3:fc26045926d9 | 184 | float speedM1 = 0.0; |
anfontanelli | 3:fc26045926d9 | 185 | float speedM2 = 0.0; |
anfontanelli | 3:fc26045926d9 | 186 | float speedM3 = 0.0; |
anfontanelli | 3:fc26045926d9 | 187 | |
anfontanelli | 3:fc26045926d9 | 188 | updateRoverKin(pipe_radius, pipeDir); |
anfontanelli | 3:fc26045926d9 | 189 | |
anfontanelli | 3:fc26045926d9 | 190 | getMotorsSpeed(speedM1, speedM2, speedM3); |
anfontanelli | 3:fc26045926d9 | 191 | |
anfontanelli | 3:fc26045926d9 | 192 | wheelsSpeed << speedM1, speedM2, speedM3; |
anfontanelli | 3:fc26045926d9 | 193 | |
anfontanelli | 3:fc26045926d9 | 194 | cartesianSpeed = S_inv*wheelsSpeed; |
anfontanelli | 3:fc26045926d9 | 195 | |
anfontanelli | 3:fc26045926d9 | 196 | forward_speed = cartesianSpeed[0]; |
anfontanelli | 3:fc26045926d9 | 197 | stabilization_speed = cartesianSpeed[1]; |
anfontanelli | 3:fc26045926d9 | 198 | asset_correction_speed = cartesianSpeed[2]; |
anfontanelli | 3:fc26045926d9 | 199 | |
anfontanelli | 3:fc26045926d9 | 200 | } |
anfontanelli | 3:fc26045926d9 | 201 | |
anfontanelli | 3:fc26045926d9 | 202 | void Rover::getRoverWheelsVelocity(float &speed_wheel_dx, float &speed_wheel_sx, float &speed_wheels_retro){ |
anfontanelli | 3:fc26045926d9 | 203 | |
anfontanelli | 3:fc26045926d9 | 204 | Eigen::Vector3f cartesianSpeed; |
anfontanelli | 3:fc26045926d9 | 205 | Eigen::Vector3f wheelsSpeed; |
anfontanelli | 3:fc26045926d9 | 206 | |
anfontanelli | 3:fc26045926d9 | 207 | float speedM1 = 0.0; |
anfontanelli | 3:fc26045926d9 | 208 | float speedM2 = 0.0; |
anfontanelli | 3:fc26045926d9 | 209 | float speedM3 = 0.0; |
anfontanelli | 3:fc26045926d9 | 210 | |
anfontanelli | 3:fc26045926d9 | 211 | getMotorsSpeed(speedM1, speedM2, speedM3); |
anfontanelli | 3:fc26045926d9 | 212 | |
anfontanelli | 3:fc26045926d9 | 213 | speed_wheel_dx = speedM1; |
anfontanelli | 3:fc26045926d9 | 214 | speed_wheel_sx = speedM2; |
anfontanelli | 3:fc26045926d9 | 215 | speed_wheels_retro = speedM3; |
anfontanelli | 3:fc26045926d9 | 216 | |
anfontanelli | 3:fc26045926d9 | 217 | |
anfontanelli | 3:fc26045926d9 | 218 | } |
anfontanelli | 3:fc26045926d9 | 219 | |
anfontanelli | 3:fc26045926d9 | 220 | void Rover::updateRoverKin(float pipe_radius, int pipeDir){ |
anfontanelli | 3:fc26045926d9 | 221 | float rm = r_meccanumWheel; |
anfontanelli | 3:fc26045926d9 | 222 | float ro = r_omniWheel; |
anfontanelli | 3:fc26045926d9 | 223 | float l = omniMeccanumCentralDistance; |
anfontanelli | 3:fc26045926d9 | 224 | float L = wheelsCntactPointDistanceFromPipeCenter; |
anfontanelli | 3:fc26045926d9 | 225 | float pr = pipe_radius; |
anfontanelli | 3:fc26045926d9 | 226 | float R_dx = 0.0; |
anfontanelli | 3:fc26045926d9 | 227 | float R_sx = 0.0; |
anfontanelli | 3:fc26045926d9 | 228 | float R_m = 0.0; |
anfontanelli | 3:fc26045926d9 | 229 | if(pipeDir ==0){ |
anfontanelli | 3:fc26045926d9 | 230 | R_dx = 1.0; |
anfontanelli | 3:fc26045926d9 | 231 | R_sx = 1.0; |
anfontanelli | 3:fc26045926d9 | 232 | R_m = 1.0; |
anfontanelli | 3:fc26045926d9 | 233 | }else if(pipeDir == 1){ |
anfontanelli | 3:fc26045926d9 | 234 | R_dx = pipeCurve_I; |
anfontanelli | 3:fc26045926d9 | 235 | R_sx = pipeCurve_E; |
anfontanelli | 3:fc26045926d9 | 236 | R_m = pipeCurve_M; |
anfontanelli | 3:fc26045926d9 | 237 | }else if(pipeDir == -1){ |
anfontanelli | 3:fc26045926d9 | 238 | R_dx = pipeCurve_E; |
anfontanelli | 3:fc26045926d9 | 239 | R_sx = pipeCurve_I; |
anfontanelli | 3:fc26045926d9 | 240 | R_m = pipeCurve_M; |
anfontanelli | 3:fc26045926d9 | 241 | |
anfontanelli | 3:fc26045926d9 | 242 | } |
anfontanelli | 3:fc26045926d9 | 243 | |
anfontanelli | 3:fc26045926d9 | 244 | S << -1.0*R_dx/(rm*R_m), pr/rm, -L/rm, |
anfontanelli | 3:fc26045926d9 | 245 | 1.0*R_sx/(rm*R_m), pr/rm, -L/rm, |
anfontanelli | 3:fc26045926d9 | 246 | 0, pr/ro, l/ro; |
anfontanelli | 3:fc26045926d9 | 247 | |
anfontanelli | 3:fc26045926d9 | 248 | S_inv << -rm/2, rm/2, 0, |
anfontanelli | 3:fc26045926d9 | 249 | l*rm/(2*pr*(L+l)), l*rm/(2*pr*(L+l)), L*ro/(pr*(L+l)), |
anfontanelli | 3:fc26045926d9 | 250 | -rm/(2*(L+l)), -rm/(2*(L+l)), ro/(L+l); |
anfontanelli | 3:fc26045926d9 | 251 | } |
anfontanelli | 3:fc26045926d9 | 252 | |
anfontanelli | 3:fc26045926d9 | 253 | void Rover::getMotorsTorque(float &torM1, float &torM2, float &torM3){ |
anfontanelli | 3:fc26045926d9 | 254 | |
anfontanelli | 3:fc26045926d9 | 255 | |
anfontanelli | 3:fc26045926d9 | 256 | torM1 = ionMcMeccanum->getMotorTorque(1); |
anfontanelli | 3:fc26045926d9 | 257 | torM2 = ionMcMeccanum->getMotorTorque(2); |
anfontanelli | 3:fc26045926d9 | 258 | torM3 = ionMcOmni->getMotorTorque(1); |
anfontanelli | 3:fc26045926d9 | 259 | |
anfontanelli | 3:fc26045926d9 | 260 | } |
anfontanelli | 3:fc26045926d9 | 261 | |
anfontanelli | 3:fc26045926d9 | 262 | void Rover::getMotorsSpeed(float &speedM1, float &speedM2, float &speedM3){ |
anfontanelli | 3:fc26045926d9 | 263 | |
anfontanelli | 3:fc26045926d9 | 264 | speedM1 = ionMcMeccanum->getMotorSpeed(1); |
anfontanelli | 3:fc26045926d9 | 265 | speedM2 = ionMcMeccanum->getMotorSpeed(2); |
anfontanelli | 3:fc26045926d9 | 266 | speedM3 = ionMcOmni->getMotorSpeed(1); |
anfontanelli | 3:fc26045926d9 | 267 | |
anfontanelli | 3:fc26045926d9 | 268 | } |
anfontanelli | 3:fc26045926d9 | 269 | |
anfontanelli | 3:fc26045926d9 | 270 | void Rover::setCentralJointsAngle(float jointFront, float jointRetro){ |
anfontanelli | 3:fc26045926d9 | 271 | |
anfontanelli | 3:fc26045926d9 | 272 | float LmRestPosFront = (9.0+0.5)/1000; //9 |
anfontanelli | 3:fc26045926d9 | 273 | float LmRestPosRetro = (9.0-0.5)/1000; //9 |
anfontanelli | 3:fc26045926d9 | 274 | float maxLenght = 0.02; |
anfontanelli | 3:fc26045926d9 | 275 | |
anfontanelli | 3:fc26045926d9 | 276 | float LmFront = LmRestPosFront - centralJoint2actuonix(jointFront); |
anfontanelli | 3:fc26045926d9 | 277 | float LmRetro = LmRestPosRetro - centralJoint2actuonix(jointRetro); |
anfontanelli | 3:fc26045926d9 | 278 | |
anfontanelli | 3:fc26045926d9 | 279 | meccanumActuonix->write(1.0 - LmFront/maxLenght); |
anfontanelli | 3:fc26045926d9 | 280 | omniActuonix->write(1.0 - LmRetro/maxLenght); |
anfontanelli | 3:fc26045926d9 | 281 | |
anfontanelli | 3:fc26045926d9 | 282 | if (jointFront > 3.14*5/180){ |
anfontanelli | 3:fc26045926d9 | 283 | pipeDir = -1; |
anfontanelli | 3:fc26045926d9 | 284 | }else if (jointFront < -3.14*5/180){ |
anfontanelli | 3:fc26045926d9 | 285 | pipeDir = 1; |
anfontanelli | 3:fc26045926d9 | 286 | }else{ |
anfontanelli | 3:fc26045926d9 | 287 | pipeDir = 0; |
anfontanelli | 3:fc26045926d9 | 288 | } |
anfontanelli | 3:fc26045926d9 | 289 | |
anfontanelli | 3:fc26045926d9 | 290 | |
anfontanelli | 3:fc26045926d9 | 291 | } |
anfontanelli | 3:fc26045926d9 | 292 | |
anfontanelli | 3:fc26045926d9 | 293 | float Rover::centralJoint2actuonix(float jointAngle){ |
anfontanelli | 3:fc26045926d9 | 294 | |
anfontanelli | 3:fc26045926d9 | 295 | float alpha = deg2rad(34.4); |
anfontanelli | 3:fc26045926d9 | 296 | float L1 = 23.022/1000; |
anfontanelli | 3:fc26045926d9 | 297 | float L2 = 62.037/1000; |
anfontanelli | 3:fc26045926d9 | 298 | float L3 = 51.013/1000; |
anfontanelli | 3:fc26045926d9 | 299 | float L4 = 4.939/1000; |
anfontanelli | 3:fc26045926d9 | 300 | |
anfontanelli | 3:fc26045926d9 | 301 | float l1 = L1*sin(alpha + jointAngle); |
anfontanelli | 3:fc26045926d9 | 302 | float l2 = L1*cos(alpha + jointAngle); |
anfontanelli | 3:fc26045926d9 | 303 | float h = sqrt( (L2 - l1)*(L2 - l1) + (l2 - L4)*(l2 - L4) ); |
anfontanelli | 3:fc26045926d9 | 304 | return L3 - h; |
anfontanelli | 3:fc26045926d9 | 305 | |
anfontanelli | 3:fc26045926d9 | 306 | } |
anfontanelli | 3:fc26045926d9 | 307 | |
anfontanelli | 3:fc26045926d9 | 308 | |
anfontanelli | 3:fc26045926d9 | 309 | float Rover::deg2rad(float deg) { |
anfontanelli | 3:fc26045926d9 | 310 | return deg * M_PI / 180.0; |
anfontanelli | 3:fc26045926d9 | 311 | } |
anfontanelli | 3:fc26045926d9 | 312 | |
anfontanelli | 3:fc26045926d9 | 313 | |
anfontanelli | 3:fc26045926d9 | 314 | |
anfontanelli | 3:fc26045926d9 | 315 | void Rover::calcImuAngles(float& pitch, float& roll, float dtImu) |
anfontanelli | 3:fc26045926d9 | 316 | { |
anfontanelli | 3:fc26045926d9 | 317 | |
anfontanelli | 3:fc26045926d9 | 318 | mpu->getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //acquisisco i dati dal sensore |
anfontanelli | 3:fc26045926d9 | 319 | a_x=float(ax)/FS_a - accBias[0]; //li converto in float, (CALCOLI EVITABILE SE INSTANZIASSI LE VARIABILI SUBITO COME FLOAT) |
anfontanelli | 3:fc26045926d9 | 320 | a_y=float(ay)/FS_a - accBias[1]; |
anfontanelli | 3:fc26045926d9 | 321 | a_z=float(az)/FS_a; |
anfontanelli | 3:fc26045926d9 | 322 | g_x=float(gx)/FS_g - gyroBias[0]; |
anfontanelli | 3:fc26045926d9 | 323 | g_y=float(gy)/FS_g - gyroBias[1]; |
anfontanelli | 3:fc26045926d9 | 324 | g_z=float(gz)/FS_g - gyroBias[2]; |
anfontanelli | 3:fc26045926d9 | 325 | |
anfontanelli | 3:fc26045926d9 | 326 | |
anfontanelli | 3:fc26045926d9 | 327 | pitch_integrated = g_x * dtImu; // Angle around the X-axis // Integrate the gyro data(deg/s) over time to get angle, usato g_x perchè movimento attorno all'asse x |
anfontanelli | 3:fc26045926d9 | 328 | roll_integrated = g_y * dtImu; // Angle around the Y-axis //uso g_y perchè ho il rollio ruotando atttorno all'asse y |
anfontanelli | 3:fc26045926d9 | 329 | |
anfontanelli | 3:fc26045926d9 | 330 | pitchAcc = atan2f(a_y, sqrt(a_z*a_z + a_x*a_x))*180/M_PI; //considerare formule paper, calcolo i contrubiti dovuti dall'accelerometro |
anfontanelli | 3:fc26045926d9 | 331 | rollAcc = -atan2f(a_x, sqrt(a_y*a_y + a_z*a_z))*180/M_PI; // |
anfontanelli | 3:fc26045926d9 | 332 | |
anfontanelli | 3:fc26045926d9 | 333 | if ( abs( pitchAcc - pitchAcc_p ) > 10){ |
anfontanelli | 3:fc26045926d9 | 334 | pitchAcc = pitch; |
anfontanelli | 3:fc26045926d9 | 335 | } |
anfontanelli | 3:fc26045926d9 | 336 | |
anfontanelli | 3:fc26045926d9 | 337 | if ( abs( rollAcc - rollAcc_p ) > 10){ |
anfontanelli | 3:fc26045926d9 | 338 | rollAcc = roll; |
anfontanelli | 3:fc26045926d9 | 339 | } |
anfontanelli | 3:fc26045926d9 | 340 | |
anfontanelli | 3:fc26045926d9 | 341 | // Apply Complementary Filter |
anfontanelli | 3:fc26045926d9 | 342 | pitch = 0.95f*( pitch + pitch_integrated) + 0.05f*pitchAcc; |
anfontanelli | 3:fc26045926d9 | 343 | roll = 0.95f*( roll + roll_integrated) + 0.05f*(rollAcc); |
anfontanelli | 3:fc26045926d9 | 344 | |
anfontanelli | 3:fc26045926d9 | 345 | pitchAcc_p = pitchAcc; |
anfontanelli | 3:fc26045926d9 | 346 | rollAcc_p = rollAcc; |
anfontanelli | 3:fc26045926d9 | 347 | |
anfontanelli | 3:fc26045926d9 | 348 | |
anfontanelli | 3:fc26045926d9 | 349 | } |
anfontanelli | 3:fc26045926d9 | 350 | |
anfontanelli | 3:fc26045926d9 | 351 | void Rover::calibrateImu() |
anfontanelli | 3:fc26045926d9 | 352 | { |
anfontanelli | 3:fc26045926d9 | 353 | const int CALIBRATION = 500; |
anfontanelli | 3:fc26045926d9 | 354 | for(int i=0;i<50;i++) |
anfontanelli | 3:fc26045926d9 | 355 | { |
anfontanelli | 3:fc26045926d9 | 356 | mpu->getMotion6(&ax, &ay, &az, &gx, &gy, &gz); |
anfontanelli | 3:fc26045926d9 | 357 | } |
anfontanelli | 3:fc26045926d9 | 358 | |
anfontanelli | 3:fc26045926d9 | 359 | |
anfontanelli | 3:fc26045926d9 | 360 | printf(" START CALIBRATION \r\n"); |
anfontanelli | 3:fc26045926d9 | 361 | |
anfontanelli | 3:fc26045926d9 | 362 | |
anfontanelli | 3:fc26045926d9 | 363 | for(int i=0;i<CALIBRATION;i++) |
anfontanelli | 3:fc26045926d9 | 364 | { |
anfontanelli | 3:fc26045926d9 | 365 | mpu->getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //acquisisco i dati dal sensore |
anfontanelli | 3:fc26045926d9 | 366 | a_x=(float)ax/FS_a; //li converto in float, (CALCOLI EVITABILE SE INSTANZIASSI LE VARIABILI SUBITO COME FLOAT) |
anfontanelli | 3:fc26045926d9 | 367 | a_y=(float)ay/FS_a; |
anfontanelli | 3:fc26045926d9 | 368 | //a_z=(float)az/FS_a; |
anfontanelli | 3:fc26045926d9 | 369 | g_x=(float)gx/FS_g; |
anfontanelli | 3:fc26045926d9 | 370 | g_y=(float)gy/FS_g;; |
anfontanelli | 3:fc26045926d9 | 371 | g_z=(float)gz/FS_g;; |
anfontanelli | 3:fc26045926d9 | 372 | |
anfontanelli | 3:fc26045926d9 | 373 | accBias[0] += a_x; |
anfontanelli | 3:fc26045926d9 | 374 | accBias[1] += a_y; |
anfontanelli | 3:fc26045926d9 | 375 | gyroBias[0] += g_x; |
anfontanelli | 3:fc26045926d9 | 376 | gyroBias[1] += g_y; |
anfontanelli | 3:fc26045926d9 | 377 | gyroBias[2] += g_z; |
anfontanelli | 3:fc26045926d9 | 378 | |
anfontanelli | 3:fc26045926d9 | 379 | } |
anfontanelli | 3:fc26045926d9 | 380 | |
anfontanelli | 3:fc26045926d9 | 381 | //BIAS |
anfontanelli | 3:fc26045926d9 | 382 | accBias[0] = accBias[0]/CALIBRATION; |
anfontanelli | 3:fc26045926d9 | 383 | accBias[1] = accBias[1]/CALIBRATION; |
anfontanelli | 3:fc26045926d9 | 384 | gyroBias[0] = gyroBias[0]/CALIBRATION; |
anfontanelli | 3:fc26045926d9 | 385 | gyroBias[1] = gyroBias[1]/CALIBRATION; |
anfontanelli | 3:fc26045926d9 | 386 | gyroBias[2] = gyroBias[2]/CALIBRATION; |
anfontanelli | 3:fc26045926d9 | 387 | |
anfontanelli | 3:fc26045926d9 | 388 | |
anfontanelli | 3:fc26045926d9 | 389 | printf(" END CALIBRATION \r\n"); |
anfontanelli | 3:fc26045926d9 | 390 | |
anfontanelli | 3:fc26045926d9 | 391 | //printf("%f\t %f\t %f\t %f\t %f\t \r\n", accBias[0], accBias[1], gyroBias[0],gyroBias[1],gyroBias[2]); //stampo le misure |
anfontanelli | 3:fc26045926d9 | 392 | |
anfontanelli | 3:fc26045926d9 | 393 | |
anfontanelli | 3:fc26045926d9 | 394 | } |
anfontanelli | 3:fc26045926d9 | 395 | |
anfontanelli | 3:fc26045926d9 | 396 | |
anfontanelli | 3:fc26045926d9 | 397 | |
anfontanelli | 3:fc26045926d9 | 398 | void Rover::startEthComunication(){ |
anfontanelli | 3:fc26045926d9 | 399 | printf("Avvio la comunicazione ethernet \r\n"); |
anfontanelli | 3:fc26045926d9 | 400 | comunicationTimer.start(); |
anfontanelli | 3:fc26045926d9 | 401 | eth_tcp->connect(); |
anfontanelli | 3:fc26045926d9 | 402 | setState(1); |
anfontanelli | 3:fc26045926d9 | 403 | |
anfontanelli | 3:fc26045926d9 | 404 | } |
anfontanelli | 3:fc26045926d9 | 405 | |
anfontanelli | 3:fc26045926d9 | 406 | void Rover::ethComunicationUpdate(float _time, float _pitch, float _vels_a, float _vels_m, float _velf_a, float _velf_m) { |
anfontanelli | 3:fc26045926d9 | 407 | |
anfontanelli | 3:fc26045926d9 | 408 | if( eth_tcp->is_connected()){ |
anfontanelli | 3:fc26045926d9 | 409 | setState(2); |
anfontanelli | 3:fc26045926d9 | 410 | printf("ETH: connected!\r\n"); |
anfontanelli | 3:fc26045926d9 | 411 | sock_open = true; |
anfontanelli | 3:fc26045926d9 | 412 | }else{ |
anfontanelli | 3:fc26045926d9 | 413 | setState(1); |
anfontanelli | 3:fc26045926d9 | 414 | } |
anfontanelli | 3:fc26045926d9 | 415 | |
anfontanelli | 3:fc26045926d9 | 416 | eth_status = eth_tcp->recv_pkt(cmd, recv_vec, n_of_int); |
anfontanelli | 3:fc26045926d9 | 417 | |
anfontanelli | 3:fc26045926d9 | 418 | eth_time = comunicationTimer.read(); |
anfontanelli | 3:fc26045926d9 | 419 | //printf("%f\r\n", (double)(eth_time - eth_time_sample_received)); |
anfontanelli | 3:fc26045926d9 | 420 | |
anfontanelli | 3:fc26045926d9 | 421 | |
anfontanelli | 3:fc26045926d9 | 422 | if(eth_status){ |
anfontanelli | 3:fc26045926d9 | 423 | eth_time_sample_received = comunicationTimer.read(); |
anfontanelli | 3:fc26045926d9 | 424 | if(cmd == 's'){ |
anfontanelli | 3:fc26045926d9 | 425 | if(n_of_int == 4){ |
anfontanelli | 3:fc26045926d9 | 426 | eth_mutex.lock(); |
anfontanelli | 3:fc26045926d9 | 427 | forward_vel = recv_vec.get(); |
anfontanelli | 3:fc26045926d9 | 428 | pitch_d = recv_vec.get(); |
anfontanelli | 3:fc26045926d9 | 429 | //jointFront = recv_vec.get(); |
anfontanelli | 3:fc26045926d9 | 430 | //jointRetro = recv_vec.get(); |
anfontanelli | 3:fc26045926d9 | 431 | enableStab = recv_vec.get(); |
anfontanelli | 3:fc26045926d9 | 432 | enableCurv= recv_vec.get(); |
anfontanelli | 3:fc26045926d9 | 433 | eth_mutex.unlock(); |
anfontanelli | 3:fc26045926d9 | 434 | } |
anfontanelli | 3:fc26045926d9 | 435 | |
anfontanelli | 3:fc26045926d9 | 436 | eth_mutex.lock(); |
anfontanelli | 3:fc26045926d9 | 437 | vec_to_send.put(_time*1000); |
anfontanelli | 3:fc26045926d9 | 438 | vec_to_send.put(_pitch*1000); |
anfontanelli | 3:fc26045926d9 | 439 | vec_to_send.put(_vels_a*1000); |
anfontanelli | 3:fc26045926d9 | 440 | vec_to_send.put(_vels_m*1000); |
anfontanelli | 3:fc26045926d9 | 441 | vec_to_send.put(_velf_a*1000); |
anfontanelli | 3:fc26045926d9 | 442 | vec_to_send.put(_velf_m*1000); |
anfontanelli | 3:fc26045926d9 | 443 | |
anfontanelli | 3:fc26045926d9 | 444 | eth_mutex.unlock(); |
anfontanelli | 3:fc26045926d9 | 445 | eth_tcp->send_vec_of_int(vec_to_send); |
anfontanelli | 3:fc26045926d9 | 446 | |
anfontanelli | 3:fc26045926d9 | 447 | vec_to_send.clear(); |
anfontanelli | 3:fc26045926d9 | 448 | |
anfontanelli | 3:fc26045926d9 | 449 | cmd = ' '; |
anfontanelli | 3:fc26045926d9 | 450 | |
anfontanelli | 3:fc26045926d9 | 451 | } |
anfontanelli | 3:fc26045926d9 | 452 | } |
anfontanelli | 3:fc26045926d9 | 453 | |
anfontanelli | 3:fc26045926d9 | 454 | if((double)(eth_time - eth_time_sample_received)>eth_time_out && sock_open == true){ |
anfontanelli | 3:fc26045926d9 | 455 | eth_tcp->reset_connection(); |
anfontanelli | 3:fc26045926d9 | 456 | sock_open = false; |
anfontanelli | 3:fc26045926d9 | 457 | eth_mutex.lock(); |
anfontanelli | 3:fc26045926d9 | 458 | forward_vel = 0.0; |
anfontanelli | 3:fc26045926d9 | 459 | pitch_d = 0.0; |
anfontanelli | 3:fc26045926d9 | 460 | //jointFront = 0.0; |
anfontanelli | 3:fc26045926d9 | 461 | //jointRetro = 0.0; |
anfontanelli | 3:fc26045926d9 | 462 | enableCurv = 0; |
anfontanelli | 3:fc26045926d9 | 463 | eth_mutex.unlock(); |
anfontanelli | 3:fc26045926d9 | 464 | } |
anfontanelli | 3:fc26045926d9 | 465 | |
anfontanelli | 3:fc26045926d9 | 466 | |
anfontanelli | 3:fc26045926d9 | 467 | } |
anfontanelli | 3:fc26045926d9 | 468 | |
anfontanelli | 3:fc26045926d9 | 469 | int Rover::getEthState(){ |
anfontanelli | 3:fc26045926d9 | 470 | int state; |
anfontanelli | 3:fc26045926d9 | 471 | |
anfontanelli | 3:fc26045926d9 | 472 | eth_mutex.lock(); |
anfontanelli | 3:fc26045926d9 | 473 | state = eth_state; |
anfontanelli | 3:fc26045926d9 | 474 | eth_mutex.unlock(); |
anfontanelli | 3:fc26045926d9 | 475 | |
anfontanelli | 3:fc26045926d9 | 476 | return state; |
anfontanelli | 3:fc26045926d9 | 477 | } |
anfontanelli | 3:fc26045926d9 | 478 | |
anfontanelli | 3:fc26045926d9 | 479 | void Rover::setState(int state){ |
anfontanelli | 3:fc26045926d9 | 480 | |
anfontanelli | 3:fc26045926d9 | 481 | eth_mutex.lock(); |
anfontanelli | 3:fc26045926d9 | 482 | eth_state = state; |
anfontanelli | 3:fc26045926d9 | 483 | eth_mutex.unlock(); |
anfontanelli | 3:fc26045926d9 | 484 | |
anfontanelli | 3:fc26045926d9 | 485 | } |
anfontanelli | 3:fc26045926d9 | 486 | |
anfontanelli | 3:fc26045926d9 | 487 |