Diff: main.cpp.orig

Revision:

20:6a9062d54eb0

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp.orig	Mon Apr 03 17:07:37 2017 +0000
@@ -0,0 +1,194 @@
+#include "mbed.h"
+#include "robot.h" // Initializes the robot. This include should be used in all main.cpp!
+#include "math.h"
+ 
+Timer t;
+
+float dist(float robot_x, float robot_y, float target_x, float target_y);
+
+int goToPointWithAngle(float target_x, float target_y, float target_angle);
+
+int randomizeAndMap();
+int updateSonarValues();
+int computeObstacle();
+
+bool map[25][25];
+float alpha; //angle error
+float rho; //distance from target
+float beta;
+float kRho=12, ka=30, kb=-13; //Kappa values
+float linear, angular, angular_left, angular_right;
+float dt=0.5;
+float temp;
+float d2;
+
+bool tooClose = false;
+
+int leftMm;
+int frontMm;
+int rightMm;
+
+
+//Diameter of a wheel and distance between the 2
+float r=3.25, b=7.2;
+
+int speed=999; // Max speed at beggining of movement
+
+//Target example x,y values
+float target_x=46.8, target_y=78.6, target_angle=1.57;
+
+//Timeout time;
+int main(){
+    i2c1.frequency(100000);
+    initRobot(); //Initializing the robot
+    pc.baud(9600); // baud for the pc communication
+
+    measure_always_on();
+    wait_ms(50);
+    
+    //Fill map
+    for (int i = 0; i<25; i++) {
+        for (int j = 0; j<25; j++) {
+            map[i][j] = false;        
+        }
+    }
+
+    //Resetting coordinates before moving
+    theta=0; 
+    X=0;
+    Y=0;
+
+    /*alpha = atan2((target_y-Y),(target_x-X))-theta;
+    alpha = atan(sin(alpha)/cos(alpha));
+    rho = dist(X, Y, target_x, target_y);
+    
+    beta = -alpha-theta+target_angle;*/
+    for (int i = 0; i<10; i++) {
+        randomizeAndMap();
+    }
+
+    //Stop at the end
+    leftMotor(1,0);
+    rightMotor(1,0);
+
+    pc.printf("\n\r %f -- arrived!", rho);
+}
+
+//Distance computation function
+float dist(float robot_x, float robot_y, float target_x, float target_y){
+    return sqrt(pow(target_y-robot_y,2) + pow(target_x-robot_x,2));
+}
+
+//Updates sonar values
+int updateSonarValues() {
+    leftMm = get_distance_left_sensor();
+    frontMm = get_distance_front_sensor();
+    rightMm = get_distance_right_sensor();
+    return 0;
+}
+
+int randomizeAndMap() {
+    target_x = (rand()%2500)/10;//for decimal precision
+    target_y = (rand()%2500)/10;
+    target_angle = ((rand()%31416)-15708)/1000;
+    pc.printf("\n\r targ_X=%f", target_x);
+    pc.printf("\n\r targ_Y=%f", target_y);
+    pc.printf("\n\r targ_Angle=%f", target_angle);
+    goToPointWithAngle(target_x, target_y, target_angle);
+    return 0;    
+}
+
+int computeObstacle() {
+    //get the sensor values
+    //compute the probabilistic shit of empty/non-empty in the direction of the sensor below 10 cm
+    //update the map object with true where it's likely to be obstacled
+    int xObstacle, yObstacle;
+    if (leftMm < 10) {
+    
+    } else if (frontMm < 10) {
+    
+    } else if (rightMm < 10) {
+    
+    }
+    
+    map[xObstacle][yObstacle] = true;
+    return 0;
+}
+
+int goToPointWithAngle(float target_x, float target_y, float target_angle) {
+     do {
+        pc.printf("\n\n\r entered while");
+        
+        //Timer stuff
+        dt = t.read();
+        t.reset();
+        t.start();
+        
+        updateSonarValues();
+        if (leftMm < 100 || frontMm < 100 || rightMm < 100) {
+            tooClose = true;    
+        }
+        
+        //Updating X,Y and theta with the odometry values
+        Odometria();
+        
+        alpha = atan2((target_y-Y),(target_x-X))-theta;
+        alpha = atan(sin(alpha)/cos(alpha));
+        rho = dist(X, Y, target_x, target_y);
+        d2 = rho;
+        beta = -alpha-theta+target_angle;        
+        
+        //Computing angle error and distance towards the target value
+        rho += dt*(-kRho*cos(alpha)*rho);
+        temp = alpha;
+        alpha += dt*(kRho*sin(alpha)-ka*alpha-kb*beta);
+        beta += dt*(-kRho*sin(temp));
+        pc.printf("\n\r d2=%f", d2);
+        pc.printf("\n\r dt=%f", dt);
+
+        //Computing linear and angular velocities
+        if(alpha>=-1.5708 && alpha<=1.5708){
+            linear=kRho*rho;
+            angular=ka*alpha+kb*beta;
+        }
+        else{
+            linear=-kRho*rho;
+            angular=-ka*alpha-kb*beta;
+        }
+        angular_left=(linear-0.5*b*angular)/r;
+        angular_right=(linear+0.5*b*angular)/r;
+
+        //Slowing down at the end for more precision
+        if (d2<25) {
+            speed = d2*30;
+        }
+        
+        //Normalize speed for motors
+        if(angular_left>angular_right) {
+            angular_right=speed*angular_right/angular_left;
+            angular_left=speed;
+        } else {
+            angular_left=speed*angular_left/angular_right;
+            angular_right=speed;
+        }
+
+        pc.printf("\n\r X=%f", X);
+        pc.printf("\n\r Y=%f", Y);
+        pc.printf("\n\r leftMm=%f", leftMm);
+        pc.printf("\n\r frontMm=%f", frontMm);
+        pc.printf("\n\r rightMm=%f", rightMm);
+
+        //Updating motor velocities
+        leftMotor(1,angular_left);
+        rightMotor(1,angular_right);
+
+        wait(0.2);
+        //Timer stuff
+        t.stop();
+    } while(d2>1 & !tooClose);
+    
+    if (tooClose) {
+        computeObstacle();
+    }
+    return 0;
+}
\ No newline at end of file