Yo Fleyt / Mbed 2 deprecated VirtualForces_with_class

with class

Dependencies:   ISR_Mini-explorer mbed

Fork of VirtualForces by Georgios Tsamis

MiniExplorerCoimbra.cpp@35:68f9edbb3cff, 2017-06-11 (annotated)

Committer:
Ludwigfr
Date:
Sun Jun 11 22:40:37 2017 +0000
Revision:
35:68f9edbb3cff
Parent:
34:c208497dd079
Child:
37:b4c45e43ad29
change so every command to the robot is in world coordinate

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 35:68f9edbb3cff 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 35:68f9edbb3cff 173 //compute_probability_t(float distanceObstacleDetected, float x, float y, float[2] robotCoordinatesInWorld)
Ludwigfr 35:68f9edbb3cff 174 this->map.update_cell_value(i,j,this->sonarRight.compute_probability_t(rightMm,xWorldCell,yWorldCell,this->xWorld,this->yWorld,theta));
Ludwigfr 35:68f9edbb3cff 175 this->map.update_cell_value(i,j,this->sonarLeft.compute_probability_t(leftMm,xWorldCell,yWorldCell,this->xWorld,yWorldCell,theta));
Ludwigfr 35:68f9edbb3cff 176 this->map.update_cell_value(i,j,this->sonarFront.compute_probability_t(frontMm,xWorldCell,yWorldCell,this->xWorld,yWorldCell,theta));
Ludwigfr 35:68f9edbb3cff 177
Ludwigfr 35:68f9edbb3cff 178 }
Ludwigfr 35:68f9edbb3cff 179 }
Ludwigfr 35:68f9edbb3cff 180 }
Ludwigfr 35:68f9edbb3cff 181
Ludwigfr 35:68f9edbb3cff 182 //Distance computation function
Ludwigfr 35:68f9edbb3cff 183 float MiniExplorerCoimbra::dist(float x1, float y1, float x2, float y2){
Ludwigfr 35:68f9edbb3cff 184 return sqrt(pow(y2-y1,2) + pow(x2-x1,2));
Ludwigfr 35:68f9edbb3cff 185 }
Ludwigfr 35:68f9edbb3cff 186
Ludwigfr 35:68f9edbb3cff 187 //use virtual force field
Ludwigfr 33:814bcd7d3cfe 188 void MiniExplorerCoimbra::try_to_reach_target(float targetXWorld,float targetYWorld){
Ludwigfr 33:814bcd7d3cfe 189 //atan2 gives the angle beetween PI and -PI
Ludwigfr 35:68f9edbb3cff 190 this->myOdometria();
Ludwigfr 35:68f9edbb3cff 191 /*
Ludwigfr 35:68f9edbb3cff 192 float deplacementOnXWorld=targetXWorld-this->xWorld;
Ludwigfr 35:68f9edbb3cff 193 float deplacementOnYWorld=targetYWorld-this->yWorld;
Ludwigfr 35:68f9edbb3cff 194 */
Ludwigfr 35:68f9edbb3cff 195 float angleToTarget=atan2(targetXWorld-this->xWorld,targetYWorld-this->yWorld);
Ludwigfr 33:814bcd7d3cfe 196 turn_to_target(angleToTarget);
Ludwigfr 33:814bcd7d3cfe 197 bool reached=false;
Ludwigfr 33:814bcd7d3cfe 198 int print=0;
Ludwigfr 33:814bcd7d3cfe 199 while (!reached) {
Ludwigfr 33:814bcd7d3cfe 200 vff(&reached,targetXWorld,targetYWorld);
Ludwigfr 33:814bcd7d3cfe 201 //test_got_to_line(&reached);
Ludwigfr 33:814bcd7d3cfe 202 if(print==10){
Ludwigfr 33:814bcd7d3cfe 203 leftMotor(1,0);
Ludwigfr 33:814bcd7d3cfe 204 rightMotor(1,0);
Ludwigfr 35:68f9edbb3cff 205 this->print_final_map_with_robot_position_and_target(targetXWorld,targetYWorld);
Ludwigfr 33:814bcd7d3cfe 206 print=0;
Ludwigfr 33:814bcd7d3cfe 207 }else
Ludwigfr 33:814bcd7d3cfe 208 print+=1;
Ludwigfr 33:814bcd7d3cfe 209 }
Ludwigfr 33:814bcd7d3cfe 210 //Stop at the end
Ludwigfr 33:814bcd7d3cfe 211 leftMotor(1,0);
Ludwigfr 33:814bcd7d3cfe 212 rightMotor(1,0);
Ludwigfr 33:814bcd7d3cfe 213 pc.printf("\r\n target reached");
Ludwigfr 33:814bcd7d3cfe 214 wait(3);//
Ludwigfr 33:814bcd7d3cfe 215 }
Ludwigfr 33:814bcd7d3cfe 216
Ludwigfr 33:814bcd7d3cfe 217 void MiniExplorerCoimbra::vff(bool* reached, float targetXWorld, float targetYWorld){
Ludwigfr 33:814bcd7d3cfe 218 float line_a;
Ludwigfr 33:814bcd7d3cfe 219 float line_b;
Ludwigfr 33:814bcd7d3cfe 220 float line_c;
Ludwigfr 33:814bcd7d3cfe 221 //Updating X,Y and theta with the odometry values
Ludwigfr 35:68f9edbb3cff 222 float forceXWorld=0;
Ludwigfr 35:68f9edbb3cff 223 float forceYWorld=0;
Ludwigfr 33:814bcd7d3cfe 224 //we update the odometrie
Ludwigfr 35:68f9edbb3cff 225 this->myOdometria();
Ludwigfr 33:814bcd7d3cfe 226 //we check the sensors
Ludwigfr 33:814bcd7d3cfe 227 float leftMm = get_distance_left_sensor();
Ludwigfr 33:814bcd7d3cfe 228 float frontMm = get_distance_front_sensor();
Ludwigfr 33:814bcd7d3cfe 229 float rightMm = get_distance_right_sensor();
Ludwigfr 33:814bcd7d3cfe 230 //update the probabilities values
Ludwigfr 33:814bcd7d3cfe 231 this->update_sonar_values(leftMm, frontMm, rightMm);
Ludwigfr 33:814bcd7d3cfe 232 //we compute the force on X and Y
Ludwigfr 35:68f9edbb3cff 233 this->compute_forceX_and_forceY(&forceXWorld, &forceYWorld,targetXWorld,targetYWorld);
Ludwigfr 33:814bcd7d3cfe 234 //we compute a new ine
Ludwigfr 35:68f9edbb3cff 235 this->calculate_line(forceXWorld, forceYWorld, &line_a,&line_b,&line_c);
Ludwigfr 33:814bcd7d3cfe 236 //Updating motor velocities
Ludwigfr 33:814bcd7d3cfe 237 this->go_to_line(line_a,line_b,line_c,targetXWorld,targetYWorld);
Ludwigfr 33:814bcd7d3cfe 238
Ludwigfr 33:814bcd7d3cfe 239 //wait(0.1);
Ludwigfr 35:68f9edbb3cff 240 this->myOdometria();
Ludwigfr 35:68f9edbb3cff 241 if(dist(this->xWorld,this->yWorld,targetXWorld,targetYWorld)<10)
Ludwigfr 33:814bcd7d3cfe 242 *reached=true;
Ludwigfr 33:814bcd7d3cfe 243 }
Ludwigfr 33:814bcd7d3cfe 244
Ludwigfr 33:814bcd7d3cfe 245 //compute the force on X and Y
Ludwigfr 35:68f9edbb3cff 246 void MiniExplorerCoimbra::compute_forceX_and_forceY(float* forceXWorld, float* forceYWorld, float targetXWorld, float targetYWorld){
Ludwigfr 33:814bcd7d3cfe 247 float forceRepulsionComputedX=0;
Ludwigfr 33:814bcd7d3cfe 248 float forceRepulsionComputedY=0;
Ludwigfr 33:814bcd7d3cfe 249 //for each cell of the map we compute a force of repulsion
Ludwigfr 33:814bcd7d3cfe 250 for(int i=0;i<this->map.nbCellWidth;i++){
Ludwigfr 33:814bcd7d3cfe 251 for(int j=0;j<this->map.nbCellHeight;j++){
Ludwigfr 35:68f9edbb3cff 252 this->update_force(i,j,&forceRepulsionComputedX,&forceRepulsionComputedY);
Ludwigfr 33:814bcd7d3cfe 253 }
Ludwigfr 33:814bcd7d3cfe 254 }
Ludwigfr 33:814bcd7d3cfe 255 //update with attraction force
Ludwigfr 35:68f9edbb3cff 256 *forceXWorld=+forceRepulsionComputedX;
Ludwigfr 35:68f9edbb3cff 257 *forceYWorld=+forceRepulsionComputedY;
Ludwigfr 35:68f9edbb3cff 258 float distanceTargetRobot=sqrt(pow(targetXWorld-this->xWorld,2)+pow(targetYWorld-this->yWorld,2));
Ludwigfr 33:814bcd7d3cfe 259 if(distanceTargetRobot != 0){
Ludwigfr 35:68f9edbb3cff 260 *forceXWorld-=this->attractionConstantForce*(targetXWorld-this->xWorld)/distanceTargetRobot;
Ludwigfr 35:68f9edbb3cff 261 *forceYWorld-=this->attractionConstantForce*(targetYWorld-this->yWorld)/distanceTargetRobot;
Ludwigfr 33:814bcd7d3cfe 262 }
Ludwigfr 35:68f9edbb3cff 263 float amplitude=sqrt(pow(*forceXWorld,2)+pow(*forceYWorld,2));
Ludwigfr 33:814bcd7d3cfe 264 if(amplitude!=0){//avoid division by 0 if forceX and forceY == 0
Ludwigfr 35:68f9edbb3cff 265 *forceXWorld=*forceXWorld/amplitude;
Ludwigfr 35:68f9edbb3cff 266 *forceYWorld=*forceYWorld/amplitude;
Ludwigfr 33:814bcd7d3cfe 267 }
Ludwigfr 33:814bcd7d3cfe 268 }
Ludwigfr 33:814bcd7d3cfe 269
Ludwigfr 35:68f9edbb3cff 270 void MiniExplorerCoimbra::update_force(int widthIndice, int heightIndice, float* forceRepulsionComputedX, float* forceRepulsionComputedY ){
Ludwigfr 33:814bcd7d3cfe 271 //get the coordonate of the map and the robot in the ortonormal space
Ludwigfr 33:814bcd7d3cfe 272 float xWorldCell=this->map.cell_width_coordinate_to_world(widthIndice);
Ludwigfr 33:814bcd7d3cfe 273 float yWorldCell=this->map.cell_height_coordinate_to_world(heightIndice);
Ludwigfr 33:814bcd7d3cfe 274 //compute the distance beetween the cell and the robot
Ludwigfr 35:68f9edbb3cff 275 float distanceCellToRobot=sqrt(pow(xWorldCell-this->xWorld,2)+pow(yWorldCell-this->yWorld,2));
Ludwigfr 33:814bcd7d3cfe 276 //check if the cell is in range
Ludwigfr 33:814bcd7d3cfe 277 if(distanceCellToRobot <= this->rangeForce) {
Ludwigfr 33:814bcd7d3cfe 278 float probaCell=this->map.get_proba_cell(widthIndice,heightIndice);
Ludwigfr 35:68f9edbb3cff 279 *forceRepulsionComputedX+=this->repulsionConstantForce*probaCell*(xWorldCell-this->xWorld)/pow(distanceCellToRobot,3);
Ludwigfr 35:68f9edbb3cff 280 *forceRepulsionComputedY+=this->repulsionConstantForce*probaCell*(yWorldCell-this->yWorld)/pow(distanceCellToRobot,3);
Ludwigfr 33:814bcd7d3cfe 281 }
Ludwigfr 33:814bcd7d3cfe 282 }
Ludwigfr 33:814bcd7d3cfe 283
Ludwigfr 35:68f9edbb3cff 284 //robotX and robotY are from this->myOdometria(), calculate line_a, line_b and line_c
Ludwigfr 33:814bcd7d3cfe 285 void MiniExplorerCoimbra::calculate_line(float forceX, float forceY, float *line_a, float *line_b, float *line_c){
Ludwigfr 33:814bcd7d3cfe 286 /*
Ludwigfr 33:814bcd7d3cfe 287 in the teachers maths it is
Ludwigfr 33:814bcd7d3cfe 288
Ludwigfr 33:814bcd7d3cfe 289 *line_a=forceY;
Ludwigfr 33:814bcd7d3cfe 290 *line_b=-forceX;
Ludwigfr 33:814bcd7d3cfe 291
Ludwigfr 33:814bcd7d3cfe 292 because a*x+b*y+c=0
Ludwigfr 33:814bcd7d3cfe 293 a impact the vertical and b the horizontal
Ludwigfr 33:814bcd7d3cfe 294 and he has to put them like this because
Ludwigfr 35:68f9edbb3cff 295 Robot space: World space:
Ludwigfr 33:814bcd7d3cfe 296 ^ ^
Ludwigfr 33:814bcd7d3cfe 297 |x |y
Ludwigfr 33:814bcd7d3cfe 298 <- R O ->
Ludwigfr 33:814bcd7d3cfe 299 y x
Ludwigfr 33:814bcd7d3cfe 300 but since our forceX, forceY are already computed in the orthonormal space I m not sure we need to
Ludwigfr 33:814bcd7d3cfe 301 */
Ludwigfr 33:814bcd7d3cfe 302 *line_a=forceX;
Ludwigfr 33:814bcd7d3cfe 303 *line_b=forceY;
Ludwigfr 33:814bcd7d3cfe 304 //because the line computed always pass by the robot center we dont need lince_c
Ludwigfr 35:68f9edbb3cff 305 //*line_c=forceX*this->yWorld+forceY*this->xWorld;
Ludwigfr 33:814bcd7d3cfe 306 *line_c=0;
Ludwigfr 33:814bcd7d3cfe 307 }
Ludwigfr 33:814bcd7d3cfe 308
Ludwigfr 33:814bcd7d3cfe 309 //currently line_c is not used
Ludwigfr 33:814bcd7d3cfe 310 void MiniExplorerCoimbra::go_to_line(float line_a, float line_b, float line_c,float targetXWorld, float targetYWorld){
Ludwigfr 33:814bcd7d3cfe 311 float lineAngle;
Ludwigfr 33:814bcd7d3cfe 312 float angleError;
Ludwigfr 33:814bcd7d3cfe 313 float linear;
Ludwigfr 33:814bcd7d3cfe 314 float angular;
Ludwigfr 33:814bcd7d3cfe 315
Ludwigfr 35:68f9edbb3cff 316 //atan2 use the deplacement on X and the deplacement on Y
Ludwigfr 35:68f9edbb3cff 317 float angleToTarget=atan2(targetXWorld-this->xWorld,targetYWorld-this->yWorld);
Ludwigfr 35:68f9edbb3cff 318 bool aligned=false;
Ludwigfr 35:68f9edbb3cff 319
Ludwigfr 35:68f9edbb3cff 320 //this condition is passed if the target is in the same direction as the robot orientation
Ludwigfr 35:68f9edbb3cff 321 if((angleToTarget>0 && this->thetaWorld > 0) || (angleToTarget<0 && this->thetaWorld <0))
Ludwigfr 35:68f9edbb3cff 322 aligned=true;
Ludwigfr 33:814bcd7d3cfe 323
Ludwigfr 35:68f9edbb3cff 324 //line angle is beetween pi/2 and -pi/2
Ludwigfr 35:68f9edbb3cff 325 /*
Ludwigfr 35:68f9edbb3cff 326 ax+by+c=0 (here c==0)
Ludwigfr 35:68f9edbb3cff 327 y=x*-a/b
Ludwigfr 35:68f9edbb3cff 328 if a*b > 0, the robot is going down
Ludwigfr 35:68f9edbb3cff 329 if a*b <0, the robot is going up
Ludwigfr 35:68f9edbb3cff 330 */
Ludwigfr 33:814bcd7d3cfe 331 if(line_b!=0){
Ludwigfr 35:68f9edbb3cff 332 if(!aligned)
Ludwigfr 33:814bcd7d3cfe 333 lineAngle=atan(-line_a/line_b);
Ludwigfr 33:814bcd7d3cfe 334 else
Ludwigfr 33:814bcd7d3cfe 335 lineAngle=atan(line_a/-line_b);
Ludwigfr 33:814bcd7d3cfe 336 }
Ludwigfr 33:814bcd7d3cfe 337 else{
Ludwigfr 33:814bcd7d3cfe 338 lineAngle=1.5708;
Ludwigfr 33:814bcd7d3cfe 339 }
Ludwigfr 33:814bcd7d3cfe 340
Ludwigfr 33:814bcd7d3cfe 341 //Computing angle error
Ludwigfr 35:68f9edbb3cff 342 angleError = lineAngle-this->thetaWorld;//TODO that I m not sure
Ludwigfr 33:814bcd7d3cfe 343 angleError = atan(sin(angleError)/cos(angleError));
Ludwigfr 33:814bcd7d3cfe 344
Ludwigfr 33:814bcd7d3cfe 345 //Calculating velocities
Ludwigfr 33:814bcd7d3cfe 346 linear=this->kv*(3.1416);
Ludwigfr 33:814bcd7d3cfe 347 angular=this->kh*angleError;
Ludwigfr 33:814bcd7d3cfe 348
Ludwigfr 33:814bcd7d3cfe 349 float angularLeft=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
Ludwigfr 33:814bcd7d3cfe 350 float angularRight=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
Ludwigfr 33:814bcd7d3cfe 351
Ludwigfr 33:814bcd7d3cfe 352 //Normalize speed for motors
Ludwigfr 33:814bcd7d3cfe 353 if(abs(angularLeft)>abs(angularRight)) {
Ludwigfr 33:814bcd7d3cfe 354 angularRight=this->speed*abs(angularRight/angularLeft)*this->sign1(angularRight);
Ludwigfr 33:814bcd7d3cfe 355 angularLeft=this->speed*this->sign1(angularLeft);
Ludwigfr 33:814bcd7d3cfe 356 }
Ludwigfr 33:814bcd7d3cfe 357 else {
Ludwigfr 33:814bcd7d3cfe 358 angularLeft=this->speed*abs(angularLeft/angularRight)*this->sign1(angularLeft);
Ludwigfr 33:814bcd7d3cfe 359 angularRight=this->speed*this->sign1(angularRight);
Ludwigfr 33:814bcd7d3cfe 360 }
Ludwigfr 33:814bcd7d3cfe 361 leftMotor(this->sign2(angularLeft),abs(angularLeft));
Ludwigfr 33:814bcd7d3cfe 362 rightMotor(this->sign2(angularRight),abs(angularRight));
Ludwigfr 33:814bcd7d3cfe 363 }
Ludwigfr 33:814bcd7d3cfe 364
Ludwigfr 33:814bcd7d3cfe 365 /*angleToTarget is obtained through atan2 so it s:
Ludwigfr 33:814bcd7d3cfe 366 < 0 if the angle is bettween PI and 2pi on a trigo circle
Ludwigfr 33:814bcd7d3cfe 367 > 0 if it is between 0 and PI
Ludwigfr 33:814bcd7d3cfe 368 */
Ludwigfr 33:814bcd7d3cfe 369 void MiniExplorerCoimbra::turn_to_target(float angleToTarget){
Ludwigfr 35:68f9edbb3cff 370 this->myOdometria();
Ludwigfr 33:814bcd7d3cfe 371 float theta_plus_h_pi=theta+PI/2;//theta is between -PI and PI
Ludwigfr 33:814bcd7d3cfe 372 if(theta_plus_h_pi > PI)
Ludwigfr 33:814bcd7d3cfe 373 theta_plus_h_pi=-(2*PI-theta_plus_h_pi);
Ludwigfr 33:814bcd7d3cfe 374 if(angleToTarget>0){
Ludwigfr 33:814bcd7d3cfe 375 leftMotor(0,1);
Ludwigfr 33:814bcd7d3cfe 376 rightMotor(1,1);
Ludwigfr 33:814bcd7d3cfe 377 }else{
Ludwigfr 33:814bcd7d3cfe 378 leftMotor(1,1);
Ludwigfr 33:814bcd7d3cfe 379 rightMotor(0,1);
Ludwigfr 33:814bcd7d3cfe 380 }
Ludwigfr 33:814bcd7d3cfe 381 while(abs(angleToTarget-theta_plus_h_pi)>0.05){
Ludwigfr 35:68f9edbb3cff 382 this->myOdometria();
Ludwigfr 33:814bcd7d3cfe 383 theta_plus_h_pi=theta+PI/2;//theta is between -PI and PI
Ludwigfr 33:814bcd7d3cfe 384 if(theta_plus_h_pi > PI)
Ludwigfr 33:814bcd7d3cfe 385 theta_plus_h_pi=-(2*PI-theta_plus_h_pi);
Ludwigfr 33:814bcd7d3cfe 386 //pc.printf("\n\r diff=%f", abs(angleToTarget-theta_plus_h_pi));
Ludwigfr 33:814bcd7d3cfe 387 }
Ludwigfr 33:814bcd7d3cfe 388 leftMotor(1,0);
Ludwigfr 33:814bcd7d3cfe 389 rightMotor(1,0);
Ludwigfr 33:814bcd7d3cfe 390 }
Ludwigfr 33:814bcd7d3cfe 391
Ludwigfr 35:68f9edbb3cff 392 void MiniExplorerCoimbra::do_half_flip(){
Ludwigfr 35:68f9edbb3cff 393 this->myOdometria();
Ludwigfr 35:68f9edbb3cff 394 float theta_plus_h_pi=theta+PI/2;//theta is between -PI and PI
Ludwigfr 35:68f9edbb3cff 395 if(theta_plus_h_pi > PI)
Ludwigfr 35:68f9edbb3cff 396 theta_plus_h_pi=-(2*PI-theta_plus_h_pi);
Ludwigfr 35:68f9edbb3cff 397 leftMotor(0,100);
Ludwigfr 35:68f9edbb3cff 398 rightMotor(1,100);
Ludwigfr 35:68f9edbb3cff 399 while(abs(theta_plus_h_pi-theta)>0.05){
Ludwigfr 35:68f9edbb3cff 400 this->myOdometria();
Ludwigfr 35:68f9edbb3cff 401 // pc.printf("\n\r diff=%f", abs(theta_plus_pi-theta));
Ludwigfr 35:68f9edbb3cff 402 }
Ludwigfr 35:68f9edbb3cff 403 leftMotor(1,0);
Ludwigfr 35:68f9edbb3cff 404 rightMotor(1,0);
Ludwigfr 35:68f9edbb3cff 405 }
Ludwigfr 33:814bcd7d3cfe 406
Ludwigfr 35:68f9edbb3cff 407 void MiniExplorerCoimbra::print_final_map_with_robot_position() {
Ludwigfr 34:c208497dd079 408 float currProba;
Ludwigfr 34:c208497dd079 409
Ludwigfr 34:c208497dd079 410 float heightIndiceInOrthonormal;
Ludwigfr 34:c208497dd079 411 float widthIndiceInOrthonormal;
Ludwigfr 34:c208497dd079 412
Ludwigfr 34:c208497dd079 413 float widthMalus=-(3*this->map.sizeCellWidth/2);
Ludwigfr 34:c208497dd079 414 float widthBonus=this->map.sizeCellWidth/2;
Ludwigfr 34:c208497dd079 415
Ludwigfr 34:c208497dd079 416 float heightMalus=-(3*this->map.sizeCellHeight/2);
Ludwigfr 34:c208497dd079 417 float heightBonus=this->map.sizeCellHeight/2;
Ludwigfr 34:c208497dd079 418
Ludwigfr 34:c208497dd079 419 pc.printf("\n\r");
Ludwigfr 34:c208497dd079 420 for (int y = this->map.nbCellHeight -1; y>-1; y--) {
Ludwigfr 34:c208497dd079 421 for (int x= 0; x<this->map.nbCellWidth; x++) {
Ludwigfr 34:c208497dd079 422 heightIndiceInOrthonormal=this->map.cell_height_coordinate_to_world(y);
Ludwigfr 34:c208497dd079 423 widthIndiceInOrthonormal=this->map.cell_width_coordinate_to_world(x);
Ludwigfr 35:68f9edbb3cff 424 if(this->yWorld >= (heightIndiceInOrthonormal+heightMalus) && this->yWorld <= (heightIndiceInOrthonormal+heightBonus) && this->xWorld >= (widthIndiceInOrthonormal+widthMalus) && this->xWorld <= (widthIndiceInOrthonormal+widthBonus))
Ludwigfr 34:c208497dd079 425 pc.printf(" R ");
Ludwigfr 34:c208497dd079 426 else{
Ludwigfr 34:c208497dd079 427 currProba=this->map.log_to_proba(this->map.cellsLogValues[x][y]);
Ludwigfr 34:c208497dd079 428 if ( currProba < 0.5)
Ludwigfr 34:c208497dd079 429 pc.printf(" ");
Ludwigfr 34:c208497dd079 430 else{
Ludwigfr 34:c208497dd079 431 if(currProba==0.5)
Ludwigfr 34:c208497dd079 432 pc.printf(" . ");
Ludwigfr 34:c208497dd079 433 else
Ludwigfr 34:c208497dd079 434 pc.printf(" X ");
Ludwigfr 34:c208497dd079 435 }
Ludwigfr 34:c208497dd079 436 }
Ludwigfr 34:c208497dd079 437 }
Ludwigfr 34:c208497dd079 438 pc.printf("\n\r");
Ludwigfr 34:c208497dd079 439 }
Ludwigfr 34:c208497dd079 440 }
Ludwigfr 34:c208497dd079 441
Ludwigfr 35:68f9edbb3cff 442 void MiniExplorerCoimbra::print_final_map_with_robot_position_and_target(float targetXWorld, float targetYWorld) {
Ludwigfr 34:c208497dd079 443 float currProba;
Ludwigfr 34:c208497dd079 444
Ludwigfr 34:c208497dd079 445 float heightIndiceInOrthonormal;
Ludwigfr 34:c208497dd079 446 float widthIndiceInOrthonormal;
Ludwigfr 34:c208497dd079 447
Ludwigfr 34:c208497dd079 448 float widthMalus=-(3*this->map.sizeCellWidth/2);
Ludwigfr 34:c208497dd079 449 float widthBonus=this->map.sizeCellWidth/2;
Ludwigfr 34:c208497dd079 450
Ludwigfr 34:c208497dd079 451 float heightMalus=-(3*this->map.sizeCellHeight/2);
Ludwigfr 34:c208497dd079 452 float heightBonus=this->map.sizeCellHeight/2;
Ludwigfr 34:c208497dd079 453
Ludwigfr 34:c208497dd079 454 pc.printf("\n\r");
Ludwigfr 34:c208497dd079 455 for (int y = this->map.nbCellHeight -1; y>-1; y--) {
Ludwigfr 34:c208497dd079 456 for (int x= 0; x<this->map.nbCellWidth; x++) {
Ludwigfr 34:c208497dd079 457 heightIndiceInOrthonormal=this->map.cell_height_coordinate_to_world(y);
Ludwigfr 34:c208497dd079 458 widthIndiceInOrthonormal=this->map.cell_width_coordinate_to_world(x);
Ludwigfr 35:68f9edbb3cff 459 if(this->yWorld >= (heightIndiceInOrthonormal+heightMalus) && this->yWorld <= (heightIndiceInOrthonormal+heightBonus) && this->xWorld >= (widthIndiceInOrthonormal+widthMalus) && this->xWorld <= (widthIndiceInOrthonormal+widthBonus))
Ludwigfr 34:c208497dd079 460 pc.printf(" R ");
Ludwigfr 34:c208497dd079 461 else{
Ludwigfr 34:c208497dd079 462 if(targetYWorld >= (heightIndiceInOrthonormal+heightMalus) && targetYWorld <= (heightIndiceInOrthonormal+heightBonus) && targetXWorld >= (widthIndiceInOrthonormal+widthMalus) && targetXWorld <= (widthIndiceInOrthonormal+widthBonus))
Ludwigfr 34:c208497dd079 463 pc.printf(" T ");
Ludwigfr 34:c208497dd079 464 else{
Ludwigfr 34:c208497dd079 465 currProba=this->map.log_to_proba(this->map.cellsLogValues[x][y]);
Ludwigfr 34:c208497dd079 466 if ( currProba < 0.5)
Ludwigfr 34:c208497dd079 467 pc.printf(" ");
Ludwigfr 34:c208497dd079 468 else{
Ludwigfr 34:c208497dd079 469 if(currProba==0.5)
Ludwigfr 34:c208497dd079 470 pc.printf(" . ");
Ludwigfr 34:c208497dd079 471 else
Ludwigfr 34:c208497dd079 472 pc.printf(" X ");
Ludwigfr 34:c208497dd079 473 }
Ludwigfr 34:c208497dd079 474 }
Ludwigfr 34:c208497dd079 475 }
Ludwigfr 34:c208497dd079 476 }
Ludwigfr 34:c208497dd079 477 pc.printf("\n\r");
Ludwigfr 34:c208497dd079 478 }
Ludwigfr 34:c208497dd079 479 }
Ludwigfr 34:c208497dd079 480
Ludwigfr 34:c208497dd079 481 void MiniExplorerCoimbra::print_final_map() {
Ludwigfr 34:c208497dd079 482 float currProba;
Ludwigfr 34:c208497dd079 483 pc.printf("\n\r");
Ludwigfr 34:c208497dd079 484 for (int y = this->map.nbCellHeight -1; y>-1; y--) {
Ludwigfr 34:c208497dd079 485 for (int x= 0; x<this->map.nbCellWidth; x++) {
Ludwigfr 34:c208497dd079 486 currProba=this->map.log_to_proba(this->map.cellsLogValues[x][y]);
Ludwigfr 34:c208497dd079 487 if ( currProba < 0.5) {
Ludwigfr 34:c208497dd079 488 pc.printf(" ");
Ludwigfr 34:c208497dd079 489 } else {
Ludwigfr 34:c208497dd079 490 if(currProba==0.5)
Ludwigfr 34:c208497dd079 491 pc.printf(" . ");
Ludwigfr 34:c208497dd079 492 else
Ludwigfr 34:c208497dd079 493 pc.printf(" X ");
Ludwigfr 34:c208497dd079 494 }
Ludwigfr 34:c208497dd079 495 }
Ludwigfr 34:c208497dd079 496 pc.printf("\n\r");
Ludwigfr 34:c208497dd079 497 }
Ludwigfr 34:c208497dd079 498 }
Ludwigfr 35:68f9edbb3cff 499
Ludwigfr 35:68f9edbb3cff 500 //return 1 if positiv, -1 if negativ
Ludwigfr 35:68f9edbb3cff 501 float MiniExplorerCoimbra::sign1(float value){
Ludwigfr 35:68f9edbb3cff 502 if(value>=0)
Ludwigfr 35:68f9edbb3cff 503 return 1;
Ludwigfr 35:68f9edbb3cff 504 else
Ludwigfr 35:68f9edbb3cff 505 return -1;
Ludwigfr 35:68f9edbb3cff 506 }
Ludwigfr 35:68f9edbb3cff 507
Ludwigfr 35:68f9edbb3cff 508 //return 1 if positiv, 0 if negativ
Ludwigfr 35:68f9edbb3cff 509 int MiniExplorerCoimbra::sign2(float value){
Ludwigfr 35:68f9edbb3cff 510 if(value>=0)
Ludwigfr 35:68f9edbb3cff 511 return 1;
Ludwigfr 35:68f9edbb3cff 512 else
Ludwigfr 35:68f9edbb3cff 513 return 0;
Ludwigfr 35:68f9edbb3cff 514 }
Ludwigfr 35:68f9edbb3cff 515
Ludwigfr 35:68f9edbb3cff 516 /*UNUSED
Ludwigfr 35:68f9edbb3cff 517 float MiniExplorerCoimbra::get_converted_robot_X_into_world(){
Ludwigfr 35:68f9edbb3cff 518 //x world coordinate
Ludwigfr 35:68f9edbb3cff 519 return this->map.nbCellWidth*this->map.sizeCellWidth-Y;
Ludwigfr 35:68f9edbb3cff 520 }
Ludwigfr 35:68f9edbb3cff 521
Ludwigfr 35:68f9edbb3cff 522 float MiniExplorerCoimbra::get_converted_robot_Y_into_world(){
Ludwigfr 35:68f9edbb3cff 523 //y worldcoordinate
Ludwigfr 35:68f9edbb3cff 524 return X;
Ludwigfr 35:68f9edbb3cff 525 }
Ludwigfr 35:68f9edbb3cff 526
Ludwigfr 35:68f9edbb3cff 527
Ludwigfr 35:68f9edbb3cff 528 //x and y passed are TargetNotMap
Ludwigfr 35:68f9edbb3cff 529 float get_error_angles(float x, float y,float theta){
Ludwigfr 35:68f9edbb3cff 530 float angleBeetweenRobotAndTarget=atan2(y,x);
Ludwigfr 35:68f9edbb3cff 531 if(y > 0){
Ludwigfr 35:68f9edbb3cff 532 if(angleBeetweenRobotAndTarget < PI/2)//up right
Ludwigfr 35:68f9edbb3cff 533 angleBeetweenRobotAndTarget=-PI/2+angleBeetweenRobotAndTarget;
Ludwigfr 35:68f9edbb3cff 534 else//up right
Ludwigfr 35:68f9edbb3cff 535 angleBeetweenRobotAndTarget=angleBeetweenRobotAndTarget-PI/2;
Ludwigfr 35:68f9edbb3cff 536 }else{
Ludwigfr 35:68f9edbb3cff 537 if(angleBeetweenRobotAndTarget > -PI/2)//lower right
Ludwigfr 35:68f9edbb3cff 538 angleBeetweenRobotAndTarget=angleBeetweenRobotAndTarget-PI/2;
Ludwigfr 35:68f9edbb3cff 539 else//lower left
Ludwigfr 35:68f9edbb3cff 540 angleBeetweenRobotAndTarget=2*PI+angleBeetweenRobotAndTarget-PI/2;
Ludwigfr 35:68f9edbb3cff 541 }
Ludwigfr 35:68f9edbb3cff 542 return angleBeetweenRobotAndTarget-theta;
Ludwigfr 35:68f9edbb3cff 543 }
Ludwigfr 35:68f9edbb3cff 544 */