Yo Fleyt / Mbed 2 deprecated VirtualForces_with_class

with class

Dependencies:   ISR_Mini-explorer mbed

Fork of VirtualForces by Georgios Tsamis

MiniExplorerCoimbra.cpp@38:5ed7c79fb724, 2017-06-15 (annotated)

Committer:
Ludwigfr
Date:
Thu Jun 15 23:17:55 2017 +0000
Revision:
38:5ed7c79fb724
Parent:
37:b4c45e43ad29
Child:
39:890439b495e3
with some part of lab 4

Who changed what in which revision?

UserRevisionLine numberNew 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 35:68f9edbb3cff 543 */