SRM / Mbed OS LabWork3Lidar

dasd

Dependencies:   BufferedSerial

ActiveCell.h@7:5e59f8a011fd, 2019-06-07 (annotated)

Committer:
shut
Date:
Fri Jun 07 15:39:06 2019 +0000
Revision:
7:5e59f8a011fd
Parent:
6:1c84602323c8 
7-6

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
shut 6:1c84602323c8 10 float cellVal;
jsobiecki 4:6ebe8982de0e 11 float forceX;
jsobiecki 4:6ebe8982de0e 12 float forceY;
jsobiecki 4:6ebe8982de0e 13 float distance;
shut 6:1c84602323c8 14 short xt, yt;
shut 6:1c84602323c8 15 static const short repulsiveForce = 20;
jsobiecki 4:6ebe8982de0e 16 //robots position
shut 6:1c84602323c8 17 short x0;
shut 6:1c84602323c8 18 short 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;
shut 6:1c84602323c8 23 short sectorK;
jsobiecki 4:6ebe8982de0e 24 //resolution - if changing change also secVal in main & calcSectors
shut 6:1c84602323c8 25 static const short 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