Yo Fleyt / Mbed 2 deprecated roboticLab_withclass_3_July

test morning

Dependencies:   ISR_Mini-explorer mbed

Fork of roboticLab_withclass_3_July by Georgios Tsamis

MiniExplorerCoimbra.cpp@1:20f48907c726, 2017-07-03 (annotated)

Committer:
geotsam
Date:
Mon Jul 03 17:06:16 2017 +0000
Revision:
1:20f48907c726
Parent:
0:9f7ee7ed13e4
Child:
2:11cd5173aa36
added first lab stuff

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Ludwigfr 0:9f7ee7ed13e4 1 #include "MiniExplorerCoimbra.hpp"
Ludwigfr 0:9f7ee7ed13e4 2 #include "robot.h"
Ludwigfr 0:9f7ee7ed13e4 3
Ludwigfr 0:9f7ee7ed13e4 4 #define PI 3.14159
Ludwigfr 0:9f7ee7ed13e4 5
Ludwigfr 0:9f7ee7ed13e4 6 MiniExplorerCoimbra::MiniExplorerCoimbra(float defaultXWorld, float defaultYWorld, float defaultThetaWorld):map(200,200,20,20),sonarLeft(10*PI/36,-4,4),sonarFront(0,0,5),sonarRight(-10*PI/36,4,4){
Ludwigfr 0:9f7ee7ed13e4 7 i2c1.frequency(100000);
Ludwigfr 0:9f7ee7ed13e4 8 initRobot(); //Initializing the robot
Ludwigfr 0:9f7ee7ed13e4 9 pc.baud(9600); // baud for the pc communication
Ludwigfr 0:9f7ee7ed13e4 10
Ludwigfr 0:9f7ee7ed13e4 11 measure_always_on();//TODO check if needed
Ludwigfr 0:9f7ee7ed13e4 12
Ludwigfr 0:9f7ee7ed13e4 13 this->setXYThetaAndXYThetaWorld(defaultXWorld,defaultYWorld,defaultThetaWorld);
Ludwigfr 0:9f7ee7ed13e4 14 this->radiusWheels=3.25;
Ludwigfr 0:9f7ee7ed13e4 15 this->distanceWheels=7.2;
Ludwigfr 0:9f7ee7ed13e4 16
geotsam 1:20f48907c726 17 this->k_linear=10;
geotsam 1:20f48907c726 18 this->k_angular=200;
Ludwigfr 0:9f7ee7ed13e4 19 this->khro=12;
Ludwigfr 0:9f7ee7ed13e4 20 this->ka=30;
Ludwigfr 0:9f7ee7ed13e4 21 this->kb=-13;
Ludwigfr 0:9f7ee7ed13e4 22 this->kv=200;
Ludwigfr 0:9f7ee7ed13e4 23 this->kh=200;
geotsam 1:20f48907c726 24 this->kd=5;
geotsam 1:20f48907c726 25 this->speed=300;
Ludwigfr 0:9f7ee7ed13e4 26
Ludwigfr 0:9f7ee7ed13e4 27 this->rangeForce=30;
Ludwigfr 0:9f7ee7ed13e4 28 this->attractionConstantForce=10000;
Ludwigfr 0:9f7ee7ed13e4 29 this->repulsionConstantForce=1;
Ludwigfr 0:9f7ee7ed13e4 30 }
Ludwigfr 0:9f7ee7ed13e4 31
Ludwigfr 0:9f7ee7ed13e4 32 void MiniExplorerCoimbra::setXYThetaAndXYThetaWorld(float defaultXWorld, float defaultYWorld, float defaultThetaWorld){
Ludwigfr 0:9f7ee7ed13e4 33 this->xWorld=defaultXWorld;
Ludwigfr 0:9f7ee7ed13e4 34 this->yWorld=defaultYWorld;
Ludwigfr 0:9f7ee7ed13e4 35 this->thetaWorld=defaultThetaWorld;
Ludwigfr 0:9f7ee7ed13e4 36 X=defaultYWorld;
Ludwigfr 0:9f7ee7ed13e4 37 Y=-defaultXWorld;
Ludwigfr 0:9f7ee7ed13e4 38 if(defaultThetaWorld < -PI/2)
Ludwigfr 0:9f7ee7ed13e4 39 theta=PI/2+PI-defaultThetaWorld;
Ludwigfr 0:9f7ee7ed13e4 40 else
Ludwigfr 0:9f7ee7ed13e4 41 theta=defaultThetaWorld-PI/2;
Ludwigfr 0:9f7ee7ed13e4 42 }
Ludwigfr 0:9f7ee7ed13e4 43
Ludwigfr 0:9f7ee7ed13e4 44 void MiniExplorerCoimbra::myOdometria(){
Ludwigfr 0:9f7ee7ed13e4 45 Odometria();
Ludwigfr 0:9f7ee7ed13e4 46 this->xWorld=-Y;
Ludwigfr 0:9f7ee7ed13e4 47 this->yWorld=X;
Ludwigfr 0:9f7ee7ed13e4 48 if(theta >PI/2)
Ludwigfr 0:9f7ee7ed13e4 49 this->thetaWorld=-PI+(theta-PI/2);
Ludwigfr 0:9f7ee7ed13e4 50 else
Ludwigfr 0:9f7ee7ed13e4 51 this->thetaWorld=theta+PI/2;
Ludwigfr 0:9f7ee7ed13e4 52 }
Ludwigfr 0:9f7ee7ed13e4 53
geotsam 1:20f48907c726 54 void MiniExplorerCoimbra::go_to_point(float targetXWorld, float targetYWorld) {
geotsam 1:20f48907c726 55
geotsam 1:20f48907c726 56 float angleError; //angle error
geotsam 1:20f48907c726 57 float d; //distance from target
geotsam 1:20f48907c726 58 float k_linear=10, k_angular=200;
geotsam 1:20f48907c726 59 float angularLeft, angularRight, linear, angular;
geotsam 1:20f48907c726 60 int speed=300;
geotsam 1:20f48907c726 61
geotsam 1:20f48907c726 62 do {
geotsam 1:20f48907c726 63 //Updating X,Y and theta with the odometry values
geotsam 1:20f48907c726 64 this->myOdometria();
geotsam 1:20f48907c726 65
geotsam 1:20f48907c726 66 //Computing angle error and distance towards the target value
geotsam 1:20f48907c726 67 angleError = atan2((targetYWorld-this->yWorld),(targetXWorld-this->xWorld))-this->thetaWorld;
geotsam 1:20f48907c726 68 if(angleError>PI) angleError=-(angleError-PI);
geotsam 1:20f48907c726 69 else if(angleError<-PI) angleError=-(angleError+PI);
geotsam 1:20f48907c726 70 pc.printf("\n\r error=%f",angleError);
geotsam 1:20f48907c726 71
geotsam 1:20f48907c726 72 d=this->dist(this->xWorld, this->yWorld, targetXWorld, targetYWorld);
geotsam 1:20f48907c726 73 pc.printf("\n\r dist=%f/n", d);
geotsam 1:20f48907c726 74
geotsam 1:20f48907c726 75 //Computing linear and angular velocities
geotsam 1:20f48907c726 76 linear=k_linear*d;
geotsam 1:20f48907c726 77 angular=k_angular*angleError;
geotsam 1:20f48907c726 78 angularLeft=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
geotsam 1:20f48907c726 79 angularRight=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
geotsam 1:20f48907c726 80
geotsam 1:20f48907c726 81
geotsam 1:20f48907c726 82 //Normalize speed for motors
geotsam 1:20f48907c726 83 if(angularLeft>angularRight) {
geotsam 1:20f48907c726 84 angularRight=speed*angularRight/angularLeft;
geotsam 1:20f48907c726 85 angularLeft=speed;
geotsam 1:20f48907c726 86 } else {
geotsam 1:20f48907c726 87 angularLeft=speed*angularLeft/angularRight;
geotsam 1:20f48907c726 88 angularRight=speed;
geotsam 1:20f48907c726 89 }
geotsam 1:20f48907c726 90
geotsam 1:20f48907c726 91 pc.printf("\n\r X=%f", this->xWorld);
geotsam 1:20f48907c726 92 pc.printf("\n\r Y=%f", this->yWorld);
geotsam 1:20f48907c726 93 pc.printf("\n\r theta=%f", this->thetaWorld);
geotsam 1:20f48907c726 94
geotsam 1:20f48907c726 95 //Updating motor velocities
geotsam 1:20f48907c726 96 if(angularLeft>0){
geotsam 1:20f48907c726 97 leftMotor(1,angularLeft);
geotsam 1:20f48907c726 98 }
geotsam 1:20f48907c726 99 else{
geotsam 1:20f48907c726 100 leftMotor(0,-angularLeft);
geotsam 1:20f48907c726 101 }
geotsam 1:20f48907c726 102
geotsam 1:20f48907c726 103 if(angularRight>0){
geotsam 1:20f48907c726 104 rightMotor(1,angularRight);
geotsam 1:20f48907c726 105 }
geotsam 1:20f48907c726 106 else{
geotsam 1:20f48907c726 107 rightMotor(0,-angularRight);
geotsam 1:20f48907c726 108 }
geotsam 1:20f48907c726 109
geotsam 1:20f48907c726 110 wait(0.5);
geotsam 1:20f48907c726 111 } while(d>1);
geotsam 1:20f48907c726 112
geotsam 1:20f48907c726 113 //Stop at the end
geotsam 1:20f48907c726 114 leftMotor(1,0);
geotsam 1:20f48907c726 115 rightMotor(1,0);
geotsam 1:20f48907c726 116 }
geotsam 1:20f48907c726 117
Ludwigfr 0:9f7ee7ed13e4 118 //generate a position randomly and makes the robot go there while updating the map
Ludwigfr 0:9f7ee7ed13e4 119 void MiniExplorerCoimbra::randomize_and_map() {
Ludwigfr 0:9f7ee7ed13e4 120 //TODO check that it's aurelien's work
Ludwigfr 0:9f7ee7ed13e4 121 float movementOnX=rand()%(int)(this->map.widthRealMap/2);
Ludwigfr 0:9f7ee7ed13e4 122 float movementOnY=rand()%(int)(this->map.heightRealMap/2);
Ludwigfr 0:9f7ee7ed13e4 123
Ludwigfr 0:9f7ee7ed13e4 124 float signOfX=rand()%2;
Ludwigfr 0:9f7ee7ed13e4 125 if(signOfX < 1)
Ludwigfr 0:9f7ee7ed13e4 126 signOfX=-1;
Ludwigfr 0:9f7ee7ed13e4 127 float signOfY=rand()%2;
Ludwigfr 0:9f7ee7ed13e4 128 if(signOfY < 1)
Ludwigfr 0:9f7ee7ed13e4 129 signOfY=-1;
Ludwigfr 0:9f7ee7ed13e4 130
Ludwigfr 0:9f7ee7ed13e4 131 float targetXWorld = movementOnX*signOfX;
Ludwigfr 0:9f7ee7ed13e4 132 float targetYWorld = movementOnY*signOfY;
Ludwigfr 0:9f7ee7ed13e4 133 float targetAngleWorld = 2*((float)(rand()%31416)-15708)/10000.0;
Ludwigfr 0:9f7ee7ed13e4 134 this->go_to_point_with_angle(targetXWorld, targetYWorld, targetAngleWorld);
Ludwigfr 0:9f7ee7ed13e4 135 }
Ludwigfr 0:9f7ee7ed13e4 136
Ludwigfr 0:9f7ee7ed13e4 137 //move of targetXWorld and targetYWorld ending in a targetAngleWorld
Ludwigfr 0:9f7ee7ed13e4 138 void MiniExplorerCoimbra::go_to_point_with_angle(float targetXWorld, float targetYWorld, float targetAngleWorld) {
Ludwigfr 0:9f7ee7ed13e4 139 bool keepGoing=true;
Ludwigfr 0:9f7ee7ed13e4 140 float leftMm;
Ludwigfr 0:9f7ee7ed13e4 141 float frontMm;
Ludwigfr 0:9f7ee7ed13e4 142 float rightMm;
Ludwigfr 0:9f7ee7ed13e4 143 float dt;
Ludwigfr 0:9f7ee7ed13e4 144 Timer t;
Ludwigfr 0:9f7ee7ed13e4 145 float distanceToTarget;
Ludwigfr 0:9f7ee7ed13e4 146 do {
Ludwigfr 0:9f7ee7ed13e4 147 //Timer stuff
Ludwigfr 0:9f7ee7ed13e4 148 dt = t.read();
Ludwigfr 0:9f7ee7ed13e4 149 t.reset();
Ludwigfr 0:9f7ee7ed13e4 150 t.start();
Ludwigfr 0:9f7ee7ed13e4 151
Ludwigfr 0:9f7ee7ed13e4 152 //Updating X,Y and theta with the odometry values
Ludwigfr 0:9f7ee7ed13e4 153 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 154 leftMm = get_distance_left_sensor();
Ludwigfr 0:9f7ee7ed13e4 155 frontMm = get_distance_front_sensor();
Ludwigfr 0:9f7ee7ed13e4 156 rightMm = get_distance_right_sensor();
Ludwigfr 0:9f7ee7ed13e4 157
Ludwigfr 0:9f7ee7ed13e4 158 //if in dangerzone
Ludwigfr 0:9f7ee7ed13e4 159 if((frontMm < 150 && frontMm > 0)|| (leftMm <150 && leftMm > 0) || (rightMm <150 && rightMm > 0) ){
Ludwigfr 0:9f7ee7ed13e4 160 leftMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 161 rightMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 162 this->update_sonar_values(leftMm, frontMm, rightMm);
Ludwigfr 0:9f7ee7ed13e4 163 //TODO Giorgos maybe you can also test the do_half_flip() function
Ludwigfr 0:9f7ee7ed13e4 164 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 165 //do a flip TODO
Ludwigfr 0:9f7ee7ed13e4 166 keepGoing=false;
Ludwigfr 0:9f7ee7ed13e4 167 this->do_half_flip();
Ludwigfr 0:9f7ee7ed13e4 168 }else{
Ludwigfr 0:9f7ee7ed13e4 169 //if not in danger zone continue as usual
Ludwigfr 0:9f7ee7ed13e4 170 this->update_sonar_values(leftMm, frontMm, rightMm);
Ludwigfr 0:9f7ee7ed13e4 171
Ludwigfr 0:9f7ee7ed13e4 172 //Updating motor velocities
Ludwigfr 0:9f7ee7ed13e4 173 distanceToTarget=this->update_angular_speed_wheels_go_to_point_with_angle(targetXWorld,targetYWorld,targetAngleWorld,dt);
Ludwigfr 0:9f7ee7ed13e4 174
Ludwigfr 0:9f7ee7ed13e4 175 wait(0.2);
Ludwigfr 0:9f7ee7ed13e4 176 //Timer stuff
Ludwigfr 0:9f7ee7ed13e4 177 t.stop();
Ludwigfr 0:9f7ee7ed13e4 178 pc.printf("\n\r dist to target= %f",distanceToTarget);
Ludwigfr 0:9f7ee7ed13e4 179 }
Ludwigfr 0:9f7ee7ed13e4 180 } while(distanceToTarget>1 && (abs(targetAngleWorld-this->thetaWorld)>0.01) && keepGoing);
Ludwigfr 0:9f7ee7ed13e4 181
Ludwigfr 0:9f7ee7ed13e4 182 //Stop at the end
Ludwigfr 0:9f7ee7ed13e4 183 leftMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 184 rightMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 185 }
Ludwigfr 0:9f7ee7ed13e4 186
geotsam 1:20f48907c726 187
geotsam 1:20f48907c726 188 //move of targetXWorld and targetYWorld ending in a targetAngleWorld
geotsam 1:20f48907c726 189 void MiniExplorerCoimbra::go_to_point_with_angle_first_lab(float targetXWorld, float targetYWorld, float targetAngleWorld) {
geotsam 1:20f48907c726 190 float dt;
geotsam 1:20f48907c726 191 Timer t;
geotsam 1:20f48907c726 192 float distanceToTarget;
geotsam 1:20f48907c726 193 do {
geotsam 1:20f48907c726 194 //Timer stuff
geotsam 1:20f48907c726 195 dt = t.read();
geotsam 1:20f48907c726 196 t.reset();
geotsam 1:20f48907c726 197 t.start();
geotsam 1:20f48907c726 198
geotsam 1:20f48907c726 199 //Updating X,Y and theta with the odometry values
geotsam 1:20f48907c726 200 this->myOdometria();
geotsam 1:20f48907c726 201
geotsam 1:20f48907c726 202
geotsam 1:20f48907c726 203 //Updating motor velocities
geotsam 1:20f48907c726 204 distanceToTarget=this->update_angular_speed_wheels_go_to_point_with_angle(targetXWorld,targetYWorld,targetAngleWorld,dt);
geotsam 1:20f48907c726 205
geotsam 1:20f48907c726 206 wait(0.2);
geotsam 1:20f48907c726 207 //Timer stuff
geotsam 1:20f48907c726 208 t.stop();
geotsam 1:20f48907c726 209 pc.printf("\n\rdist to target= %f",distanceToTarget);
geotsam 1:20f48907c726 210
geotsam 1:20f48907c726 211 } while(distanceToTarget>1 || (abs(targetAngleWorld-this->thetaWorld)>0.1));
geotsam 1:20f48907c726 212
geotsam 1:20f48907c726 213 //Stop at the end
geotsam 1:20f48907c726 214 leftMotor(1,0);
geotsam 1:20f48907c726 215 rightMotor(1,0);
geotsam 1:20f48907c726 216 pc.printf("\r\nReached Target!");
geotsam 1:20f48907c726 217 }
geotsam 1:20f48907c726 218
Ludwigfr 0:9f7ee7ed13e4 219 float MiniExplorerCoimbra::update_angular_speed_wheels_go_to_point_with_angle(float targetXWorld, float targetYWorld, float targetAngleWorld, float dt){
Ludwigfr 0:9f7ee7ed13e4 220 //compute_angles_and_distance
Ludwigfr 0:9f7ee7ed13e4 221 //atan2 take the deplacement on x and the deplacement on y as parameters
Ludwigfr 0:9f7ee7ed13e4 222 float angleToPoint = atan2((targetYWorld-this->yWorld),(targetXWorld-this->xWorld))-this->thetaWorld;
geotsam 1:20f48907c726 223
geotsam 1:20f48907c726 224 if(angleToPoint>PI) angleToPoint=-(angleToPoint-PI);
geotsam 1:20f48907c726 225 else if(angleToPoint<-PI) angleToPoint=-(angleToPoint+PI); //rho is the distance to the point of arrival
geotsam 1:20f48907c726 226
Ludwigfr 0:9f7ee7ed13e4 227 float rho = dist(targetXWorld,targetYWorld,this->xWorld,this->yWorld);
Ludwigfr 0:9f7ee7ed13e4 228 float distanceToTarget = rho;
Ludwigfr 0:9f7ee7ed13e4 229 //TODO check that
Ludwigfr 0:9f7ee7ed13e4 230 float beta = targetAngleWorld-angleToPoint-this->thetaWorld;
Ludwigfr 0:9f7ee7ed13e4 231
Ludwigfr 0:9f7ee7ed13e4 232 //Computing angle error and distance towards the target value
Ludwigfr 0:9f7ee7ed13e4 233 rho += dt*(-this->khro*cos(angleToPoint)*rho);
Ludwigfr 0:9f7ee7ed13e4 234 float temp = angleToPoint;
Ludwigfr 0:9f7ee7ed13e4 235 angleToPoint += dt*(this->khro*sin(angleToPoint)-this->ka*angleToPoint-this->kb*beta);
Ludwigfr 0:9f7ee7ed13e4 236 beta += dt*(-this->khro*sin(temp));
Ludwigfr 0:9f7ee7ed13e4 237
Ludwigfr 0:9f7ee7ed13e4 238 //Computing linear and angular velocities
Ludwigfr 0:9f7ee7ed13e4 239 float linear;
Ludwigfr 0:9f7ee7ed13e4 240 float angular;
Ludwigfr 0:9f7ee7ed13e4 241 if(angleToPoint>=-1.5708 && angleToPoint<=1.5708){
Ludwigfr 0:9f7ee7ed13e4 242 linear=this->khro*rho;
Ludwigfr 0:9f7ee7ed13e4 243 angular=this->ka*angleToPoint+this->kb*beta;
Ludwigfr 0:9f7ee7ed13e4 244 }
Ludwigfr 0:9f7ee7ed13e4 245 else{
Ludwigfr 0:9f7ee7ed13e4 246 linear=-this->khro*rho;
Ludwigfr 0:9f7ee7ed13e4 247 angular=-this->ka*angleToPoint-this->kb*beta;
Ludwigfr 0:9f7ee7ed13e4 248 }
geotsam 1:20f48907c726 249
geotsam 1:20f48907c726 250 float angularLeft=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
geotsam 1:20f48907c726 251 float angularRight=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
geotsam 1:20f48907c726 252
geotsam 1:20f48907c726 253 //Slowing down at the end for more precision
geotsam 1:20f48907c726 254 if (distanceToTarget<30) {
geotsam 1:20f48907c726 255 this->speed = distanceToTarget*10;
geotsam 1:20f48907c726 256 }
Ludwigfr 0:9f7ee7ed13e4 257
Ludwigfr 0:9f7ee7ed13e4 258 //Normalize speed for motors
geotsam 1:20f48907c726 259 if(angularLeft>angularRight) {
geotsam 1:20f48907c726 260 angularRight=this->speed*angularRight/angularLeft;
geotsam 1:20f48907c726 261 angularLeft=this->speed;
Ludwigfr 0:9f7ee7ed13e4 262 } else {
geotsam 1:20f48907c726 263 angularLeft=this->speed*angularLeft/angularRight;
geotsam 1:20f48907c726 264 angularRight=this->speed;
Ludwigfr 0:9f7ee7ed13e4 265 }
Ludwigfr 0:9f7ee7ed13e4 266
Ludwigfr 0:9f7ee7ed13e4 267 //compute_linear_angular_velocities
geotsam 1:20f48907c726 268 leftMotor(1,angularLeft);
geotsam 1:20f48907c726 269 rightMotor(1,angularRight);
Ludwigfr 0:9f7ee7ed13e4 270
Ludwigfr 0:9f7ee7ed13e4 271 return distanceToTarget;
Ludwigfr 0:9f7ee7ed13e4 272 }
Ludwigfr 0:9f7ee7ed13e4 273
Ludwigfr 0:9f7ee7ed13e4 274 void MiniExplorerCoimbra::update_sonar_values(float leftMm,float frontMm,float rightMm){
Ludwigfr 0:9f7ee7ed13e4 275 float xWorldCell;
Ludwigfr 0:9f7ee7ed13e4 276 float yWorldCell;
Ludwigfr 0:9f7ee7ed13e4 277 for(int i=0;i<this->map.nbCellWidth;i++){
Ludwigfr 0:9f7ee7ed13e4 278 for(int j=0;j<this->map.nbCellHeight;j++){
Ludwigfr 0:9f7ee7ed13e4 279 xWorldCell=this->map.cell_width_coordinate_to_world(i);
Ludwigfr 0:9f7ee7ed13e4 280 yWorldCell=this->map.cell_height_coordinate_to_world(j);
Ludwigfr 0:9f7ee7ed13e4 281 this->map.update_cell_value(i,j,this->sonarRight.compute_probability_t(rightMm/10,xWorldCell,yWorldCell,this->xWorld,this->yWorld,this->thetaWorld));
Ludwigfr 0:9f7ee7ed13e4 282 this->map.update_cell_value(i,j,this->sonarLeft.compute_probability_t(leftMm/10,xWorldCell,yWorldCell,this->xWorld,this->yWorld,this->thetaWorld));
Ludwigfr 0:9f7ee7ed13e4 283 this->map.update_cell_value(i,j,this->sonarFront.compute_probability_t(frontMm/10,xWorldCell,yWorldCell,this->xWorld,this->yWorld,this->thetaWorld));
Ludwigfr 0:9f7ee7ed13e4 284
Ludwigfr 0:9f7ee7ed13e4 285 }
Ludwigfr 0:9f7ee7ed13e4 286 }
Ludwigfr 0:9f7ee7ed13e4 287 }
Ludwigfr 0:9f7ee7ed13e4 288
Ludwigfr 0:9f7ee7ed13e4 289 void MiniExplorerCoimbra::do_half_flip(){
Ludwigfr 0:9f7ee7ed13e4 290 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 291 float theta_plus_h_pi=theta+PI/2;//theta is between -PI and PI
Ludwigfr 0:9f7ee7ed13e4 292 if(theta_plus_h_pi > PI)
Ludwigfr 0:9f7ee7ed13e4 293 theta_plus_h_pi=-(2*PI-theta_plus_h_pi);
Ludwigfr 0:9f7ee7ed13e4 294 leftMotor(0,100);
Ludwigfr 0:9f7ee7ed13e4 295 rightMotor(1,100);
Ludwigfr 0:9f7ee7ed13e4 296 while(abs(theta_plus_h_pi-theta)>0.05){
Ludwigfr 0:9f7ee7ed13e4 297 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 298 // pc.printf("\n\r diff=%f", abs(theta_plus_pi-theta));
Ludwigfr 0:9f7ee7ed13e4 299 }
Ludwigfr 0:9f7ee7ed13e4 300 leftMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 301 rightMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 302 }
Ludwigfr 0:9f7ee7ed13e4 303
Ludwigfr 0:9f7ee7ed13e4 304 //Distance computation function
Ludwigfr 0:9f7ee7ed13e4 305 float MiniExplorerCoimbra::dist(float x1, float y1, float x2, float y2){
Ludwigfr 0:9f7ee7ed13e4 306 return sqrt(pow(y2-y1,2) + pow(x2-x1,2));
Ludwigfr 0:9f7ee7ed13e4 307 }
Ludwigfr 0:9f7ee7ed13e4 308
Ludwigfr 0:9f7ee7ed13e4 309 //use virtual force field
Ludwigfr 0:9f7ee7ed13e4 310 void MiniExplorerCoimbra::try_to_reach_target(float targetXWorld,float targetYWorld){
Ludwigfr 0:9f7ee7ed13e4 311 //atan2 gives the angle beetween PI and -PI
Ludwigfr 0:9f7ee7ed13e4 312 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 313 /*
Ludwigfr 0:9f7ee7ed13e4 314 float deplacementOnXWorld=targetXWorld-this->xWorld;
Ludwigfr 0:9f7ee7ed13e4 315 float deplacementOnYWorld=targetYWorld-this->yWorld;
Ludwigfr 0:9f7ee7ed13e4 316 */
Ludwigfr 0:9f7ee7ed13e4 317 float angleToTarget=atan2(targetYWorld-this->yWorld,targetXWorld-this->xWorld);
Ludwigfr 0:9f7ee7ed13e4 318 turn_to_target(angleToTarget);
Ludwigfr 0:9f7ee7ed13e4 319 bool reached=false;
Ludwigfr 0:9f7ee7ed13e4 320 int print=0;
Ludwigfr 0:9f7ee7ed13e4 321 while (!reached) {
Ludwigfr 0:9f7ee7ed13e4 322 this->vff(&reached,targetXWorld,targetYWorld);
Ludwigfr 0:9f7ee7ed13e4 323 //test_got_to_line(&reached);
Ludwigfr 0:9f7ee7ed13e4 324 if(print==10){
Ludwigfr 0:9f7ee7ed13e4 325 leftMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 326 rightMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 327 this->print_map_with_robot_position_and_target(targetXWorld,targetYWorld);
Ludwigfr 0:9f7ee7ed13e4 328 print=0;
Ludwigfr 0:9f7ee7ed13e4 329 }else
Ludwigfr 0:9f7ee7ed13e4 330 print+=1;
Ludwigfr 0:9f7ee7ed13e4 331 }
Ludwigfr 0:9f7ee7ed13e4 332 //Stop at the end
Ludwigfr 0:9f7ee7ed13e4 333 leftMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 334 rightMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 335 pc.printf("\r\n target reached");
Ludwigfr 0:9f7ee7ed13e4 336 wait(3);//
Ludwigfr 0:9f7ee7ed13e4 337 }
Ludwigfr 0:9f7ee7ed13e4 338
Ludwigfr 0:9f7ee7ed13e4 339 void MiniExplorerCoimbra::vff(bool* reached, float targetXWorld, float targetYWorld){
Ludwigfr 0:9f7ee7ed13e4 340 float line_a;
Ludwigfr 0:9f7ee7ed13e4 341 float line_b;
Ludwigfr 0:9f7ee7ed13e4 342 float line_c;
Ludwigfr 0:9f7ee7ed13e4 343 //Updating X,Y and theta with the odometry values
Ludwigfr 0:9f7ee7ed13e4 344 float forceXWorld=0;
Ludwigfr 0:9f7ee7ed13e4 345 float forceYWorld=0;
Ludwigfr 0:9f7ee7ed13e4 346 //we update the odometrie
Ludwigfr 0:9f7ee7ed13e4 347 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 348 //we check the sensors
Ludwigfr 0:9f7ee7ed13e4 349 float leftMm = get_distance_left_sensor();
Ludwigfr 0:9f7ee7ed13e4 350 float frontMm = get_distance_front_sensor();
Ludwigfr 0:9f7ee7ed13e4 351 float rightMm = get_distance_right_sensor();
Ludwigfr 0:9f7ee7ed13e4 352 //update the probabilities values
Ludwigfr 0:9f7ee7ed13e4 353 this->update_sonar_values(leftMm, frontMm, rightMm);
Ludwigfr 0:9f7ee7ed13e4 354 //we compute the force on X and Y
Ludwigfr 0:9f7ee7ed13e4 355 this->compute_forceX_and_forceY(&forceXWorld, &forceYWorld,targetXWorld,targetYWorld);
Ludwigfr 0:9f7ee7ed13e4 356 //we compute a new ine
Ludwigfr 0:9f7ee7ed13e4 357 this->calculate_line(forceXWorld, forceYWorld, &line_a,&line_b,&line_c);
Ludwigfr 0:9f7ee7ed13e4 358 //Updating motor velocities
Ludwigfr 0:9f7ee7ed13e4 359 this->go_to_line(line_a,line_b,line_c,targetXWorld,targetYWorld);
Ludwigfr 0:9f7ee7ed13e4 360
Ludwigfr 0:9f7ee7ed13e4 361 //wait(0.1);
Ludwigfr 0:9f7ee7ed13e4 362 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 363 if(dist(this->xWorld,this->yWorld,targetXWorld,targetYWorld)<10)
Ludwigfr 0:9f7ee7ed13e4 364 *reached=true;
Ludwigfr 0:9f7ee7ed13e4 365 }
Ludwigfr 0:9f7ee7ed13e4 366
Ludwigfr 0:9f7ee7ed13e4 367 /*angleToTarget is obtained through atan2 so it s:
Ludwigfr 0:9f7ee7ed13e4 368 < 0 if the angle is bettween PI and 2pi on a trigo circle
Ludwigfr 0:9f7ee7ed13e4 369 > 0 if it is between 0 and PI
Ludwigfr 0:9f7ee7ed13e4 370 */
Ludwigfr 0:9f7ee7ed13e4 371 void MiniExplorerCoimbra::turn_to_target(float angleToTarget){
Ludwigfr 0:9f7ee7ed13e4 372 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 373 float theta_plus_h_pi=theta+PI/2;//theta is between -PI and PI
Ludwigfr 0:9f7ee7ed13e4 374 if(theta_plus_h_pi > PI)
Ludwigfr 0:9f7ee7ed13e4 375 theta_plus_h_pi=-(2*PI-theta_plus_h_pi);
Ludwigfr 0:9f7ee7ed13e4 376 if(angleToTarget>0){
Ludwigfr 0:9f7ee7ed13e4 377 leftMotor(0,1);
Ludwigfr 0:9f7ee7ed13e4 378 rightMotor(1,1);
Ludwigfr 0:9f7ee7ed13e4 379 }else{
Ludwigfr 0:9f7ee7ed13e4 380 leftMotor(1,1);
Ludwigfr 0:9f7ee7ed13e4 381 rightMotor(0,1);
Ludwigfr 0:9f7ee7ed13e4 382 }
Ludwigfr 0:9f7ee7ed13e4 383 while(abs(angleToTarget-theta_plus_h_pi)>0.05){
Ludwigfr 0:9f7ee7ed13e4 384 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 385 theta_plus_h_pi=theta+PI/2;//theta is between -PI and PI
Ludwigfr 0:9f7ee7ed13e4 386 if(theta_plus_h_pi > PI)
Ludwigfr 0:9f7ee7ed13e4 387 theta_plus_h_pi=-(2*PI-theta_plus_h_pi);
Ludwigfr 0:9f7ee7ed13e4 388 //pc.printf("\n\r diff=%f", abs(angleToTarget-theta_plus_h_pi));
Ludwigfr 0:9f7ee7ed13e4 389 }
Ludwigfr 0:9f7ee7ed13e4 390 leftMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 391 rightMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 392 }
Ludwigfr 0:9f7ee7ed13e4 393
Ludwigfr 0:9f7ee7ed13e4 394
Ludwigfr 0:9f7ee7ed13e4 395 void MiniExplorerCoimbra::print_map_with_robot_position_and_target(float targetXWorld, float targetYWorld) {
Ludwigfr 0:9f7ee7ed13e4 396 float currProba;
Ludwigfr 0:9f7ee7ed13e4 397
Ludwigfr 0:9f7ee7ed13e4 398 float heightIndiceInOrthonormal;
Ludwigfr 0:9f7ee7ed13e4 399 float widthIndiceInOrthonormal;
Ludwigfr 0:9f7ee7ed13e4 400
Ludwigfr 0:9f7ee7ed13e4 401 float widthMalus=-(3*this->map.sizeCellWidth/2);
Ludwigfr 0:9f7ee7ed13e4 402 float widthBonus=this->map.sizeCellWidth/2;
Ludwigfr 0:9f7ee7ed13e4 403
Ludwigfr 0:9f7ee7ed13e4 404 float heightMalus=-(3*this->map.sizeCellHeight/2);
Ludwigfr 0:9f7ee7ed13e4 405 float heightBonus=this->map.sizeCellHeight/2;
Ludwigfr 0:9f7ee7ed13e4 406
Ludwigfr 0:9f7ee7ed13e4 407 pc.printf("\n\r");
Ludwigfr 0:9f7ee7ed13e4 408 for (int y = this->map.nbCellHeight -1; y>-1; y--) {
Ludwigfr 0:9f7ee7ed13e4 409 for (int x= 0; x<this->map.nbCellWidth; x++) {
Ludwigfr 0:9f7ee7ed13e4 410 heightIndiceInOrthonormal=this->map.cell_height_coordinate_to_world(y);
Ludwigfr 0:9f7ee7ed13e4 411 widthIndiceInOrthonormal=this->map.cell_width_coordinate_to_world(x);
Ludwigfr 0:9f7ee7ed13e4 412 if(this->yWorld >= (heightIndiceInOrthonormal+heightMalus) && this->yWorld <= (heightIndiceInOrthonormal+heightBonus) && this->xWorld >= (widthIndiceInOrthonormal+widthMalus) && this->xWorld <= (widthIndiceInOrthonormal+widthBonus))
Ludwigfr 0:9f7ee7ed13e4 413 pc.printf(" R ");
Ludwigfr 0:9f7ee7ed13e4 414 else{
Ludwigfr 0:9f7ee7ed13e4 415 if(targetYWorld >= (heightIndiceInOrthonormal+heightMalus) && targetYWorld <= (heightIndiceInOrthonormal+heightBonus) && targetXWorld >= (widthIndiceInOrthonormal+widthMalus) && targetXWorld <= (widthIndiceInOrthonormal+widthBonus))
Ludwigfr 0:9f7ee7ed13e4 416 pc.printf(" T ");
Ludwigfr 0:9f7ee7ed13e4 417 else{
Ludwigfr 0:9f7ee7ed13e4 418 currProba=this->map.log_to_proba(this->map.cellsLogValues[x][y]);
Ludwigfr 0:9f7ee7ed13e4 419 if ( currProba < 0.5)
Ludwigfr 0:9f7ee7ed13e4 420 pc.printf(" ");
Ludwigfr 0:9f7ee7ed13e4 421 else{
Ludwigfr 0:9f7ee7ed13e4 422 if(currProba==0.5)
Ludwigfr 0:9f7ee7ed13e4 423 pc.printf(" . ");
Ludwigfr 0:9f7ee7ed13e4 424 else
Ludwigfr 0:9f7ee7ed13e4 425 pc.printf(" X ");
Ludwigfr 0:9f7ee7ed13e4 426 }
Ludwigfr 0:9f7ee7ed13e4 427 }
Ludwigfr 0:9f7ee7ed13e4 428 }
Ludwigfr 0:9f7ee7ed13e4 429 }
Ludwigfr 0:9f7ee7ed13e4 430 pc.printf("\n\r");
Ludwigfr 0:9f7ee7ed13e4 431 }
Ludwigfr 0:9f7ee7ed13e4 432 }
Ludwigfr 0:9f7ee7ed13e4 433
Ludwigfr 0:9f7ee7ed13e4 434
Ludwigfr 0:9f7ee7ed13e4 435 //robotX and robotY are from this->myOdometria(), calculate line_a, line_b and line_c
Ludwigfr 0:9f7ee7ed13e4 436 void MiniExplorerCoimbra::calculate_line(float forceX, float forceY, float *line_a, float *line_b, float *line_c){
Ludwigfr 0:9f7ee7ed13e4 437 /*
Ludwigfr 0:9f7ee7ed13e4 438 in the teachers maths it is
Ludwigfr 0:9f7ee7ed13e4 439
Ludwigfr 0:9f7ee7ed13e4 440 *line_a=forceY;
Ludwigfr 0:9f7ee7ed13e4 441 *line_b=-forceX;
Ludwigfr 0:9f7ee7ed13e4 442
Ludwigfr 0:9f7ee7ed13e4 443 because a*x+b*y+c=0
Ludwigfr 0:9f7ee7ed13e4 444 a impact the vertical and b the horizontal
Ludwigfr 0:9f7ee7ed13e4 445 and he has to put them like this because
Ludwigfr 0:9f7ee7ed13e4 446 Robot space: World space:
Ludwigfr 0:9f7ee7ed13e4 447 ^ ^
Ludwigfr 0:9f7ee7ed13e4 448 |x |y
Ludwigfr 0:9f7ee7ed13e4 449 <- R O ->
Ludwigfr 0:9f7ee7ed13e4 450 y x
Ludwigfr 0:9f7ee7ed13e4 451 but since our forceX, forceY are already computed in the orthonormal space I m not sure we need to
Ludwigfr 0:9f7ee7ed13e4 452 */
Ludwigfr 0:9f7ee7ed13e4 453 *line_a=forceX;
Ludwigfr 0:9f7ee7ed13e4 454 *line_b=forceY;
Ludwigfr 0:9f7ee7ed13e4 455 //because the line computed always pass by the robot center we dont need lince_c
Ludwigfr 0:9f7ee7ed13e4 456 //*line_c=forceX*this->yWorld+forceY*this->xWorld;
Ludwigfr 0:9f7ee7ed13e4 457 *line_c=0;
Ludwigfr 0:9f7ee7ed13e4 458 }
Ludwigfr 0:9f7ee7ed13e4 459 //compute the force on X and Y
Ludwigfr 0:9f7ee7ed13e4 460 void MiniExplorerCoimbra::compute_forceX_and_forceY(float* forceXWorld, float* forceYWorld, float targetXWorld, float targetYWorld){
Ludwigfr 0:9f7ee7ed13e4 461 float forceRepulsionComputedX=0;
Ludwigfr 0:9f7ee7ed13e4 462 float forceRepulsionComputedY=0;
Ludwigfr 0:9f7ee7ed13e4 463 for(int i=0;i<this->map.nbCellWidth;i++){ //for each cell of the map we compute a force of repulsion
Ludwigfr 0:9f7ee7ed13e4 464 for(int j=0;j<this->map.nbCellHeight;j++){
Ludwigfr 0:9f7ee7ed13e4 465 this->update_force(i,j,&forceRepulsionComputedX,&forceRepulsionComputedY);
Ludwigfr 0:9f7ee7ed13e4 466 }
Ludwigfr 0:9f7ee7ed13e4 467 }
Ludwigfr 0:9f7ee7ed13e4 468 //update with attraction force
Ludwigfr 0:9f7ee7ed13e4 469 *forceXWorld=+forceRepulsionComputedX;
Ludwigfr 0:9f7ee7ed13e4 470 *forceYWorld=+forceRepulsionComputedY;
Ludwigfr 0:9f7ee7ed13e4 471 float distanceTargetRobot=sqrt(pow(targetXWorld-this->xWorld,2)+pow(targetYWorld-this->yWorld,2));
Ludwigfr 0:9f7ee7ed13e4 472 if(distanceTargetRobot != 0){
Ludwigfr 0:9f7ee7ed13e4 473 *forceXWorld-=this->attractionConstantForce*(targetXWorld-this->xWorld)/distanceTargetRobot;
Ludwigfr 0:9f7ee7ed13e4 474 *forceYWorld-=this->attractionConstantForce*(targetYWorld-this->yWorld)/distanceTargetRobot;
Ludwigfr 0:9f7ee7ed13e4 475 }
Ludwigfr 0:9f7ee7ed13e4 476 float amplitude=sqrt(pow(*forceXWorld,2)+pow(*forceYWorld,2));
Ludwigfr 0:9f7ee7ed13e4 477 if(amplitude!=0){//avoid division by 0 if forceX and forceY == 0
Ludwigfr 0:9f7ee7ed13e4 478 *forceXWorld=*forceXWorld/amplitude;
Ludwigfr 0:9f7ee7ed13e4 479 *forceYWorld=*forceYWorld/amplitude;
Ludwigfr 0:9f7ee7ed13e4 480 }
Ludwigfr 0:9f7ee7ed13e4 481 }
Ludwigfr 0:9f7ee7ed13e4 482
Ludwigfr 0:9f7ee7ed13e4 483 //currently line_c is not used
Ludwigfr 0:9f7ee7ed13e4 484 void MiniExplorerCoimbra::go_to_line(float line_a, float line_b, float line_c,float targetXWorld, float targetYWorld){
Ludwigfr 0:9f7ee7ed13e4 485 float lineAngle;
Ludwigfr 0:9f7ee7ed13e4 486 float angleError;
Ludwigfr 0:9f7ee7ed13e4 487 float linear;
Ludwigfr 0:9f7ee7ed13e4 488 float angular;
Ludwigfr 0:9f7ee7ed13e4 489
Ludwigfr 0:9f7ee7ed13e4 490 //atan2 use the deplacement on X and the deplacement on Y
Ludwigfr 0:9f7ee7ed13e4 491 float angleToTarget=atan2(targetYWorld-this->yWorld,targetXWorld-this->xWorld);
Ludwigfr 0:9f7ee7ed13e4 492 bool aligned=false;
Ludwigfr 0:9f7ee7ed13e4 493
Ludwigfr 0:9f7ee7ed13e4 494 //this condition is passed if the target is in the same direction as the robot orientation
Ludwigfr 0:9f7ee7ed13e4 495 if((angleToTarget>0 && this->thetaWorld > 0) || (angleToTarget<0 && this->thetaWorld <0))
Ludwigfr 0:9f7ee7ed13e4 496 aligned=true;
Ludwigfr 0:9f7ee7ed13e4 497
Ludwigfr 0:9f7ee7ed13e4 498 //line angle is beetween pi/2 and -pi/2
Ludwigfr 0:9f7ee7ed13e4 499 /*
Ludwigfr 0:9f7ee7ed13e4 500 ax+by+c=0 (here c==0)
Ludwigfr 0:9f7ee7ed13e4 501 y=x*-a/b
Ludwigfr 0:9f7ee7ed13e4 502 if a*b > 0, the robot is going down
Ludwigfr 0:9f7ee7ed13e4 503 if a*b <0, the robot is going up
Ludwigfr 0:9f7ee7ed13e4 504 */
Ludwigfr 0:9f7ee7ed13e4 505 if(line_b!=0){
Ludwigfr 0:9f7ee7ed13e4 506 if(!aligned)
Ludwigfr 0:9f7ee7ed13e4 507 lineAngle=atan(-line_a/line_b);
Ludwigfr 0:9f7ee7ed13e4 508 else
Ludwigfr 0:9f7ee7ed13e4 509 lineAngle=atan(line_a/-line_b);
Ludwigfr 0:9f7ee7ed13e4 510 }
Ludwigfr 0:9f7ee7ed13e4 511 else{
Ludwigfr 0:9f7ee7ed13e4 512 lineAngle=1.5708;
Ludwigfr 0:9f7ee7ed13e4 513 }
Ludwigfr 0:9f7ee7ed13e4 514
Ludwigfr 0:9f7ee7ed13e4 515 //Computing angle error
Ludwigfr 0:9f7ee7ed13e4 516 angleError = lineAngle-this->thetaWorld;//TODO that I m not sure
Ludwigfr 0:9f7ee7ed13e4 517 angleError = atan(sin(angleError)/cos(angleError));
Ludwigfr 0:9f7ee7ed13e4 518
Ludwigfr 0:9f7ee7ed13e4 519 //Calculating velocities
Ludwigfr 0:9f7ee7ed13e4 520 linear=this->kv*(3.1416);
Ludwigfr 0:9f7ee7ed13e4 521 angular=this->kh*angleError;
Ludwigfr 0:9f7ee7ed13e4 522
Ludwigfr 0:9f7ee7ed13e4 523 float angularLeft=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
Ludwigfr 0:9f7ee7ed13e4 524 float angularRight=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
Ludwigfr 0:9f7ee7ed13e4 525
Ludwigfr 0:9f7ee7ed13e4 526 //Normalize speed for motors
Ludwigfr 0:9f7ee7ed13e4 527 if(abs(angularLeft)>abs(angularRight)) {
Ludwigfr 0:9f7ee7ed13e4 528 angularRight=this->speed*abs(angularRight/angularLeft)*this->sign1(angularRight);
Ludwigfr 0:9f7ee7ed13e4 529 angularLeft=this->speed*this->sign1(angularLeft);
Ludwigfr 0:9f7ee7ed13e4 530 }
Ludwigfr 0:9f7ee7ed13e4 531 else {
Ludwigfr 0:9f7ee7ed13e4 532 angularLeft=this->speed*abs(angularLeft/angularRight)*this->sign1(angularLeft);
Ludwigfr 0:9f7ee7ed13e4 533 angularRight=this->speed*this->sign1(angularRight);
Ludwigfr 0:9f7ee7ed13e4 534 }
Ludwigfr 0:9f7ee7ed13e4 535 leftMotor(this->sign2(angularLeft),abs(angularLeft));
Ludwigfr 0:9f7ee7ed13e4 536 rightMotor(this->sign2(angularRight),abs(angularRight));
Ludwigfr 0:9f7ee7ed13e4 537 }
Ludwigfr 0:9f7ee7ed13e4 538
geotsam 1:20f48907c726 539 void MiniExplorerCoimbra::go_to_line_first_lab(float line_a, float line_b, float line_c){
geotsam 1:20f48907c726 540 float lineAngle;
geotsam 1:20f48907c726 541 float angleError;
geotsam 1:20f48907c726 542 float linear;
geotsam 1:20f48907c726 543 float angular;
geotsam 1:20f48907c726 544 float d;
geotsam 1:20f48907c726 545
geotsam 1:20f48907c726 546 //line angle is beetween pi/2 and -pi/2
geotsam 1:20f48907c726 547
geotsam 1:20f48907c726 548 if(line_b!=0){
geotsam 1:20f48907c726 549 lineAngle=atan(line_a/-line_b);
geotsam 1:20f48907c726 550 }
geotsam 1:20f48907c726 551 else{
geotsam 1:20f48907c726 552 lineAngle=1.5708;
geotsam 1:20f48907c726 553 }
geotsam 1:20f48907c726 554
geotsam 1:20f48907c726 555 do{
geotsam 1:20f48907c726 556 this->myOdometria();
geotsam 1:20f48907c726 557 //Computing angle error
geotsam 1:20f48907c726 558 angleError = lineAngle-this->thetaWorld;//TODO that I m not sure
geotsam 1:20f48907c726 559
geotsam 1:20f48907c726 560 if(angleError>PI) angleError=-(angleError-PI);
geotsam 1:20f48907c726 561 else if(angleError<-PI) angleError=-(angleError+PI);
geotsam 1:20f48907c726 562
geotsam 1:20f48907c726 563 d=this->distFromLine(xWorld, yWorld, line_a, line_b, line_c);
geotsam 1:20f48907c726 564
geotsam 1:20f48907c726 565 //Calculating velocities
geotsam 1:20f48907c726 566 linear= this->kv*(3.14);
geotsam 1:20f48907c726 567 angular=-this->kd*d + this->kh*angleError;
geotsam 1:20f48907c726 568
geotsam 1:20f48907c726 569 float angularLeft=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
geotsam 1:20f48907c726 570 float angularRight=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
geotsam 1:20f48907c726 571
geotsam 1:20f48907c726 572 //Normalize speed for motors
geotsam 1:20f48907c726 573 if(abs(angularLeft)>abs(angularRight)) {
geotsam 1:20f48907c726 574 angularRight=this->speed*abs(angularRight/angularLeft)*this->sign1(angularRight);
geotsam 1:20f48907c726 575 angularLeft=this->speed*this->sign1(angularLeft);
geotsam 1:20f48907c726 576 }
geotsam 1:20f48907c726 577 else {
geotsam 1:20f48907c726 578 angularLeft=this->speed*abs(angularLeft/angularRight)*this->sign1(angularLeft);
geotsam 1:20f48907c726 579 angularRight=this->speed*this->sign1(angularRight);
geotsam 1:20f48907c726 580 }
geotsam 1:20f48907c726 581
geotsam 1:20f48907c726 582 pc.printf("\r\nd = %f", d);
geotsam 1:20f48907c726 583 pc.printf("\r\nerror = %f\n", angleError);
geotsam 1:20f48907c726 584
geotsam 1:20f48907c726 585 leftMotor(this->sign2(angularLeft),abs(angularLeft));
geotsam 1:20f48907c726 586 rightMotor(this->sign2(angularRight),abs(angularRight));
geotsam 1:20f48907c726 587 }while(1);
geotsam 1:20f48907c726 588 }
geotsam 1:20f48907c726 589
Ludwigfr 0:9f7ee7ed13e4 590 void MiniExplorerCoimbra::update_force(int widthIndice, int heightIndice, float* forceRepulsionComputedX, float* forceRepulsionComputedY ){
Ludwigfr 0:9f7ee7ed13e4 591 //get the coordonate of the map and the robot in the ortonormal space
Ludwigfr 0:9f7ee7ed13e4 592 float xWorldCell=this->map.cell_width_coordinate_to_world(widthIndice);
Ludwigfr 0:9f7ee7ed13e4 593 float yWorldCell=this->map.cell_height_coordinate_to_world(heightIndice);
Ludwigfr 0:9f7ee7ed13e4 594 //compute the distance beetween the cell and the robot
Ludwigfr 0:9f7ee7ed13e4 595 float distanceCellToRobot=sqrt(pow(xWorldCell-this->xWorld,2)+pow(yWorldCell-this->yWorld,2));
Ludwigfr 0:9f7ee7ed13e4 596 //check if the cell is in range
Ludwigfr 0:9f7ee7ed13e4 597 if(distanceCellToRobot <= this->rangeForce) {
Ludwigfr 0:9f7ee7ed13e4 598 float probaCell=this->map.get_proba_cell(widthIndice,heightIndice);
Ludwigfr 0:9f7ee7ed13e4 599 *forceRepulsionComputedX+=this->repulsionConstantForce*probaCell*(xWorldCell-this->xWorld)/pow(distanceCellToRobot,3);
Ludwigfr 0:9f7ee7ed13e4 600 *forceRepulsionComputedY+=this->repulsionConstantForce*probaCell*(yWorldCell-this->yWorld)/pow(distanceCellToRobot,3);
Ludwigfr 0:9f7ee7ed13e4 601 }
Ludwigfr 0:9f7ee7ed13e4 602 }
Ludwigfr 0:9f7ee7ed13e4 603
Ludwigfr 0:9f7ee7ed13e4 604 //return 1 if positiv, -1 if negativ
Ludwigfr 0:9f7ee7ed13e4 605 float MiniExplorerCoimbra::sign1(float value){
Ludwigfr 0:9f7ee7ed13e4 606 if(value>=0)
Ludwigfr 0:9f7ee7ed13e4 607 return 1;
Ludwigfr 0:9f7ee7ed13e4 608 else
Ludwigfr 0:9f7ee7ed13e4 609 return -1;
Ludwigfr 0:9f7ee7ed13e4 610 }
Ludwigfr 0:9f7ee7ed13e4 611
Ludwigfr 0:9f7ee7ed13e4 612 //return 1 if positiv, 0 if negativ
Ludwigfr 0:9f7ee7ed13e4 613 int MiniExplorerCoimbra::sign2(float value){
Ludwigfr 0:9f7ee7ed13e4 614 if(value>=0)
Ludwigfr 0:9f7ee7ed13e4 615 return 1;
Ludwigfr 0:9f7ee7ed13e4 616 else
Ludwigfr 0:9f7ee7ed13e4 617 return 0;
Ludwigfr 0:9f7ee7ed13e4 618 }
Ludwigfr 0:9f7ee7ed13e4 619
geotsam 1:20f48907c726 620 float MiniExplorerCoimbra::distFromLine(float robot_x, float robot_y, float line_a, float line_b, float line_c){
geotsam 1:20f48907c726 621 return abs((line_a*robot_x+line_b*robot_y+line_c)/sqrt(line_a*line_a+line_b*line_b));
geotsam 1:20f48907c726 622 }
geotsam 1:20f48907c726 623
Ludwigfr 0:9f7ee7ed13e4 624 /*
Ludwigfr 0:9f7ee7ed13e4 625 void MiniExplorerCoimbra::test_procedure_lab_4(){
Ludwigfr 0:9f7ee7ed13e4 626 this->map.fill_map_with_initial();
Ludwigfr 0:9f7ee7ed13e4 627 float xEstimatedK=this->xWorld;
Ludwigfr 0:9f7ee7ed13e4 628 float yEstimatedK=this->yWorld;
Ludwigfr 0:9f7ee7ed13e4 629 float thetaWorldEstimatedK = this->thetaWorld;
Ludwigfr 0:9f7ee7ed13e4 630 float distanceMoved;
Ludwigfr 0:9f7ee7ed13e4 631 float angleMoved;
Ludwigfr 0:9f7ee7ed13e4 632 float PreviousCovarianceOdometricPositionEstimate[3][3];
Ludwigfr 0:9f7ee7ed13e4 633 PreviousCovarianceOdometricPositionEstimate[0][0]=0;
Ludwigfr 0:9f7ee7ed13e4 634 PreviousCovarianceOdometricPositionEstimate[0][1]=0;
Ludwigfr 0:9f7ee7ed13e4 635 PreviousCovarianceOdometricPositionEstimate[0][2]=0;
Ludwigfr 0:9f7ee7ed13e4 636 PreviousCovarianceOdometricPositionEstimate[1][0]=0;
Ludwigfr 0:9f7ee7ed13e4 637 PreviousCovarianceOdometricPositionEstimate[1][1]=0;
Ludwigfr 0:9f7ee7ed13e4 638 PreviousCovarianceOdometricPositionEstimate[1][2]=0;
Ludwigfr 0:9f7ee7ed13e4 639 PreviousCovarianceOdometricPositionEstimate[2][0]=0;
Ludwigfr 0:9f7ee7ed13e4 640 PreviousCovarianceOdometricPositionEstimate[2][1]=0;
Ludwigfr 0:9f7ee7ed13e4 641 PreviousCovarianceOdometricPositionEstimate[2][2]=0;
Ludwigfr 0:9f7ee7ed13e4 642 while(1){
Ludwigfr 0:9f7ee7ed13e4 643 distanceMoved=50;
Ludwigfr 0:9f7ee7ed13e4 644 angleMoved=0;
Ludwigfr 0:9f7ee7ed13e4 645 this->go_straight_line(50);
Ludwigfr 0:9f7ee7ed13e4 646 wait(1);
Ludwigfr 0:9f7ee7ed13e4 647 procedure_lab_4(xEstimatedK,yEstimatedK,thetaWorldEstimatedK,distanceMoved,angleMoved,PreviousCovarianceOdometricPositionEstimate);
Ludwigfr 0:9f7ee7ed13e4 648 pc.printf("\n\r x:%f,y:%f,theta:%f",this->xWorld,this->yWorld,this->thetaWorld);
Ludwigfr 0:9f7ee7ed13e4 649 pc.printf("\n\r xEstim:%f,yEstim:%f,thetaEstim:%f",xEstimatedK,yEstimatedK,thetaWorldEstimatedK);
Ludwigfr 0:9f7ee7ed13e4 650 this->turn_to_target(PI/2);
Ludwigfr 0:9f7ee7ed13e4 651 distanceMoved=0;
Ludwigfr 0:9f7ee7ed13e4 652 angleMoved=PI/2;
Ludwigfr 0:9f7ee7ed13e4 653 procedure_lab_4(xEstimatedK,yEstimatedK,thetaWorldEstimatedK,distanceMoved,angleMoved,PreviousCovarianceOdometricPositionEstimate);
Ludwigfr 0:9f7ee7ed13e4 654 pc.printf("\n\r x:%f,y:%f,theta:%f",this->xWorld,this->yWorld,this->thetaWorld);
Ludwigfr 0:9f7ee7ed13e4 655 pc.printf("\n\r xEstim:%f,yEstim:%f,thetaEstim:%f",xEstimatedK,yEstimatedK,thetaWorldEstimatedK);
Ludwigfr 0:9f7ee7ed13e4 656 }
Ludwigfr 0:9f7ee7ed13e4 657 }
Ludwigfr 0:9f7ee7ed13e4 658
Ludwigfr 0:9f7ee7ed13e4 659 void MiniExplorerCoimbra::go_straight_line(float distanceCm){
Ludwigfr 0:9f7ee7ed13e4 660 leftMotor(1,1);
Ludwigfr 0:9f7ee7ed13e4 661 rightMotor(1,1);
Ludwigfr 0:9f7ee7ed13e4 662 float xEstimated=this->xWorld+distanceCm*cos(this->thetaWorld);
Ludwigfr 0:9f7ee7ed13e4 663 float yEstimated=this->yWorld+distanceCm*sin(this->thetaWorld);
Ludwigfr 0:9f7ee7ed13e4 664 while(1){
Ludwigfr 0:9f7ee7ed13e4 665 this->myOdometria();
Ludwigfr 0:9f7ee7ed13e4 666 if(abs(xEstimated-this->xWorld) < 0.1 && abs(yEstimated-this->yWorld) < 0.1)
Ludwigfr 0:9f7ee7ed13e4 667 break;
Ludwigfr 0:9f7ee7ed13e4 668 }
Ludwigfr 0:9f7ee7ed13e4 669 leftMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 670 rightMotor(1,0);
Ludwigfr 0:9f7ee7ed13e4 671 }
Ludwigfr 0:9f7ee7ed13e4 672
Ludwigfr 0:9f7ee7ed13e4 673 //compute predicted localisation (assume movements are either straight lines or pure rotation), update PreviousCovarianceOdometricPositionEstimate
Ludwigfr 0:9f7ee7ed13e4 674 //TODO tweak constant rewritte it good
Ludwigfr 0:9f7ee7ed13e4 675 void MiniExplorerCoimbra::procedure_lab_4(float xEstimatedK,float yEstimatedK, float thetaWorldEstimatedK, float distanceMoved, float angleMoved, float PreviousCovarianceOdometricPositionEstimate[3][3]){
Ludwigfr 0:9f7ee7ed13e4 676
Ludwigfr 0:9f7ee7ed13e4 677 float distanceMovedByLeftWheel=distanceMoved/2;
Ludwigfr 0:9f7ee7ed13e4 678 float distanceMovedByRightWheel=distanceMoved/2;
Ludwigfr 0:9f7ee7ed13e4 679 if(angleMoved !=0){
Ludwigfr 0:9f7ee7ed13e4 680 //TODO check that not sure
Ludwigfr 0:9f7ee7ed13e4 681 distanceMovedByLeftWheel=-angleMoved/(2*this->distanceWheels);
Ludwigfr 0:9f7ee7ed13e4 682 distanceMovedByRightWheel=angleMoved/(2*this->distanceWheels);
Ludwigfr 0:9f7ee7ed13e4 683 }else{
Ludwigfr 0:9f7ee7ed13e4 684 distanceMovedByLeftWheel=distanceMoved/2;
Ludwigfr 0:9f7ee7ed13e4 685 distanceMovedByRightWheel=distanceMoved/2;
Ludwigfr 0:9f7ee7ed13e4 686 }
Ludwigfr 0:9f7ee7ed13e4 687
Ludwigfr 0:9f7ee7ed13e4 688 float xEstimatedKNext=xEstimatedK+distanceMoved*cos(angleMoved);
Ludwigfr 0:9f7ee7ed13e4 689 float yEstimatedKNext=yEstimatedK+distanceMoved*sin(angleMoved);
Ludwigfr 0:9f7ee7ed13e4 690 float thetaWorldEstimatedKNext=thetaWorldEstimatedK+angleMoved;
Ludwigfr 0:9f7ee7ed13e4 691
Ludwigfr 0:9f7ee7ed13e4 692 float kRight=0.1;//error constant
Ludwigfr 0:9f7ee7ed13e4 693 float kLeft=0.1;//error constant
Ludwigfr 0:9f7ee7ed13e4 694 float motionIncrementCovarianceMatrix[2][2];
Ludwigfr 0:9f7ee7ed13e4 695 motionIncrementCovarianceMatrix[0][0]=kRight*abs(distanceMovedByRightWheel);
Ludwigfr 0:9f7ee7ed13e4 696 motionIncrementCovarianceMatrix[0][1]=0;
Ludwigfr 0:9f7ee7ed13e4 697 motionIncrementCovarianceMatrix[1][0]=0;
Ludwigfr 0:9f7ee7ed13e4 698 motionIncrementCovarianceMatrix[1][1]=kLeft*abs(distanceMovedByLeftWheel);
Ludwigfr 0:9f7ee7ed13e4 699
Ludwigfr 0:9f7ee7ed13e4 700 float jacobianFp[3][3];
Ludwigfr 0:9f7ee7ed13e4 701 jacobianFp[0][0]=1;
Ludwigfr 0:9f7ee7ed13e4 702 jacobianFp[0][1]=0;
Ludwigfr 0:9f7ee7ed13e4 703 jacobianFp[0][2]=-distanceMoved*sin(thetaWorldEstimatedK+angleMoved/2);
Ludwigfr 0:9f7ee7ed13e4 704 jacobianFp[1][0]=0;
Ludwigfr 0:9f7ee7ed13e4 705 jacobianFp[1][1]=1;
Ludwigfr 0:9f7ee7ed13e4 706 jacobianFp[1][2]=distanceMoved*cos(thetaWorldEstimatedK+angleMoved/2);;
Ludwigfr 0:9f7ee7ed13e4 707 jacobianFp[2][0]=0;
Ludwigfr 0:9f7ee7ed13e4 708 jacobianFp[2][1]=0;
Ludwigfr 0:9f7ee7ed13e4 709 jacobianFp[2][2]=1;
Ludwigfr 0:9f7ee7ed13e4 710
Ludwigfr 0:9f7ee7ed13e4 711 float jacobianFpTranspose[3][3];
Ludwigfr 0:9f7ee7ed13e4 712 jacobianFpTranspose[0][0]=1;
Ludwigfr 0:9f7ee7ed13e4 713 jacobianFpTranspose[0][1]=0;
Ludwigfr 0:9f7ee7ed13e4 714 jacobianFpTranspose[0][2]=0;
Ludwigfr 0:9f7ee7ed13e4 715 jacobianFpTranspose[1][0]=0;
Ludwigfr 0:9f7ee7ed13e4 716 jacobianFpTranspose[1][1]=1;
Ludwigfr 0:9f7ee7ed13e4 717 jacobianFpTranspose[1][2]=0;
Ludwigfr 0:9f7ee7ed13e4 718 jacobianFpTranspose[2][0]=-distanceMoved*sin(thetaWorldEstimatedK+angleMoved/2);
Ludwigfr 0:9f7ee7ed13e4 719 jacobianFpTranspose[2][1]=distanceMoved*cos(thetaWorldEstimatedK+angleMoved/2);
Ludwigfr 0:9f7ee7ed13e4 720 jacobianFpTranspose[2][2]=1;
Ludwigfr 0:9f7ee7ed13e4 721
Ludwigfr 0:9f7ee7ed13e4 722 float jacobianFDeltarl[3][2];
Ludwigfr 0:9f7ee7ed13e4 723 jacobianFDeltarl[0][0]=(1/2)*cos(thetaWorldEstimatedK+angleMoved/2)-(distanceMoved/(2*this->distanceWheels))*sin(thetaWorldEstimatedK+angleMoved/2);
Ludwigfr 0:9f7ee7ed13e4 724 jacobianFDeltarl[0][1]=(1/2)*cos(thetaWorldEstimatedK+angleMoved/2)+(distanceMoved/(2*this->distanceWheels))*sin(thetaWorldEstimatedK+angleMoved/2);
Ludwigfr 0:9f7ee7ed13e4 725 jacobianFDeltarl[1][0]=(1/2)*sin(thetaWorldEstimatedK+angleMoved/2)+(distanceMoved/(2*this->distanceWheels))*cos(thetaWorldEstimatedK+angleMoved/2);
Ludwigfr 0:9f7ee7ed13e4 726 jacobianFDeltarl[1][1]=(1/2)*sin(thetaWorldEstimatedK+angleMoved/2)-(distanceMoved/(2*this->distanceWheels))*cos(thetaWorldEstimatedK+angleMoved/2);
Ludwigfr 0:9f7ee7ed13e4 727 jacobianFDeltarl[2][0]=1/this->distanceWheels;
Ludwigfr 0:9f7ee7ed13e4 728 jacobianFDeltarl[2][1]=-1/this->distanceWheels;
Ludwigfr 0:9f7ee7ed13e4 729
Ludwigfr 0:9f7ee7ed13e4 730 float jacobianFDeltarlTranspose[2][3];//1,3,5;2,4,6
Ludwigfr 0:9f7ee7ed13e4 731 jacobianFDeltarlTranspose[0][0]=(1/2)*cos(thetaWorldEstimatedK+angleMoved/2)-(distanceMoved/(2*this->distanceWheels))*sin(thetaWorldEstimatedK+angleMoved/2);
Ludwigfr 0:9f7ee7ed13e4 732 jacobianFDeltarlTranspose[0][1]=(1/2)*sin(thetaWorldEstimatedK+angleMoved/2)+(distanceMoved/(2*this->distanceWheels))*cos(thetaWorldEstimatedK+angleMoved/2);
Ludwigfr 0:9f7ee7ed13e4 733 jacobianFDeltarlTranspose[0][2]=1/this->distanceWheels;
Ludwigfr 0:9f7ee7ed13e4 734 jacobianFDeltarlTranspose[1][0]=(1/2)*cos(thetaWorldEstimatedK+angleMoved/2)+(distanceMoved/(2*this->distanceWheels))*sin(thetaWorldEstimatedK+angleMoved/2);
Ludwigfr 0:9f7ee7ed13e4 735 jacobianFDeltarlTranspose[1][1]=(1/2)*sin(thetaWorldEstimatedK+angleMoved/2)-(distanceMoved/(2*this->distanceWheels))*cos(thetaWorldEstimatedK+angleMoved/2);
Ludwigfr 0:9f7ee7ed13e4 736 jacobianFDeltarlTranspose[1][2]=-1/this->distanceWheels;
Ludwigfr 0:9f7ee7ed13e4 737
Ludwigfr 0:9f7ee7ed13e4 738 float tempMatrix3_3[3][3];
Ludwigfr 0:9f7ee7ed13e4 739 int i;
Ludwigfr 0:9f7ee7ed13e4 740 int j;
Ludwigfr 0:9f7ee7ed13e4 741 int k;
Ludwigfr 0:9f7ee7ed13e4 742 for( i = 0; i < 3; ++i){//mult 3*3, 3*3
Ludwigfr 0:9f7ee7ed13e4 743 for(j = 0; j < 3; ++j){
Ludwigfr 0:9f7ee7ed13e4 744 for(k = 0; k < 3; ++k){
Ludwigfr 0:9f7ee7ed13e4 745 tempMatrix3_3[i][j] += jacobianFp[i][k] * PreviousCovarianceOdometricPositionEstimate[k][j];
Ludwigfr 0:9f7ee7ed13e4 746 }
Ludwigfr 0:9f7ee7ed13e4 747 }
Ludwigfr 0:9f7ee7ed13e4 748 }
Ludwigfr 0:9f7ee7ed13e4 749 float sndTempMatrix3_3[3][3];
Ludwigfr 0:9f7ee7ed13e4 750 for(i = 0; i < 3; ++i){//mult 3*3, 3*3
Ludwigfr 0:9f7ee7ed13e4 751 for(j = 0; j < 3; ++j){
Ludwigfr 0:9f7ee7ed13e4 752 for(k = 0; k < 3; ++k){
Ludwigfr 0:9f7ee7ed13e4 753 sndTempMatrix3_3[i][j] += tempMatrix3_3[i][k] * jacobianFpTranspose[k][j];
Ludwigfr 0:9f7ee7ed13e4 754 }
Ludwigfr 0:9f7ee7ed13e4 755 }
Ludwigfr 0:9f7ee7ed13e4 756 }
Ludwigfr 0:9f7ee7ed13e4 757 float tempMatrix3_2[3][2];
Ludwigfr 0:9f7ee7ed13e4 758 for(i = 0; i < 3; ++i){//mult 3*2 , 2,2
Ludwigfr 0:9f7ee7ed13e4 759 for(j = 0; j < 2; ++j){
Ludwigfr 0:9f7ee7ed13e4 760 for(k = 0; k < 2; ++k){
Ludwigfr 0:9f7ee7ed13e4 761 tempMatrix3_2[i][j] += jacobianFDeltarl[i][k] * motionIncrementCovarianceMatrix[k][j];
Ludwigfr 0:9f7ee7ed13e4 762 }
Ludwigfr 0:9f7ee7ed13e4 763 }
Ludwigfr 0:9f7ee7ed13e4 764 }
Ludwigfr 0:9f7ee7ed13e4 765 float thrdTempMatrix3_3[3][3];
Ludwigfr 0:9f7ee7ed13e4 766 for(i = 0; i < 3; ++i){//mult 3*2 , 2,3
Ludwigfr 0:9f7ee7ed13e4 767 for(j = 0; j < 3; ++j){
Ludwigfr 0:9f7ee7ed13e4 768 for(k = 0; k < 2; ++k){
Ludwigfr 0:9f7ee7ed13e4 769 thrdTempMatrix3_3[i][j] += tempMatrix3_2[i][k] * jacobianFDeltarlTranspose[k][j];
Ludwigfr 0:9f7ee7ed13e4 770 }
Ludwigfr 0:9f7ee7ed13e4 771 }
Ludwigfr 0:9f7ee7ed13e4 772 }
Ludwigfr 0:9f7ee7ed13e4 773 float newCovarianceOdometricPositionEstimate[3][3];
Ludwigfr 0:9f7ee7ed13e4 774 for(i = 0; i < 3; ++i){//add 3*3 , 3*3
Ludwigfr 0:9f7ee7ed13e4 775 for(j = 0; j < 3; ++j){
Ludwigfr 0:9f7ee7ed13e4 776 newCovarianceOdometricPositionEstimate[i][j]=sndTempMatrix3_3[i][j]+thrdTempMatrix3_3[i][j];
Ludwigfr 0:9f7ee7ed13e4 777 }
Ludwigfr 0:9f7ee7ed13e4 778 }
Ludwigfr 0:9f7ee7ed13e4 779
Ludwigfr 0:9f7ee7ed13e4 780 //check measurements from sonars, see if they passed the validation gate
Ludwigfr 0:9f7ee7ed13e4 781 float leftCm = get_distance_left_sensor()/10;
Ludwigfr 0:9f7ee7ed13e4 782 float frontCm = get_distance_front_sensor()/10;
Ludwigfr 0:9f7ee7ed13e4 783 float rightCm = get_distance_right_sensor()/10;
Ludwigfr 0:9f7ee7ed13e4 784 //if superior to sonar range, put the value to sonar max range + 1
Ludwigfr 0:9f7ee7ed13e4 785 if(leftCm > this->sonarLeft.maxRange)
Ludwigfr 0:9f7ee7ed13e4 786 leftCm=this->sonarLeft.maxRange;
Ludwigfr 0:9f7ee7ed13e4 787 if(frontCm > this->sonarFront.maxRange)
Ludwigfr 0:9f7ee7ed13e4 788 frontCm=this->sonarFront.maxRange;
Ludwigfr 0:9f7ee7ed13e4 789 if(rightCm > this->sonarRight.maxRange)
Ludwigfr 0:9f7ee7ed13e4 790 rightCm=this->sonarRight.maxRange;
Ludwigfr 0:9f7ee7ed13e4 791 //get the estimated measure we should have according to our knowledge of the map and the previously computed localisation
Ludwigfr 0:9f7ee7ed13e4 792 float leftCmEstimated=this->sonarLeft.maxRange;
Ludwigfr 0:9f7ee7ed13e4 793 float frontCmEstimated=this->sonarFront.maxRange;
Ludwigfr 0:9f7ee7ed13e4 794 float rightCmEstimated=this->sonarRight.maxRange;
Ludwigfr 0:9f7ee7ed13e4 795 float xWorldCell;
Ludwigfr 0:9f7ee7ed13e4 796 float yWorldCell;
Ludwigfr 0:9f7ee7ed13e4 797 float currDistance;
Ludwigfr 0:9f7ee7ed13e4 798 float xClosestCellLeft;
Ludwigfr 0:9f7ee7ed13e4 799 float yClosestCellLeft;
Ludwigfr 0:9f7ee7ed13e4 800 float xClosestCellFront;
Ludwigfr 0:9f7ee7ed13e4 801 float yClosestCellFront;
Ludwigfr 0:9f7ee7ed13e4 802 float xClosestCellRight;
Ludwigfr 0:9f7ee7ed13e4 803 float yClosestCellRight;
Ludwigfr 0:9f7ee7ed13e4 804 for(int i=0;i<this->map.nbCellWidth;i++){
Ludwigfr 0:9f7ee7ed13e4 805 for(int j=0;j<this->map.nbCellHeight;j++){
Ludwigfr 0:9f7ee7ed13e4 806 //check if occupied, if not discard
Ludwigfr 0:9f7ee7ed13e4 807 if(this->map.get_proba_cell(i,j)==1){
Ludwigfr 0:9f7ee7ed13e4 808 //check if in sonar range
Ludwigfr 0:9f7ee7ed13e4 809 xWorldCell=this->map.cell_width_coordinate_to_world(i);
Ludwigfr 0:9f7ee7ed13e4 810 yWorldCell=this->map.cell_height_coordinate_to_world(j);
Ludwigfr 0:9f7ee7ed13e4 811 //check left
Ludwigfr 0:9f7ee7ed13e4 812 currDistance=this->sonarLeft.isInRange(xWorldCell,yWorldCell,xEstimatedKNext,yEstimatedKNext,thetaWorldEstimatedKNext);
Ludwigfr 0:9f7ee7ed13e4 813 if((currDistance < this->sonarLeft.maxRange) && currDistance!=-1){
Ludwigfr 0:9f7ee7ed13e4 814 //check if distance is lower than previous, update lowest if so
Ludwigfr 0:9f7ee7ed13e4 815 if(currDistance < leftCmEstimated){
Ludwigfr 0:9f7ee7ed13e4 816 leftCmEstimated= currDistance;
Ludwigfr 0:9f7ee7ed13e4 817 xClosestCellLeft=xWorldCell;
Ludwigfr 0:9f7ee7ed13e4 818 yClosestCellLeft=yWorldCell;
Ludwigfr 0:9f7ee7ed13e4 819 }
Ludwigfr 0:9f7ee7ed13e4 820 }
Ludwigfr 0:9f7ee7ed13e4 821 //check front
Ludwigfr 0:9f7ee7ed13e4 822 currDistance=this->sonarFront.isInRange(xWorldCell,yWorldCell,xEstimatedKNext,yEstimatedKNext,thetaWorldEstimatedKNext);
Ludwigfr 0:9f7ee7ed13e4 823 if((currDistance < this->sonarFront.maxRange) && currDistance!=-1){
Ludwigfr 0:9f7ee7ed13e4 824 //check if distance is lower than previous, update lowest if so
Ludwigfr 0:9f7ee7ed13e4 825 if(currDistance < frontCmEstimated){
Ludwigfr 0:9f7ee7ed13e4 826 frontCmEstimated= currDistance;
Ludwigfr 0:9f7ee7ed13e4 827 xClosestCellFront=xWorldCell;
Ludwigfr 0:9f7ee7ed13e4 828 yClosestCellFront=yWorldCell;
Ludwigfr 0:9f7ee7ed13e4 829 }
Ludwigfr 0:9f7ee7ed13e4 830 }
Ludwigfr 0:9f7ee7ed13e4 831 //check right
Ludwigfr 0:9f7ee7ed13e4 832 currDistance=this->sonarRight.isInRange(xWorldCell,yWorldCell,xEstimatedKNext,yEstimatedKNext,thetaWorldEstimatedKNext);
Ludwigfr 0:9f7ee7ed13e4 833 if((currDistance < this->sonarRight.maxRange) && currDistance!=-1){
Ludwigfr 0:9f7ee7ed13e4 834 //check if distance is lower than previous, update lowest if so
Ludwigfr 0:9f7ee7ed13e4 835 if(currDistance < rightCmEstimated){
Ludwigfr 0:9f7ee7ed13e4 836 rightCmEstimated= currDistance;
Ludwigfr 0:9f7ee7ed13e4 837 xClosestCellRight=xWorldCell;
Ludwigfr 0:9f7ee7ed13e4 838 yClosestCellRight=yWorldCell;
Ludwigfr 0:9f7ee7ed13e4 839 }
Ludwigfr 0:9f7ee7ed13e4 840 }
Ludwigfr 0:9f7ee7ed13e4 841 }
Ludwigfr 0:9f7ee7ed13e4 842 }
Ludwigfr 0:9f7ee7ed13e4 843 }
Ludwigfr 0:9f7ee7ed13e4 844 //get the innoncation: the value of the difference between the actual measure and what we anticipated
Ludwigfr 0:9f7ee7ed13e4 845 float innovationLeft=leftCm-leftCmEstimated;
Ludwigfr 0:9f7ee7ed13e4 846 float innovationFront=frontCm-frontCmEstimated;
Ludwigfr 0:9f7ee7ed13e4 847 float innovationRight=-rightCm-rightCmEstimated;
Ludwigfr 0:9f7ee7ed13e4 848 //compute jacobian of observation
Ludwigfr 0:9f7ee7ed13e4 849 float jacobianOfObservationLeft[3];
Ludwigfr 0:9f7ee7ed13e4 850 float jacobianOfObservationRight[3];
Ludwigfr 0:9f7ee7ed13e4 851 float jacobianOfObservationFront[3];
Ludwigfr 0:9f7ee7ed13e4 852 float xSonarLeft=xEstimatedKNext+this->sonarLeft.distanceX;
Ludwigfr 0:9f7ee7ed13e4 853 float ySonarLeft=yEstimatedKNext+this->sonarLeft.distanceY;
Ludwigfr 0:9f7ee7ed13e4 854 //left
Ludwigfr 0:9f7ee7ed13e4 855 jacobianOfObservationLeft[0]=(xSonarLeft-xClosestCellLeft)/leftCmEstimated;
Ludwigfr 0:9f7ee7ed13e4 856 jacobianOfObservationLeft[1]=(ySonarLeft-yClosestCellLeft)/leftCmEstimated;
Ludwigfr 0:9f7ee7ed13e4 857 jacobianOfObservationLeft[2]=(xClosestCellLeft-xSonarLeft)*(xSonarLeft*sin(thetaWorldEstimatedKNext)+ySonarLeft*cos(thetaWorldEstimatedKNext))+(yClosestCellLeft-ySonarLeft)*(-xSonarLeft*cos(thetaWorldEstimatedKNext)+ySonarLeft*sin(thetaWorldEstimatedKNext));
Ludwigfr 0:9f7ee7ed13e4 858 //front
Ludwigfr 0:9f7ee7ed13e4 859 float xSonarFront=xEstimatedKNext+this->sonarFront.distanceX;
Ludwigfr 0:9f7ee7ed13e4 860 float ySonarFront=yEstimatedKNext+this->sonarFront.distanceY;
Ludwigfr 0:9f7ee7ed13e4 861 jacobianOfObservationFront[0]=(xSonarFront-xClosestCellFront)/frontCmEstimated;
Ludwigfr 0:9f7ee7ed13e4 862 jacobianOfObservationFront[1]=(ySonarFront-yClosestCellFront)/frontCmEstimated;
Ludwigfr 0:9f7ee7ed13e4 863 jacobianOfObservationFront[2]=(xClosestCellFront-xSonarFront)*(xSonarFront*sin(thetaWorldEstimatedKNext)+ySonarFront*cos(thetaWorldEstimatedKNext))+(yClosestCellFront-ySonarFront)*(-xSonarFront*cos(thetaWorldEstimatedKNext)+ySonarFront*sin(thetaWorldEstimatedKNext));
Ludwigfr 0:9f7ee7ed13e4 864 //right
Ludwigfr 0:9f7ee7ed13e4 865 float xSonarRight=xEstimatedKNext+this->sonarRight.distanceX;
Ludwigfr 0:9f7ee7ed13e4 866 float ySonarRight=yEstimatedKNext+this->sonarRight.distanceY;
Ludwigfr 0:9f7ee7ed13e4 867 jacobianOfObservationRight[0]=(xSonarRight-xClosestCellRight)/rightCmEstimated;
Ludwigfr 0:9f7ee7ed13e4 868 jacobianOfObservationRight[1]=(ySonarRight-yClosestCellRight)/rightCmEstimated;
Ludwigfr 0:9f7ee7ed13e4 869 jacobianOfObservationRight[2]=(xClosestCellRight-xSonarRight)*(xSonarRight*sin(thetaWorldEstimatedKNext)+ySonarRight*cos(thetaWorldEstimatedKNext))+(yClosestCellRight-ySonarRight)*(-xSonarRight*cos(thetaWorldEstimatedKNext)+ySonarRight*sin(thetaWorldEstimatedKNext));
Ludwigfr 0:9f7ee7ed13e4 870
Ludwigfr 0:9f7ee7ed13e4 871 //left
Ludwigfr 0:9f7ee7ed13e4 872 float TempMatrix3_3InnovationLeft[3];
Ludwigfr 0:9f7ee7ed13e4 873 TempMatrix3_3InnovationLeft[0]=jacobianOfObservationLeft[0]*newCovarianceOdometricPositionEstimate[0][0]+jacobianOfObservationLeft[1]*newCovarianceOdometricPositionEstimate[1][0]+jacobianOfObservationLeft[2]*newCovarianceOdometricPositionEstimate[2][0];
Ludwigfr 0:9f7ee7ed13e4 874 TempMatrix3_3InnovationLeft[1]=jacobianOfObservationLeft[0]*newCovarianceOdometricPositionEstimate[0][1]+jacobianOfObservationLeft[1]*newCovarianceOdometricPositionEstimate[1][1]+jacobianOfObservationLeft[2]*newCovarianceOdometricPositionEstimate[2][1];
Ludwigfr 0:9f7ee7ed13e4 875 TempMatrix3_3InnovationLeft[2]=jacobianOfObservationLeft[0]*newCovarianceOdometricPositionEstimate[0][2]+jacobianOfObservationLeft[1]*newCovarianceOdometricPositionEstimate[1][2]+jacobianOfObservationLeft[2]*newCovarianceOdometricPositionEstimate[2][2];
Ludwigfr 0:9f7ee7ed13e4 876 float innovationConvarianceLeft=TempMatrix3_3InnovationLeft[0]*jacobianOfObservationLeft[0]+TempMatrix3_3InnovationLeft[1]*jacobianOfObservationLeft[1]+TempMatrix3_3InnovationLeft[2]*jacobianOfObservationLeft[2];
Ludwigfr 0:9f7ee7ed13e4 877 //front
Ludwigfr 0:9f7ee7ed13e4 878 float TempMatrix3_3InnovationFront[3];
Ludwigfr 0:9f7ee7ed13e4 879 TempMatrix3_3InnovationFront[0]=jacobianOfObservationFront[0]*newCovarianceOdometricPositionEstimate[0][0]+jacobianOfObservationFront[1]*newCovarianceOdometricPositionEstimate[1][0]+jacobianOfObservationFront[2]*newCovarianceOdometricPositionEstimate[2][0];
Ludwigfr 0:9f7ee7ed13e4 880 TempMatrix3_3InnovationFront[1]=jacobianOfObservationFront[0]*newCovarianceOdometricPositionEstimate[0][1]+jacobianOfObservationFront[1]*newCovarianceOdometricPositionEstimate[1][1]+jacobianOfObservationFront[2]*newCovarianceOdometricPositionEstimate[2][1];
Ludwigfr 0:9f7ee7ed13e4 881 TempMatrix3_3InnovationFront[2]=jacobianOfObservationFront[0]*newCovarianceOdometricPositionEstimate[0][2]+jacobianOfObservationFront[1]*newCovarianceOdometricPositionEstimate[1][2]+jacobianOfObservationFront[2]*newCovarianceOdometricPositionEstimate[2][2];
Ludwigfr 0:9f7ee7ed13e4 882 float innovationConvarianceFront=TempMatrix3_3InnovationFront[0]*jacobianOfObservationFront[0]+TempMatrix3_3InnovationFront[1]*jacobianOfObservationFront[1]+TempMatrix3_3InnovationFront[2]*jacobianOfObservationFront[2];
Ludwigfr 0:9f7ee7ed13e4 883 //right
Ludwigfr 0:9f7ee7ed13e4 884 float TempMatrix3_3InnovationRight[3];
Ludwigfr 0:9f7ee7ed13e4 885 TempMatrix3_3InnovationRight[0]=jacobianOfObservationRight[0]*newCovarianceOdometricPositionEstimate[0][0]+jacobianOfObservationRight[1]*newCovarianceOdometricPositionEstimate[1][0]+jacobianOfObservationRight[2]*newCovarianceOdometricPositionEstimate[2][0];
Ludwigfr 0:9f7ee7ed13e4 886 TempMatrix3_3InnovationRight[1]=jacobianOfObservationRight[0]*newCovarianceOdometricPositionEstimate[0][1]+jacobianOfObservationRight[1]*newCovarianceOdometricPositionEstimate[1][1]+jacobianOfObservationRight[2]*newCovarianceOdometricPositionEstimate[2][1];
Ludwigfr 0:9f7ee7ed13e4 887 TempMatrix3_3InnovationRight[2]=jacobianOfObservationRight[0]*newCovarianceOdometricPositionEstimate[0][2]+jacobianOfObservationRight[1]*newCovarianceOdometricPositionEstimate[1][2]+jacobianOfObservationRight[2]*newCovarianceOdometricPositionEstimate[2][2];
Ludwigfr 0:9f7ee7ed13e4 888 float innovationConvarianceRight=TempMatrix3_3InnovationRight[0]*jacobianOfObservationRight[0]+TempMatrix3_3InnovationRight[1]*jacobianOfObservationRight[1]+TempMatrix3_3InnovationRight[2]*jacobianOfObservationRight[2];
Ludwigfr 0:9f7ee7ed13e4 889
Ludwigfr 0:9f7ee7ed13e4 890 //check if it pass the validation gate
Ludwigfr 0:9f7ee7ed13e4 891 float gateScoreLeft=innovationLeft*innovationLeft/innovationConvarianceLeft;
Ludwigfr 0:9f7ee7ed13e4 892 float gateScoreFront=innovationFront*innovationFront/innovationConvarianceFront;
Ludwigfr 0:9f7ee7ed13e4 893 float gateScoreRight=innovationRight*innovationRight/innovationConvarianceRight;
Ludwigfr 0:9f7ee7ed13e4 894 int leftPassed=0;
Ludwigfr 0:9f7ee7ed13e4 895 int frontPassed=0;
Ludwigfr 0:9f7ee7ed13e4 896 int rightPassed=0;
Ludwigfr 0:9f7ee7ed13e4 897 int e=10;//constant
Ludwigfr 0:9f7ee7ed13e4 898 if(gateScoreLeft < e)
Ludwigfr 0:9f7ee7ed13e4 899 leftPassed=1;
Ludwigfr 0:9f7ee7ed13e4 900 if(gateScoreFront < e)
Ludwigfr 0:9f7ee7ed13e4 901 frontPassed=10;
Ludwigfr 0:9f7ee7ed13e4 902 if(gateScoreRight < e)
Ludwigfr 0:9f7ee7ed13e4 903 rightPassed=100;
Ludwigfr 0:9f7ee7ed13e4 904 //for those who passed
Ludwigfr 0:9f7ee7ed13e4 905 //compute composite innovation
Ludwigfr 0:9f7ee7ed13e4 906 float compositeInnovation[3];
Ludwigfr 0:9f7ee7ed13e4 907 int nbPassed=leftPassed+frontPassed+rightPassed;
Ludwigfr 0:9f7ee7ed13e4 908 switch(nbPassed){
Ludwigfr 0:9f7ee7ed13e4 909 case 0://none
Ludwigfr 0:9f7ee7ed13e4 910 compositeInnovation[0]=0;
Ludwigfr 0:9f7ee7ed13e4 911 compositeInnovation[1]=0;
Ludwigfr 0:9f7ee7ed13e4 912 compositeInnovation[2]=0;
Ludwigfr 0:9f7ee7ed13e4 913 break;
Ludwigfr 0:9f7ee7ed13e4 914 case 1://left
Ludwigfr 0:9f7ee7ed13e4 915 compositeInnovation[0]=innovationLeft;
Ludwigfr 0:9f7ee7ed13e4 916 compositeInnovation[1]=0;
Ludwigfr 0:9f7ee7ed13e4 917 compositeInnovation[2]=0;
Ludwigfr 0:9f7ee7ed13e4 918 break;
Ludwigfr 0:9f7ee7ed13e4 919 case 10://front
Ludwigfr 0:9f7ee7ed13e4 920 compositeInnovation[0]=0;
Ludwigfr 0:9f7ee7ed13e4 921 compositeInnovation[1]=innovationFront;
Ludwigfr 0:9f7ee7ed13e4 922 compositeInnovation[2]=0;
Ludwigfr 0:9f7ee7ed13e4 923 break;
Ludwigfr 0:9f7ee7ed13e4 924 case 11://left and front
Ludwigfr 0:9f7ee7ed13e4 925 compositeInnovation[0]=innovationLeft;
Ludwigfr 0:9f7ee7ed13e4 926 compositeInnovation[1]=innovationFront;
Ludwigfr 0:9f7ee7ed13e4 927 compositeInnovation[2]=0;
Ludwigfr 0:9f7ee7ed13e4 928 break;
Ludwigfr 0:9f7ee7ed13e4 929 case 100://right
Ludwigfr 0:9f7ee7ed13e4 930 compositeInnovation[0]=0;
Ludwigfr 0:9f7ee7ed13e4 931 compositeInnovation[1]=0;
Ludwigfr 0:9f7ee7ed13e4 932 compositeInnovation[2]=innovationRight;
Ludwigfr 0:9f7ee7ed13e4 933 break;
Ludwigfr 0:9f7ee7ed13e4 934 case 101://right and left
Ludwigfr 0:9f7ee7ed13e4 935 compositeInnovation[0]=innovationLeft;
Ludwigfr 0:9f7ee7ed13e4 936 compositeInnovation[1]=0;
Ludwigfr 0:9f7ee7ed13e4 937 compositeInnovation[2]=innovationRight;
Ludwigfr 0:9f7ee7ed13e4 938 break;
Ludwigfr 0:9f7ee7ed13e4 939 case 110://right and front
Ludwigfr 0:9f7ee7ed13e4 940 compositeInnovation[0]=0;
Ludwigfr 0:9f7ee7ed13e4 941 compositeInnovation[1]=innovationFront;
Ludwigfr 0:9f7ee7ed13e4 942 compositeInnovation[2]=innovationRight;
Ludwigfr 0:9f7ee7ed13e4 943 break;
Ludwigfr 0:9f7ee7ed13e4 944 case 111://right front and left
Ludwigfr 0:9f7ee7ed13e4 945 compositeInnovation[0]=innovationLeft;
Ludwigfr 0:9f7ee7ed13e4 946 compositeInnovation[1]=innovationFront;
Ludwigfr 0:9f7ee7ed13e4 947 compositeInnovation[2]=innovationRight;
Ludwigfr 0:9f7ee7ed13e4 948 break;
Ludwigfr 0:9f7ee7ed13e4 949 }
Ludwigfr 0:9f7ee7ed13e4 950 //compute composite measurement Jacobian
Ludwigfr 0:9f7ee7ed13e4 951 float *compositeMeasurementJacobian;
Ludwigfr 0:9f7ee7ed13e4 952 switch(nbPassed){
Ludwigfr 0:9f7ee7ed13e4 953 case 0://none
Ludwigfr 0:9f7ee7ed13e4 954 break;
Ludwigfr 0:9f7ee7ed13e4 955 case 1://left
Ludwigfr 0:9f7ee7ed13e4 956 //compositeMeasurementJacobian = jacobianOfObservationLeft
Ludwigfr 0:9f7ee7ed13e4 957 break;
Ludwigfr 0:9f7ee7ed13e4 958 case 10://front
Ludwigfr 0:9f7ee7ed13e4 959 //compositeMeasurementJacobian = jacobianOfObservationFront
Ludwigfr 0:9f7ee7ed13e4 960 break;
Ludwigfr 0:9f7ee7ed13e4 961 case 11://left and front
Ludwigfr 0:9f7ee7ed13e4 962 compositeMeasurementJacobian=new float[6];
Ludwigfr 0:9f7ee7ed13e4 963 compositeMeasurementJacobian[0]= jacobianOfObservationLeft[0];
Ludwigfr 0:9f7ee7ed13e4 964 compositeMeasurementJacobian[1]= jacobianOfObservationLeft[1];
Ludwigfr 0:9f7ee7ed13e4 965 compositeMeasurementJacobian[2]= jacobianOfObservationLeft[2];
Ludwigfr 0:9f7ee7ed13e4 966 compositeMeasurementJacobian[3]= jacobianOfObservationFront[0];
Ludwigfr 0:9f7ee7ed13e4 967 compositeMeasurementJacobian[4]= jacobianOfObservationFront[1];
Ludwigfr 0:9f7ee7ed13e4 968 compositeMeasurementJacobian[5]= jacobianOfObservationFront[2];
Ludwigfr 0:9f7ee7ed13e4 969 break;
Ludwigfr 0:9f7ee7ed13e4 970 case 100://right
Ludwigfr 0:9f7ee7ed13e4 971 //compositeMeasurementJacobian = jacobianOfObservationRight
Ludwigfr 0:9f7ee7ed13e4 972 break;
Ludwigfr 0:9f7ee7ed13e4 973 case 101://right and left
Ludwigfr 0:9f7ee7ed13e4 974 compositeMeasurementJacobian=new float[6];
Ludwigfr 0:9f7ee7ed13e4 975 compositeMeasurementJacobian[0]= jacobianOfObservationLeft[0];
Ludwigfr 0:9f7ee7ed13e4 976 compositeMeasurementJacobian[1]= jacobianOfObservationLeft[1];
Ludwigfr 0:9f7ee7ed13e4 977 compositeMeasurementJacobian[2]= jacobianOfObservationLeft[2];
Ludwigfr 0:9f7ee7ed13e4 978 compositeMeasurementJacobian[3]= jacobianOfObservationRight[0];
Ludwigfr 0:9f7ee7ed13e4 979 compositeMeasurementJacobian[4]= jacobianOfObservationRight[1];
Ludwigfr 0:9f7ee7ed13e4 980 compositeMeasurementJacobian[5]= jacobianOfObservationRight[2];
Ludwigfr 0:9f7ee7ed13e4 981
Ludwigfr 0:9f7ee7ed13e4 982 break;
Ludwigfr 0:9f7ee7ed13e4 983 case 110://right and front
Ludwigfr 0:9f7ee7ed13e4 984 compositeMeasurementJacobian=new float[6];
Ludwigfr 0:9f7ee7ed13e4 985 compositeMeasurementJacobian[0]= jacobianOfObservationFront[0];
Ludwigfr 0:9f7ee7ed13e4 986 compositeMeasurementJacobian[1]= jacobianOfObservationFront[1];
Ludwigfr 0:9f7ee7ed13e4 987 compositeMeasurementJacobian[2]= jacobianOfObservationFront[2];
Ludwigfr 0:9f7ee7ed13e4 988 compositeMeasurementJacobian[3]= jacobianOfObservationRight[0];
Ludwigfr 0:9f7ee7ed13e4 989 compositeMeasurementJacobian[4]= jacobianOfObservationRight[1];
Ludwigfr 0:9f7ee7ed13e4 990 compositeMeasurementJacobian[5]= jacobianOfObservationRight[2];
Ludwigfr 0:9f7ee7ed13e4 991
Ludwigfr 0:9f7ee7ed13e4 992 break;
Ludwigfr 0:9f7ee7ed13e4 993 case 111://right front and left
Ludwigfr 0:9f7ee7ed13e4 994 compositeMeasurementJacobian=new float[9];
Ludwigfr 0:9f7ee7ed13e4 995 compositeMeasurementJacobian[0]= jacobianOfObservationLeft[0];
Ludwigfr 0:9f7ee7ed13e4 996 compositeMeasurementJacobian[1]= jacobianOfObservationLeft[1];
Ludwigfr 0:9f7ee7ed13e4 997 compositeMeasurementJacobian[2]= jacobianOfObservationLeft[2];
Ludwigfr 0:9f7ee7ed13e4 998 compositeMeasurementJacobian[3]= jacobianOfObservationFront[0];
Ludwigfr 0:9f7ee7ed13e4 999 compositeMeasurementJacobian[4]= jacobianOfObservationFront[1];
Ludwigfr 0:9f7ee7ed13e4 1000 compositeMeasurementJacobian[5]= jacobianOfObservationFront[2];
Ludwigfr 0:9f7ee7ed13e4 1001 compositeMeasurementJacobian[6]= jacobianOfObservationRight[0];
Ludwigfr 0:9f7ee7ed13e4 1002 compositeMeasurementJacobian[7]= jacobianOfObservationRight[1];
Ludwigfr 0:9f7ee7ed13e4 1003 compositeMeasurementJacobian[8]= jacobianOfObservationRight[2];
Ludwigfr 0:9f7ee7ed13e4 1004 break;
Ludwigfr 0:9f7ee7ed13e4 1005 }
Ludwigfr 0:9f7ee7ed13e4 1006 //compute composite innovation covariance TODO dont have time, I assume you can just multiply them, can also simply all this easily
Ludwigfr 0:9f7ee7ed13e4 1007 float compositeInnovationCovariance;
Ludwigfr 0:9f7ee7ed13e4 1008 switch(nbPassed){
Ludwigfr 0:9f7ee7ed13e4 1009 case 0://none
Ludwigfr 0:9f7ee7ed13e4 1010 compositeInnovationCovariance=1;
Ludwigfr 0:9f7ee7ed13e4 1011 break;
Ludwigfr 0:9f7ee7ed13e4 1012 case 1://left
Ludwigfr 0:9f7ee7ed13e4 1013 compositeInnovationCovariance = innovationConvarianceLeft;
Ludwigfr 0:9f7ee7ed13e4 1014
Ludwigfr 0:9f7ee7ed13e4 1015 break;
Ludwigfr 0:9f7ee7ed13e4 1016 case 10://front
Ludwigfr 0:9f7ee7ed13e4 1017 compositeInnovationCovariance = innovationConvarianceFront;
Ludwigfr 0:9f7ee7ed13e4 1018 break;
Ludwigfr 0:9f7ee7ed13e4 1019 case 11://left and front
Ludwigfr 0:9f7ee7ed13e4 1020 compositeInnovationCovariance=innovationConvarianceLeft*innovationConvarianceFront;
Ludwigfr 0:9f7ee7ed13e4 1021
Ludwigfr 0:9f7ee7ed13e4 1022 break;
Ludwigfr 0:9f7ee7ed13e4 1023 case 100://right
Ludwigfr 0:9f7ee7ed13e4 1024 compositeInnovationCovariance = innovationConvarianceRight;
Ludwigfr 0:9f7ee7ed13e4 1025 break;
Ludwigfr 0:9f7ee7ed13e4 1026 case 101://right and left
Ludwigfr 0:9f7ee7ed13e4 1027 compositeInnovationCovariance=innovationConvarianceLeft*innovationConvarianceRight;
Ludwigfr 0:9f7ee7ed13e4 1028 break;
Ludwigfr 0:9f7ee7ed13e4 1029 case 110://right and front
Ludwigfr 0:9f7ee7ed13e4 1030 compositeInnovationCovariance=innovationConvarianceFront*innovationConvarianceRight;
Ludwigfr 0:9f7ee7ed13e4 1031
Ludwigfr 0:9f7ee7ed13e4 1032
Ludwigfr 0:9f7ee7ed13e4 1033 break;
Ludwigfr 0:9f7ee7ed13e4 1034 case 111://right front and left
Ludwigfr 0:9f7ee7ed13e4 1035 compositeInnovationCovariance=innovationConvarianceLeft*innovationConvarianceFront*innovationConvarianceRight;
Ludwigfr 0:9f7ee7ed13e4 1036
Ludwigfr 0:9f7ee7ed13e4 1037 break;
Ludwigfr 0:9f7ee7ed13e4 1038 }
Ludwigfr 0:9f7ee7ed13e4 1039
Ludwigfr 0:9f7ee7ed13e4 1040 //compute kalman gain
Ludwigfr 0:9f7ee7ed13e4 1041 float kalmanGain[3][3];
Ludwigfr 0:9f7ee7ed13e4 1042 switch(nbPassed){
Ludwigfr 0:9f7ee7ed13e4 1043 //mult nbSonarPassed*3 , 3*3
Ludwigfr 0:9f7ee7ed13e4 1044 case 0://none
Ludwigfr 0:9f7ee7ed13e4 1045 break;
Ludwigfr 0:9f7ee7ed13e4 1046 case 1://left
Ludwigfr 0:9f7ee7ed13e4 1047
Ludwigfr 0:9f7ee7ed13e4 1048 for(i = 0; i < 3; ++i){//mult 3*3, 3*1
Ludwigfr 0:9f7ee7ed13e4 1049 for(j = 0; j < 1; ++j){
Ludwigfr 0:9f7ee7ed13e4 1050 for(k = 0; k < 3; ++k){
Ludwigfr 0:9f7ee7ed13e4 1051 kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k];
Ludwigfr 0:9f7ee7ed13e4 1052 }
Ludwigfr 0:9f7ee7ed13e4 1053 }
Ludwigfr 0:9f7ee7ed13e4 1054 }
Ludwigfr 0:9f7ee7ed13e4 1055
Ludwigfr 0:9f7ee7ed13e4 1056 break;
Ludwigfr 0:9f7ee7ed13e4 1057 case 10://front
Ludwigfr 0:9f7ee7ed13e4 1058 for(i = 0; i < 3; ++i){//mult 3*3, 3*1
Ludwigfr 0:9f7ee7ed13e4 1059 for(j = 0; j < 1; ++j){
Ludwigfr 0:9f7ee7ed13e4 1060 for(k = 0; k < 3; ++k){
Ludwigfr 0:9f7ee7ed13e4 1061 kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k];
Ludwigfr 0:9f7ee7ed13e4 1062 }
Ludwigfr 0:9f7ee7ed13e4 1063 }
Ludwigfr 0:9f7ee7ed13e4 1064 }
Ludwigfr 0:9f7ee7ed13e4 1065 break;
Ludwigfr 0:9f7ee7ed13e4 1066 case 11://left and front
Ludwigfr 0:9f7ee7ed13e4 1067 for(i = 0; i < 3; ++i){//mult 3*3, 3*2
Ludwigfr 0:9f7ee7ed13e4 1068 for(j = 0; j < 2; ++j){
Ludwigfr 0:9f7ee7ed13e4 1069 for(k = 0; k < 3; ++k){
Ludwigfr 0:9f7ee7ed13e4 1070 kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k+j*3];
Ludwigfr 0:9f7ee7ed13e4 1071 }
Ludwigfr 0:9f7ee7ed13e4 1072 }
Ludwigfr 0:9f7ee7ed13e4 1073 }
Ludwigfr 0:9f7ee7ed13e4 1074
Ludwigfr 0:9f7ee7ed13e4 1075 break;
Ludwigfr 0:9f7ee7ed13e4 1076 case 100://right
Ludwigfr 0:9f7ee7ed13e4 1077 for(i = 0; i < 3; ++i){//mult 3*3, 3*1
Ludwigfr 0:9f7ee7ed13e4 1078 for(j = 0; j < 1; ++j){
Ludwigfr 0:9f7ee7ed13e4 1079 for(k = 0; k < 3; ++k){
Ludwigfr 0:9f7ee7ed13e4 1080 kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k];
Ludwigfr 0:9f7ee7ed13e4 1081 }
Ludwigfr 0:9f7ee7ed13e4 1082 }
Ludwigfr 0:9f7ee7ed13e4 1083 }
Ludwigfr 0:9f7ee7ed13e4 1084 break;
Ludwigfr 0:9f7ee7ed13e4 1085 case 101://right and left
Ludwigfr 0:9f7ee7ed13e4 1086 for(i = 0; i < 3; ++i){//mult 3*3, 3*2
Ludwigfr 0:9f7ee7ed13e4 1087 for(j = 0; j < 2; ++j){
Ludwigfr 0:9f7ee7ed13e4 1088 for(k = 0; k < 3; ++k){
Ludwigfr 0:9f7ee7ed13e4 1089 kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k+j*3];
Ludwigfr 0:9f7ee7ed13e4 1090 }
Ludwigfr 0:9f7ee7ed13e4 1091 }
Ludwigfr 0:9f7ee7ed13e4 1092 }
Ludwigfr 0:9f7ee7ed13e4 1093
Ludwigfr 0:9f7ee7ed13e4 1094 break;
Ludwigfr 0:9f7ee7ed13e4 1095 case 110://right and front
Ludwigfr 0:9f7ee7ed13e4 1096 for(i = 0; i < 3; ++i){//mult 3*3, 3*2
Ludwigfr 0:9f7ee7ed13e4 1097 for(j = 0; j < 2; ++j){
Ludwigfr 0:9f7ee7ed13e4 1098 for(k = 0; k < 3; ++k){
Ludwigfr 0:9f7ee7ed13e4 1099 kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k+j*3];
Ludwigfr 0:9f7ee7ed13e4 1100 }
Ludwigfr 0:9f7ee7ed13e4 1101 }
Ludwigfr 0:9f7ee7ed13e4 1102 }
Ludwigfr 0:9f7ee7ed13e4 1103
Ludwigfr 0:9f7ee7ed13e4 1104
Ludwigfr 0:9f7ee7ed13e4 1105 break;
Ludwigfr 0:9f7ee7ed13e4 1106 case 111://right front and left
Ludwigfr 0:9f7ee7ed13e4 1107 for(i = 0; i < 3; ++i){//mult 3*3, 3*3
Ludwigfr 0:9f7ee7ed13e4 1108 for(j = 0; j < 3; ++j){
Ludwigfr 0:9f7ee7ed13e4 1109 for(k = 0; k < 3; ++k){
Ludwigfr 0:9f7ee7ed13e4 1110 kalmanGain[i][j] += newCovarianceOdometricPositionEstimate[i][k] * compositeInnovationCovariance[k+j*3];
Ludwigfr 0:9f7ee7ed13e4 1111 }
Ludwigfr 0:9f7ee7ed13e4 1112 }
Ludwigfr 0:9f7ee7ed13e4 1113 }
Ludwigfr 0:9f7ee7ed13e4 1114
Ludwigfr 0:9f7ee7ed13e4 1115 break;
Ludwigfr 0:9f7ee7ed13e4 1116 }
Ludwigfr 0:9f7ee7ed13e4 1117
Ludwigfr 0:9f7ee7ed13e4 1118 for(i = 0; i < 3; ++i){//mult 3*3, 3*3
Ludwigfr 0:9f7ee7ed13e4 1119 for(j = 0; j < 3; ++j){
Ludwigfr 0:9f7ee7ed13e4 1120 kalmanGain[i][j] = kalmanGain[i][j]/compositeInnovationCovariance;
Ludwigfr 0:9f7ee7ed13e4 1121 }
Ludwigfr 0:9f7ee7ed13e4 1122 }
Ludwigfr 0:9f7ee7ed13e4 1123 //compute kalman gain * composite innovation
Ludwigfr 0:9f7ee7ed13e4 1124 //mult 3*3 , 3*1
Ludwigfr 0:9f7ee7ed13e4 1125 float result3_1[3];
Ludwigfr 0:9f7ee7ed13e4 1126 for(i = 0; i < 3; ++i){//mult 3*3, 3*1
Ludwigfr 0:9f7ee7ed13e4 1127 for(k = 0; k < 3; ++k){
Ludwigfr 0:9f7ee7ed13e4 1128 result3_1[i] += kalmanGain[i][k] * compositeInnovation[k];
Ludwigfr 0:9f7ee7ed13e4 1129 }
Ludwigfr 0:9f7ee7ed13e4 1130 }
Ludwigfr 0:9f7ee7ed13e4 1131 //compute updated robot position estimate
Ludwigfr 0:9f7ee7ed13e4 1132 float xEstimatedKLast=xEstimatedKNext+result3_1[0];
Ludwigfr 0:9f7ee7ed13e4 1133 float yEstimatedKLast=yEstimatedKNext+result3_1[1];
Ludwigfr 0:9f7ee7ed13e4 1134 float thetaWorldEstimatedKLast=thetaWorldEstimatedKNext+result3_1[2];
Ludwigfr 0:9f7ee7ed13e4 1135 //store the resultant covariance for next estimation
Ludwigfr 0:9f7ee7ed13e4 1136
Ludwigfr 0:9f7ee7ed13e4 1137 float kalmanGainTranspose[3][3];
Ludwigfr 0:9f7ee7ed13e4 1138 kalmanGainTranspose[0][0]=kalmanGain[0][0];
Ludwigfr 0:9f7ee7ed13e4 1139 kalmanGainTranspose[0][1]=kalmanGain[1][0];
Ludwigfr 0:9f7ee7ed13e4 1140 kalmanGainTranspose[0][2]=kalmanGain[2][0];
Ludwigfr 0:9f7ee7ed13e4 1141 kalmanGainTranspose[1][0]=kalmanGain[0][1];
Ludwigfr 0:9f7ee7ed13e4 1142 kalmanGainTranspose[1][1]=kalmanGain[1][1];
Ludwigfr 0:9f7ee7ed13e4 1143 kalmanGainTranspose[1][2]=kalmanGain[2][1];
Ludwigfr 0:9f7ee7ed13e4 1144 kalmanGainTranspose[2][0]=kalmanGain[0][2];
Ludwigfr 0:9f7ee7ed13e4 1145 kalmanGainTranspose[2][1]=kalmanGain[1][2];
Ludwigfr 0:9f7ee7ed13e4 1146 kalmanGainTranspose[2][2]=kalmanGain[2][2];
Ludwigfr 0:9f7ee7ed13e4 1147
Ludwigfr 0:9f7ee7ed13e4 1148 //mult 3*3 , 3*3
Ludwigfr 0:9f7ee7ed13e4 1149 float fithTempMatrix3_3[3][3];
Ludwigfr 0:9f7ee7ed13e4 1150 for(i = 0; i < 3; ++i){//mult 3*3 , 3*3
Ludwigfr 0:9f7ee7ed13e4 1151 for(j = 0; j < 3; ++j){
Ludwigfr 0:9f7ee7ed13e4 1152 for(k = 0; k < 3; ++k){
Ludwigfr 0:9f7ee7ed13e4 1153 fithTempMatrix3_3[i][j] += kalmanGain[i][k] * kalmanGainTranspose[k][j];
Ludwigfr 0:9f7ee7ed13e4 1154 }
Ludwigfr 0:9f7ee7ed13e4 1155 }
Ludwigfr 0:9f7ee7ed13e4 1156 }
Ludwigfr 0:9f7ee7ed13e4 1157 for(i = 0; i < 3; ++i){//add 3*3 , 3*3
Ludwigfr 0:9f7ee7ed13e4 1158 for(j = 0; j < 3; ++j){
Ludwigfr 0:9f7ee7ed13e4 1159 fithTempMatrix3_3[i][j]=fithTempMatrix3_3[i][j]*compositeInnovationCovariance;
Ludwigfr 0:9f7ee7ed13e4 1160 }
Ludwigfr 0:9f7ee7ed13e4 1161 }
Ludwigfr 0:9f7ee7ed13e4 1162 float covariancePositionEsimatedLast[3][3];
Ludwigfr 0:9f7ee7ed13e4 1163 for(i = 0; i < 3; ++i){//add 3*3 , 3*3
Ludwigfr 0:9f7ee7ed13e4 1164 for(j = 0; j < 3; ++j){
Ludwigfr 0:9f7ee7ed13e4 1165 covariancePositionEsimatedLast[i][j]=fithTempMatrix3_3[i][j]+newCovarianceOdometricPositionEstimate[i][j];
Ludwigfr 0:9f7ee7ed13e4 1166 }
Ludwigfr 0:9f7ee7ed13e4 1167 }
Ludwigfr 0:9f7ee7ed13e4 1168 //update PreviousCovarianceOdometricPositionEstimate
Ludwigfr 0:9f7ee7ed13e4 1169 for(i = 0; i < 3; ++i){
Ludwigfr 0:9f7ee7ed13e4 1170 for(j = 0; j < 3; ++j){
Ludwigfr 0:9f7ee7ed13e4 1171 PreviousCovarianceOdometricPositionEstimate[i][j]=covariancePositionEsimatedLast[i][j];
Ludwigfr 0:9f7ee7ed13e4 1172 }
Ludwigfr 0:9f7ee7ed13e4 1173 }
Ludwigfr 0:9f7ee7ed13e4 1174
Ludwigfr 0:9f7ee7ed13e4 1175 xEstimatedK=xEstimatedKLast;
Ludwigfr 0:9f7ee7ed13e4 1176 yEstimatedK=yEstimatedKLast;
Ludwigfr 0:9f7ee7ed13e4 1177 thetaWorldEstimatedK=thetaWorldEstimatedKLast;
Ludwigfr 0:9f7ee7ed13e4 1178 }
Ludwigfr 0:9f7ee7ed13e4 1179 */