Clone per testare Mbed studio
Dependencies: PwmIn IONMcMotor MPU6050 Eigen ROVER
Fork of Hyfliers_Completo_testato by
Robots/Rover/Rover.cpp@4:3f22193053d0, 2020-01-08 (annotated)
- Committer:
- anfontanelli
- Date:
- Wed Jan 08 11:05:36 2020 +0000
- Revision:
- 4:3f22193053d0
- Parent:
- 3:fc26045926d9
- Child:
- 6:584653235830
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 | 4:3f22193053d0 | 6 | frontBoardAddress = 128; |
anfontanelli | 4:3f22193053d0 | 7 | frontMotorGearBoxRatio = 103; |
anfontanelli | 4:3f22193053d0 | 8 | frontEncoderPulse = 512; |
anfontanelli | 4:3f22193053d0 | 9 | frontKt = 0.00667; |
anfontanelli | 4:3f22193053d0 | 10 | frontTransmissionRatio = 1.0; |
anfontanelli | 4:3f22193053d0 | 11 | retroTransmissionRatio = 1.0; |
anfontanelli | 4:3f22193053d0 | 12 | retroBoardAddress = 129; |
anfontanelli | 4:3f22193053d0 | 13 | retroMotorGearBoxRatio = 103; |
anfontanelli | 4:3f22193053d0 | 14 | retroEncoderPulse = 512; |
anfontanelli | 4:3f22193053d0 | 15 | retroKt = 0.00667; |
anfontanelli | 3:fc26045926d9 | 16 | |
anfontanelli | 4:3f22193053d0 | 17 | r_frontWheel = 0.03; |
anfontanelli | 4:3f22193053d0 | 18 | r_retroWheel = 0.03; |
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 | 4:3f22193053d0 | 26 | retroFrontCentralDistance = 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 | 4:3f22193053d0 | 31 | S = Eigen::Matrix<float, 4, 3>::Zero(); |
anfontanelli | 3:fc26045926d9 | 32 | |
anfontanelli | 3:fc26045926d9 | 33 | |
anfontanelli | 4:3f22193053d0 | 34 | ionMcFront = new IONMcMotors(frontBoardAddress,ionMcBoudRate, PG_14, PG_9, frontMotorGearBoxRatio, frontEncoderPulse, frontKt, frontKt); |
anfontanelli | 4:3f22193053d0 | 35 | ionMcRetro = new IONMcMotors(retroBoardAddress,ionMcBoudRate, PB_9, PB_8, retroMotorGearBoxRatio, retroEncoderPulse, retroKt, retroKt); |
anfontanelli | 4:3f22193053d0 | 36 | frontActuonix = new Servo(PE_11); |
anfontanelli | 4:3f22193053d0 | 37 | retroActuonix = 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 | float frontVel = 0.05*frontDistance; |
anfontanelli | 3:fc26045926d9 | 76 | float retroVel = -0.04*retroDistance; |
anfontanelli | 3:fc26045926d9 | 77 | |
anfontanelli | 3:fc26045926d9 | 78 | |
anfontanelli | 3:fc26045926d9 | 79 | if(enableCurv==1){ |
anfontanelli | 3:fc26045926d9 | 80 | frontPos = frontPos + frontVel*0.005; |
anfontanelli | 3:fc26045926d9 | 81 | retroPos = retroPos + retroVel*0.005; |
anfontanelli | 3:fc26045926d9 | 82 | }else{ |
anfontanelli | 3:fc26045926d9 | 83 | frontPos = 0.0; |
anfontanelli | 3:fc26045926d9 | 84 | retroPos = 0.0; |
anfontanelli | 3:fc26045926d9 | 85 | } |
anfontanelli | 3:fc26045926d9 | 86 | |
anfontanelli | 3:fc26045926d9 | 87 | if(frontPos > 0.5) frontPos=0.5; |
anfontanelli | 3:fc26045926d9 | 88 | if(frontPos < -0.5) frontPos=-0.5; |
anfontanelli | 3:fc26045926d9 | 89 | if(retroPos > 0.5) retroPos=0.5; |
anfontanelli | 3:fc26045926d9 | 90 | if(retroPos < -0.5) retroPos=-0.5; |
anfontanelli | 3:fc26045926d9 | 91 | |
anfontanelli | 3:fc26045926d9 | 92 | setCentralJointsAngle(frontPos,retroPos); |
anfontanelli | 3:fc26045926d9 | 93 | |
anfontanelli | 3:fc26045926d9 | 94 | } |
anfontanelli | 3:fc26045926d9 | 95 | |
anfontanelli | 3:fc26045926d9 | 96 | |
anfontanelli | 3:fc26045926d9 | 97 | void Rover::initializeImu(){ |
anfontanelli | 3:fc26045926d9 | 98 | |
anfontanelli | 3:fc26045926d9 | 99 | printf("Initialize IMU \r\n"); |
anfontanelli | 3:fc26045926d9 | 100 | //IMU |
anfontanelli | 3:fc26045926d9 | 101 | FS_a = 8192; //per avere letture in g, sarebbe 32768/4 perchè 4g è il fondo scala |
anfontanelli | 3:fc26045926d9 | 102 | FS_g = 131.072; //per avere letture in gradi/s, sarebbe 32768/250 perchè 250 è il fondo scala |
anfontanelli | 3:fc26045926d9 | 103 | |
anfontanelli | 3:fc26045926d9 | 104 | |
anfontanelli | 3:fc26045926d9 | 105 | accBias[0] = 0.0; |
anfontanelli | 3:fc26045926d9 | 106 | accBias[1] = 0.0; |
anfontanelli | 3:fc26045926d9 | 107 | accBias[2] = 0.0; |
anfontanelli | 3:fc26045926d9 | 108 | gyroBias[0] = 0.0; |
anfontanelli | 3:fc26045926d9 | 109 | gyroBias[1] = 0.0; |
anfontanelli | 3:fc26045926d9 | 110 | gyroBias[2] = 0.0; |
anfontanelli | 3:fc26045926d9 | 111 | |
anfontanelli | 3:fc26045926d9 | 112 | |
anfontanelli | 3:fc26045926d9 | 113 | pitchAcc = 0.0; |
anfontanelli | 3:fc26045926d9 | 114 | rollAcc = 0.0; |
anfontanelli | 3:fc26045926d9 | 115 | |
anfontanelli | 3:fc26045926d9 | 116 | pitch_integrated = 0.0; |
anfontanelli | 3:fc26045926d9 | 117 | roll_integrated = 0.0; |
anfontanelli | 3:fc26045926d9 | 118 | |
anfontanelli | 3:fc26045926d9 | 119 | mpu->initialize(); |
anfontanelli | 3:fc26045926d9 | 120 | bool mpu6050TestResult = mpu->testConnection(); |
anfontanelli | 3:fc26045926d9 | 121 | if(mpu6050TestResult) { |
anfontanelli | 3:fc26045926d9 | 122 | printf("MPU6050 test passed \r\n"); |
anfontanelli | 3:fc26045926d9 | 123 | } else { |
anfontanelli | 3:fc26045926d9 | 124 | printf("MPU6050 test failed \r\n"); |
anfontanelli | 3:fc26045926d9 | 125 | } |
anfontanelli | 3:fc26045926d9 | 126 | |
anfontanelli | 3:fc26045926d9 | 127 | |
anfontanelli | 3:fc26045926d9 | 128 | calibrateImu(); |
anfontanelli | 3:fc26045926d9 | 129 | } |
anfontanelli | 3:fc26045926d9 | 130 | |
anfontanelli | 3:fc26045926d9 | 131 | void Rover::setWheelsVelocity(float forward_speed, float stabilization_speed, float asset_correction_speed, float pipe_radius, float maxWheelAcceleration){ |
anfontanelli | 3:fc26045926d9 | 132 | |
anfontanelli | 3:fc26045926d9 | 133 | Eigen::Vector3f cartesianSpeed; |
anfontanelli | 4:3f22193053d0 | 134 | Eigen::Vector4f wheelsSpeed; |
anfontanelli | 3:fc26045926d9 | 135 | |
anfontanelli | 3:fc26045926d9 | 136 | updateRoverKin(pipe_radius, pipeDir); |
anfontanelli | 3:fc26045926d9 | 137 | |
anfontanelli | 3:fc26045926d9 | 138 | cartesianSpeed << forward_speed, stabilization_speed, asset_correction_speed; |
anfontanelli | 3:fc26045926d9 | 139 | wheelsSpeed = S*cartesianSpeed; |
anfontanelli | 4:3f22193053d0 | 140 | ionMcFront->setSpeed(1, wheelsSpeed[0],maxWheelAcceleration); |
anfontanelli | 4:3f22193053d0 | 141 | ionMcFront->setSpeed(2, wheelsSpeed[1],maxWheelAcceleration); |
anfontanelli | 4:3f22193053d0 | 142 | ionMcRetro->setSpeed(1,wheelsSpeed[2],maxWheelAcceleration); |
anfontanelli | 4:3f22193053d0 | 143 | ionMcRetro->setSpeed(2,wheelsSpeed[3],maxWheelAcceleration); |
anfontanelli | 3:fc26045926d9 | 144 | } |
anfontanelli | 3:fc26045926d9 | 145 | |
anfontanelli | 3:fc26045926d9 | 146 | |
anfontanelli | 3:fc26045926d9 | 147 | int Rover::get_forward_vel(){ |
anfontanelli | 3:fc26045926d9 | 148 | int fv; |
anfontanelli | 3:fc26045926d9 | 149 | eth_mutex.lock(); |
anfontanelli | 3:fc26045926d9 | 150 | fv = forward_vel; |
anfontanelli | 3:fc26045926d9 | 151 | eth_mutex.unlock(); |
anfontanelli | 3:fc26045926d9 | 152 | return fv; |
anfontanelli | 3:fc26045926d9 | 153 | |
anfontanelli | 3:fc26045926d9 | 154 | } |
anfontanelli | 3:fc26045926d9 | 155 | int Rover::get_pitch(){ |
anfontanelli | 3:fc26045926d9 | 156 | int pit; |
anfontanelli | 3:fc26045926d9 | 157 | eth_mutex.lock(); |
anfontanelli | 3:fc26045926d9 | 158 | pit = pitch_d; |
anfontanelli | 3:fc26045926d9 | 159 | eth_mutex.unlock(); |
anfontanelli | 3:fc26045926d9 | 160 | return pit; |
anfontanelli | 3:fc26045926d9 | 161 | } |
anfontanelli | 3:fc26045926d9 | 162 | |
anfontanelli | 3:fc26045926d9 | 163 | int Rover::get_jointFront(){ |
anfontanelli | 3:fc26045926d9 | 164 | int jf; |
anfontanelli | 3:fc26045926d9 | 165 | eth_mutex.lock(); |
anfontanelli | 3:fc26045926d9 | 166 | jf = jointFront; |
anfontanelli | 3:fc26045926d9 | 167 | eth_mutex.unlock(); |
anfontanelli | 3:fc26045926d9 | 168 | return jf; |
anfontanelli | 3:fc26045926d9 | 169 | } |
anfontanelli | 3:fc26045926d9 | 170 | int Rover::get_jointRetro(){ |
anfontanelli | 3:fc26045926d9 | 171 | int jr; |
anfontanelli | 3:fc26045926d9 | 172 | eth_mutex.lock(); |
anfontanelli | 3:fc26045926d9 | 173 | jr = jointRetro; |
anfontanelli | 3:fc26045926d9 | 174 | eth_mutex.unlock(); |
anfontanelli | 3:fc26045926d9 | 175 | return jr; |
anfontanelli | 3:fc26045926d9 | 176 | } |
anfontanelli | 3:fc26045926d9 | 177 | |
anfontanelli | 3:fc26045926d9 | 178 | void Rover::getRoverVelocity(float &forward_speed, float &stabilization_speed, float &asset_correction_speed, float pipe_radius){ |
anfontanelli | 3:fc26045926d9 | 179 | |
anfontanelli | 3:fc26045926d9 | 180 | Eigen::Vector3f cartesianSpeed; |
anfontanelli | 4:3f22193053d0 | 181 | Eigen::Vector4f wheelsSpeed; |
anfontanelli | 3:fc26045926d9 | 182 | |
anfontanelli | 3:fc26045926d9 | 183 | float speedM1 = 0.0; |
anfontanelli | 3:fc26045926d9 | 184 | float speedM2 = 0.0; |
anfontanelli | 3:fc26045926d9 | 185 | float speedM3 = 0.0; |
anfontanelli | 4:3f22193053d0 | 186 | float speedM4 = 0.0; |
anfontanelli | 4:3f22193053d0 | 187 | |
anfontanelli | 3:fc26045926d9 | 188 | updateRoverKin(pipe_radius, pipeDir); |
anfontanelli | 3:fc26045926d9 | 189 | |
anfontanelli | 4:3f22193053d0 | 190 | getMotorsSpeed(speedM1, speedM2, speedM3, speedM3); |
anfontanelli | 3:fc26045926d9 | 191 | |
anfontanelli | 4:3f22193053d0 | 192 | wheelsSpeed << speedM1, speedM2, speedM3, speedM4; |
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 | 4:3f22193053d0 | 202 | void Rover::getRoverWheelsVelocity(float &front_dx, float &front_sx, float &retro_dx, float &retro_sx){ |
anfontanelli | 3:fc26045926d9 | 203 | |
anfontanelli | 3:fc26045926d9 | 204 | Eigen::Vector3f cartesianSpeed; |
anfontanelli | 4:3f22193053d0 | 205 | Eigen::Vector4f 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 | 4:3f22193053d0 | 210 | float speedM4 = 0.0; |
anfontanelli | 4:3f22193053d0 | 211 | |
anfontanelli | 4:3f22193053d0 | 212 | getMotorsSpeed(speedM1, speedM2, speedM3, speedM4); |
anfontanelli | 3:fc26045926d9 | 213 | |
anfontanelli | 4:3f22193053d0 | 214 | front_dx = speedM1; |
anfontanelli | 4:3f22193053d0 | 215 | front_sx = speedM2; |
anfontanelli | 4:3f22193053d0 | 216 | retro_dx = speedM3; |
anfontanelli | 4:3f22193053d0 | 217 | retro_sx = speedM4; |
anfontanelli | 3:fc26045926d9 | 218 | |
anfontanelli | 3:fc26045926d9 | 219 | } |
anfontanelli | 3:fc26045926d9 | 220 | |
anfontanelli | 3:fc26045926d9 | 221 | void Rover::updateRoverKin(float pipe_radius, int pipeDir){ |
anfontanelli | 4:3f22193053d0 | 222 | float rf = r_frontWheel; |
anfontanelli | 4:3f22193053d0 | 223 | float rr = r_retroWheel; |
anfontanelli | 4:3f22193053d0 | 224 | float l = retroFrontCentralDistance; |
anfontanelli | 3:fc26045926d9 | 225 | float L = wheelsCntactPointDistanceFromPipeCenter; |
anfontanelli | 3:fc26045926d9 | 226 | float pr = pipe_radius; |
anfontanelli | 3:fc26045926d9 | 227 | float R_dx = 0.0; |
anfontanelli | 3:fc26045926d9 | 228 | float R_sx = 0.0; |
anfontanelli | 3:fc26045926d9 | 229 | float R_m = 0.0; |
anfontanelli | 3:fc26045926d9 | 230 | if(pipeDir ==0){ |
anfontanelli | 3:fc26045926d9 | 231 | R_dx = 1.0; |
anfontanelli | 3:fc26045926d9 | 232 | R_sx = 1.0; |
anfontanelli | 3:fc26045926d9 | 233 | R_m = 1.0; |
anfontanelli | 3:fc26045926d9 | 234 | }else if(pipeDir == 1){ |
anfontanelli | 3:fc26045926d9 | 235 | R_dx = pipeCurve_I; |
anfontanelli | 3:fc26045926d9 | 236 | R_sx = pipeCurve_E; |
anfontanelli | 3:fc26045926d9 | 237 | R_m = pipeCurve_M; |
anfontanelli | 3:fc26045926d9 | 238 | }else if(pipeDir == -1){ |
anfontanelli | 3:fc26045926d9 | 239 | R_dx = pipeCurve_E; |
anfontanelli | 3:fc26045926d9 | 240 | R_sx = pipeCurve_I; |
anfontanelli | 3:fc26045926d9 | 241 | R_m = pipeCurve_M; |
anfontanelli | 3:fc26045926d9 | 242 | |
anfontanelli | 3:fc26045926d9 | 243 | } |
anfontanelli | 4:3f22193053d0 | 244 | //AGGIORNARE la S |
anfontanelli | 4:3f22193053d0 | 245 | S << -1.0*R_dx/(rf*R_m), pr/rf, -(L+l)/rf, |
anfontanelli | 4:3f22193053d0 | 246 | 1.0*R_sx/(rf*R_m), pr/rf, -(L+l)/rf, |
anfontanelli | 4:3f22193053d0 | 247 | -1.0*R_dx/(rr*R_m), -pr/rr, -(L+l)/rr, |
anfontanelli | 4:3f22193053d0 | 248 | 1.0*R_sx/(rr*R_m), -pr/rr, -(L+l)/rr; |
anfontanelli | 3:fc26045926d9 | 249 | |
anfontanelli | 4:3f22193053d0 | 250 | |
anfontanelli | 4:3f22193053d0 | 251 | Eigen::FullPivLU<Matrix4f> Core_S(S*S.transpose()); |
anfontanelli | 4:3f22193053d0 | 252 | S_inv = S.transpose()*Core_S.inverse(); |
anfontanelli | 4:3f22193053d0 | 253 | |
anfontanelli | 3:fc26045926d9 | 254 | } |
anfontanelli | 3:fc26045926d9 | 255 | |
anfontanelli | 4:3f22193053d0 | 256 | void Rover::getMotorsTorque(float &torM1, float &torM2, float &torM3, float &torM4){ |
anfontanelli | 3:fc26045926d9 | 257 | |
anfontanelli | 3:fc26045926d9 | 258 | |
anfontanelli | 4:3f22193053d0 | 259 | torM1 = ionMcFront->getMotorTorque(1); |
anfontanelli | 4:3f22193053d0 | 260 | torM2 = ionMcFront->getMotorTorque(2); |
anfontanelli | 4:3f22193053d0 | 261 | torM3 = ionMcRetro->getMotorTorque(1); |
anfontanelli | 4:3f22193053d0 | 262 | torM4 = ionMcRetro->getMotorTorque(2); |
anfontanelli | 3:fc26045926d9 | 263 | } |
anfontanelli | 3:fc26045926d9 | 264 | |
anfontanelli | 4:3f22193053d0 | 265 | void Rover::getMotorsSpeed(float &speedM1, float &speedM2, float &speedM3, float &speedM4){ |
anfontanelli | 3:fc26045926d9 | 266 | |
anfontanelli | 4:3f22193053d0 | 267 | speedM1 = ionMcFront->getMotorSpeed(1); |
anfontanelli | 4:3f22193053d0 | 268 | speedM2 = ionMcFront->getMotorSpeed(2); |
anfontanelli | 4:3f22193053d0 | 269 | speedM3 = ionMcRetro->getMotorSpeed(1); |
anfontanelli | 4:3f22193053d0 | 270 | speedM4 = ionMcRetro->getMotorSpeed(2); |
anfontanelli | 3:fc26045926d9 | 271 | |
anfontanelli | 3:fc26045926d9 | 272 | } |
anfontanelli | 3:fc26045926d9 | 273 | |
anfontanelli | 3:fc26045926d9 | 274 | void Rover::setCentralJointsAngle(float jointFront, float jointRetro){ |
anfontanelli | 3:fc26045926d9 | 275 | |
anfontanelli | 3:fc26045926d9 | 276 | float LmRestPosFront = (9.0+0.5)/1000; //9 |
anfontanelli | 3:fc26045926d9 | 277 | float LmRestPosRetro = (9.0-0.5)/1000; //9 |
anfontanelli | 3:fc26045926d9 | 278 | float maxLenght = 0.02; |
anfontanelli | 3:fc26045926d9 | 279 | |
anfontanelli | 3:fc26045926d9 | 280 | float LmFront = LmRestPosFront - centralJoint2actuonix(jointFront); |
anfontanelli | 3:fc26045926d9 | 281 | float LmRetro = LmRestPosRetro - centralJoint2actuonix(jointRetro); |
anfontanelli | 3:fc26045926d9 | 282 | |
anfontanelli | 4:3f22193053d0 | 283 | frontActuonix->write(1.0 - LmFront/maxLenght); |
anfontanelli | 4:3f22193053d0 | 284 | retroActuonix->write(1.0 - LmRetro/maxLenght); |
anfontanelli | 3:fc26045926d9 | 285 | |
anfontanelli | 3:fc26045926d9 | 286 | if (jointFront > 3.14*5/180){ |
anfontanelli | 3:fc26045926d9 | 287 | pipeDir = -1; |
anfontanelli | 3:fc26045926d9 | 288 | }else if (jointFront < -3.14*5/180){ |
anfontanelli | 3:fc26045926d9 | 289 | pipeDir = 1; |
anfontanelli | 3:fc26045926d9 | 290 | }else{ |
anfontanelli | 3:fc26045926d9 | 291 | pipeDir = 0; |
anfontanelli | 3:fc26045926d9 | 292 | } |
anfontanelli | 3:fc26045926d9 | 293 | |
anfontanelli | 3:fc26045926d9 | 294 | |
anfontanelli | 3:fc26045926d9 | 295 | } |
anfontanelli | 3:fc26045926d9 | 296 | |
anfontanelli | 3:fc26045926d9 | 297 | float Rover::centralJoint2actuonix(float jointAngle){ |
anfontanelli | 3:fc26045926d9 | 298 | |
anfontanelli | 3:fc26045926d9 | 299 | float alpha = deg2rad(34.4); |
anfontanelli | 3:fc26045926d9 | 300 | float L1 = 23.022/1000; |
anfontanelli | 3:fc26045926d9 | 301 | float L2 = 62.037/1000; |
anfontanelli | 3:fc26045926d9 | 302 | float L3 = 51.013/1000; |
anfontanelli | 3:fc26045926d9 | 303 | float L4 = 4.939/1000; |
anfontanelli | 3:fc26045926d9 | 304 | |
anfontanelli | 3:fc26045926d9 | 305 | float l1 = L1*sin(alpha + jointAngle); |
anfontanelli | 3:fc26045926d9 | 306 | float l2 = L1*cos(alpha + jointAngle); |
anfontanelli | 3:fc26045926d9 | 307 | float h = sqrt( (L2 - l1)*(L2 - l1) + (l2 - L4)*(l2 - L4) ); |
anfontanelli | 3:fc26045926d9 | 308 | return L3 - h; |
anfontanelli | 3:fc26045926d9 | 309 | |
anfontanelli | 3:fc26045926d9 | 310 | } |
anfontanelli | 3:fc26045926d9 | 311 | |
anfontanelli | 3:fc26045926d9 | 312 | |
anfontanelli | 3:fc26045926d9 | 313 | float Rover::deg2rad(float deg) { |
anfontanelli | 3:fc26045926d9 | 314 | return deg * M_PI / 180.0; |
anfontanelli | 3:fc26045926d9 | 315 | } |
anfontanelli | 3:fc26045926d9 | 316 | |
anfontanelli | 3:fc26045926d9 | 317 | |
anfontanelli | 3:fc26045926d9 | 318 | |
anfontanelli | 3:fc26045926d9 | 319 | void Rover::calcImuAngles(float& pitch, float& roll, float dtImu) |
anfontanelli | 3:fc26045926d9 | 320 | { |
anfontanelli | 3:fc26045926d9 | 321 | |
anfontanelli | 3:fc26045926d9 | 322 | mpu->getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //acquisisco i dati dal sensore |
anfontanelli | 3:fc26045926d9 | 323 | a_x=float(ax)/FS_a - accBias[0]; //li converto in float, (CALCOLI EVITABILE SE INSTANZIASSI LE VARIABILI SUBITO COME FLOAT) |
anfontanelli | 3:fc26045926d9 | 324 | a_y=float(ay)/FS_a - accBias[1]; |
anfontanelli | 3:fc26045926d9 | 325 | a_z=float(az)/FS_a; |
anfontanelli | 3:fc26045926d9 | 326 | g_x=float(gx)/FS_g - gyroBias[0]; |
anfontanelli | 3:fc26045926d9 | 327 | g_y=float(gy)/FS_g - gyroBias[1]; |
anfontanelli | 3:fc26045926d9 | 328 | g_z=float(gz)/FS_g - gyroBias[2]; |
anfontanelli | 3:fc26045926d9 | 329 | |
anfontanelli | 3:fc26045926d9 | 330 | |
anfontanelli | 3:fc26045926d9 | 331 | 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 | 332 | 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 | 333 | |
anfontanelli | 3:fc26045926d9 | 334 | 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 | 335 | rollAcc = -atan2f(a_x, sqrt(a_y*a_y + a_z*a_z))*180/M_PI; // |
anfontanelli | 3:fc26045926d9 | 336 | |
anfontanelli | 3:fc26045926d9 | 337 | if ( abs( pitchAcc - pitchAcc_p ) > 10){ |
anfontanelli | 3:fc26045926d9 | 338 | pitchAcc = pitch; |
anfontanelli | 3:fc26045926d9 | 339 | } |
anfontanelli | 3:fc26045926d9 | 340 | |
anfontanelli | 3:fc26045926d9 | 341 | if ( abs( rollAcc - rollAcc_p ) > 10){ |
anfontanelli | 3:fc26045926d9 | 342 | rollAcc = roll; |
anfontanelli | 3:fc26045926d9 | 343 | } |
anfontanelli | 3:fc26045926d9 | 344 | |
anfontanelli | 3:fc26045926d9 | 345 | // Apply Complementary Filter |
anfontanelli | 3:fc26045926d9 | 346 | pitch = 0.95f*( pitch + pitch_integrated) + 0.05f*pitchAcc; |
anfontanelli | 3:fc26045926d9 | 347 | roll = 0.95f*( roll + roll_integrated) + 0.05f*(rollAcc); |
anfontanelli | 3:fc26045926d9 | 348 | |
anfontanelli | 3:fc26045926d9 | 349 | pitchAcc_p = pitchAcc; |
anfontanelli | 3:fc26045926d9 | 350 | rollAcc_p = rollAcc; |
anfontanelli | 3:fc26045926d9 | 351 | |
anfontanelli | 3:fc26045926d9 | 352 | |
anfontanelli | 3:fc26045926d9 | 353 | } |
anfontanelli | 3:fc26045926d9 | 354 | |
anfontanelli | 3:fc26045926d9 | 355 | void Rover::calibrateImu() |
anfontanelli | 3:fc26045926d9 | 356 | { |
anfontanelli | 3:fc26045926d9 | 357 | const int CALIBRATION = 500; |
anfontanelli | 3:fc26045926d9 | 358 | for(int i=0;i<50;i++) |
anfontanelli | 3:fc26045926d9 | 359 | { |
anfontanelli | 3:fc26045926d9 | 360 | mpu->getMotion6(&ax, &ay, &az, &gx, &gy, &gz); |
anfontanelli | 3:fc26045926d9 | 361 | } |
anfontanelli | 3:fc26045926d9 | 362 | |
anfontanelli | 3:fc26045926d9 | 363 | |
anfontanelli | 3:fc26045926d9 | 364 | printf(" START CALIBRATION \r\n"); |
anfontanelli | 3:fc26045926d9 | 365 | |
anfontanelli | 3:fc26045926d9 | 366 | |
anfontanelli | 3:fc26045926d9 | 367 | for(int i=0;i<CALIBRATION;i++) |
anfontanelli | 3:fc26045926d9 | 368 | { |
anfontanelli | 3:fc26045926d9 | 369 | mpu->getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //acquisisco i dati dal sensore |
anfontanelli | 3:fc26045926d9 | 370 | a_x=(float)ax/FS_a; //li converto in float, (CALCOLI EVITABILE SE INSTANZIASSI LE VARIABILI SUBITO COME FLOAT) |
anfontanelli | 3:fc26045926d9 | 371 | a_y=(float)ay/FS_a; |
anfontanelli | 3:fc26045926d9 | 372 | //a_z=(float)az/FS_a; |
anfontanelli | 3:fc26045926d9 | 373 | g_x=(float)gx/FS_g; |
anfontanelli | 3:fc26045926d9 | 374 | g_y=(float)gy/FS_g;; |
anfontanelli | 3:fc26045926d9 | 375 | g_z=(float)gz/FS_g;; |
anfontanelli | 3:fc26045926d9 | 376 | |
anfontanelli | 3:fc26045926d9 | 377 | accBias[0] += a_x; |
anfontanelli | 3:fc26045926d9 | 378 | accBias[1] += a_y; |
anfontanelli | 3:fc26045926d9 | 379 | gyroBias[0] += g_x; |
anfontanelli | 3:fc26045926d9 | 380 | gyroBias[1] += g_y; |
anfontanelli | 3:fc26045926d9 | 381 | gyroBias[2] += g_z; |
anfontanelli | 3:fc26045926d9 | 382 | |
anfontanelli | 3:fc26045926d9 | 383 | } |
anfontanelli | 3:fc26045926d9 | 384 | |
anfontanelli | 3:fc26045926d9 | 385 | //BIAS |
anfontanelli | 3:fc26045926d9 | 386 | accBias[0] = accBias[0]/CALIBRATION; |
anfontanelli | 3:fc26045926d9 | 387 | accBias[1] = accBias[1]/CALIBRATION; |
anfontanelli | 3:fc26045926d9 | 388 | gyroBias[0] = gyroBias[0]/CALIBRATION; |
anfontanelli | 3:fc26045926d9 | 389 | gyroBias[1] = gyroBias[1]/CALIBRATION; |
anfontanelli | 3:fc26045926d9 | 390 | gyroBias[2] = gyroBias[2]/CALIBRATION; |
anfontanelli | 3:fc26045926d9 | 391 | |
anfontanelli | 3:fc26045926d9 | 392 | |
anfontanelli | 3:fc26045926d9 | 393 | printf(" END CALIBRATION \r\n"); |
anfontanelli | 3:fc26045926d9 | 394 | |
anfontanelli | 3:fc26045926d9 | 395 | //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 | 396 | |
anfontanelli | 3:fc26045926d9 | 397 | |
anfontanelli | 3:fc26045926d9 | 398 | } |
anfontanelli | 3:fc26045926d9 | 399 | |
anfontanelli | 3:fc26045926d9 | 400 | |
anfontanelli | 3:fc26045926d9 | 401 | |
anfontanelli | 3:fc26045926d9 | 402 | void Rover::startEthComunication(){ |
anfontanelli | 3:fc26045926d9 | 403 | printf("Avvio la comunicazione ethernet \r\n"); |
anfontanelli | 3:fc26045926d9 | 404 | comunicationTimer.start(); |
anfontanelli | 3:fc26045926d9 | 405 | eth_tcp->connect(); |
anfontanelli | 3:fc26045926d9 | 406 | setState(1); |
anfontanelli | 3:fc26045926d9 | 407 | |
anfontanelli | 3:fc26045926d9 | 408 | } |
anfontanelli | 3:fc26045926d9 | 409 | |
anfontanelli | 3:fc26045926d9 | 410 | void Rover::ethComunicationUpdate(float _time, float _pitch, float _vels_a, float _vels_m, float _velf_a, float _velf_m) { |
anfontanelli | 3:fc26045926d9 | 411 | |
anfontanelli | 3:fc26045926d9 | 412 | if( eth_tcp->is_connected()){ |
anfontanelli | 3:fc26045926d9 | 413 | setState(2); |
anfontanelli | 3:fc26045926d9 | 414 | printf("ETH: connected!\r\n"); |
anfontanelli | 3:fc26045926d9 | 415 | sock_open = true; |
anfontanelli | 3:fc26045926d9 | 416 | }else{ |
anfontanelli | 3:fc26045926d9 | 417 | setState(1); |
anfontanelli | 3:fc26045926d9 | 418 | } |
anfontanelli | 3:fc26045926d9 | 419 | |
anfontanelli | 3:fc26045926d9 | 420 | eth_status = eth_tcp->recv_pkt(cmd, recv_vec, n_of_int); |
anfontanelli | 3:fc26045926d9 | 421 | |
anfontanelli | 3:fc26045926d9 | 422 | eth_time = comunicationTimer.read(); |
anfontanelli | 3:fc26045926d9 | 423 | //printf("%f\r\n", (double)(eth_time - eth_time_sample_received)); |
anfontanelli | 3:fc26045926d9 | 424 | |
anfontanelli | 3:fc26045926d9 | 425 | |
anfontanelli | 3:fc26045926d9 | 426 | if(eth_status){ |
anfontanelli | 3:fc26045926d9 | 427 | eth_time_sample_received = comunicationTimer.read(); |
anfontanelli | 3:fc26045926d9 | 428 | if(cmd == 's'){ |
anfontanelli | 3:fc26045926d9 | 429 | if(n_of_int == 4){ |
anfontanelli | 3:fc26045926d9 | 430 | eth_mutex.lock(); |
anfontanelli | 3:fc26045926d9 | 431 | forward_vel = recv_vec.get(); |
anfontanelli | 3:fc26045926d9 | 432 | pitch_d = recv_vec.get(); |
anfontanelli | 3:fc26045926d9 | 433 | //jointFront = recv_vec.get(); |
anfontanelli | 3:fc26045926d9 | 434 | //jointRetro = recv_vec.get(); |
anfontanelli | 3:fc26045926d9 | 435 | enableStab = recv_vec.get(); |
anfontanelli | 3:fc26045926d9 | 436 | enableCurv= recv_vec.get(); |
anfontanelli | 3:fc26045926d9 | 437 | eth_mutex.unlock(); |
anfontanelli | 3:fc26045926d9 | 438 | } |
anfontanelli | 3:fc26045926d9 | 439 | |
anfontanelli | 3:fc26045926d9 | 440 | eth_mutex.lock(); |
anfontanelli | 3:fc26045926d9 | 441 | vec_to_send.put(_time*1000); |
anfontanelli | 3:fc26045926d9 | 442 | vec_to_send.put(_pitch*1000); |
anfontanelli | 3:fc26045926d9 | 443 | vec_to_send.put(_vels_a*1000); |
anfontanelli | 3:fc26045926d9 | 444 | vec_to_send.put(_vels_m*1000); |
anfontanelli | 3:fc26045926d9 | 445 | vec_to_send.put(_velf_a*1000); |
anfontanelli | 3:fc26045926d9 | 446 | vec_to_send.put(_velf_m*1000); |
anfontanelli | 3:fc26045926d9 | 447 | |
anfontanelli | 3:fc26045926d9 | 448 | eth_mutex.unlock(); |
anfontanelli | 3:fc26045926d9 | 449 | eth_tcp->send_vec_of_int(vec_to_send); |
anfontanelli | 3:fc26045926d9 | 450 | |
anfontanelli | 3:fc26045926d9 | 451 | vec_to_send.clear(); |
anfontanelli | 3:fc26045926d9 | 452 | |
anfontanelli | 3:fc26045926d9 | 453 | cmd = ' '; |
anfontanelli | 3:fc26045926d9 | 454 | |
anfontanelli | 3:fc26045926d9 | 455 | } |
anfontanelli | 3:fc26045926d9 | 456 | } |
anfontanelli | 3:fc26045926d9 | 457 | |
anfontanelli | 3:fc26045926d9 | 458 | if((double)(eth_time - eth_time_sample_received)>eth_time_out && sock_open == true){ |
anfontanelli | 3:fc26045926d9 | 459 | eth_tcp->reset_connection(); |
anfontanelli | 3:fc26045926d9 | 460 | sock_open = false; |
anfontanelli | 3:fc26045926d9 | 461 | eth_mutex.lock(); |
anfontanelli | 3:fc26045926d9 | 462 | forward_vel = 0.0; |
anfontanelli | 3:fc26045926d9 | 463 | pitch_d = 0.0; |
anfontanelli | 3:fc26045926d9 | 464 | //jointFront = 0.0; |
anfontanelli | 3:fc26045926d9 | 465 | //jointRetro = 0.0; |
anfontanelli | 3:fc26045926d9 | 466 | enableCurv = 0; |
anfontanelli | 3:fc26045926d9 | 467 | eth_mutex.unlock(); |
anfontanelli | 3:fc26045926d9 | 468 | } |
anfontanelli | 3:fc26045926d9 | 469 | |
anfontanelli | 3:fc26045926d9 | 470 | |
anfontanelli | 3:fc26045926d9 | 471 | } |
anfontanelli | 3:fc26045926d9 | 472 | |
anfontanelli | 3:fc26045926d9 | 473 | int Rover::getEthState(){ |
anfontanelli | 3:fc26045926d9 | 474 | int state; |
anfontanelli | 3:fc26045926d9 | 475 | |
anfontanelli | 3:fc26045926d9 | 476 | eth_mutex.lock(); |
anfontanelli | 3:fc26045926d9 | 477 | state = eth_state; |
anfontanelli | 3:fc26045926d9 | 478 | eth_mutex.unlock(); |
anfontanelli | 3:fc26045926d9 | 479 | |
anfontanelli | 3:fc26045926d9 | 480 | return state; |
anfontanelli | 3:fc26045926d9 | 481 | } |
anfontanelli | 3:fc26045926d9 | 482 | |
anfontanelli | 3:fc26045926d9 | 483 | void Rover::setState(int state){ |
anfontanelli | 3:fc26045926d9 | 484 | |
anfontanelli | 3:fc26045926d9 | 485 | eth_mutex.lock(); |
anfontanelli | 3:fc26045926d9 | 486 | eth_state = state; |
anfontanelli | 3:fc26045926d9 | 487 | eth_mutex.unlock(); |
anfontanelli | 3:fc26045926d9 | 488 | |
anfontanelli | 3:fc26045926d9 | 489 | } |
anfontanelli | 3:fc26045926d9 | 490 | |
anfontanelli | 3:fc26045926d9 | 491 |