Marco De Silva / ROVER

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?

UserRevisionLine numberNew 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 }