SRM / Mbed 2 deprecated LabWork2VFH_valleytry

vaLLEY TRY

Dependencies:   mbed

ActiveCell.h@9:699054d8510b, 2019-04-08 (annotated)

Committer:
shut
Date:
Mon Apr 08 21:25:14 2019 +0000
Revision:
9:699054d8510b
Parent:
7:3a755ebe4eaf
Child:
10:32c65de8ff37
qqqqq

Who changed what in which revision?

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