Yo Fleyt / Mbed 2 deprecated VirtualForces_with_class

with class

Dependencies:   ISR_Mini-explorer mbed

Fork of VirtualForces by Georgios Tsamis

MiniExplorerCoimbra.cpp@33:814bcd7d3cfe, 2017-06-09 (annotated)

Committer:
Ludwigfr
Date:
Fri Jun 09 00:28:32 2017 +0000
Revision:
33:814bcd7d3cfe
Child:
34:c208497dd079
version with class

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Ludwigfr 33:814bcd7d3cfe 1 #include "MiniExplorerCoimbra.hpp"
Ludwigfr 33:814bcd7d3cfe 2
Ludwigfr 33:814bcd7d3cfe 3 #define PI 3.14159
Ludwigfr 33:814bcd7d3cfe 4
Ludwigfr 33:814bcd7d3cfe 5 MiniExplorerCoimbra:: MiniExplorerCoimbra(float defaultX, float defaultY, float defaultTheta):map(200,200,20,20),sonarLeft(10*PI/36,-4,4),sonarFront(0,0,5),sonarRight(-10*PI/36,4,4){
Ludwigfr 33:814bcd7d3cfe 6 i2c1.frequency(100000);
Ludwigfr 33:814bcd7d3cfe 7 initRobot(); //Initializing the robot
Ludwigfr 33:814bcd7d3cfe 8 pc.baud(9600); // baud for the pc communication
Ludwigfr 33:814bcd7d3cfe 9
Ludwigfr 33:814bcd7d3cfe 10 measure_always_on();//TODO check if needed
Ludwigfr 33:814bcd7d3cfe 11
Ludwigfr 33:814bcd7d3cfe 12 this->setXYTheta(defaultX,defaultY,defaultTheta);
Ludwigfr 33:814bcd7d3cfe 13 this->radiusWheels=3.25;
Ludwigfr 33:814bcd7d3cfe 14 this->distanceWheels=7.2;
Ludwigfr 33:814bcd7d3cfe 15
Ludwigfr 33:814bcd7d3cfe 16 this->khro=12;
Ludwigfr 33:814bcd7d3cfe 17 this->ka=30;
Ludwigfr 33:814bcd7d3cfe 18 this->kb=-13;
Ludwigfr 33:814bcd7d3cfe 19 this->kv=200;
Ludwigfr 33:814bcd7d3cfe 20 this->kh=200;
Ludwigfr 33:814bcd7d3cfe 21
Ludwigfr 33:814bcd7d3cfe 22 this->rangeForce=30;
Ludwigfr 33:814bcd7d3cfe 23 this->attractionConstantForce=10000;
Ludwigfr 33:814bcd7d3cfe 24 this->repulsionConstantForce=1;
Ludwigfr 33:814bcd7d3cfe 25 }
Ludwigfr 33:814bcd7d3cfe 26
Ludwigfr 33:814bcd7d3cfe 27 void MiniExplorerCoimbra::setXYTheta(float x, float y, float theta){
Ludwigfr 33:814bcd7d3cfe 28 X=x;
Ludwigfr 33:814bcd7d3cfe 29 Y=y;
Ludwigfr 33:814bcd7d3cfe 30 theta=theta;
Ludwigfr 33:814bcd7d3cfe 31 }
Ludwigfr 33:814bcd7d3cfe 32
Ludwigfr 33:814bcd7d3cfe 33 //generate a position randomly and makes the robot go there while updating the map
Ludwigfr 33:814bcd7d3cfe 34 void MiniExplorerCoimbra::randomize_and_map() {
Ludwigfr 33:814bcd7d3cfe 35 //TODO check that it's aurelien's work
Ludwigfr 33:814bcd7d3cfe 36 float movementOnX=rand()%(int)(this->map.widthRealMap/2);
Ludwigfr 33:814bcd7d3cfe 37 float movementOnY=rand()%(int)(this->map.heightRealMap/2);
Ludwigfr 33:814bcd7d3cfe 38
Ludwigfr 33:814bcd7d3cfe 39 float signOfX=rand()%2;
Ludwigfr 33:814bcd7d3cfe 40 if(signOfX < 1)
Ludwigfr 33:814bcd7d3cfe 41 signOfX=-1;
Ludwigfr 33:814bcd7d3cfe 42 float signOfY=rand()%2;
Ludwigfr 33:814bcd7d3cfe 43 if(signOfY < 1)
Ludwigfr 33:814bcd7d3cfe 44 signOfY=-1;
Ludwigfr 33:814bcd7d3cfe 45
Ludwigfr 33:814bcd7d3cfe 46 float targetX = movementOnX*signOfX;
Ludwigfr 33:814bcd7d3cfe 47 float targetY = movementOnY*signOfY;
Ludwigfr 33:814bcd7d3cfe 48 float targetAngle = 2*((float)(rand()%31416)-15708)/10000.0;
Ludwigfr 33:814bcd7d3cfe 49 this->go_to_point_with_angle(targetX, targetY, targetAngle);
Ludwigfr 33:814bcd7d3cfe 50 }
Ludwigfr 33:814bcd7d3cfe 51
Ludwigfr 33:814bcd7d3cfe 52 //go the the given position while updating the map
Ludwigfr 33:814bcd7d3cfe 53 void MiniExplorerCoimbra::go_to_point_with_angle(float targetX, float targetY, float targetAngle) {
Ludwigfr 33:814bcd7d3cfe 54 bool keep_going=true;
Ludwigfr 33:814bcd7d3cfe 55 float leftMm;
Ludwigfr 33:814bcd7d3cfe 56 float frontMm;
Ludwigfr 33:814bcd7d3cfe 57 float rightMm;
Ludwigfr 33:814bcd7d3cfe 58 float dt;
Ludwigfr 33:814bcd7d3cfe 59 Timer t;
Ludwigfr 33:814bcd7d3cfe 60 float d2;
Ludwigfr 33:814bcd7d3cfe 61 do {
Ludwigfr 33:814bcd7d3cfe 62 //Timer stuff
Ludwigfr 33:814bcd7d3cfe 63 dt = t.read();
Ludwigfr 33:814bcd7d3cfe 64 t.reset();
Ludwigfr 33:814bcd7d3cfe 65 t.start();
Ludwigfr 33:814bcd7d3cfe 66
Ludwigfr 33:814bcd7d3cfe 67 //Updating X,Y and theta with the odometry values
Ludwigfr 33:814bcd7d3cfe 68 Odometria();
Ludwigfr 33:814bcd7d3cfe 69 leftMm = get_distance_left_sensor();
Ludwigfr 33:814bcd7d3cfe 70 frontMm = get_distance_front_sensor();
Ludwigfr 33:814bcd7d3cfe 71 rightMm = get_distance_right_sensor();
Ludwigfr 33:814bcd7d3cfe 72
Ludwigfr 33:814bcd7d3cfe 73 //if in dangerzone
Ludwigfr 33:814bcd7d3cfe 74 if((frontMm < 150 && frontMm > 0)|| (leftMm <150 && leftMm > 0) || (rightMm <150 && rightMm > 0) ){
Ludwigfr 33:814bcd7d3cfe 75 leftMotor(1,0);
Ludwigfr 33:814bcd7d3cfe 76 rightMotor(1,0);
Ludwigfr 33:814bcd7d3cfe 77 this->update_sonar_values(leftMm, frontMm, rightMm);
Ludwigfr 33:814bcd7d3cfe 78 //TODO Giorgos maybe you can also test the do_half_flip() function
Ludwigfr 33:814bcd7d3cfe 79 Odometria();
Ludwigfr 33:814bcd7d3cfe 80 //do a flip TODO
Ludwigfr 33:814bcd7d3cfe 81 keep_going=false;
Ludwigfr 33:814bcd7d3cfe 82 this->do_half_flip();
Ludwigfr 33:814bcd7d3cfe 83 }else{
Ludwigfr 33:814bcd7d3cfe 84 //if not in danger zone continue as usual
Ludwigfr 33:814bcd7d3cfe 85 this->update_sonar_values(leftMm, frontMm, rightMm);
Ludwigfr 33:814bcd7d3cfe 86
Ludwigfr 33:814bcd7d3cfe 87 //Updating motor velocities
Ludwigfr 33:814bcd7d3cfe 88 d2=this->update_angular_speed_wheels_go_to_point_with_angle(targetX,targetY,targetAngle,dt);
Ludwigfr 33:814bcd7d3cfe 89
Ludwigfr 33:814bcd7d3cfe 90 wait(0.2);
Ludwigfr 33:814bcd7d3cfe 91 //Timer stuff
Ludwigfr 33:814bcd7d3cfe 92 t.stop();
Ludwigfr 33:814bcd7d3cfe 93 pc.printf("\n\r dist to target= %f",d2);
Ludwigfr 33:814bcd7d3cfe 94 }
Ludwigfr 33:814bcd7d3cfe 95 } while(d2>1 && (abs(targetAngle-theta)>0.01) && keep_going);
Ludwigfr 33:814bcd7d3cfe 96
Ludwigfr 33:814bcd7d3cfe 97 //Stop at the end
Ludwigfr 33:814bcd7d3cfe 98 leftMotor(1,0);
Ludwigfr 33:814bcd7d3cfe 99 rightMotor(1,0);
Ludwigfr 33:814bcd7d3cfe 100 }
Ludwigfr 33:814bcd7d3cfe 101
Ludwigfr 33:814bcd7d3cfe 102 void MiniExplorerCoimbra::update_sonar_values(float leftMm,float frontMm,float rightMm){
Ludwigfr 33:814bcd7d3cfe 103 float xWorldCell;
Ludwigfr 33:814bcd7d3cfe 104 float yWorldCell;
Ludwigfr 33:814bcd7d3cfe 105 for(int i=0;i<this->map.nbCellWidth;i++){
Ludwigfr 33:814bcd7d3cfe 106 for(int j=0;j<this->map.nbCellHeight;j++){
Ludwigfr 33:814bcd7d3cfe 107 xWorldCell=this->map.cell_width_coordinate_to_world(i);
Ludwigfr 33:814bcd7d3cfe 108 yWorldCell=this->map.cell_height_coordinate_to_world(j);
Ludwigfr 33:814bcd7d3cfe 109 //compute_probability_t(float distanceObstacleDetected, float x, float y, float[2] robotCoordinatesInWorld)
Ludwigfr 33:814bcd7d3cfe 110 this->map.update_cell_value(i,j,this->sonarRight.compute_probability_t(rightMm,xWorldCell,yWorldCell,this->get_converted_robot_X_into_world(),this->get_converted_robot_Y_into_world()));
Ludwigfr 33:814bcd7d3cfe 111 this->map.update_cell_value(i,j,this->sonarLeft.compute_probability_t(leftMm,xWorldCell,yWorldCell,this->get_converted_robot_X_into_world(),this->get_converted_robot_Y_into_world()));
Ludwigfr 33:814bcd7d3cfe 112 this->map.update_cell_value(i,j,this->sonarFront.compute_probability_t(frontMm,xWorldCell,yWorldCell,this->get_converted_robot_X_into_world(),this->get_converted_robot_Y_into_world()));
Ludwigfr 33:814bcd7d3cfe 113
Ludwigfr 33:814bcd7d3cfe 114 }
Ludwigfr 33:814bcd7d3cfe 115 }
Ludwigfr 33:814bcd7d3cfe 116 }
Ludwigfr 33:814bcd7d3cfe 117
Ludwigfr 33:814bcd7d3cfe 118 float MiniExplorerCoimbra::get_converted_robot_X_into_world(){
Ludwigfr 33:814bcd7d3cfe 119 //x world coordinate
Ludwigfr 33:814bcd7d3cfe 120 return this->map.nbCellWidth*this->map.sizeCellWidth-Y;
Ludwigfr 33:814bcd7d3cfe 121 }
Ludwigfr 33:814bcd7d3cfe 122
Ludwigfr 33:814bcd7d3cfe 123 float MiniExplorerCoimbra::get_converted_robot_Y_into_world(){
Ludwigfr 33:814bcd7d3cfe 124 //y worldcoordinate
Ludwigfr 33:814bcd7d3cfe 125 return X;
Ludwigfr 33:814bcd7d3cfe 126 }
Ludwigfr 33:814bcd7d3cfe 127
Ludwigfr 33:814bcd7d3cfe 128 void MiniExplorerCoimbra::do_half_flip(){
Ludwigfr 33:814bcd7d3cfe 129 Odometria();
Ludwigfr 33:814bcd7d3cfe 130 float theta_plus_h_pi=theta+PI/2;//theta is between -PI and PI
Ludwigfr 33:814bcd7d3cfe 131 if(theta_plus_h_pi > PI)
Ludwigfr 33:814bcd7d3cfe 132 theta_plus_h_pi=-(2*PI-theta_plus_h_pi);
Ludwigfr 33:814bcd7d3cfe 133 leftMotor(0,100);
Ludwigfr 33:814bcd7d3cfe 134 rightMotor(1,100);
Ludwigfr 33:814bcd7d3cfe 135 while(abs(theta_plus_h_pi-theta)>0.05){
Ludwigfr 33:814bcd7d3cfe 136 Odometria();
Ludwigfr 33:814bcd7d3cfe 137 // pc.printf("\n\r diff=%f", abs(theta_plus_pi-theta));
Ludwigfr 33:814bcd7d3cfe 138 }
Ludwigfr 33:814bcd7d3cfe 139 leftMotor(1,0);
Ludwigfr 33:814bcd7d3cfe 140 rightMotor(1,0);
Ludwigfr 33:814bcd7d3cfe 141 }
Ludwigfr 33:814bcd7d3cfe 142
Ludwigfr 33:814bcd7d3cfe 143 //Distance computation function
Ludwigfr 33:814bcd7d3cfe 144 float MiniExplorerCoimbra::dist(float robotX, float robotY, float targetX, float targetY){
Ludwigfr 33:814bcd7d3cfe 145 //pc.printf("%f, %f, %f, %f",robotX,robotY,targetX,targetY);
Ludwigfr 33:814bcd7d3cfe 146 return sqrt(pow(targetY-robotY,2) + pow(targetX-robotX,2));
Ludwigfr 33:814bcd7d3cfe 147 }
Ludwigfr 33:814bcd7d3cfe 148
Ludwigfr 33:814bcd7d3cfe 149 float MiniExplorerCoimbra::update_angular_speed_wheels_go_to_point_with_angle(float targetX, float targetY, float targetAngle, float dt){
Ludwigfr 33:814bcd7d3cfe 150 //compute_angles_and_distance
Ludwigfr 33:814bcd7d3cfe 151 float alpha = atan2((targetY-Y),(targetX-X))-theta;
Ludwigfr 33:814bcd7d3cfe 152 alpha = atan(sin(alpha)/cos(alpha));
Ludwigfr 33:814bcd7d3cfe 153 float rho = this->dist(X, Y, targetX, targetY);
Ludwigfr 33:814bcd7d3cfe 154 float d2 = rho;
Ludwigfr 33:814bcd7d3cfe 155 float beta = -alpha-theta+targetAngle;
Ludwigfr 33:814bcd7d3cfe 156
Ludwigfr 33:814bcd7d3cfe 157 //Computing angle error and distance towards the target value
Ludwigfr 33:814bcd7d3cfe 158 rho += dt*(-this->khro*cos(alpha)*rho);
Ludwigfr 33:814bcd7d3cfe 159 float temp = alpha;
Ludwigfr 33:814bcd7d3cfe 160 alpha += dt*(this->khro*sin(alpha)-this->ka*alpha-this->kb*beta);
Ludwigfr 33:814bcd7d3cfe 161 beta += dt*(-this->khro*sin(temp));
Ludwigfr 33:814bcd7d3cfe 162
Ludwigfr 33:814bcd7d3cfe 163 //Computing linear and angular velocities
Ludwigfr 33:814bcd7d3cfe 164 float linear;
Ludwigfr 33:814bcd7d3cfe 165 float angular;
Ludwigfr 33:814bcd7d3cfe 166 if(alpha>=-1.5708 && alpha<=1.5708){
Ludwigfr 33:814bcd7d3cfe 167 linear=this->khro*rho;
Ludwigfr 33:814bcd7d3cfe 168 angular=this->ka*alpha+this->kb*beta;
Ludwigfr 33:814bcd7d3cfe 169 }
Ludwigfr 33:814bcd7d3cfe 170 else{
Ludwigfr 33:814bcd7d3cfe 171 linear=-this->khro*rho;
Ludwigfr 33:814bcd7d3cfe 172 angular=-this->ka*alpha-this->kb*beta;
Ludwigfr 33:814bcd7d3cfe 173 }
Ludwigfr 33:814bcd7d3cfe 174 float angular_left=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
Ludwigfr 33:814bcd7d3cfe 175 float angular_right=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
Ludwigfr 33:814bcd7d3cfe 176
Ludwigfr 33:814bcd7d3cfe 177 //Normalize speed for motors
Ludwigfr 33:814bcd7d3cfe 178 if(angular_left>angular_right) {
Ludwigfr 33:814bcd7d3cfe 179 angular_right=this->speed*angular_right/angular_left;
Ludwigfr 33:814bcd7d3cfe 180 angular_left=this->speed;
Ludwigfr 33:814bcd7d3cfe 181 } else {
Ludwigfr 33:814bcd7d3cfe 182 angular_left=this->speed*angular_left/angular_right;
Ludwigfr 33:814bcd7d3cfe 183 angular_right=this->speed;
Ludwigfr 33:814bcd7d3cfe 184 }
Ludwigfr 33:814bcd7d3cfe 185
Ludwigfr 33:814bcd7d3cfe 186 //compute_linear_angular_velocities
Ludwigfr 33:814bcd7d3cfe 187 leftMotor(1,angular_left);
Ludwigfr 33:814bcd7d3cfe 188 rightMotor(1,angular_right);
Ludwigfr 33:814bcd7d3cfe 189
Ludwigfr 33:814bcd7d3cfe 190 return d2;
Ludwigfr 33:814bcd7d3cfe 191 }
Ludwigfr 33:814bcd7d3cfe 192
Ludwigfr 33:814bcd7d3cfe 193 void MiniExplorerCoimbra::try_to_reach_target(float targetXWorld,float targetYWorld){
Ludwigfr 33:814bcd7d3cfe 194 //atan2 gives the angle beetween PI and -PI
Ludwigfr 33:814bcd7d3cfe 195 float angleToTarget=atan2(targetXWorld,targetYWorld);
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 33:814bcd7d3cfe 205 /*
Ludwigfr 33:814bcd7d3cfe 206 pc.printf("\r\n theta=%f", theta);
Ludwigfr 33:814bcd7d3cfe 207 pc.printf("\r\n IN ORTHO:");
Ludwigfr 33:814bcd7d3cfe 208 pc.printf("\r\n X Robot=%f", this->get_converted_robot_X_into_world());
Ludwigfr 33:814bcd7d3cfe 209 pc.printf("\r\n Y Robot=%f", this->get_converted_robot_Y_into_world());
Ludwigfr 33:814bcd7d3cfe 210 pc.printf("\r\n X Target=%f", targetXWorld);
Ludwigfr 33:814bcd7d3cfe 211 pc.printf("\r\n Y Target=%f", targetYWorld);
Ludwigfr 33:814bcd7d3cfe 212 */
Ludwigfr 33:814bcd7d3cfe 213 print_final_map_with_robot_position_and_target();
Ludwigfr 33:814bcd7d3cfe 214 print=0;
Ludwigfr 33:814bcd7d3cfe 215 }else
Ludwigfr 33:814bcd7d3cfe 216 print+=1;
Ludwigfr 33:814bcd7d3cfe 217 }
Ludwigfr 33:814bcd7d3cfe 218 //Stop at the end
Ludwigfr 33:814bcd7d3cfe 219 leftMotor(1,0);
Ludwigfr 33:814bcd7d3cfe 220 rightMotor(1,0);
Ludwigfr 33:814bcd7d3cfe 221 pc.printf("\r\n target reached");
Ludwigfr 33:814bcd7d3cfe 222 wait(3);//
Ludwigfr 33:814bcd7d3cfe 223 }
Ludwigfr 33:814bcd7d3cfe 224
Ludwigfr 33:814bcd7d3cfe 225 void MiniExplorerCoimbra::vff(bool* reached, float targetXWorld, float targetYWorld){
Ludwigfr 33:814bcd7d3cfe 226 float line_a;
Ludwigfr 33:814bcd7d3cfe 227 float line_b;
Ludwigfr 33:814bcd7d3cfe 228 float line_c;
Ludwigfr 33:814bcd7d3cfe 229 //Updating X,Y and theta with the odometry values
Ludwigfr 33:814bcd7d3cfe 230 float forceX=0;
Ludwigfr 33:814bcd7d3cfe 231 float forceY=0;
Ludwigfr 33:814bcd7d3cfe 232 //we update the odometrie
Ludwigfr 33:814bcd7d3cfe 233 Odometria();
Ludwigfr 33:814bcd7d3cfe 234 //we check the sensors
Ludwigfr 33:814bcd7d3cfe 235 float leftMm = get_distance_left_sensor();
Ludwigfr 33:814bcd7d3cfe 236 float frontMm = get_distance_front_sensor();
Ludwigfr 33:814bcd7d3cfe 237 float rightMm = get_distance_right_sensor();
Ludwigfr 33:814bcd7d3cfe 238 //update the probabilities values
Ludwigfr 33:814bcd7d3cfe 239 this->update_sonar_values(leftMm, frontMm, rightMm);
Ludwigfr 33:814bcd7d3cfe 240 //we compute the force on X and Y
Ludwigfr 33:814bcd7d3cfe 241 this->compute_forceX_and_forceY(&forceX, &forceY,targetXWorld,targetYWorld);
Ludwigfr 33:814bcd7d3cfe 242 //we compute a new ine
Ludwigfr 33:814bcd7d3cfe 243 this->calculate_line(forceX, forceY, &line_a,&line_b,&line_c);
Ludwigfr 33:814bcd7d3cfe 244 //Updating motor velocities
Ludwigfr 33:814bcd7d3cfe 245 this->go_to_line(line_a,line_b,line_c,targetXWorld,targetYWorld);
Ludwigfr 33:814bcd7d3cfe 246
Ludwigfr 33:814bcd7d3cfe 247 //wait(0.1);
Ludwigfr 33:814bcd7d3cfe 248 Odometria();
Ludwigfr 33:814bcd7d3cfe 249 if(dist(this->get_converted_robot_X_into_world(),this->get_converted_robot_Y_into_world(),targetXWorld,targetYWorld)<10)
Ludwigfr 33:814bcd7d3cfe 250 *reached=true;
Ludwigfr 33:814bcd7d3cfe 251 }
Ludwigfr 33:814bcd7d3cfe 252
Ludwigfr 33:814bcd7d3cfe 253 //compute the force on X and Y
Ludwigfr 33:814bcd7d3cfe 254 void MiniExplorerCoimbra::compute_forceX_and_forceY(float* forceX, float* forceY, float targetXWorld, float targetYWorld){
Ludwigfr 33:814bcd7d3cfe 255 float xRobotWorld=this->get_converted_robot_X_into_world();
Ludwigfr 33:814bcd7d3cfe 256 float yRobotWorld=this->get_converted_robot_Y_into_world();
Ludwigfr 33:814bcd7d3cfe 257
Ludwigfr 33:814bcd7d3cfe 258 float forceRepulsionComputedX=0;
Ludwigfr 33:814bcd7d3cfe 259 float forceRepulsionComputedY=0;
Ludwigfr 33:814bcd7d3cfe 260 //for each cell of the map we compute a force of repulsion
Ludwigfr 33:814bcd7d3cfe 261 for(int i=0;i<this->map.nbCellWidth;i++){
Ludwigfr 33:814bcd7d3cfe 262 for(int j=0;j<this->map.nbCellHeight;j++){
Ludwigfr 33:814bcd7d3cfe 263 this->update_force(i,j,&forceRepulsionComputedX,&forceRepulsionComputedY,xRobotWorld,yRobotWorld);
Ludwigfr 33:814bcd7d3cfe 264 }
Ludwigfr 33:814bcd7d3cfe 265 }
Ludwigfr 33:814bcd7d3cfe 266 //update with attraction force
Ludwigfr 33:814bcd7d3cfe 267 *forceX=+forceRepulsionComputedX;
Ludwigfr 33:814bcd7d3cfe 268 *forceY=+forceRepulsionComputedY;
Ludwigfr 33:814bcd7d3cfe 269 float distanceTargetRobot=sqrt(pow(targetXWorld-xRobotWorld,2)+pow(targetYWorld-yRobotWorld,2));
Ludwigfr 33:814bcd7d3cfe 270 if(distanceTargetRobot != 0){
Ludwigfr 33:814bcd7d3cfe 271 *forceX-=this->attractionConstantForce*(targetXWorld-xRobotWorld)/distanceTargetRobot;
Ludwigfr 33:814bcd7d3cfe 272 *forceY-=this->attractionConstantForce*(targetYWorld-yRobotWorld)/distanceTargetRobot;
Ludwigfr 33:814bcd7d3cfe 273 }
Ludwigfr 33:814bcd7d3cfe 274 float amplitude=sqrt(pow(*forceX,2)+pow(*forceY,2));
Ludwigfr 33:814bcd7d3cfe 275 if(amplitude!=0){//avoid division by 0 if forceX and forceY == 0
Ludwigfr 33:814bcd7d3cfe 276 *forceX=*forceX/amplitude;
Ludwigfr 33:814bcd7d3cfe 277 *forceY=*forceY/amplitude;
Ludwigfr 33:814bcd7d3cfe 278 }
Ludwigfr 33:814bcd7d3cfe 279 }
Ludwigfr 33:814bcd7d3cfe 280
Ludwigfr 33:814bcd7d3cfe 281 void MiniExplorerCoimbra::update_force(int widthIndice, int heightIndice, float* forceX, float* forceY, float xRobotWorld, float yRobotWorld ){
Ludwigfr 33:814bcd7d3cfe 282 //get the coordonate of the map and the robot in the ortonormal space
Ludwigfr 33:814bcd7d3cfe 283 float xWorldCell=this->map.cell_width_coordinate_to_world(widthIndice);
Ludwigfr 33:814bcd7d3cfe 284 float yWorldCell=this->map.cell_height_coordinate_to_world(heightIndice);
Ludwigfr 33:814bcd7d3cfe 285
Ludwigfr 33:814bcd7d3cfe 286 //compute the distance beetween the cell and the robot
Ludwigfr 33:814bcd7d3cfe 287 float distanceCellToRobot=sqrt(pow(xWorldCell-xRobotWorld,2)+pow(yWorldCell-yRobotWorld,2));
Ludwigfr 33:814bcd7d3cfe 288 //check if the cell is in range
Ludwigfr 33:814bcd7d3cfe 289 if(distanceCellToRobot <= this->rangeForce) {
Ludwigfr 33:814bcd7d3cfe 290 float probaCell=this->map.get_proba_cell(widthIndice,heightIndice);
Ludwigfr 33:814bcd7d3cfe 291 float xForceComputed=this->repulsionConstantForce*probaCell*(xWorldCell-xRobotWorld)/pow(distanceCellToRobot,3);
Ludwigfr 33:814bcd7d3cfe 292 float yForceComputed=this->repulsionConstantForce*probaCell*(yWorldCell-yRobotWorld)/pow(distanceCellToRobot,3);
Ludwigfr 33:814bcd7d3cfe 293 *forceX+=xForceComputed;
Ludwigfr 33:814bcd7d3cfe 294 *forceY+=yForceComputed;
Ludwigfr 33:814bcd7d3cfe 295 }
Ludwigfr 33:814bcd7d3cfe 296 }
Ludwigfr 33:814bcd7d3cfe 297
Ludwigfr 33:814bcd7d3cfe 298 //robotX and robotY are from Odometria, calculate line_a, line_b and line_c
Ludwigfr 33:814bcd7d3cfe 299 void MiniExplorerCoimbra::calculate_line(float forceX, float forceY, float *line_a, float *line_b, float *line_c){
Ludwigfr 33:814bcd7d3cfe 300 /*
Ludwigfr 33:814bcd7d3cfe 301 in the teachers maths it is
Ludwigfr 33:814bcd7d3cfe 302
Ludwigfr 33:814bcd7d3cfe 303 *line_a=forceY;
Ludwigfr 33:814bcd7d3cfe 304 *line_b=-forceX;
Ludwigfr 33:814bcd7d3cfe 305
Ludwigfr 33:814bcd7d3cfe 306 because a*x+b*y+c=0
Ludwigfr 33:814bcd7d3cfe 307 a impact the vertical and b the horizontal
Ludwigfr 33:814bcd7d3cfe 308 and he has to put them like this because
Ludwigfr 33:814bcd7d3cfe 309 Robot space: orthonormal space:
Ludwigfr 33:814bcd7d3cfe 310 ^ ^
Ludwigfr 33:814bcd7d3cfe 311 |x |y
Ludwigfr 33:814bcd7d3cfe 312 <- R O ->
Ludwigfr 33:814bcd7d3cfe 313 y x
Ludwigfr 33:814bcd7d3cfe 314 but since our forceX, forceY are already computed in the orthonormal space I m not sure we need to
Ludwigfr 33:814bcd7d3cfe 315 */
Ludwigfr 33:814bcd7d3cfe 316 *line_a=forceX;
Ludwigfr 33:814bcd7d3cfe 317 *line_b=forceY;
Ludwigfr 33:814bcd7d3cfe 318 //because the line computed always pass by the robot center we dont need lince_c
Ludwigfr 33:814bcd7d3cfe 319 //*line_c=forceX*yRobotWorld+forceY*xRobotWorld;
Ludwigfr 33:814bcd7d3cfe 320 *line_c=0;
Ludwigfr 33:814bcd7d3cfe 321 }
Ludwigfr 33:814bcd7d3cfe 322
Ludwigfr 33:814bcd7d3cfe 323 //currently line_c is not used
Ludwigfr 33:814bcd7d3cfe 324 void MiniExplorerCoimbra::go_to_line(float line_a, float line_b, float line_c,float targetXWorld, float targetYWorld){
Ludwigfr 33:814bcd7d3cfe 325 float lineAngle;
Ludwigfr 33:814bcd7d3cfe 326 float angleError;
Ludwigfr 33:814bcd7d3cfe 327 float linear;
Ludwigfr 33:814bcd7d3cfe 328 float angular;
Ludwigfr 33:814bcd7d3cfe 329
Ludwigfr 33:814bcd7d3cfe 330 bool direction=true;
Ludwigfr 33:814bcd7d3cfe 331
Ludwigfr 33:814bcd7d3cfe 332 //take as parameter how much the robot is supposed to move on x and y (world)
Ludwigfr 33:814bcd7d3cfe 333 float angleToTarget=atan2(targetXWorld-this->get_converted_robot_X_into_world(),targetYWorld-this->get_converted_robot_Y_into_world());
Ludwigfr 33:814bcd7d3cfe 334 bool inFront=true;
Ludwigfr 33:814bcd7d3cfe 335 if(angleToTarget < 0)//the target is in front
Ludwigfr 33:814bcd7d3cfe 336 inFront=false;
Ludwigfr 33:814bcd7d3cfe 337
Ludwigfr 33:814bcd7d3cfe 338 if(theta > 0){
Ludwigfr 33:814bcd7d3cfe 339 //the robot is oriented to the left
Ludwigfr 33:814bcd7d3cfe 340 if(theta > PI/2){
Ludwigfr 33:814bcd7d3cfe 341 //the robot is oriented lower left
Ludwigfr 33:814bcd7d3cfe 342 if(inFront)
Ludwigfr 33:814bcd7d3cfe 343 direction=false;//robot and target not oriented the same way
Ludwigfr 33:814bcd7d3cfe 344 }else{
Ludwigfr 33:814bcd7d3cfe 345 //the robot is oriented upper left
Ludwigfr 33:814bcd7d3cfe 346 if(!inFront)
Ludwigfr 33:814bcd7d3cfe 347 direction=false;//robot and target not oriented the same way
Ludwigfr 33:814bcd7d3cfe 348 }
Ludwigfr 33:814bcd7d3cfe 349 }else{
Ludwigfr 33:814bcd7d3cfe 350 //the robot is oriented to the right
Ludwigfr 33:814bcd7d3cfe 351 if(theta < -PI/2){
Ludwigfr 33:814bcd7d3cfe 352 //the robot is oriented lower right
Ludwigfr 33:814bcd7d3cfe 353 if(inFront)
Ludwigfr 33:814bcd7d3cfe 354 direction=false;//robot and target not oriented the same way
Ludwigfr 33:814bcd7d3cfe 355 }else{
Ludwigfr 33:814bcd7d3cfe 356 //the robot is oriented upper right
Ludwigfr 33:814bcd7d3cfe 357 if(!inFront)
Ludwigfr 33:814bcd7d3cfe 358 direction=false;//robot and target not oriented the same way
Ludwigfr 33:814bcd7d3cfe 359 }
Ludwigfr 33:814bcd7d3cfe 360 }
Ludwigfr 33:814bcd7d3cfe 361 //pc.printf("\r\n Target is in front");
Ludwigfr 33:814bcd7d3cfe 362
Ludwigfr 33:814bcd7d3cfe 363 if(line_b!=0){
Ludwigfr 33:814bcd7d3cfe 364 if(!direction)
Ludwigfr 33:814bcd7d3cfe 365 lineAngle=atan(-line_a/line_b);
Ludwigfr 33:814bcd7d3cfe 366 else
Ludwigfr 33:814bcd7d3cfe 367 lineAngle=atan(line_a/-line_b);
Ludwigfr 33:814bcd7d3cfe 368 }
Ludwigfr 33:814bcd7d3cfe 369 else{
Ludwigfr 33:814bcd7d3cfe 370 lineAngle=1.5708;
Ludwigfr 33:814bcd7d3cfe 371 }
Ludwigfr 33:814bcd7d3cfe 372
Ludwigfr 33:814bcd7d3cfe 373 //Computing angle error
Ludwigfr 33:814bcd7d3cfe 374 angleError = lineAngle-theta;
Ludwigfr 33:814bcd7d3cfe 375 angleError = atan(sin(angleError)/cos(angleError));
Ludwigfr 33:814bcd7d3cfe 376
Ludwigfr 33:814bcd7d3cfe 377 //Calculating velocities
Ludwigfr 33:814bcd7d3cfe 378 linear=this->kv*(3.1416);
Ludwigfr 33:814bcd7d3cfe 379 angular=this->kh*angleError;
Ludwigfr 33:814bcd7d3cfe 380
Ludwigfr 33:814bcd7d3cfe 381 float angularLeft=(linear-0.5*this->distanceWheels*angular)/this->radiusWheels;
Ludwigfr 33:814bcd7d3cfe 382 float angularRight=(linear+0.5*this->distanceWheels*angular)/this->radiusWheels;
Ludwigfr 33:814bcd7d3cfe 383
Ludwigfr 33:814bcd7d3cfe 384 //Normalize speed for motors
Ludwigfr 33:814bcd7d3cfe 385 if(abs(angularLeft)>abs(angularRight)) {
Ludwigfr 33:814bcd7d3cfe 386 angularRight=this->speed*abs(angularRight/angularLeft)*this->sign1(angularRight);
Ludwigfr 33:814bcd7d3cfe 387 angularLeft=this->speed*this->sign1(angularLeft);
Ludwigfr 33:814bcd7d3cfe 388 }
Ludwigfr 33:814bcd7d3cfe 389 else {
Ludwigfr 33:814bcd7d3cfe 390 angularLeft=this->speed*abs(angularLeft/angularRight)*this->sign1(angularLeft);
Ludwigfr 33:814bcd7d3cfe 391 angularRight=this->speed*this->sign1(angularRight);
Ludwigfr 33:814bcd7d3cfe 392 }
Ludwigfr 33:814bcd7d3cfe 393 leftMotor(this->sign2(angularLeft),abs(angularLeft));
Ludwigfr 33:814bcd7d3cfe 394 rightMotor(this->sign2(angularRight),abs(angularRight));
Ludwigfr 33:814bcd7d3cfe 395 }
Ludwigfr 33:814bcd7d3cfe 396
Ludwigfr 33:814bcd7d3cfe 397 //return 1 if positiv, -1 if negativ
Ludwigfr 33:814bcd7d3cfe 398 float MiniExplorerCoimbra::sign1(float value){
Ludwigfr 33:814bcd7d3cfe 399 if(value>=0)
Ludwigfr 33:814bcd7d3cfe 400 return 1;
Ludwigfr 33:814bcd7d3cfe 401 else
Ludwigfr 33:814bcd7d3cfe 402 return -1;
Ludwigfr 33:814bcd7d3cfe 403 }
Ludwigfr 33:814bcd7d3cfe 404
Ludwigfr 33:814bcd7d3cfe 405 //return 1 if positiv, 0 if negativ
Ludwigfr 33:814bcd7d3cfe 406 int MiniExplorerCoimbra::sign2(float value){
Ludwigfr 33:814bcd7d3cfe 407 if(value>=0)
Ludwigfr 33:814bcd7d3cfe 408 return 1;
Ludwigfr 33:814bcd7d3cfe 409 else
Ludwigfr 33:814bcd7d3cfe 410 return 0;
Ludwigfr 33:814bcd7d3cfe 411 }
Ludwigfr 33:814bcd7d3cfe 412
Ludwigfr 33:814bcd7d3cfe 413 /*angleToTarget is obtained through atan2 so it s:
Ludwigfr 33:814bcd7d3cfe 414 < 0 if the angle is bettween PI and 2pi on a trigo circle
Ludwigfr 33:814bcd7d3cfe 415 > 0 if it is between 0 and PI
Ludwigfr 33:814bcd7d3cfe 416 */
Ludwigfr 33:814bcd7d3cfe 417 void MiniExplorerCoimbra::turn_to_target(float angleToTarget){
Ludwigfr 33:814bcd7d3cfe 418 Odometria();
Ludwigfr 33:814bcd7d3cfe 419 float theta_plus_h_pi=theta+PI/2;//theta is between -PI and PI
Ludwigfr 33:814bcd7d3cfe 420 if(theta_plus_h_pi > PI)
Ludwigfr 33:814bcd7d3cfe 421 theta_plus_h_pi=-(2*PI-theta_plus_h_pi);
Ludwigfr 33:814bcd7d3cfe 422 if(angleToTarget>0){
Ludwigfr 33:814bcd7d3cfe 423 leftMotor(0,1);
Ludwigfr 33:814bcd7d3cfe 424 rightMotor(1,1);
Ludwigfr 33:814bcd7d3cfe 425 }else{
Ludwigfr 33:814bcd7d3cfe 426 leftMotor(1,1);
Ludwigfr 33:814bcd7d3cfe 427 rightMotor(0,1);
Ludwigfr 33:814bcd7d3cfe 428 }
Ludwigfr 33:814bcd7d3cfe 429 while(abs(angleToTarget-theta_plus_h_pi)>0.05){
Ludwigfr 33:814bcd7d3cfe 430 Odometria();
Ludwigfr 33:814bcd7d3cfe 431 theta_plus_h_pi=theta+PI/2;//theta is between -PI and PI
Ludwigfr 33:814bcd7d3cfe 432 if(theta_plus_h_pi > PI)
Ludwigfr 33:814bcd7d3cfe 433 theta_plus_h_pi=-(2*PI-theta_plus_h_pi);
Ludwigfr 33:814bcd7d3cfe 434 //pc.printf("\n\r diff=%f", abs(angleToTarget-theta_plus_h_pi));
Ludwigfr 33:814bcd7d3cfe 435 }
Ludwigfr 33:814bcd7d3cfe 436 leftMotor(1,0);
Ludwigfr 33:814bcd7d3cfe 437 rightMotor(1,0);
Ludwigfr 33:814bcd7d3cfe 438 }
Ludwigfr 33:814bcd7d3cfe 439
Ludwigfr 33:814bcd7d3cfe 440
Ludwigfr 33:814bcd7d3cfe 441 /*
Ludwigfr 33:814bcd7d3cfe 442 //x and y passed are TargetNotMap
Ludwigfr 33:814bcd7d3cfe 443 float get_error_angles(float x, float y,float theta){
Ludwigfr 33:814bcd7d3cfe 444 float angleBeetweenRobotAndTarget=atan2(y,x);
Ludwigfr 33:814bcd7d3cfe 445 if(y > 0){
Ludwigfr 33:814bcd7d3cfe 446 if(angleBeetweenRobotAndTarget < PI/2)//up right
Ludwigfr 33:814bcd7d3cfe 447 angleBeetweenRobotAndTarget=-PI/2+angleBeetweenRobotAndTarget;
Ludwigfr 33:814bcd7d3cfe 448 else//up right
Ludwigfr 33:814bcd7d3cfe 449 angleBeetweenRobotAndTarget=angleBeetweenRobotAndTarget-PI/2;
Ludwigfr 33:814bcd7d3cfe 450 }else{
Ludwigfr 33:814bcd7d3cfe 451 if(angleBeetweenRobotAndTarget > -PI/2)//lower right
Ludwigfr 33:814bcd7d3cfe 452 angleBeetweenRobotAndTarget=angleBeetweenRobotAndTarget-PI/2;
Ludwigfr 33:814bcd7d3cfe 453 else//lower left
Ludwigfr 33:814bcd7d3cfe 454 angleBeetweenRobotAndTarget=2*PI+angleBeetweenRobotAndTarget-PI/2;
Ludwigfr 33:814bcd7d3cfe 455 }
Ludwigfr 33:814bcd7d3cfe 456 return angleBeetweenRobotAndTarget-theta;
Ludwigfr 33:814bcd7d3cfe 457 }
Ludwigfr 33:814bcd7d3cfe 458 */