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