SRM / Mbed OS LabWork3Lidar

Dependencies:   BufferedSerial

ActiveCell.h@4:6ebe8982de0e, 2019-05-06 (annotated)

Committer:
jsobiecki
Date:
Mon May 06 17:15:52 2019 +0000
Revision:
4:6ebe8982de0e
Child:
6:1c84602323c8
add lidar

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jsobiecki 4:6ebe8982de0e 1 #ifndef UNTITLED2_ACTIVECELL_H
jsobiecki 4:6ebe8982de0e 2 #define UNTITLED2_ACTIVECELL_H
jsobiecki 4:6ebe8982de0e 3 #include <iostream>
jsobiecki 4:6ebe8982de0e 4 #include <iomanip>
jsobiecki 4:6ebe8982de0e 5 #include <math.h>
jsobiecki 4:6ebe8982de0e 6
jsobiecki 4:6ebe8982de0e 7 class ActiveCell {
jsobiecki 4:6ebe8982de0e 8 public:
jsobiecki 4:6ebe8982de0e 9 //cell dimension = 5x5cm
jsobiecki 4:6ebe8982de0e 10 int cellVal;
jsobiecki 4:6ebe8982de0e 11 float forceX;
jsobiecki 4:6ebe8982de0e 12 float forceY;
jsobiecki 4:6ebe8982de0e 13 float distance;
jsobiecki 4:6ebe8982de0e 14 int xt, yt;
jsobiecki 4:6ebe8982de0e 15 static const float repulsiveForce = 20;
jsobiecki 4:6ebe8982de0e 16 //robots position
jsobiecki 4:6ebe8982de0e 17 int x0;
jsobiecki 4:6ebe8982de0e 18 int y0;
jsobiecki 4:6ebe8982de0e 19 ///////////VFH//////////////
jsobiecki 4:6ebe8982de0e 20 static const int a=70,b=2;
jsobiecki 4:6ebe8982de0e 21 float angle;
jsobiecki 4:6ebe8982de0e 22 float amplitude;
jsobiecki 4:6ebe8982de0e 23 int sectorK;
jsobiecki 4:6ebe8982de0e 24 //resolution - if changing change also secVal in main & calcSectors
jsobiecki 4:6ebe8982de0e 25 static const int res=10;
jsobiecki 4:6ebe8982de0e 26
jsobiecki 4:6ebe8982de0e 27 ActiveCell(){
jsobiecki 4:6ebe8982de0e 28 angle=0;
jsobiecki 4:6ebe8982de0e 29 amplitude=0;
jsobiecki 4:6ebe8982de0e 30 sectorK=0;
jsobiecki 4:6ebe8982de0e 31 cellVal=0;
jsobiecki 4:6ebe8982de0e 32 forceX=0;
jsobiecki 4:6ebe8982de0e 33 forceY=0;
jsobiecki 4:6ebe8982de0e 34 x0=0;
jsobiecki 4:6ebe8982de0e 35 y0=0;
jsobiecki 4:6ebe8982de0e 36 }
jsobiecki 4:6ebe8982de0e 37
jsobiecki 4:6ebe8982de0e 38
jsobiecki 4:6ebe8982de0e 39 void calForce() {
jsobiecki 4:6ebe8982de0e 40 forceX = (repulsiveForce * cellVal / pow(distance, 2)) * (xt - x0) / distance;
jsobiecki 4:6ebe8982de0e 41 forceY = (repulsiveForce * cellVal / pow(distance, 2)) * (yt - y0) / distance;
jsobiecki 4:6ebe8982de0e 42 amplitude = cellVal*cellVal*(a-b*distance);
jsobiecki 4:6ebe8982de0e 43 }
jsobiecki 4:6ebe8982de0e 44 //Calculating distance from the robot
jsobiecki 4:6ebe8982de0e 45 void calDist(int idX, int idY) {
jsobiecki 4:6ebe8982de0e 46 if (idX < 5) {
jsobiecki 4:6ebe8982de0e 47 xt = x0 - (5 - idX) * 5;
jsobiecki 4:6ebe8982de0e 48 } else if (idX == 5) {
jsobiecki 4:6ebe8982de0e 49 xt = x0;
jsobiecki 4:6ebe8982de0e 50 } else {
jsobiecki 4:6ebe8982de0e 51 xt = x0 + (idX - 5) * 5;
jsobiecki 4:6ebe8982de0e 52 }
jsobiecki 4:6ebe8982de0e 53 if (idY > 5) {
jsobiecki 4:6ebe8982de0e 54 yt = y0 - (5 - idY) * 5;
jsobiecki 4:6ebe8982de0e 55 } else if (idY == 5) {
jsobiecki 4:6ebe8982de0e 56 yt = y0;
jsobiecki 4:6ebe8982de0e 57 } else {
jsobiecki 4:6ebe8982de0e 58 yt = y0 + (idY - 5) * 5;
jsobiecki 4:6ebe8982de0e 59 }
jsobiecki 4:6ebe8982de0e 60 distance = sqrt(pow((float)abs(x0 - xt), 2) + pow((float)abs(y0 - yt), 2));
jsobiecki 4:6ebe8982de0e 61
jsobiecki 4:6ebe8982de0e 62 //////angle/////////////
jsobiecki 4:6ebe8982de0e 63 angle = atan2((float)yt-y0,(float)xt-x0)*180/3.14159265358979323846f;
jsobiecki 4:6ebe8982de0e 64 sectorK=angle/res;
jsobiecki 4:6ebe8982de0e 65 if(sectorK<0)
jsobiecki 4:6ebe8982de0e 66 sectorK=36+sectorK;
jsobiecki 4:6ebe8982de0e 67 }
jsobiecki 4:6ebe8982de0e 68 };
jsobiecki 4:6ebe8982de0e 69
jsobiecki 4:6ebe8982de0e 70
jsobiecki 4:6ebe8982de0e 71 #endif //UNTITLED2_ACTIVECELL_H