PRISMA Lab / Mbed OS Hyfliers_Completo_testato

Clone per testare Mbed studio

Dependencies:   PwmIn IONMcMotor MPU6050 Eigen ROVER

Fork of Hyfliers_Completo_testato by Marco De Silva

Robots/Rover/Rover.cpp@6:584653235830, 2021-09-14 (annotated)

Committer:
anfontanelli
Date:
Tue Sep 14 11:18:35 2021 +0000
Revision:
6:584653235830
Parent:
4:3f22193053d0 
A

Who changed what in which revision?

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