a
Rover.cpp@0:65943c77d1dc, 2021-10-04 (annotated)
- Committer:
- marcodesilva
- Date:
- Mon Oct 04 12:52:17 2021 +0000
- Revision:
- 0:65943c77d1dc
a;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
marcodesilva | 0:65943c77d1dc | 1 | #include "Rover.h" |
marcodesilva | 0:65943c77d1dc | 2 | #include <iostream> |
marcodesilva | 0:65943c77d1dc | 3 | |
marcodesilva | 0:65943c77d1dc | 4 | Rover::Rover(){ |
marcodesilva | 0:65943c77d1dc | 5 | |
marcodesilva | 0:65943c77d1dc | 6 | pipeDir = 0; |
marcodesilva | 0:65943c77d1dc | 7 | |
marcodesilva | 0:65943c77d1dc | 8 | S = Eigen::Matrix<float, 4, 3>::Zero(); |
marcodesilva | 0:65943c77d1dc | 9 | |
marcodesilva | 0:65943c77d1dc | 10 | ionMcFront = new IONMcMotors(frontBoardAddress,ionMcBoudRate, PB_9, PB_8, frontMotorGearBoxRatio, frontEncoderPulse, frontKt, frontKt); |
marcodesilva | 0:65943c77d1dc | 11 | ionMcRetro = new IONMcMotors(retroBoardAddress,ionMcBoudRate, PG_14, PG_9, retroMotorGearBoxRatio, retroEncoderPulse, retroKt, retroKt); |
marcodesilva | 0:65943c77d1dc | 12 | |
marcodesilva | 0:65943c77d1dc | 13 | frontActuonix = new Servo(PE_11); |
marcodesilva | 0:65943c77d1dc | 14 | retroActuonix = new Servo(PE_9); |
marcodesilva | 0:65943c77d1dc | 15 | mpu = new MPU6050(PF_15, PF_14); |
marcodesilva | 0:65943c77d1dc | 16 | |
marcodesilva | 0:65943c77d1dc | 17 | tofs = new TOFs(); |
marcodesilva | 0:65943c77d1dc | 18 | |
marcodesilva | 0:65943c77d1dc | 19 | enableCurv = 1; |
marcodesilva | 0:65943c77d1dc | 20 | g_x_old = 0.0; |
marcodesilva | 0:65943c77d1dc | 21 | |
marcodesilva | 0:65943c77d1dc | 22 | } |
marcodesilva | 0:65943c77d1dc | 23 | |
marcodesilva | 0:65943c77d1dc | 24 | void Rover::initializeTofs(){ |
marcodesilva | 0:65943c77d1dc | 25 | |
marcodesilva | 0:65943c77d1dc | 26 | tofs->TOFs_init(); |
marcodesilva | 0:65943c77d1dc | 27 | ThisThread::sleep_for(1000); |
marcodesilva | 0:65943c77d1dc | 28 | //tofs->TOFs_offset(); |
marcodesilva | 0:65943c77d1dc | 29 | |
marcodesilva | 0:65943c77d1dc | 30 | |
marcodesilva | 0:65943c77d1dc | 31 | } |
marcodesilva | 0:65943c77d1dc | 32 | |
marcodesilva | 0:65943c77d1dc | 33 | void Rover::setParameters(Eigen::VectorXd roverParameters){ |
marcodesilva | 0:65943c77d1dc | 34 | r_frontWheel = roverParameters(0); |
marcodesilva | 0:65943c77d1dc | 35 | r_retroWheel = roverParameters(1); |
marcodesilva | 0:65943c77d1dc | 36 | pipeCurve_I = roverParameters(2); |
marcodesilva | 0:65943c77d1dc | 37 | pipeCurve_E = roverParameters(3); |
marcodesilva | 0:65943c77d1dc | 38 | pipeCurve_M = roverParameters(4); |
marcodesilva | 0:65943c77d1dc | 39 | retroFrontCentralDistance = roverParameters(5); |
marcodesilva | 0:65943c77d1dc | 40 | wheelsCntactPointDistanceFromPipeCenter = roverParameters(6); |
marcodesilva | 0:65943c77d1dc | 41 | |
marcodesilva | 0:65943c77d1dc | 42 | } |
marcodesilva | 0:65943c77d1dc | 43 | |
marcodesilva | 0:65943c77d1dc | 44 | |
marcodesilva | 0:65943c77d1dc | 45 | float Rover::computeStabilizationVel(float pitch_d, float pitch, float pitch_vel, float Kp, float Kd){ |
marcodesilva | 0:65943c77d1dc | 46 | |
marcodesilva | 0:65943c77d1dc | 47 | vels_a = -Kp*M_PI*(pitch_d-pitch)/180 -Kd*M_PI*(0-pitch_vel)/180; |
marcodesilva | 0:65943c77d1dc | 48 | return vels_a; |
marcodesilva | 0:65943c77d1dc | 49 | |
marcodesilva | 0:65943c77d1dc | 50 | } |
marcodesilva | 0:65943c77d1dc | 51 | |
marcodesilva | 0:65943c77d1dc | 52 | |
marcodesilva | 0:65943c77d1dc | 53 | void Rover::acquireTofs(float &roverLaserFrontDx, float &roverLaserFrontSx, float &roverLaserRetroDx, float &roverLaserRetroSx, float &roverFrontDistance, float &roverRetroDistance){ |
marcodesilva | 0:65943c77d1dc | 54 | |
marcodesilva | 0:65943c77d1dc | 55 | tofs->TOFs_acquireFiltr(); |
marcodesilva | 0:65943c77d1dc | 56 | roverLaserFrontDx = tofs->getLaserFrontDx(); |
marcodesilva | 0:65943c77d1dc | 57 | roverLaserFrontSx = tofs->getLaserFrontSx(); |
marcodesilva | 0:65943c77d1dc | 58 | roverLaserRetroDx = tofs->getLaserRetroDx(); |
marcodesilva | 0:65943c77d1dc | 59 | roverLaserRetroSx = tofs->getLaserRetroSx(); |
marcodesilva | 0:65943c77d1dc | 60 | |
marcodesilva | 0:65943c77d1dc | 61 | roverFrontDistance = tofs->getFrontDistance(); |
marcodesilva | 0:65943c77d1dc | 62 | roverRetroDistance = tofs->getRetroDistance(); |
marcodesilva | 0:65943c77d1dc | 63 | |
marcodesilva | 0:65943c77d1dc | 64 | |
marcodesilva | 0:65943c77d1dc | 65 | } |
marcodesilva | 0:65943c77d1dc | 66 | |
marcodesilva | 0:65943c77d1dc | 67 | void Rover::computeCentralJointsFromTofs(float &roverLaserFrontDx,float &roverLaserFrontSx,float &roverLaserRetroDx,float &roverLaserRetroSx){ |
marcodesilva | 0:65943c77d1dc | 68 | |
marcodesilva | 0:65943c77d1dc | 69 | float frontDistance; |
marcodesilva | 0:65943c77d1dc | 70 | float retroDistance; |
marcodesilva | 0:65943c77d1dc | 71 | |
marcodesilva | 0:65943c77d1dc | 72 | acquireTofs(roverLaserFrontDx, roverLaserFrontSx, roverLaserRetroDx, roverLaserRetroSx, roverFrontDistance, roverRetroDistance); |
marcodesilva | 0:65943c77d1dc | 73 | |
marcodesilva | 0:65943c77d1dc | 74 | frontDistance = roverFrontDistance; |
marcodesilva | 0:65943c77d1dc | 75 | retroDistance = roverRetroDistance; |
marcodesilva | 0:65943c77d1dc | 76 | float frontVel = 0.05*frontDistance; |
marcodesilva | 0:65943c77d1dc | 77 | float retroVel = -0.04*retroDistance; |
marcodesilva | 0:65943c77d1dc | 78 | |
marcodesilva | 0:65943c77d1dc | 79 | if(enableCurv==1){ |
marcodesilva | 0:65943c77d1dc | 80 | frontPos = frontPos + frontVel*0.02; |
marcodesilva | 0:65943c77d1dc | 81 | retroPos = retroPos + retroVel*0.02; |
marcodesilva | 0:65943c77d1dc | 82 | }else{ |
marcodesilva | 0:65943c77d1dc | 83 | frontPos = 0.0; |
marcodesilva | 0:65943c77d1dc | 84 | retroPos = 0.0; |
marcodesilva | 0:65943c77d1dc | 85 | } |
marcodesilva | 0:65943c77d1dc | 86 | |
marcodesilva | 0:65943c77d1dc | 87 | if(frontPos > 0.5) frontPos=0.5; |
marcodesilva | 0:65943c77d1dc | 88 | if(frontPos < -0.5) frontPos=-0.5; |
marcodesilva | 0:65943c77d1dc | 89 | if(retroPos > 0.5) retroPos=0.5; |
marcodesilva | 0:65943c77d1dc | 90 | if(retroPos < -0.5) retroPos=-0.5; |
marcodesilva | 0:65943c77d1dc | 91 | |
marcodesilva | 0:65943c77d1dc | 92 | setCentralJointsAngle(frontPos,retroPos); |
marcodesilva | 0:65943c77d1dc | 93 | |
marcodesilva | 0:65943c77d1dc | 94 | } |
marcodesilva | 0:65943c77d1dc | 95 | |
marcodesilva | 0:65943c77d1dc | 96 | |
marcodesilva | 0:65943c77d1dc | 97 | void Rover::initializeImu(){ |
marcodesilva | 0:65943c77d1dc | 98 | |
marcodesilva | 0:65943c77d1dc | 99 | printf("Initialize IMU \r\n"); |
marcodesilva | 0:65943c77d1dc | 100 | //IMU |
marcodesilva | 0:65943c77d1dc | 101 | |
marcodesilva | 0:65943c77d1dc | 102 | accBias[0] = 0.0; |
marcodesilva | 0:65943c77d1dc | 103 | accBias[1] = 0.0; |
marcodesilva | 0:65943c77d1dc | 104 | accBias[2] = 0.0; |
marcodesilva | 0:65943c77d1dc | 105 | gyroBias[0] = 0.0; |
marcodesilva | 0:65943c77d1dc | 106 | gyroBias[1] = 0.0; |
marcodesilva | 0:65943c77d1dc | 107 | gyroBias[2] = 0.0; |
marcodesilva | 0:65943c77d1dc | 108 | |
marcodesilva | 0:65943c77d1dc | 109 | mpu->initialize(); |
marcodesilva | 0:65943c77d1dc | 110 | bool mpu6050TestResult = mpu->testConnection(); |
marcodesilva | 0:65943c77d1dc | 111 | if(mpu6050TestResult) { |
marcodesilva | 0:65943c77d1dc | 112 | printf("MPU6050 test passed \r\n"); |
marcodesilva | 0:65943c77d1dc | 113 | } else { |
marcodesilva | 0:65943c77d1dc | 114 | printf("MPU6050 test failed \r\n"); |
marcodesilva | 0:65943c77d1dc | 115 | } |
marcodesilva | 0:65943c77d1dc | 116 | |
marcodesilva | 0:65943c77d1dc | 117 | |
marcodesilva | 0:65943c77d1dc | 118 | calibrateImu(); |
marcodesilva | 0:65943c77d1dc | 119 | } |
marcodesilva | 0:65943c77d1dc | 120 | |
marcodesilva | 0:65943c77d1dc | 121 | void Rover::setRoverVelocity(float forward_speed, float stabilization_speed, float asset_correction_speed, float pipe_radius, float maxWheelAcceleration){ |
marcodesilva | 0:65943c77d1dc | 122 | |
marcodesilva | 0:65943c77d1dc | 123 | Eigen::Vector3f cartesianSpeed; |
marcodesilva | 0:65943c77d1dc | 124 | Eigen::Vector4f wheelsSpeed; |
marcodesilva | 0:65943c77d1dc | 125 | |
marcodesilva | 0:65943c77d1dc | 126 | updateRoverKin(pipe_radius, pipeDir); |
marcodesilva | 0:65943c77d1dc | 127 | |
marcodesilva | 0:65943c77d1dc | 128 | cartesianSpeed << forward_speed, stabilization_speed, asset_correction_speed; |
marcodesilva | 0:65943c77d1dc | 129 | wheelsSpeed = S*cartesianSpeed; |
marcodesilva | 0:65943c77d1dc | 130 | //std::cout << "des: " << wheelsSpeed.transpose() << std::endl; |
marcodesilva | 0:65943c77d1dc | 131 | ionMcFront->setSpeed(1, wheelsSpeed[0],maxWheelAcceleration); |
marcodesilva | 0:65943c77d1dc | 132 | |
marcodesilva | 0:65943c77d1dc | 133 | ionMcFront->setSpeed(2, wheelsSpeed[1],maxWheelAcceleration); |
marcodesilva | 0:65943c77d1dc | 134 | |
marcodesilva | 0:65943c77d1dc | 135 | ionMcRetro->setSpeed(1,wheelsSpeed[2],maxWheelAcceleration); |
marcodesilva | 0:65943c77d1dc | 136 | |
marcodesilva | 0:65943c77d1dc | 137 | ionMcRetro->setSpeed(2,wheelsSpeed[3],maxWheelAcceleration); |
marcodesilva | 0:65943c77d1dc | 138 | |
marcodesilva | 0:65943c77d1dc | 139 | } |
marcodesilva | 0:65943c77d1dc | 140 | |
marcodesilva | 0:65943c77d1dc | 141 | |
marcodesilva | 0:65943c77d1dc | 142 | |
marcodesilva | 0:65943c77d1dc | 143 | void Rover::getRoverVelocity(float &forward_speed, float &stabilization_speed, float &asset_correction_speed, float pipe_radius){ |
marcodesilva | 0:65943c77d1dc | 144 | |
marcodesilva | 0:65943c77d1dc | 145 | Eigen::Vector3f cartesianSpeed; |
marcodesilva | 0:65943c77d1dc | 146 | Eigen::Vector4f wheelsSpeed; |
marcodesilva | 0:65943c77d1dc | 147 | |
marcodesilva | 0:65943c77d1dc | 148 | float speedM1 = 0.0; |
marcodesilva | 0:65943c77d1dc | 149 | float speedM2 = 0.0; |
marcodesilva | 0:65943c77d1dc | 150 | float speedM3 = 0.0; |
marcodesilva | 0:65943c77d1dc | 151 | float speedM4 = 0.0; |
marcodesilva | 0:65943c77d1dc | 152 | |
marcodesilva | 0:65943c77d1dc | 153 | updateRoverKin(pipe_radius, pipeDir); |
marcodesilva | 0:65943c77d1dc | 154 | |
marcodesilva | 0:65943c77d1dc | 155 | getMotorsSpeed(speedM1, speedM2, speedM3, speedM4); |
marcodesilva | 0:65943c77d1dc | 156 | |
marcodesilva | 0:65943c77d1dc | 157 | wheelsSpeed << speedM1, speedM2, speedM3, speedM4; |
marcodesilva | 0:65943c77d1dc | 158 | |
marcodesilva | 0:65943c77d1dc | 159 | //td::cout << "mis: " << wheelsSpeed.transpose() << std::endl; |
marcodesilva | 0:65943c77d1dc | 160 | |
marcodesilva | 0:65943c77d1dc | 161 | |
marcodesilva | 0:65943c77d1dc | 162 | cartesianSpeed = S_inv*wheelsSpeed; |
marcodesilva | 0:65943c77d1dc | 163 | |
marcodesilva | 0:65943c77d1dc | 164 | forward_speed = cartesianSpeed[0]; |
marcodesilva | 0:65943c77d1dc | 165 | stabilization_speed = cartesianSpeed[1]; |
marcodesilva | 0:65943c77d1dc | 166 | asset_correction_speed = cartesianSpeed[2]; |
marcodesilva | 0:65943c77d1dc | 167 | |
marcodesilva | 0:65943c77d1dc | 168 | } |
marcodesilva | 0:65943c77d1dc | 169 | |
marcodesilva | 0:65943c77d1dc | 170 | void Rover::getRoverWheelsVelocity(float &front_dx, float &front_sx, float &retro_dx, float &retro_sx){ |
marcodesilva | 0:65943c77d1dc | 171 | |
marcodesilva | 0:65943c77d1dc | 172 | Eigen::Vector3f cartesianSpeed; |
marcodesilva | 0:65943c77d1dc | 173 | Eigen::Vector4f wheelsSpeed; |
marcodesilva | 0:65943c77d1dc | 174 | |
marcodesilva | 0:65943c77d1dc | 175 | float speedM1 = 0.0; |
marcodesilva | 0:65943c77d1dc | 176 | float speedM2 = 0.0; |
marcodesilva | 0:65943c77d1dc | 177 | float speedM3 = 0.0; |
marcodesilva | 0:65943c77d1dc | 178 | float speedM4 = 0.0; |
marcodesilva | 0:65943c77d1dc | 179 | |
marcodesilva | 0:65943c77d1dc | 180 | getMotorsSpeed(speedM1, speedM2, speedM3, speedM4); |
marcodesilva | 0:65943c77d1dc | 181 | |
marcodesilva | 0:65943c77d1dc | 182 | front_dx = speedM1; |
marcodesilva | 0:65943c77d1dc | 183 | front_sx = speedM2; |
marcodesilva | 0:65943c77d1dc | 184 | retro_dx = speedM3; |
marcodesilva | 0:65943c77d1dc | 185 | retro_sx = speedM4; |
marcodesilva | 0:65943c77d1dc | 186 | |
marcodesilva | 0:65943c77d1dc | 187 | } |
marcodesilva | 0:65943c77d1dc | 188 | |
marcodesilva | 0:65943c77d1dc | 189 | void Rover::updateRoverKin(float pipe_radius, int pipeDir){ |
marcodesilva | 0:65943c77d1dc | 190 | float rf = r_frontWheel; |
marcodesilva | 0:65943c77d1dc | 191 | float rr = r_retroWheel; |
marcodesilva | 0:65943c77d1dc | 192 | float l = retroFrontCentralDistance; |
marcodesilva | 0:65943c77d1dc | 193 | float L = wheelsCntactPointDistanceFromPipeCenter; |
marcodesilva | 0:65943c77d1dc | 194 | float pr = pipe_radius; |
marcodesilva | 0:65943c77d1dc | 195 | float R_dx = 0.0; |
marcodesilva | 0:65943c77d1dc | 196 | float R_sx = 0.0; |
marcodesilva | 0:65943c77d1dc | 197 | float R_m = 0.0; |
marcodesilva | 0:65943c77d1dc | 198 | if(pipeDir ==0){ |
marcodesilva | 0:65943c77d1dc | 199 | R_dx = 1.0; |
marcodesilva | 0:65943c77d1dc | 200 | R_sx = 1.0; |
marcodesilva | 0:65943c77d1dc | 201 | R_m = 1.0; |
marcodesilva | 0:65943c77d1dc | 202 | }else if(pipeDir == 1){ |
marcodesilva | 0:65943c77d1dc | 203 | R_dx = pipeCurve_I; |
marcodesilva | 0:65943c77d1dc | 204 | R_sx = pipeCurve_E; |
marcodesilva | 0:65943c77d1dc | 205 | R_m = pipeCurve_M; |
marcodesilva | 0:65943c77d1dc | 206 | }else if(pipeDir == -1){ |
marcodesilva | 0:65943c77d1dc | 207 | R_dx = pipeCurve_E; |
marcodesilva | 0:65943c77d1dc | 208 | R_sx = pipeCurve_I; |
marcodesilva | 0:65943c77d1dc | 209 | R_m = pipeCurve_M; |
marcodesilva | 0:65943c77d1dc | 210 | |
marcodesilva | 0:65943c77d1dc | 211 | } |
marcodesilva | 0:65943c77d1dc | 212 | //AGGIORNARE la S |
marcodesilva | 0:65943c77d1dc | 213 | S << 1.0*R_dx/(rf*R_m), pr/rf, -(L+l)/rf, |
marcodesilva | 0:65943c77d1dc | 214 | -1.0*R_sx/(rf*R_m), pr/rf, -(L+l)/rf, |
marcodesilva | 0:65943c77d1dc | 215 | 1.0*R_dx/(rr*R_m), -pr/rr, -(L+l)/rr, |
marcodesilva | 0:65943c77d1dc | 216 | -1.0*R_sx/(rr*R_m), -pr/rr, -(L+l)/rr; |
marcodesilva | 0:65943c77d1dc | 217 | |
marcodesilva | 0:65943c77d1dc | 218 | |
marcodesilva | 0:65943c77d1dc | 219 | Eigen::FullPivLU<Matrix3f> Core_S(S.transpose()*S); |
marcodesilva | 0:65943c77d1dc | 220 | S_inv = Core_S.inverse()*S.transpose(); |
marcodesilva | 0:65943c77d1dc | 221 | |
marcodesilva | 0:65943c77d1dc | 222 | } |
marcodesilva | 0:65943c77d1dc | 223 | |
marcodesilva | 0:65943c77d1dc | 224 | void Rover::getMotorsTorque(float &torM1, float &torM2, float &torM3, float &torM4){ |
marcodesilva | 0:65943c77d1dc | 225 | |
marcodesilva | 0:65943c77d1dc | 226 | |
marcodesilva | 0:65943c77d1dc | 227 | torM1 = ionMcFront->getMotorTorque(1); |
marcodesilva | 0:65943c77d1dc | 228 | torM2 = ionMcFront->getMotorTorque(2); |
marcodesilva | 0:65943c77d1dc | 229 | torM3 = ionMcRetro->getMotorTorque(1); |
marcodesilva | 0:65943c77d1dc | 230 | torM4 = ionMcRetro->getMotorTorque(2); |
marcodesilva | 0:65943c77d1dc | 231 | } |
marcodesilva | 0:65943c77d1dc | 232 | |
marcodesilva | 0:65943c77d1dc | 233 | void Rover::getMotorsSpeed(float &speedM1, float &speedM2, float &speedM3, float &speedM4){ |
marcodesilva | 0:65943c77d1dc | 234 | |
marcodesilva | 0:65943c77d1dc | 235 | speedM1 = ionMcFront->getMotorSpeed(1); |
marcodesilva | 0:65943c77d1dc | 236 | speedM2 = ionMcFront->getMotorSpeed(2); |
marcodesilva | 0:65943c77d1dc | 237 | speedM3 = ionMcRetro->getMotorSpeed(1); |
marcodesilva | 0:65943c77d1dc | 238 | speedM4 = ionMcRetro->getMotorSpeed(2); |
marcodesilva | 0:65943c77d1dc | 239 | |
marcodesilva | 0:65943c77d1dc | 240 | } |
marcodesilva | 0:65943c77d1dc | 241 | |
marcodesilva | 0:65943c77d1dc | 242 | void Rover::setCentralJointsAngle(float jointFront, float jointRetro){ |
marcodesilva | 0:65943c77d1dc | 243 | |
marcodesilva | 0:65943c77d1dc | 244 | float LmRestPosFront = (9.0)/1000; //9 |
marcodesilva | 0:65943c77d1dc | 245 | float LmRestPosRetro = (9.0-0.5)/1000; //9 |
marcodesilva | 0:65943c77d1dc | 246 | float maxLenght = 0.02; |
marcodesilva | 0:65943c77d1dc | 247 | |
marcodesilva | 0:65943c77d1dc | 248 | float LmFront = LmRestPosFront - centralJoint2actuonix(jointFront); |
marcodesilva | 0:65943c77d1dc | 249 | float LmRetro = LmRestPosRetro - centralJoint2actuonix(jointRetro); |
marcodesilva | 0:65943c77d1dc | 250 | |
marcodesilva | 0:65943c77d1dc | 251 | frontActuonix->write(1.0 - LmFront/maxLenght); |
marcodesilva | 0:65943c77d1dc | 252 | retroActuonix->write(1.0 - LmRetro/maxLenght); |
marcodesilva | 0:65943c77d1dc | 253 | |
marcodesilva | 0:65943c77d1dc | 254 | if (jointFront > 3.14*5/180){ |
marcodesilva | 0:65943c77d1dc | 255 | pipeDir = -1; |
marcodesilva | 0:65943c77d1dc | 256 | }else if (jointFront < -3.14*5/180){ |
marcodesilva | 0:65943c77d1dc | 257 | pipeDir = 1; |
marcodesilva | 0:65943c77d1dc | 258 | }else{ |
marcodesilva | 0:65943c77d1dc | 259 | pipeDir = 0; |
marcodesilva | 0:65943c77d1dc | 260 | } |
marcodesilva | 0:65943c77d1dc | 261 | |
marcodesilva | 0:65943c77d1dc | 262 | pipeDir = 0;///////////////////////////////////////////////////////////// |
marcodesilva | 0:65943c77d1dc | 263 | |
marcodesilva | 0:65943c77d1dc | 264 | |
marcodesilva | 0:65943c77d1dc | 265 | } |
marcodesilva | 0:65943c77d1dc | 266 | |
marcodesilva | 0:65943c77d1dc | 267 | float Rover::centralJoint2actuonix(float jointAngle){ |
marcodesilva | 0:65943c77d1dc | 268 | |
marcodesilva | 0:65943c77d1dc | 269 | float alpha = deg2rad(34.4); |
marcodesilva | 0:65943c77d1dc | 270 | float L1 = 23.022/1000; |
marcodesilva | 0:65943c77d1dc | 271 | float L2 = 62.037/1000; |
marcodesilva | 0:65943c77d1dc | 272 | float L3 = 51.013/1000; |
marcodesilva | 0:65943c77d1dc | 273 | float L4 = 4.939/1000; |
marcodesilva | 0:65943c77d1dc | 274 | |
marcodesilva | 0:65943c77d1dc | 275 | float l1 = L1*sin(alpha + jointAngle); |
marcodesilva | 0:65943c77d1dc | 276 | float l2 = L1*cos(alpha + jointAngle); |
marcodesilva | 0:65943c77d1dc | 277 | float h = sqrt( (L2 - l1)*(L2 - l1) + (l2 - L4)*(l2 - L4) ); |
marcodesilva | 0:65943c77d1dc | 278 | return L3 - h; |
marcodesilva | 0:65943c77d1dc | 279 | |
marcodesilva | 0:65943c77d1dc | 280 | } |
marcodesilva | 0:65943c77d1dc | 281 | |
marcodesilva | 0:65943c77d1dc | 282 | |
marcodesilva | 0:65943c77d1dc | 283 | float Rover::deg2rad(float deg) { |
marcodesilva | 0:65943c77d1dc | 284 | return deg * M_PI / 180.0; |
marcodesilva | 0:65943c77d1dc | 285 | } |
marcodesilva | 0:65943c77d1dc | 286 | |
marcodesilva | 0:65943c77d1dc | 287 | |
marcodesilva | 0:65943c77d1dc | 288 | |
marcodesilva | 0:65943c77d1dc | 289 | void Rover::calcImuAngles(float& pitch, float& pitch_vel, float& roll, float dtImu) |
marcodesilva | 0:65943c77d1dc | 290 | { |
marcodesilva | 0:65943c77d1dc | 291 | int16_t ax = 0; |
marcodesilva | 0:65943c77d1dc | 292 | int16_t ay = 0; |
marcodesilva | 0:65943c77d1dc | 293 | int16_t az = 0; |
marcodesilva | 0:65943c77d1dc | 294 | int16_t gx = 0; |
marcodesilva | 0:65943c77d1dc | 295 | int16_t gy = 0; |
marcodesilva | 0:65943c77d1dc | 296 | int16_t gz = 0; |
marcodesilva | 0:65943c77d1dc | 297 | float pitchAcc = 0.0; |
marcodesilva | 0:65943c77d1dc | 298 | float rollAcc = 0.0; |
marcodesilva | 0:65943c77d1dc | 299 | float pitchAcc_p = 0.0; |
marcodesilva | 0:65943c77d1dc | 300 | float rollAcc_p = 0.0; |
marcodesilva | 0:65943c77d1dc | 301 | float pitch_integrated = 0.0; |
marcodesilva | 0:65943c77d1dc | 302 | float roll_integrated = 0.0; |
marcodesilva | 0:65943c77d1dc | 303 | mpu->getMotion6(ax, ay, az, gx, gy, gz); //acquisisco i dati dal sensore |
marcodesilva | 0:65943c77d1dc | 304 | float a_x=float(ax)/FS_a - accBias[0]; //li converto in float, (CALCOLI EVITABILE SE INSTANZIASSI LE VARIABILI SUBITO COME FLOAT) |
marcodesilva | 0:65943c77d1dc | 305 | float a_y=float(ay)/FS_a - accBias[1]; |
marcodesilva | 0:65943c77d1dc | 306 | float a_z=float(az)/FS_a; |
marcodesilva | 0:65943c77d1dc | 307 | float g_x=float(gx)/FS_g - gyroBias[0]; |
marcodesilva | 0:65943c77d1dc | 308 | float g_y=float(gy)/FS_g - gyroBias[1]; |
marcodesilva | 0:65943c77d1dc | 309 | float g_z=float(gz)/FS_g - gyroBias[2]; |
marcodesilva | 0:65943c77d1dc | 310 | if(abs(g_x) > 100.0){ |
marcodesilva | 0:65943c77d1dc | 311 | pitch_vel = 0; |
marcodesilva | 0:65943c77d1dc | 312 | }else{ |
marcodesilva | 0:65943c77d1dc | 313 | pitch_vel = g_x; |
marcodesilva | 0:65943c77d1dc | 314 | } |
marcodesilva | 0:65943c77d1dc | 315 | /*g_x_old = g_x;*/ |
marcodesilva | 0:65943c77d1dc | 316 | //pitch_vel = g_x; |
marcodesilva | 0:65943c77d1dc | 317 | |
marcodesilva | 0:65943c77d1dc | 318 | |
marcodesilva | 0:65943c77d1dc | 319 | 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 |
marcodesilva | 0:65943c77d1dc | 320 | roll_integrated = g_y * dtImu; // Angle around the Y-axis //uso g_y perchè ho il rollio ruotando atttorno all'asse y |
marcodesilva | 0:65943c77d1dc | 321 | |
marcodesilva | 0:65943c77d1dc | 322 | 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 |
marcodesilva | 0:65943c77d1dc | 323 | rollAcc = -atan2f(a_x, sqrt(a_y*a_y + a_z*a_z))*180/M_PI; // |
marcodesilva | 0:65943c77d1dc | 324 | |
marcodesilva | 0:65943c77d1dc | 325 | if ( abs( pitchAcc - pitchAcc_p ) > 10){ |
marcodesilva | 0:65943c77d1dc | 326 | pitchAcc = pitch; |
marcodesilva | 0:65943c77d1dc | 327 | } |
marcodesilva | 0:65943c77d1dc | 328 | |
marcodesilva | 0:65943c77d1dc | 329 | if ( abs( rollAcc - rollAcc_p ) > 10){ |
marcodesilva | 0:65943c77d1dc | 330 | rollAcc = roll; |
marcodesilva | 0:65943c77d1dc | 331 | } |
marcodesilva | 0:65943c77d1dc | 332 | |
marcodesilva | 0:65943c77d1dc | 333 | // Apply Complementary Filter |
marcodesilva | 0:65943c77d1dc | 334 | pitch = 0.95f*( pitch + pitch_integrated) + 0.05f*pitchAcc; |
marcodesilva | 0:65943c77d1dc | 335 | roll = 0.95f*( roll + roll_integrated) + 0.05f*(rollAcc); |
marcodesilva | 0:65943c77d1dc | 336 | |
marcodesilva | 0:65943c77d1dc | 337 | pitchAcc_p = pitchAcc; |
marcodesilva | 0:65943c77d1dc | 338 | rollAcc_p = rollAcc; |
marcodesilva | 0:65943c77d1dc | 339 | |
marcodesilva | 0:65943c77d1dc | 340 | |
marcodesilva | 0:65943c77d1dc | 341 | } |
marcodesilva | 0:65943c77d1dc | 342 | |
marcodesilva | 0:65943c77d1dc | 343 | void Rover::calibrateImu() |
marcodesilva | 0:65943c77d1dc | 344 | { |
marcodesilva | 0:65943c77d1dc | 345 | |
marcodesilva | 0:65943c77d1dc | 346 | //Imu variables |
marcodesilva | 0:65943c77d1dc | 347 | int16_t ax = 0; |
marcodesilva | 0:65943c77d1dc | 348 | int16_t ay = 0; |
marcodesilva | 0:65943c77d1dc | 349 | int16_t az = 0; |
marcodesilva | 0:65943c77d1dc | 350 | int16_t gx = 0; |
marcodesilva | 0:65943c77d1dc | 351 | int16_t gy = 0; |
marcodesilva | 0:65943c77d1dc | 352 | int16_t gz = 0; |
marcodesilva | 0:65943c77d1dc | 353 | float a_x,a_y,a_z; |
marcodesilva | 0:65943c77d1dc | 354 | float g_x, g_y, g_z; |
marcodesilva | 0:65943c77d1dc | 355 | |
marcodesilva | 0:65943c77d1dc | 356 | const int CALIBRATION = 500; |
marcodesilva | 0:65943c77d1dc | 357 | |
marcodesilva | 0:65943c77d1dc | 358 | for(int i=0;i<50;i++) |
marcodesilva | 0:65943c77d1dc | 359 | { |
marcodesilva | 0:65943c77d1dc | 360 | mpu->getMotion6(ax, ay, az, gx, gy, gz); |
marcodesilva | 0:65943c77d1dc | 361 | } |
marcodesilva | 0:65943c77d1dc | 362 | |
marcodesilva | 0:65943c77d1dc | 363 | |
marcodesilva | 0:65943c77d1dc | 364 | printf(" START CALIBRATION \r\n"); |
marcodesilva | 0:65943c77d1dc | 365 | |
marcodesilva | 0:65943c77d1dc | 366 | |
marcodesilva | 0:65943c77d1dc | 367 | for(int i=0;i<CALIBRATION;i++) |
marcodesilva | 0:65943c77d1dc | 368 | { |
marcodesilva | 0:65943c77d1dc | 369 | mpu->getMotion6(ax, ay, az, gx, gy, gz); //acquisisco i dati dal sensore |
marcodesilva | 0:65943c77d1dc | 370 | a_x=(float)ax/FS_a; //li converto in float, (CALCOLI EVITABILE SE INSTANZIASSI LE VARIABILI SUBITO COME FLOAT) |
marcodesilva | 0:65943c77d1dc | 371 | a_y=(float)ay/FS_a; |
marcodesilva | 0:65943c77d1dc | 372 | //a_z=(float)az/FS_a; |
marcodesilva | 0:65943c77d1dc | 373 | g_x=(float)gx/FS_g; |
marcodesilva | 0:65943c77d1dc | 374 | g_y=(float)gy/FS_g;; |
marcodesilva | 0:65943c77d1dc | 375 | g_z=(float)gz/FS_g;; |
marcodesilva | 0:65943c77d1dc | 376 | |
marcodesilva | 0:65943c77d1dc | 377 | accBias[0] += a_x; |
marcodesilva | 0:65943c77d1dc | 378 | accBias[1] += a_y; |
marcodesilva | 0:65943c77d1dc | 379 | gyroBias[0] += g_x; |
marcodesilva | 0:65943c77d1dc | 380 | gyroBias[1] += g_y; |
marcodesilva | 0:65943c77d1dc | 381 | gyroBias[2] += g_z; |
marcodesilva | 0:65943c77d1dc | 382 | |
marcodesilva | 0:65943c77d1dc | 383 | } |
marcodesilva | 0:65943c77d1dc | 384 | |
marcodesilva | 0:65943c77d1dc | 385 | //BIAS |
marcodesilva | 0:65943c77d1dc | 386 | accBias[0] = accBias[0]/CALIBRATION; |
marcodesilva | 0:65943c77d1dc | 387 | accBias[1] = accBias[1]/CALIBRATION; |
marcodesilva | 0:65943c77d1dc | 388 | gyroBias[0] = gyroBias[0]/CALIBRATION; |
marcodesilva | 0:65943c77d1dc | 389 | gyroBias[1] = gyroBias[1]/CALIBRATION; |
marcodesilva | 0:65943c77d1dc | 390 | gyroBias[2] = gyroBias[2]/CALIBRATION; |
marcodesilva | 0:65943c77d1dc | 391 | |
marcodesilva | 0:65943c77d1dc | 392 | |
marcodesilva | 0:65943c77d1dc | 393 | printf(" END CALIBRATION \r\n"); |
marcodesilva | 0:65943c77d1dc | 394 | |
marcodesilva | 0:65943c77d1dc | 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 |
marcodesilva | 0:65943c77d1dc | 396 | |
marcodesilva | 0:65943c77d1dc | 397 | } |