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@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?

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