Marco De Silva / ROVER

a

old/Rover_omni_cpp.txt@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
marcodesilva 0:65943c77d1dc 3 Rover::Rover(){
marcodesilva 0:65943c77d1dc 4
marcodesilva 0:65943c77d1dc 5 ionMcBoudRate = 460800;
marcodesilva 0:65943c77d1dc 6 meccanumBoardAddress = 128;
marcodesilva 0:65943c77d1dc 7 meccanumMotorGearBoxRatio = 103;
marcodesilva 0:65943c77d1dc 8 meccanumEncoderPulse = 512;
marcodesilva 0:65943c77d1dc 9 meccanumKt = 0.00667;
marcodesilva 0:65943c77d1dc 10 meccanumTransmissionRatio = 1.0;
marcodesilva 0:65943c77d1dc 11 omniTransmissionRatio = 1.0;
marcodesilva 0:65943c77d1dc 12 omniBoardAddress = 129;
marcodesilva 0:65943c77d1dc 13 omniMotorGearBoxRatio = 103;
marcodesilva 0:65943c77d1dc 14 omniEncoderPulse = 512;
marcodesilva 0:65943c77d1dc 15 omniKt = 0.00667;
marcodesilva 0:65943c77d1dc 16
marcodesilva 0:65943c77d1dc 17 r_meccanumWheel = 0.03;
marcodesilva 0:65943c77d1dc 18 r_omniWheel = 0.019;
marcodesilva 0:65943c77d1dc 19
marcodesilva 0:65943c77d1dc 20 pipeCurve_I = 0.165;
marcodesilva 0:65943c77d1dc 21 pipeCurve_E = 0.2925;
marcodesilva 0:65943c77d1dc 22 pipeCurve_M = 0.228;
marcodesilva 0:65943c77d1dc 23
marcodesilva 0:65943c77d1dc 24 pipeDir = 0;
marcodesilva 0:65943c77d1dc 25
marcodesilva 0:65943c77d1dc 26 omniMeccanumCentralDistance = 0.25;
marcodesilva 0:65943c77d1dc 27 wheelsCntactPointDistanceFromPipeCenter = 0.08;
marcodesilva 0:65943c77d1dc 28
marcodesilva 0:65943c77d1dc 29 eth_time_out = 2.0;
marcodesilva 0:65943c77d1dc 30
marcodesilva 0:65943c77d1dc 31 S = Eigen::Matrix3f::Zero();
marcodesilva 0:65943c77d1dc 32
marcodesilva 0:65943c77d1dc 33
marcodesilva 0:65943c77d1dc 34 ionMcMeccanum = new IONMcMotors(meccanumBoardAddress,ionMcBoudRate, PG_14, PG_9, meccanumMotorGearBoxRatio, meccanumEncoderPulse, meccanumKt, meccanumKt);
marcodesilva 0:65943c77d1dc 35 ionMcOmni = new IONMcMotors(omniBoardAddress,ionMcBoudRate, PB_9, PB_8, omniMotorGearBoxRatio, omniEncoderPulse, omniKt, omniKt);
marcodesilva 0:65943c77d1dc 36 meccanumActuonix = new Servo(PE_11);
marcodesilva 0:65943c77d1dc 37 omniActuonix = new Servo(PE_9);
marcodesilva 0:65943c77d1dc 38 mpu = new MPU6050(PF_15, PF_14);
marcodesilva 0:65943c77d1dc 39 eth_tcp = new Eth_tcp(3154, 500);
marcodesilva 0:65943c77d1dc 40
marcodesilva 0:65943c77d1dc 41 distance_sensors = new distanceSensors();
marcodesilva 0:65943c77d1dc 42
marcodesilva 0:65943c77d1dc 43
marcodesilva 0:65943c77d1dc 44 eth_state = -1;
marcodesilva 0:65943c77d1dc 45
marcodesilva 0:65943c77d1dc 46 eth_status = false;
marcodesilva 0:65943c77d1dc 47
marcodesilva 0:65943c77d1dc 48
marcodesilva 0:65943c77d1dc 49 }
marcodesilva 0:65943c77d1dc 50
marcodesilva 0:65943c77d1dc 51 void Rover::initializeTofs(){
marcodesilva 0:65943c77d1dc 52 distance_sensors->TOFs_init();
marcodesilva 0:65943c77d1dc 53 distance_sensors->TOFs_offset();
marcodesilva 0:65943c77d1dc 54 }
marcodesilva 0:65943c77d1dc 55
marcodesilva 0:65943c77d1dc 56 void Rover::acquireTofs(float &frontDistance, float &retroDistance){
marcodesilva 0:65943c77d1dc 57
marcodesilva 0:65943c77d1dc 58 distance_sensors->TOFs_acquireFiltr();
marcodesilva 0:65943c77d1dc 59 frontDistance = distance_sensors->getFrontDistance();
marcodesilva 0:65943c77d1dc 60 retroDistance = distance_sensors->getRetroDistance();
marcodesilva 0:65943c77d1dc 61
marcodesilva 0:65943c77d1dc 62
marcodesilva 0:65943c77d1dc 63
marcodesilva 0:65943c77d1dc 64 }
marcodesilva 0:65943c77d1dc 65
marcodesilva 0:65943c77d1dc 66
marcodesilva 0:65943c77d1dc 67 void Rover::computeCentralJointsFromTofs(){
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(frontDistance, retroDistance);
marcodesilva 0:65943c77d1dc 73
marcodesilva 0:65943c77d1dc 74
marcodesilva 0:65943c77d1dc 75
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
marcodesilva 0:65943c77d1dc 80 if(enableCurv==1){
marcodesilva 0:65943c77d1dc 81 frontPos = frontPos + frontVel*0.005;
marcodesilva 0:65943c77d1dc 82 retroPos = retroPos + retroVel*0.005;
marcodesilva 0:65943c77d1dc 83 }else{
marcodesilva 0:65943c77d1dc 84 frontPos = 0.0;
marcodesilva 0:65943c77d1dc 85 retroPos = 0.0;
marcodesilva 0:65943c77d1dc 86 }
marcodesilva 0:65943c77d1dc 87
marcodesilva 0:65943c77d1dc 88 if(frontPos > 0.5) frontPos=0.5;
marcodesilva 0:65943c77d1dc 89 if(frontPos < -0.5) frontPos=-0.5;
marcodesilva 0:65943c77d1dc 90 if(retroPos > 0.5) retroPos=0.5;
marcodesilva 0:65943c77d1dc 91 if(retroPos < -0.5) retroPos=-0.5;
marcodesilva 0:65943c77d1dc 92
marcodesilva 0:65943c77d1dc 93 setCentralJointsAngle(frontPos,retroPos);
marcodesilva 0:65943c77d1dc 94
marcodesilva 0:65943c77d1dc 95 }
marcodesilva 0:65943c77d1dc 96
marcodesilva 0:65943c77d1dc 97
marcodesilva 0:65943c77d1dc 98 void Rover::initializeImu(){
marcodesilva 0:65943c77d1dc 99
marcodesilva 0:65943c77d1dc 100 printf("Initialize IMU \r\n");
marcodesilva 0:65943c77d1dc 101 //IMU
marcodesilva 0:65943c77d1dc 102 FS_a = 8192; //per avere letture in g, sarebbe 32768/4 perchè 4g è il fondo scala
marcodesilva 0:65943c77d1dc 103 FS_g = 131.072; //per avere letture in gradi/s, sarebbe 32768/250 perchè 250 è il fondo scala
marcodesilva 0:65943c77d1dc 104
marcodesilva 0:65943c77d1dc 105
marcodesilva 0:65943c77d1dc 106 accBias[0] = 0.0;
marcodesilva 0:65943c77d1dc 107 accBias[1] = 0.0;
marcodesilva 0:65943c77d1dc 108 accBias[2] = 0.0;
marcodesilva 0:65943c77d1dc 109 gyroBias[0] = 0.0;
marcodesilva 0:65943c77d1dc 110 gyroBias[1] = 0.0;
marcodesilva 0:65943c77d1dc 111 gyroBias[2] = 0.0;
marcodesilva 0:65943c77d1dc 112
marcodesilva 0:65943c77d1dc 113
marcodesilva 0:65943c77d1dc 114 pitchAcc = 0.0;
marcodesilva 0:65943c77d1dc 115 rollAcc = 0.0;
marcodesilva 0:65943c77d1dc 116
marcodesilva 0:65943c77d1dc 117 pitch_integrated = 0.0;
marcodesilva 0:65943c77d1dc 118 roll_integrated = 0.0;
marcodesilva 0:65943c77d1dc 119
marcodesilva 0:65943c77d1dc 120 mpu->initialize();
marcodesilva 0:65943c77d1dc 121 bool mpu6050TestResult = mpu->testConnection();
marcodesilva 0:65943c77d1dc 122 if(mpu6050TestResult) {
marcodesilva 0:65943c77d1dc 123 printf("MPU6050 test passed \r\n");
marcodesilva 0:65943c77d1dc 124 } else {
marcodesilva 0:65943c77d1dc 125 printf("MPU6050 test failed \r\n");
marcodesilva 0:65943c77d1dc 126 }
marcodesilva 0:65943c77d1dc 127
marcodesilva 0:65943c77d1dc 128
marcodesilva 0:65943c77d1dc 129 calibrateImu();
marcodesilva 0:65943c77d1dc 130 }
marcodesilva 0:65943c77d1dc 131
marcodesilva 0:65943c77d1dc 132 void Rover::setWheelsVelocity(float forward_speed, float stabilization_speed, float asset_correction_speed, float pipe_radius, float maxWheelAcceleration){
marcodesilva 0:65943c77d1dc 133
marcodesilva 0:65943c77d1dc 134 Eigen::Vector3f cartesianSpeed;
marcodesilva 0:65943c77d1dc 135 Eigen::Vector3f wheelsSpeed;
marcodesilva 0:65943c77d1dc 136
marcodesilva 0:65943c77d1dc 137 updateRoverKin(pipe_radius, pipeDir);
marcodesilva 0:65943c77d1dc 138
marcodesilva 0:65943c77d1dc 139 cartesianSpeed << forward_speed, stabilization_speed, asset_correction_speed;
marcodesilva 0:65943c77d1dc 140 wheelsSpeed = S*cartesianSpeed;
marcodesilva 0:65943c77d1dc 141 ionMcMeccanum->setSpeed(1, wheelsSpeed[0],maxWheelAcceleration);
marcodesilva 0:65943c77d1dc 142 ionMcMeccanum->setSpeed(2, wheelsSpeed[1],maxWheelAcceleration);
marcodesilva 0:65943c77d1dc 143 ionMcOmni->setSpeed(1,wheelsSpeed[2],maxWheelAcceleration);
marcodesilva 0:65943c77d1dc 144
marcodesilva 0:65943c77d1dc 145 }
marcodesilva 0:65943c77d1dc 146
marcodesilva 0:65943c77d1dc 147
marcodesilva 0:65943c77d1dc 148 int Rover::get_forward_vel(){
marcodesilva 0:65943c77d1dc 149 int fv;
marcodesilva 0:65943c77d1dc 150 eth_mutex.lock();
marcodesilva 0:65943c77d1dc 151 fv = forward_vel;
marcodesilva 0:65943c77d1dc 152 eth_mutex.unlock();
marcodesilva 0:65943c77d1dc 153 return fv;
marcodesilva 0:65943c77d1dc 154
marcodesilva 0:65943c77d1dc 155 }
marcodesilva 0:65943c77d1dc 156 int Rover::get_pitch(){
marcodesilva 0:65943c77d1dc 157 int pit;
marcodesilva 0:65943c77d1dc 158 eth_mutex.lock();
marcodesilva 0:65943c77d1dc 159 pit = pitch_d;
marcodesilva 0:65943c77d1dc 160 eth_mutex.unlock();
marcodesilva 0:65943c77d1dc 161 return pit;
marcodesilva 0:65943c77d1dc 162 }
marcodesilva 0:65943c77d1dc 163
marcodesilva 0:65943c77d1dc 164 int Rover::get_jointFront(){
marcodesilva 0:65943c77d1dc 165 int jf;
marcodesilva 0:65943c77d1dc 166 eth_mutex.lock();
marcodesilva 0:65943c77d1dc 167 jf = jointFront;
marcodesilva 0:65943c77d1dc 168 eth_mutex.unlock();
marcodesilva 0:65943c77d1dc 169 return jf;
marcodesilva 0:65943c77d1dc 170 }
marcodesilva 0:65943c77d1dc 171 int Rover::get_jointRetro(){
marcodesilva 0:65943c77d1dc 172 int jr;
marcodesilva 0:65943c77d1dc 173 eth_mutex.lock();
marcodesilva 0:65943c77d1dc 174 jr = jointRetro;
marcodesilva 0:65943c77d1dc 175 eth_mutex.unlock();
marcodesilva 0:65943c77d1dc 176 return jr;
marcodesilva 0:65943c77d1dc 177 }
marcodesilva 0:65943c77d1dc 178
marcodesilva 0:65943c77d1dc 179 void Rover::getRoverVelocity(float &forward_speed, float &stabilization_speed, float &asset_correction_speed, float pipe_radius){
marcodesilva 0:65943c77d1dc 180
marcodesilva 0:65943c77d1dc 181 Eigen::Vector3f cartesianSpeed;
marcodesilva 0:65943c77d1dc 182 Eigen::Vector3f wheelsSpeed;
marcodesilva 0:65943c77d1dc 183
marcodesilva 0:65943c77d1dc 184 float speedM1 = 0.0;
marcodesilva 0:65943c77d1dc 185 float speedM2 = 0.0;
marcodesilva 0:65943c77d1dc 186 float speedM3 = 0.0;
marcodesilva 0:65943c77d1dc 187
marcodesilva 0:65943c77d1dc 188 updateRoverKin(pipe_radius, pipeDir);
marcodesilva 0:65943c77d1dc 189
marcodesilva 0:65943c77d1dc 190 getMotorsSpeed(speedM1, speedM2, speedM3);
marcodesilva 0:65943c77d1dc 191
marcodesilva 0:65943c77d1dc 192 wheelsSpeed << speedM1, speedM2, speedM3;
marcodesilva 0:65943c77d1dc 193
marcodesilva 0:65943c77d1dc 194 cartesianSpeed = S_inv*wheelsSpeed;
marcodesilva 0:65943c77d1dc 195
marcodesilva 0:65943c77d1dc 196 forward_speed = cartesianSpeed[0];
marcodesilva 0:65943c77d1dc 197 stabilization_speed = cartesianSpeed[1];
marcodesilva 0:65943c77d1dc 198 asset_correction_speed = cartesianSpeed[2];
marcodesilva 0:65943c77d1dc 199
marcodesilva 0:65943c77d1dc 200 }
marcodesilva 0:65943c77d1dc 201
marcodesilva 0:65943c77d1dc 202 void Rover::getRoverWheelsVelocity(float &speed_wheel_dx, float &speed_wheel_sx, float &speed_wheels_retro){
marcodesilva 0:65943c77d1dc 203
marcodesilva 0:65943c77d1dc 204 Eigen::Vector3f cartesianSpeed;
marcodesilva 0:65943c77d1dc 205 Eigen::Vector3f wheelsSpeed;
marcodesilva 0:65943c77d1dc 206
marcodesilva 0:65943c77d1dc 207 float speedM1 = 0.0;
marcodesilva 0:65943c77d1dc 208 float speedM2 = 0.0;
marcodesilva 0:65943c77d1dc 209 float speedM3 = 0.0;
marcodesilva 0:65943c77d1dc 210
marcodesilva 0:65943c77d1dc 211 getMotorsSpeed(speedM1, speedM2, speedM3);
marcodesilva 0:65943c77d1dc 212
marcodesilva 0:65943c77d1dc 213 speed_wheel_dx = speedM1;
marcodesilva 0:65943c77d1dc 214 speed_wheel_sx = speedM2;
marcodesilva 0:65943c77d1dc 215 speed_wheels_retro = speedM3;
marcodesilva 0:65943c77d1dc 216
marcodesilva 0:65943c77d1dc 217
marcodesilva 0:65943c77d1dc 218 }
marcodesilva 0:65943c77d1dc 219
marcodesilva 0:65943c77d1dc 220 void Rover::updateRoverKin(float pipe_radius, int pipeDir){
marcodesilva 0:65943c77d1dc 221 float rm = r_meccanumWheel;
marcodesilva 0:65943c77d1dc 222 float ro = r_omniWheel;
marcodesilva 0:65943c77d1dc 223 float l = omniMeccanumCentralDistance;
marcodesilva 0:65943c77d1dc 224 float L = wheelsCntactPointDistanceFromPipeCenter;
marcodesilva 0:65943c77d1dc 225 float pr = pipe_radius;
marcodesilva 0:65943c77d1dc 226 float R_dx = 0.0;
marcodesilva 0:65943c77d1dc 227 float R_sx = 0.0;
marcodesilva 0:65943c77d1dc 228 float R_m = 0.0;
marcodesilva 0:65943c77d1dc 229 if(pipeDir ==0){
marcodesilva 0:65943c77d1dc 230 R_dx = 1.0;
marcodesilva 0:65943c77d1dc 231 R_sx = 1.0;
marcodesilva 0:65943c77d1dc 232 R_m = 1.0;
marcodesilva 0:65943c77d1dc 233 }else if(pipeDir == 1){
marcodesilva 0:65943c77d1dc 234 R_dx = pipeCurve_I;
marcodesilva 0:65943c77d1dc 235 R_sx = pipeCurve_E;
marcodesilva 0:65943c77d1dc 236 R_m = pipeCurve_M;
marcodesilva 0:65943c77d1dc 237 }else if(pipeDir == -1){
marcodesilva 0:65943c77d1dc 238 R_dx = pipeCurve_E;
marcodesilva 0:65943c77d1dc 239 R_sx = pipeCurve_I;
marcodesilva 0:65943c77d1dc 240 R_m = pipeCurve_M;
marcodesilva 0:65943c77d1dc 241
marcodesilva 0:65943c77d1dc 242 }
marcodesilva 0:65943c77d1dc 243
marcodesilva 0:65943c77d1dc 244 S << -1.0*R_dx/(rm*R_m), pr/rm, -L/rm,
marcodesilva 0:65943c77d1dc 245 1.0*R_sx/(rm*R_m), pr/rm, -L/rm,
marcodesilva 0:65943c77d1dc 246 0, pr/ro, l/ro;
marcodesilva 0:65943c77d1dc 247
marcodesilva 0:65943c77d1dc 248 S_inv << -rm/2, rm/2, 0,
marcodesilva 0:65943c77d1dc 249 l*rm/(2*pr*(L+l)), l*rm/(2*pr*(L+l)), L*ro/(pr*(L+l)),
marcodesilva 0:65943c77d1dc 250 -rm/(2*(L+l)), -rm/(2*(L+l)), ro/(L+l);
marcodesilva 0:65943c77d1dc 251 }
marcodesilva 0:65943c77d1dc 252
marcodesilva 0:65943c77d1dc 253 void Rover::getMotorsTorque(float &torM1, float &torM2, float &torM3){
marcodesilva 0:65943c77d1dc 254
marcodesilva 0:65943c77d1dc 255
marcodesilva 0:65943c77d1dc 256 torM1 = ionMcMeccanum->getMotorTorque(1);
marcodesilva 0:65943c77d1dc 257 torM2 = ionMcMeccanum->getMotorTorque(2);
marcodesilva 0:65943c77d1dc 258 torM3 = ionMcOmni->getMotorTorque(1);
marcodesilva 0:65943c77d1dc 259
marcodesilva 0:65943c77d1dc 260 }
marcodesilva 0:65943c77d1dc 261
marcodesilva 0:65943c77d1dc 262 void Rover::getMotorsSpeed(float &speedM1, float &speedM2, float &speedM3){
marcodesilva 0:65943c77d1dc 263
marcodesilva 0:65943c77d1dc 264 speedM1 = ionMcMeccanum->getMotorSpeed(1);
marcodesilva 0:65943c77d1dc 265 speedM2 = ionMcMeccanum->getMotorSpeed(2);
marcodesilva 0:65943c77d1dc 266 speedM3 = ionMcOmni->getMotorSpeed(1);
marcodesilva 0:65943c77d1dc 267
marcodesilva 0:65943c77d1dc 268 }
marcodesilva 0:65943c77d1dc 269
marcodesilva 0:65943c77d1dc 270 void Rover::setCentralJointsAngle(float jointFront, float jointRetro){
marcodesilva 0:65943c77d1dc 271
marcodesilva 0:65943c77d1dc 272 float LmRestPosFront = (9.0+0.5)/1000; //9
marcodesilva 0:65943c77d1dc 273 float LmRestPosRetro = (9.0-0.5)/1000; //9
marcodesilva 0:65943c77d1dc 274 float maxLenght = 0.02;
marcodesilva 0:65943c77d1dc 275
marcodesilva 0:65943c77d1dc 276 float LmFront = LmRestPosFront - centralJoint2actuonix(jointFront);
marcodesilva 0:65943c77d1dc 277 float LmRetro = LmRestPosRetro - centralJoint2actuonix(jointRetro);
marcodesilva 0:65943c77d1dc 278
marcodesilva 0:65943c77d1dc 279 meccanumActuonix->write(1.0 - LmFront/maxLenght);
marcodesilva 0:65943c77d1dc 280 omniActuonix->write(1.0 - LmRetro/maxLenght);
marcodesilva 0:65943c77d1dc 281
marcodesilva 0:65943c77d1dc 282 if (jointFront > 3.14*5/180){
marcodesilva 0:65943c77d1dc 283 pipeDir = -1;
marcodesilva 0:65943c77d1dc 284 }else if (jointFront < -3.14*5/180){
marcodesilva 0:65943c77d1dc 285 pipeDir = 1;
marcodesilva 0:65943c77d1dc 286 }else{
marcodesilva 0:65943c77d1dc 287 pipeDir = 0;
marcodesilva 0:65943c77d1dc 288 }
marcodesilva 0:65943c77d1dc 289
marcodesilva 0:65943c77d1dc 290
marcodesilva 0:65943c77d1dc 291 }
marcodesilva 0:65943c77d1dc 292
marcodesilva 0:65943c77d1dc 293 float Rover::centralJoint2actuonix(float jointAngle){
marcodesilva 0:65943c77d1dc 294
marcodesilva 0:65943c77d1dc 295 float alpha = deg2rad(34.4);
marcodesilva 0:65943c77d1dc 296 float L1 = 23.022/1000;
marcodesilva 0:65943c77d1dc 297 float L2 = 62.037/1000;
marcodesilva 0:65943c77d1dc 298 float L3 = 51.013/1000;
marcodesilva 0:65943c77d1dc 299 float L4 = 4.939/1000;
marcodesilva 0:65943c77d1dc 300
marcodesilva 0:65943c77d1dc 301 float l1 = L1*sin(alpha + jointAngle);
marcodesilva 0:65943c77d1dc 302 float l2 = L1*cos(alpha + jointAngle);
marcodesilva 0:65943c77d1dc 303 float h = sqrt( (L2 - l1)*(L2 - l1) + (l2 - L4)*(l2 - L4) );
marcodesilva 0:65943c77d1dc 304 return L3 - h;
marcodesilva 0:65943c77d1dc 305
marcodesilva 0:65943c77d1dc 306 }
marcodesilva 0:65943c77d1dc 307
marcodesilva 0:65943c77d1dc 308
marcodesilva 0:65943c77d1dc 309 float Rover::deg2rad(float deg) {
marcodesilva 0:65943c77d1dc 310 return deg * M_PI / 180.0;
marcodesilva 0:65943c77d1dc 311 }
marcodesilva 0:65943c77d1dc 312
marcodesilva 0:65943c77d1dc 313
marcodesilva 0:65943c77d1dc 314
marcodesilva 0:65943c77d1dc 315 void Rover::calcImuAngles(float& pitch, float& roll, float dtImu)
marcodesilva 0:65943c77d1dc 316 {
marcodesilva 0:65943c77d1dc 317
marcodesilva 0:65943c77d1dc 318 mpu->getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //acquisisco i dati dal sensore
marcodesilva 0:65943c77d1dc 319 a_x=float(ax)/FS_a - accBias[0]; //li converto in float, (CALCOLI EVITABILE SE INSTANZIASSI LE VARIABILI SUBITO COME FLOAT)
marcodesilva 0:65943c77d1dc 320 a_y=float(ay)/FS_a - accBias[1];
marcodesilva 0:65943c77d1dc 321 a_z=float(az)/FS_a;
marcodesilva 0:65943c77d1dc 322 g_x=float(gx)/FS_g - gyroBias[0];
marcodesilva 0:65943c77d1dc 323 g_y=float(gy)/FS_g - gyroBias[1];
marcodesilva 0:65943c77d1dc 324 g_z=float(gz)/FS_g - gyroBias[2];
marcodesilva 0:65943c77d1dc 325
marcodesilva 0:65943c77d1dc 326
marcodesilva 0:65943c77d1dc 327 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 328 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 329
marcodesilva 0:65943c77d1dc 330 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 331 rollAcc = -atan2f(a_x, sqrt(a_y*a_y + a_z*a_z))*180/M_PI; //
marcodesilva 0:65943c77d1dc 332
marcodesilva 0:65943c77d1dc 333 if ( abs( pitchAcc - pitchAcc_p ) > 10){
marcodesilva 0:65943c77d1dc 334 pitchAcc = pitch;
marcodesilva 0:65943c77d1dc 335 }
marcodesilva 0:65943c77d1dc 336
marcodesilva 0:65943c77d1dc 337 if ( abs( rollAcc - rollAcc_p ) > 10){
marcodesilva 0:65943c77d1dc 338 rollAcc = roll;
marcodesilva 0:65943c77d1dc 339 }
marcodesilva 0:65943c77d1dc 340
marcodesilva 0:65943c77d1dc 341 // Apply Complementary Filter
marcodesilva 0:65943c77d1dc 342 pitch = 0.95f*( pitch + pitch_integrated) + 0.05f*pitchAcc;
marcodesilva 0:65943c77d1dc 343 roll = 0.95f*( roll + roll_integrated) + 0.05f*(rollAcc);
marcodesilva 0:65943c77d1dc 344
marcodesilva 0:65943c77d1dc 345 pitchAcc_p = pitchAcc;
marcodesilva 0:65943c77d1dc 346 rollAcc_p = rollAcc;
marcodesilva 0:65943c77d1dc 347
marcodesilva 0:65943c77d1dc 348
marcodesilva 0:65943c77d1dc 349 }
marcodesilva 0:65943c77d1dc 350
marcodesilva 0:65943c77d1dc 351 void Rover::calibrateImu()
marcodesilva 0:65943c77d1dc 352 {
marcodesilva 0:65943c77d1dc 353 const int CALIBRATION = 500;
marcodesilva 0:65943c77d1dc 354 for(int i=0;i<50;i++)
marcodesilva 0:65943c77d1dc 355 {
marcodesilva 0:65943c77d1dc 356 mpu->getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
marcodesilva 0:65943c77d1dc 357 }
marcodesilva 0:65943c77d1dc 358
marcodesilva 0:65943c77d1dc 359
marcodesilva 0:65943c77d1dc 360 printf(" START CALIBRATION \r\n");
marcodesilva 0:65943c77d1dc 361
marcodesilva 0:65943c77d1dc 362
marcodesilva 0:65943c77d1dc 363 for(int i=0;i<CALIBRATION;i++)
marcodesilva 0:65943c77d1dc 364 {
marcodesilva 0:65943c77d1dc 365 mpu->getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //acquisisco i dati dal sensore
marcodesilva 0:65943c77d1dc 366 a_x=(float)ax/FS_a; //li converto in float, (CALCOLI EVITABILE SE INSTANZIASSI LE VARIABILI SUBITO COME FLOAT)
marcodesilva 0:65943c77d1dc 367 a_y=(float)ay/FS_a;
marcodesilva 0:65943c77d1dc 368 //a_z=(float)az/FS_a;
marcodesilva 0:65943c77d1dc 369 g_x=(float)gx/FS_g;
marcodesilva 0:65943c77d1dc 370 g_y=(float)gy/FS_g;;
marcodesilva 0:65943c77d1dc 371 g_z=(float)gz/FS_g;;
marcodesilva 0:65943c77d1dc 372
marcodesilva 0:65943c77d1dc 373 accBias[0] += a_x;
marcodesilva 0:65943c77d1dc 374 accBias[1] += a_y;
marcodesilva 0:65943c77d1dc 375 gyroBias[0] += g_x;
marcodesilva 0:65943c77d1dc 376 gyroBias[1] += g_y;
marcodesilva 0:65943c77d1dc 377 gyroBias[2] += g_z;
marcodesilva 0:65943c77d1dc 378
marcodesilva 0:65943c77d1dc 379 }
marcodesilva 0:65943c77d1dc 380
marcodesilva 0:65943c77d1dc 381 //BIAS
marcodesilva 0:65943c77d1dc 382 accBias[0] = accBias[0]/CALIBRATION;
marcodesilva 0:65943c77d1dc 383 accBias[1] = accBias[1]/CALIBRATION;
marcodesilva 0:65943c77d1dc 384 gyroBias[0] = gyroBias[0]/CALIBRATION;
marcodesilva 0:65943c77d1dc 385 gyroBias[1] = gyroBias[1]/CALIBRATION;
marcodesilva 0:65943c77d1dc 386 gyroBias[2] = gyroBias[2]/CALIBRATION;
marcodesilva 0:65943c77d1dc 387
marcodesilva 0:65943c77d1dc 388
marcodesilva 0:65943c77d1dc 389 printf(" END CALIBRATION \r\n");
marcodesilva 0:65943c77d1dc 390
marcodesilva 0:65943c77d1dc 391 //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 392
marcodesilva 0:65943c77d1dc 393
marcodesilva 0:65943c77d1dc 394 }
marcodesilva 0:65943c77d1dc 395
marcodesilva 0:65943c77d1dc 396
marcodesilva 0:65943c77d1dc 397
marcodesilva 0:65943c77d1dc 398 void Rover::startEthComunication(){
marcodesilva 0:65943c77d1dc 399 printf("Avvio la comunicazione ethernet \r\n");
marcodesilva 0:65943c77d1dc 400 comunicationTimer.start();
marcodesilva 0:65943c77d1dc 401 eth_tcp->connect();
marcodesilva 0:65943c77d1dc 402 setState(1);
marcodesilva 0:65943c77d1dc 403
marcodesilva 0:65943c77d1dc 404 }
marcodesilva 0:65943c77d1dc 405
marcodesilva 0:65943c77d1dc 406 void Rover::ethComunicationUpdate(float _time, float _pitch, float _vels_a, float _vels_m, float _velf_a, float _velf_m) {
marcodesilva 0:65943c77d1dc 407
marcodesilva 0:65943c77d1dc 408 if( eth_tcp->is_connected()){
marcodesilva 0:65943c77d1dc 409 setState(2);
marcodesilva 0:65943c77d1dc 410 printf("ETH: connected!\r\n");
marcodesilva 0:65943c77d1dc 411 sock_open = true;
marcodesilva 0:65943c77d1dc 412 }else{
marcodesilva 0:65943c77d1dc 413 setState(1);
marcodesilva 0:65943c77d1dc 414 }
marcodesilva 0:65943c77d1dc 415
marcodesilva 0:65943c77d1dc 416 eth_status = eth_tcp->recv_pkt(cmd, recv_vec, n_of_int);
marcodesilva 0:65943c77d1dc 417
marcodesilva 0:65943c77d1dc 418 eth_time = comunicationTimer.read();
marcodesilva 0:65943c77d1dc 419 //printf("%f\r\n", (double)(eth_time - eth_time_sample_received));
marcodesilva 0:65943c77d1dc 420
marcodesilva 0:65943c77d1dc 421
marcodesilva 0:65943c77d1dc 422 if(eth_status){
marcodesilva 0:65943c77d1dc 423 eth_time_sample_received = comunicationTimer.read();
marcodesilva 0:65943c77d1dc 424 if(cmd == 's'){
marcodesilva 0:65943c77d1dc 425 if(n_of_int == 4){
marcodesilva 0:65943c77d1dc 426 eth_mutex.lock();
marcodesilva 0:65943c77d1dc 427 forward_vel = recv_vec.get();
marcodesilva 0:65943c77d1dc 428 pitch_d = recv_vec.get();
marcodesilva 0:65943c77d1dc 429 //jointFront = recv_vec.get();
marcodesilva 0:65943c77d1dc 430 //jointRetro = recv_vec.get();
marcodesilva 0:65943c77d1dc 431 enableStab = recv_vec.get();
marcodesilva 0:65943c77d1dc 432 enableCurv= recv_vec.get();
marcodesilva 0:65943c77d1dc 433 eth_mutex.unlock();
marcodesilva 0:65943c77d1dc 434 }
marcodesilva 0:65943c77d1dc 435
marcodesilva 0:65943c77d1dc 436 eth_mutex.lock();
marcodesilva 0:65943c77d1dc 437 vec_to_send.put(_time*1000);
marcodesilva 0:65943c77d1dc 438 vec_to_send.put(_pitch*1000);
marcodesilva 0:65943c77d1dc 439 vec_to_send.put(_vels_a*1000);
marcodesilva 0:65943c77d1dc 440 vec_to_send.put(_vels_m*1000);
marcodesilva 0:65943c77d1dc 441 vec_to_send.put(_velf_a*1000);
marcodesilva 0:65943c77d1dc 442 vec_to_send.put(_velf_m*1000);
marcodesilva 0:65943c77d1dc 443
marcodesilva 0:65943c77d1dc 444 eth_mutex.unlock();
marcodesilva 0:65943c77d1dc 445 eth_tcp->send_vec_of_int(vec_to_send);
marcodesilva 0:65943c77d1dc 446
marcodesilva 0:65943c77d1dc 447 vec_to_send.clear();
marcodesilva 0:65943c77d1dc 448
marcodesilva 0:65943c77d1dc 449 cmd = ' ';
marcodesilva 0:65943c77d1dc 450
marcodesilva 0:65943c77d1dc 451 }
marcodesilva 0:65943c77d1dc 452 }
marcodesilva 0:65943c77d1dc 453
marcodesilva 0:65943c77d1dc 454 if((double)(eth_time - eth_time_sample_received)>eth_time_out && sock_open == true){
marcodesilva 0:65943c77d1dc 455 eth_tcp->reset_connection();
marcodesilva 0:65943c77d1dc 456 sock_open = false;
marcodesilva 0:65943c77d1dc 457 eth_mutex.lock();
marcodesilva 0:65943c77d1dc 458 forward_vel = 0.0;
marcodesilva 0:65943c77d1dc 459 pitch_d = 0.0;
marcodesilva 0:65943c77d1dc 460 //jointFront = 0.0;
marcodesilva 0:65943c77d1dc 461 //jointRetro = 0.0;
marcodesilva 0:65943c77d1dc 462 enableCurv = 0;
marcodesilva 0:65943c77d1dc 463 eth_mutex.unlock();
marcodesilva 0:65943c77d1dc 464 }
marcodesilva 0:65943c77d1dc 465
marcodesilva 0:65943c77d1dc 466
marcodesilva 0:65943c77d1dc 467 }
marcodesilva 0:65943c77d1dc 468
marcodesilva 0:65943c77d1dc 469 int Rover::getEthState(){
marcodesilva 0:65943c77d1dc 470 int state;
marcodesilva 0:65943c77d1dc 471
marcodesilva 0:65943c77d1dc 472 eth_mutex.lock();
marcodesilva 0:65943c77d1dc 473 state = eth_state;
marcodesilva 0:65943c77d1dc 474 eth_mutex.unlock();
marcodesilva 0:65943c77d1dc 475
marcodesilva 0:65943c77d1dc 476 return state;
marcodesilva 0:65943c77d1dc 477 }
marcodesilva 0:65943c77d1dc 478
marcodesilva 0:65943c77d1dc 479 void Rover::setState(int state){
marcodesilva 0:65943c77d1dc 480
marcodesilva 0:65943c77d1dc 481 eth_mutex.lock();
marcodesilva 0:65943c77d1dc 482 eth_state = state;
marcodesilva 0:65943c77d1dc 483 eth_mutex.unlock();
marcodesilva 0:65943c77d1dc 484
marcodesilva 0:65943c77d1dc 485 }
marcodesilva 0:65943c77d1dc 486
marcodesilva 0:65943c77d1dc 487