SRM / Mbed 2 deprecated LabWork4

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@0:25f4809c2729, 2019-06-09 (annotated)

Committer:
shut
Date:
Sun Jun 09 14:54:11 2019 +0000
Revision:
0:25f4809c2729
observer

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shut 0:25f4809c2729 1 #include "mbed.h"
shut 0:25f4809c2729 2 #include "BufferedSerial.h"
shut 0:25f4809c2729 3 #include "rplidar.h"
shut 0:25f4809c2729 4 #include "Robot.h"
shut 0:25f4809c2729 5 #include "Communication.h"
shut 0:25f4809c2729 6 #include "math.h"
shut 0:25f4809c2729 7 #include "ActiveCell.h"
shut 0:25f4809c2729 8 #include "HistogramCell.h"
shut 0:25f4809c2729 9 #define M_PI 3.14159265358979323846f
shut 0:25f4809c2729 10 // LABWORK 4 v0.1
shut 0:25f4809c2729 11 // LUIS CRUZ 2011164454
shut 0:25f4809c2729 12 // JACEK SOBECKI
shut 0:25f4809c2729 13 RPLidar lidar;
shut 0:25f4809c2729 14 BufferedSerial se_lidar(PA_9, PA_10);
shut 0:25f4809c2729 15 PwmOut rplidar_motor(D3);
shut 0:25f4809c2729 16 struct RPLidarMeasurement data;
shut 0:25f4809c2729 17 Serial pc(SERIAL_TX, SERIAL_RX, 115200);
shut 0:25f4809c2729 18 DigitalIn button(PC_13);
shut 0:25f4809c2729 19 void poseEst(float p[], float radius, float enc_res, float b);
shut 0:25f4809c2729 20 void SpeedLim(float w[]);
shut 0:25f4809c2729 21 void initializeArrays();
shut 0:25f4809c2729 22 int Bresenham(int x1, int y1, int x2, int y2, int cX[], int cY[]);
shut 0:25f4809c2729 23 void observe();
shut 0:25f4809c2729 24
shut 0:25f4809c2729 25 const int hSize = 80, aSize = 11;//
shut 0:25f4809c2729 26 ActiveCell activeReg[aSize][aSize];
shut 0:25f4809c2729 27 HistogramCell histogram[hSize][hSize];
shut 0:25f4809c2729 28
shut 0:25f4809c2729 29 int main(){
shut 0:25f4809c2729 30
shut 0:25f4809c2729 31 button.mode(PullUp);
shut 0:25f4809c2729 32 getCountsAndReset();
shut 0:25f4809c2729 33 setSpeeds(0, 0);
shut 0:25f4809c2729 34 initializeArrays();
shut 0:25f4809c2729 35 while(button==1);
shut 0:25f4809c2729 36 rplidar_motor.period(0.001f);
shut 0:25f4809c2729 37 lidar.begin(se_lidar);
shut 0:25f4809c2729 38 lidar.setAngle(0,360);
shut 0:25f4809c2729 39 rplidar_motor.write(0.9f);
shut 0:25f4809c2729 40 pc.printf("Program started.\n");
shut 0:25f4809c2729 41 lidar.startThreadScan();
shut 0:25f4809c2729 42 //w[0] = Omega | w[1] = Left | w[2] = Right
shut 0:25f4809c2729 43 //p[0] = X | p[1] = Y | p[2] = Theta
shut 0:25f4809c2729 44 //b = Distance between wheels, enc_res = Encoder Resolution, v = Calculated speed
shut 0:25f4809c2729 45 //k_v = Speed gain, k_s = Curvature gain, wratio = Angular speed ratio control command
shut 0:25f4809c2729 46 //Cells dim: 5x5cm |
shut 0:25f4809c2729 47 float w[3], v, p[3], p_obj[3], theta, theta_error;
shut 0:25f4809c2729 48 const float radius = 3.5, b = 13.3, enc_res = 1440, k_v = 8/*7*/,
shut 0:25f4809c2729 49 k_s = 12/*10*/, sample_time = 0.05;
shut 0:25f4809c2729 50 // ===============================================================================
shut 0:25f4809c2729 51 // =================================== COORDS ====================================
shut 0:25f4809c2729 52 // ===============================================================================
shut 0:25f4809c2729 53 //Target coordinates
shut 0:25f4809c2729 54 p_obj[0] = 100, p_obj[1] = 20, p_obj[2] = 0;
shut 0:25f4809c2729 55 //Initial coordinates:
shut 0:25f4809c2729 56 p[0] = 20, p[1] = 20, p[2] = 0;
shut 0:25f4809c2729 57 // ===============================================================================
shut 0:25f4809c2729 58 // =================================== EXECUTION =================================
shut 0:25f4809c2729 59 // ===============================================================================
shut 0:25f4809c2729 60 while(1){
shut 0:25f4809c2729 61 getCountsAndReset();
shut 0:25f4809c2729 62 pc.printf("Speeds: Left=%lf Right=%lf\n", w[1], w[2]);
shut 0:25f4809c2729 63 pc.printf("OBJECTIVE X: %lf OBJECTIVE Y: %lf\n", p_obj[0], p_obj[1]);
shut 0:25f4809c2729 64 pc.printf("Position: X=%lf Y=%lf Theta=%lf\n\n", p[0], p[1], p[2]);
shut 0:25f4809c2729 65 if(lidar.pollSensorData(&data) == 0) pc.printf("dist:%f angle:%f\n", data.distance, data.angle); // Prints one lidar measurement.
shut 0:25f4809c2729 66 //Path calculation
shut 0:25f4809c2729 67 poseEst(p, radius, enc_res, b);
shut 0:25f4809c2729 68 theta = atan2(p_obj[1]-p[1],p_obj[0]-p[0]);
shut 0:25f4809c2729 69 theta = atan2(sin(theta),cos(theta));
shut 0:25f4809c2729 70 p[2] = atan2(sin(p[2]),cos(p[2]));
shut 0:25f4809c2729 71 theta_error = theta-p[2];
shut 0:25f4809c2729 72 w[0] = k_s*(theta_error);
shut 0:25f4809c2729 73 //pc.printf("\nOmega:%lf a:%lf\n", w[0], theta); //DEBUG
shut 0:25f4809c2729 74 v = k_v*sqrt(pow((p_obj[0]-p[0]),2)+pow((p_obj[1]-p[1]),2));
shut 0:25f4809c2729 75 w[1] = (v-(b/2)*w[0])/radius;
shut 0:25f4809c2729 76 w[2] = (v+(b/2)*w[0])/radius;
shut 0:25f4809c2729 77 SpeedLim(w);
shut 0:25f4809c2729 78 if((fabs(p[0]-p_obj[0])+fabs(p[1]-p_obj[1])) < 5){
shut 0:25f4809c2729 79 setSpeeds(0,0);
shut 0:25f4809c2729 80 }
shut 0:25f4809c2729 81 else{
shut 0:25f4809c2729 82 setSpeeds(w[1], w[2]);
shut 0:25f4809c2729 83 }
shut 0:25f4809c2729 84 wait(sample_time);
shut 0:25f4809c2729 85 }
shut 0:25f4809c2729 86 }
shut 0:25f4809c2729 87 // ===============================================================================
shut 0:25f4809c2729 88 // =================================== FUNCTIONS =================================
shut 0:25f4809c2729 89 // ===============================================================================
shut 0:25f4809c2729 90 //Pose Estimation function
shut 0:25f4809c2729 91 void poseEst(float p[], float radius, float enc_res, float b){
shut 0:25f4809c2729 92 float deltaDl, deltaDr, deltaD, deltaT;
shut 0:25f4809c2729 93 deltaDl = ((float)countsLeft)*(2.0f*M_PI*radius/enc_res);
shut 0:25f4809c2729 94 deltaDr = ((float)countsRight)*(2.0f*M_PI*radius/enc_res);
shut 0:25f4809c2729 95 deltaD = (deltaDr + deltaDl)/2.0f;
shut 0:25f4809c2729 96 deltaT = (deltaDr - deltaDl)/b;
shut 0:25f4809c2729 97 if(fabs(deltaT) == 0){
shut 0:25f4809c2729 98 p[0] = p[0] + deltaD*cos(p[2]) + deltaT/2;
shut 0:25f4809c2729 99 p[1] = p[1] + deltaD*sin(p[2]) + deltaT/2;
shut 0:25f4809c2729 100 return;
shut 0:25f4809c2729 101 }
shut 0:25f4809c2729 102 p[0] += deltaD*cos(p[2]+deltaT/2.0f);
shut 0:25f4809c2729 103 p[1] += deltaD*sin(p[2]+deltaT/2.0f);
shut 0:25f4809c2729 104 p[2] += deltaT;
shut 0:25f4809c2729 105 }
shut 0:25f4809c2729 106 //Speed limiter function
shut 0:25f4809c2729 107 void SpeedLim(float w[]){
shut 0:25f4809c2729 108 float wratio;
shut 0:25f4809c2729 109 wratio = fabs(w[2]/w[1]);
shut 0:25f4809c2729 110 if(w[2] > 150 || w[1] > 150){
shut 0:25f4809c2729 111 if(wratio < 1){
shut 0:25f4809c2729 112 w[1] = 150;
shut 0:25f4809c2729 113 w[2] = w[1]*wratio;
shut 0:25f4809c2729 114 }
shut 0:25f4809c2729 115 else if(wratio > 1){
shut 0:25f4809c2729 116 w[2] = 150;
shut 0:25f4809c2729 117 w[1] = w[2]/wratio;
shut 0:25f4809c2729 118 }
shut 0:25f4809c2729 119 else{
shut 0:25f4809c2729 120 w[2] = 150;
shut 0:25f4809c2729 121 w[1] = 150;
shut 0:25f4809c2729 122 }
shut 0:25f4809c2729 123 }
shut 0:25f4809c2729 124 if(w[2] < 50 || w[1] < 50){
shut 0:25f4809c2729 125 if(wratio < 1){
shut 0:25f4809c2729 126 w[1] = 50;
shut 0:25f4809c2729 127 w[2] = w[1]*wratio;
shut 0:25f4809c2729 128 }
shut 0:25f4809c2729 129 else if(wratio > 1){
shut 0:25f4809c2729 130 w[2] = 50;
shut 0:25f4809c2729 131 w[1] = w[2]/wratio;
shut 0:25f4809c2729 132 }
shut 0:25f4809c2729 133 else{
shut 0:25f4809c2729 134 w[2] = 50;
shut 0:25f4809c2729 135 w[1] = 50;
shut 0:25f4809c2729 136 }
shut 0:25f4809c2729 137 }
shut 0:25f4809c2729 138 }
shut 0:25f4809c2729 139 //Initialize apriori knowledge of map
shut 0:25f4809c2729 140 void initializeArrays(){
shut 0:25f4809c2729 141 for (int i = 0; i < aSize; i++) {
shut 0:25f4809c2729 142 for (int j = 0; j < aSize; j++) {
shut 0:25f4809c2729 143 activeReg[i][j].calDist(i, j);
shut 0:25f4809c2729 144 }
shut 0:25f4809c2729 145 }
shut 0:25f4809c2729 146 }
shut 0:25f4809c2729 147 //Bresenham algo -> trace the Lidar's reading line into a bitmap a.k.a. cell's index
shut 0:25f4809c2729 148 int Bresenham(int x1, int y1, int x2, int y2, int cX[], int cY[]){
shut 0:25f4809c2729 149 int aux = 0;
shut 0:25f4809c2729 150 int delta_x(x2 - x1);
shut 0:25f4809c2729 151 // if xR == x2, then it does not matter what we set here
shut 0:25f4809c2729 152 signed char const ix((delta_x > 0) - (delta_x < 0));
shut 0:25f4809c2729 153 delta_x = std::abs(delta_x) << 1;
shut 0:25f4809c2729 154 int delta_y(y2 - y1);
shut 0:25f4809c2729 155 // if yR == y2, then it does not matter what we set here
shut 0:25f4809c2729 156 signed char const iy((delta_y > 0) - (delta_y < 0));
shut 0:25f4809c2729 157 delta_y = std::abs(delta_y) << 1;
shut 0:25f4809c2729 158 cX[aux] = x1, cY[aux] = y1, aux++;
shut 0:25f4809c2729 159 //pc.printf("x = %d | y = %d | xF = %d | yF = %d | aux = %d \n", x1, y1, x2, y2, aux-1);
shut 0:25f4809c2729 160 if (delta_x >= delta_y){
shut 0:25f4809c2729 161 // error may go below zero
shut 0:25f4809c2729 162 int error(delta_y - (delta_x >> 1));
shut 0:25f4809c2729 163 while (x1 != x2)
shut 0:25f4809c2729 164 {
shut 0:25f4809c2729 165 // reduce error, while taking into account the corner case of error == 0
shut 0:25f4809c2729 166 if ((error > 0) || (!error && (ix > 0))){
shut 0:25f4809c2729 167 error -= delta_x;
shut 0:25f4809c2729 168 y1 += iy;
shut 0:25f4809c2729 169 }
shut 0:25f4809c2729 170 error += delta_y;
shut 0:25f4809c2729 171 x1 += ix;
shut 0:25f4809c2729 172 cX[aux] = x1, cY[aux] = y1, aux++;
shut 0:25f4809c2729 173 }
shut 0:25f4809c2729 174 }
shut 0:25f4809c2729 175 else{
shut 0:25f4809c2729 176 // error may go below zero
shut 0:25f4809c2729 177 int error(delta_x - (delta_y >> 1));
shut 0:25f4809c2729 178 while (y1 != y2){
shut 0:25f4809c2729 179 // reduce error, while taking into account the corner case of error == 0
shut 0:25f4809c2729 180 if ((error > 0) || (!error && (iy > 0))){
shut 0:25f4809c2729 181 error -= delta_y;
shut 0:25f4809c2729 182 x1 += ix;
shut 0:25f4809c2729 183 }
shut 0:25f4809c2729 184 error += delta_x;
shut 0:25f4809c2729 185 y1 += iy;
shut 0:25f4809c2729 186 cX[aux] = x1, cY[aux] = y1, aux++;
shut 0:25f4809c2729 187 //pc.printf("x = %d | y = %d | cx = %d | cy = %d | aux = %d \n", x1, y1, cX[aux-1], cY[aux-1], aux-1); //DEBUG
shut 0:25f4809c2729 188 }
shut 0:25f4809c2729 189 }
shut 0:25f4809c2729 190 return aux;
shut 0:25f4809c2729 191 }
shut 0:25f4809c2729 192 void observe(){
shut 0:25f4809c2729 193 float thetaR_deg, readAngle, lDist, lAng;
shut 0:25f4809c2729 194 int xR, yR, xL, yL, xF, yF;
shut 0:25f4809c2729 195 //------------Processing LIDAR readings------------
shut 0:25f4809c2729 196 lDist = data.distance / 10; //mm -> cm
shut 0:25f4809c2729 197 if(lDist == 0 || lDist > 2000) return;
shut 0:25f4809c2729 198 lAng = data.angle;
shut 0:25f4809c2729 199 thetaR_deg = (p[2] * 180.0f) / M_PI;
shut 0:25f4809c2729 200 if(thetaR_deg < 0) thetaR_deg = 360 + thetaR_deg;
shut 0:25f4809c2729 201 readAngle = 270 - lAng + thetaR_deg; //Align LIDAR's frame with robot's one
shut 0:25f4809c2729 202 if(readAngle > 360) readAngle -= 360; // " " " " "
shut 0:25f4809c2729 203 else if(readAngle < 0) readAngle += 360;// " " " " "
shut 0:25f4809c2729 204 //pc.printf("ReadAngle: %f | data_deg: %f | Distance: %f | pos: (%f,%f)\n", readAngle, lAng, lDist, p[0], p[1]); //DEBUG
shut 0:25f4809c2729 205 readAngle = (readAngle * M_PI) /180.0f; // deg -> rads
shut 0:25f4809c2729 206 xL = lDist * cos(readAngle)/5;//X_coord Lidar's reading (cell unit)
shut 0:25f4809c2729 207 yL = lDist * sin(readAngle)/5;//Y_coord Lidar's reading (cell unit)
shut 0:25f4809c2729 208 xR = p[0]/5, yR = p[1]/5; //Robot's coordinates cell
shut 0:25f4809c2729 209 xF = xR + xL;
shut 0:25f4809c2729 210 yF = yR + yL;
shut 0:25f4809c2729 211 //if(xF < 0 || yF < 0 || xF > 80 || yF > 80) return;
shut 0:25f4809c2729 212 npts = Bresenham(xR, yR, xF, yF, cX, cY);
shut 0:25f4809c2729 213 }