Revision 0:25f4809c2729, committed 2019-06-09

Comitter:

shut

Date:

Sun Jun 09 14:54:11 2019 +0000

Commit message:

observer

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ActiveCell.h	Sun Jun 09 14:54:11 2019 +0000
@@ -0,0 +1,71 @@
+#ifndef UNTITLED2_ACTIVECELL_H
+#define UNTITLED2_ACTIVECELL_H
+#include <iostream>
+#include <iomanip>
+#include <math.h>
+
+class ActiveCell {
+public:
+    //cell dimension = 5x5cm
+    float cellVal;
+    float forceX;
+    float forceY;
+    float distance;
+    short xt, yt;
+    static const short repulsiveForce = 20;
+    //robots position
+    short x0;
+    short y0;
+    ///////////VFH//////////////
+    static const int a=70,b=2;
+    float angle;
+    float amplitude;
+    short sectorK;
+    //resolution - if changing change also secVal in main & calcSectors
+    static const short res=10;
+
+    ActiveCell(){
+        angle=0;
+        amplitude=0;
+        sectorK=0;
+        cellVal=0;
+        forceX=0;
+        forceY=0;
+        x0=0;
+        y0=0;
+    }
+
+
+    void calForce() {
+        forceX = (repulsiveForce * cellVal / pow(distance, 2)) * (xt - x0) / distance;
+        forceY = (repulsiveForce * cellVal / pow(distance, 2)) * (yt - y0) / distance;
+        amplitude = cellVal*cellVal*(a-b*distance);
+    }
+//Calculating distance from the robot 
+    void calDist(int idX, int idY) {
+        if (idX < 5) {
+            xt = x0 - (5 - idX) * 5;
+        } else if (idX == 5) {
+            xt = x0;
+        } else {
+            xt = x0 + (idX - 5) * 5;
+        }
+        if (idY > 5) {
+            yt = y0 - (5 - idY) * 5;
+        } else if (idY == 5) {
+            yt = y0;
+        } else {
+            yt = y0 + (idY - 5) * 5;
+        }
+        distance = sqrt(pow((float)abs(x0 - xt), 2) + pow((float)abs(y0 - yt), 2));
+        
+        //////angle/////////////
+        angle = atan2((float)yt-y0,(float)xt-x0)*180/3.14159265358979323846f;
+        sectorK=angle/res;
+        if(sectorK<0)
+            sectorK=36+sectorK;
+    }
+};
+
+
+#endif //UNTITLED2_ACTIVECELL_H

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/HistogramCell.h	Sun Jun 09 14:54:11 2019 +0000
@@ -0,0 +1,18 @@
+#ifndef UNTITLED2_HISTOGRAMCELL_H
+#define UNTITLED2_HISTOGRAMCELL_H
+
+
+class HistogramCell {
+public:
+    float cellVal;
+    float logodds; 
+
+    HistogramCell(){
+       cellVal = 0;
+        logodds = 0;
+    }
+};
+
+
+
+#endif //UNTITLED2_HISTOGRAMCELL_H

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Robot.cpp	Sun Jun 09 14:54:11 2019 +0000
@@ -0,0 +1,66 @@
+#include "Robot.h"
+#include "mbed.h"
+
+I2C i2c(I2C_SDA, I2C_SCL );
+const int addr8bit = 20 << 1; // 7bit I2C address is 20; 8bit I2C address is 40 (decimal).
+
+int16_t countsLeft = 0;
+int16_t countsRight = 0;
+
+
+void setSpeeds(int16_t leftSpeed, int16_t rightSpeed)
+{
+    char buffer[5];
+    
+    buffer[0] = 0xA1;
+    memcpy(&buffer[1], &leftSpeed, sizeof(leftSpeed));
+    memcpy(&buffer[3], &rightSpeed, sizeof(rightSpeed));
+
+    i2c.write(addr8bit, buffer, 5); // 5 bytes
+}
+
+void setLeftSpeed(int16_t speed)
+{
+    char buffer[3];
+    
+    buffer[0] = 0xA2;
+    memcpy(&buffer[1], &speed, sizeof(speed));
+
+    i2c.write(addr8bit, buffer, 3); // 3 bytes
+}
+
+void setRightSpeed(int16_t speed)
+{
+    char buffer[3];
+    
+    buffer[0] = 0xA3;
+    memcpy(&buffer[1], &speed, sizeof(speed));
+
+    i2c.write(addr8bit, buffer, 3); // 3 bytes
+}
+
+void getCounts()
+{
+    char write_buffer[2];
+    char read_buffer[4];
+    
+    write_buffer[0] = 0xA0;
+    i2c.write(addr8bit, write_buffer, 1); wait_us(100);
+    i2c.read( addr8bit, read_buffer, 4);
+    countsLeft = (int16_t((read_buffer[0]<<8)|read_buffer[1]));
+    countsRight = (int16_t((read_buffer[2]<<8)|read_buffer[3]));
+}
+
+void getCountsAndReset()
+{
+    char write_buffer[2];
+    char read_buffer[4];
+    
+    write_buffer[0] = 0xA4;
+    i2c.write(addr8bit, write_buffer, 1); wait_us(100);
+    i2c.read( addr8bit, read_buffer, 4);
+    countsLeft = (int16_t((read_buffer[0]<<8)|read_buffer[1]));
+    countsRight = (int16_t((read_buffer[2]<<8)|read_buffer[3]));
+}
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Robot.h	Sun Jun 09 14:54:11 2019 +0000
@@ -0,0 +1,52 @@
+/** @file */
+#ifndef ROBOT_H_
+#define ROBOT_H_
+
+#include "mbed.h"
+
+extern int16_t countsLeft;
+extern int16_t countsRight;
+
+/** \brief Sets the speed for both motors.
+ *
+ * \param leftSpeed A number from -300 to 300 representing the speed and
+ * direction of the left motor. Values of -300 or less result in full speed
+ * reverse, and values of 300 or more result in full speed forward.
+ * \param rightSpeed A number from -300 to 300 representing the speed and
+ * direction of the right motor. Values of -300 or less result in full speed
+ * reverse, and values of 300 or more result in full speed forward. */
+void setSpeeds(int16_t leftSpeed, int16_t rightSpeed);
+
+/** \brief Sets the speed for the left motor.
+ *
+ * \param speed A number from -300 to 300 representing the speed and
+ * direction of the left motor.  Values of -300 or less result in full speed
+ * reverse, and values of 300 or more result in full speed forward. */
+void setLeftSpeed(int16_t speed);
+
+/** \brief Sets the speed for the right motor.
+ *
+ * \param speed A number from -300 to 300 representing the speed and
+ * direction of the right motor. Values of -300 or less result in full speed
+ * reverse, and values of 300 or more result in full speed forward. */
+void setRightSpeed(int16_t speed);
+
+/*! Returns the number of counts that have been detected from the both
+ * encoders.  These counts start at 0.  Positive counts correspond to forward
+ * movement of the wheel of the Romi, while negative counts correspond
+ * to backwards movement.
+ *
+ * The count is returned as a signed 16-bit integer.  When the count goes
+ * over 32767, it will overflow down to -32768.  When the count goes below
+ * -32768, it will overflow up to 32767. */
+void getCounts();
+
+/*! This function is just like getCounts() except it also clears the
+ *  counts before returning.  If you call this frequently enough, you will
+ *  not have to worry about the count overflowing. */
+void getCountsAndReset();
+//Pose estimation [time,omega,x,y,phi,v] -> [xk,yk,phik]
+
+double limitSpeed(double speed);
+
+#endif /* ROBOT_H_ */
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Jun 09 14:54:11 2019 +0000
@@ -0,0 +1,213 @@
+#include "mbed.h"
+#include "BufferedSerial.h"
+#include "rplidar.h"
+#include "Robot.h"
+#include "Communication.h"
+#include "math.h"
+#include "ActiveCell.h"
+#include "HistogramCell.h"
+#define M_PI 3.14159265358979323846f
+// LABWORK 4 v0.1
+// LUIS CRUZ 2011164454
+// JACEK SOBECKI
+RPLidar lidar;
+BufferedSerial se_lidar(PA_9, PA_10);
+PwmOut rplidar_motor(D3);
+struct RPLidarMeasurement data;
+Serial pc(SERIAL_TX, SERIAL_RX, 115200);
+DigitalIn button(PC_13);
+void poseEst(float p[], float radius, float enc_res, float b);
+void SpeedLim(float w[]);
+void initializeArrays();
+int Bresenham(int x1, int y1, int x2, int y2, int cX[], int cY[]);
+void observe();
+
+const int hSize = 80, aSize = 11;//
+ActiveCell activeReg[aSize][aSize];
+HistogramCell histogram[hSize][hSize];
+
+int main(){
+
+    button.mode(PullUp);
+    getCountsAndReset();
+    setSpeeds(0, 0);
+    initializeArrays();
+    while(button==1);
+    rplidar_motor.period(0.001f);
+    lidar.begin(se_lidar);
+    lidar.setAngle(0,360);
+    rplidar_motor.write(0.9f);
+    pc.printf("Program started.\n");
+    lidar.startThreadScan();
+    //w[0] = Omega     | w[1] = Left  | w[2] = Right
+    //p[0] = X         | p[1] = Y     | p[2] = Theta
+    //b = Distance between wheels, enc_res = Encoder Resolution, v = Calculated speed
+    //k_v = Speed gain, k_s = Curvature gain, wratio = Angular speed ratio control command
+    //Cells dim: 5x5cm |
+    float  w[3], v, p[3], p_obj[3], theta, theta_error;
+    const float radius = 3.5, b = 13.3, enc_res = 1440, k_v = 8/*7*/, 
+    k_s = 12/*10*/, sample_time = 0.05;
+// ===============================================================================
+// =================================== COORDS ====================================
+// =============================================================================== 
+    //Target coordinates
+    p_obj[0] = 100, p_obj[1] = 20, p_obj[2] = 0;
+    //Initial coordinates:
+    p[0] = 20, p[1] = 20, p[2] = 0;
+// ===============================================================================
+// =================================== EXECUTION =================================
+// ===============================================================================
+    while(1){
+        getCountsAndReset();
+        pc.printf("Speeds: Left=%lf   Right=%lf\n", w[1], w[2]);
+        pc.printf("OBJECTIVE X: %lf  OBJECTIVE Y: %lf\n", p_obj[0], p_obj[1]);
+        pc.printf("Position: X=%lf   Y=%lf   Theta=%lf\n\n", p[0], p[1], p[2]);
+        if(lidar.pollSensorData(&data) == 0) pc.printf("dist:%f  angle:%f\n", data.distance, data.angle); // Prints one lidar measurement.
+        //Path calculation
+        poseEst(p, radius, enc_res, b);
+        theta = atan2(p_obj[1]-p[1],p_obj[0]-p[0]);
+        theta = atan2(sin(theta),cos(theta));
+        p[2] = atan2(sin(p[2]),cos(p[2]));
+        theta_error = theta-p[2];
+        w[0] = k_s*(theta_error);
+        //pc.printf("\nOmega:%lf     a:%lf\n", w[0], theta); //DEBUG
+        v = k_v*sqrt(pow((p_obj[0]-p[0]),2)+pow((p_obj[1]-p[1]),2));
+        w[1] = (v-(b/2)*w[0])/radius;
+        w[2] = (v+(b/2)*w[0])/radius;
+        SpeedLim(w);
+        if((fabs(p[0]-p_obj[0])+fabs(p[1]-p_obj[1])) < 5){
+            setSpeeds(0,0);
+        }
+        else{
+            setSpeeds(w[1], w[2]);
+            }
+        wait(sample_time); 
+    }
+}
+// ===============================================================================
+// =================================== FUNCTIONS =================================
+// ===============================================================================
+//Pose Estimation function
+void poseEst(float p[], float radius, float enc_res, float b){
+    float deltaDl, deltaDr, deltaD, deltaT;
+    deltaDl = ((float)countsLeft)*(2.0f*M_PI*radius/enc_res);
+    deltaDr = ((float)countsRight)*(2.0f*M_PI*radius/enc_res);
+    deltaD = (deltaDr + deltaDl)/2.0f;
+    deltaT = (deltaDr - deltaDl)/b;
+    if(fabs(deltaT) == 0){
+        p[0] = p[0] + deltaD*cos(p[2]) + deltaT/2;
+        p[1] = p[1] + deltaD*sin(p[2]) + deltaT/2;
+        return;
+    }
+    p[0] += deltaD*cos(p[2]+deltaT/2.0f);
+    p[1] += deltaD*sin(p[2]+deltaT/2.0f);
+    p[2] += deltaT;
+}
+//Speed limiter function
+void SpeedLim(float w[]){
+    float wratio;
+    wratio = fabs(w[2]/w[1]);
+    if(w[2] > 150 || w[1] > 150){
+        if(wratio < 1){
+            w[1] = 150;
+            w[2] = w[1]*wratio;
+        }
+        else if(wratio > 1){
+            w[2] = 150;
+            w[1] = w[2]/wratio;
+        }
+        else{
+            w[2] = 150;
+            w[1] = 150;
+        }
+    }
+    if(w[2] < 50 || w[1] < 50){
+        if(wratio < 1){
+            w[1] = 50;
+            w[2] = w[1]*wratio;
+        }
+        else if(wratio > 1){
+            w[2] = 50;
+            w[1] = w[2]/wratio;
+        }
+        else{
+            w[2] = 50;
+            w[1] = 50;
+        }
+    }
+}
+//Initialize apriori knowledge of map
+void initializeArrays(){
+    for (int i = 0; i < aSize; i++) {
+        for (int j = 0; j < aSize; j++) {
+            activeReg[i][j].calDist(i, j);
+        }
+    }
+}
+//Bresenham algo -> trace the Lidar's reading line into a bitmap a.k.a. cell's index
+int Bresenham(int x1, int y1, int x2, int y2, int cX[], int cY[]){
+    int aux = 0;
+    int delta_x(x2 - x1);
+    // if xR == x2, then it does not matter what we set here
+    signed char const ix((delta_x > 0) - (delta_x < 0));
+    delta_x = std::abs(delta_x) << 1;
+    int delta_y(y2 - y1);
+    // if yR == y2, then it does not matter what we set here
+    signed char const iy((delta_y > 0) - (delta_y < 0));
+    delta_y = std::abs(delta_y) << 1;
+    cX[aux] = x1, cY[aux] = y1, aux++;
+    //pc.printf("x = %d | y = %d | xF = %d | yF = %d | aux = %d \n", x1, y1, x2, y2, aux-1);
+    if (delta_x >= delta_y){
+        // error may go below zero
+        int error(delta_y - (delta_x >> 1));
+        while (x1 != x2)
+        {
+            // reduce error, while taking into account the corner case of error == 0
+            if ((error > 0) || (!error && (ix > 0))){
+                error -= delta_x;
+                y1 += iy;
+            }
+            error += delta_y;
+            x1 += ix;
+            cX[aux] = x1, cY[aux] = y1, aux++;
+        }
+    }
+    else{
+        // error may go below zero
+        int error(delta_x - (delta_y >> 1));
+        while (y1 != y2){
+            // reduce error, while taking into account the corner case of error == 0
+            if ((error > 0) || (!error && (iy > 0))){
+                error -= delta_y;
+                x1 += ix;
+            }
+            error += delta_x;
+            y1 += iy;
+            cX[aux] = x1, cY[aux] = y1, aux++;
+            //pc.printf("x = %d | y = %d | cx = %d | cy = %d | aux = %d \n", x1, y1, cX[aux-1], cY[aux-1], aux-1); //DEBUG
+        }
+    }
+    return aux;
+}
+void observe(){
+    float thetaR_deg, readAngle, lDist, lAng;
+    int xR, yR, xL, yL, xF, yF;
+    //------------Processing LIDAR readings------------
+    lDist = data.distance / 10; //mm -> cm
+    if(lDist == 0 || lDist > 2000) return;
+    lAng = data.angle;
+    thetaR_deg = (p[2] * 180.0f) / M_PI;
+    if(thetaR_deg < 0) thetaR_deg = 360 + thetaR_deg;
+    readAngle = 270 - lAng + thetaR_deg; //Align LIDAR's frame with robot's one
+    if(readAngle > 360) readAngle -= 360; //   "     "      "      "      "
+    else if(readAngle < 0) readAngle += 360;// "     "      "      "      "
+    //pc.printf("ReadAngle: %f | data_deg: %f | Distance: %f | pos: (%f,%f)\n", readAngle, lAng, lDist, p[0], p[1]); //DEBUG
+    readAngle = (readAngle * M_PI) /180.0f; // deg -> rads
+    xL = lDist * cos(readAngle)/5;//X_coord Lidar's reading (cell unit)
+    yL = lDist * sin(readAngle)/5;//Y_coord Lidar's reading (cell unit)
+    xR = p[0]/5, yR = p[1]/5; //Robot's coordinates cell
+    xF = xR + xL;
+    yF = yR + yL;
+    //if(xF < 0 || yF < 0 || xF > 80 || yF > 80) return;
+    npts = Bresenham(xR, yR, xF, yF, cX, cY);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sun Jun 09 14:54:11 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file