SRM / Mbed 2 deprecated LabWork2VFH_valleytry

vaLLEY TRY

Dependencies:   mbed

main.cpp@7:3a755ebe4eaf, 2019-04-05 (annotated)

Committer:
jsobiecki
Date:
Fri Apr 05 14:45:04 2019 +0000
Revision:
7:3a755ebe4eaf
Parent:
6:2cd6ae395c0f
Child:
8:ed59eb8437c4
commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jsobiecki 0:719ea21609f1 1 #include "mbed.h"
jsobiecki 0:719ea21609f1 2 #include "Robot.h"
jsobiecki 0:719ea21609f1 3 #include "math.h"
jsobiecki 4:5a892f5ab5a8 4 #include "ActiveCell.h"
jsobiecki 4:5a892f5ab5a8 5 #include "HistogramCell.h"
jsobiecki 0:719ea21609f1 6 #define M_PI 3.14159265358979323846
jsobiecki 0:719ea21609f1 7 //EXERCICIO 1
jsobiecki 0:719ea21609f1 8 //Luis Cruz N2011164454
jsobiecki 4:5a892f5ab5a8 9 //Jacek Sobecki N2018319609
jsobiecki 0:719ea21609f1 10 Serial pc(SERIAL_TX, SERIAL_RX, 115200);
jsobiecki 0:719ea21609f1 11 DigitalIn button(PC_13);
jsobiecki 0:719ea21609f1 12 void poseEst(float p[], float radius, float enc_res, float b);
jsobiecki 0:719ea21609f1 13 void SpeedLim(float w[]);
jsobiecki 4:5a892f5ab5a8 14 void initializeArrays();
jsobiecki 7:3a755ebe4eaf 15 void calcSectors();
shut 6:2cd6ae395c0f 16 void sumForces();
shut 6:2cd6ae395c0f 17 void updateActive(float xR, float yR);
shut 2:c507076bfd93 18 //int ReadSensors();
jsobiecki 5:1649f59c37de 19 //const int m = 200, n = 200, activeSize = 11;
shut 6:2cd6ae395c0f 20 //histogram size | aSize active region size
shut 6:2cd6ae395c0f 21 const int hSize = 80, aSize = 11;
jsobiecki 4:5a892f5ab5a8 22 ActiveCell activeReg[aSize][aSize];
jsobiecki 4:5a892f5ab5a8 23 HistogramCell histogram[hSize][hSize];
jsobiecki 5:1649f59c37de 24 //Repulsive force sums
shut 6:2cd6ae395c0f 25 float p[3], p_obj[3], p_final[3], fX, fY;
shut 6:2cd6ae395c0f 26 const float Fca=6;/*5*/
jsobiecki 7:3a755ebe4eaf 27
jsobiecki 7:3a755ebe4eaf 28 //VFH
jsobiecki 7:3a755ebe4eaf 29 const int L=2;
jsobiecki 7:3a755ebe4eaf 30 float secVal[30];
jsobiecki 7:3a755ebe4eaf 31 float smooth[30];
jsobiecki 0:719ea21609f1 32 int main(){
jsobiecki 0:719ea21609f1 33
jsobiecki 0:719ea21609f1 34 button.mode(PullUp);
jsobiecki 0:719ea21609f1 35 getCountsAndReset();
jsobiecki 0:719ea21609f1 36 setSpeeds(0, 0);
jsobiecki 4:5a892f5ab5a8 37 initializeArrays();
jsobiecki 0:719ea21609f1 38 while(button==1);
shut 6:2cd6ae395c0f 39 //w[0] = Omega | w[1] = Left | w[2] = Right
jsobiecki 0:719ea21609f1 40 //p[0] = X | p[1] = Y | p[2] = Theta
jsobiecki 0:719ea21609f1 41 //p_obj[0] = X | p_obj[1] = Y | p_obj[2] = Theta
jsobiecki 0:719ea21609f1 42 //b = Distance between wheels, enc_res = Encoder Resolution, v = Calculated speed
jsobiecki 0:719ea21609f1 43 //k_v = Speed gain, k_s = Curvature gain, wratio = Angular speed ratio control command
shut 2:c507076bfd93 44 //Cells dim: 5x5cm |
shut 6:2cd6ae395c0f 45 float w[3], v, theta, theta_error, err, integral = 0.0, k_i = 0.01/*0.02*/;
shut 6:2cd6ae395c0f 46 const float radius = 3.5, b = 13.3, enc_res = 1440, k_v = 8/*7*/,
jsobiecki 7:3a755ebe4eaf 47 k_s = 12/*10*/, sample_time = 0.05, d_stalker = 7.5, k_f = 2.5; /*2.5*/ //VFF
jsobiecki 0:719ea21609f1 48 // ===============================================================================
jsobiecki 0:719ea21609f1 49 // =================================== COORDS ====================================
shut 2:c507076bfd93 50 // ===============================================================================
jsobiecki 0:719ea21609f1 51 //Target coordinates
shut 6:2cd6ae395c0f 52 p_final[0] = 150, p_final[1] = 200, p_final[2] = 0;
shut 6:2cd6ae395c0f 53 //p_obj[0] = 20, p_obj[1] = 20, p_obj[2] = 0;
jsobiecki 0:719ea21609f1 54 //Initial coordinates:
shut 6:2cd6ae395c0f 55 p[0] = 95, p[1] = 20, p[2] = 0;
jsobiecki 0:719ea21609f1 56 // ===============================================================================
jsobiecki 0:719ea21609f1 57 // =================================== EXECUTION =================================
jsobiecki 0:719ea21609f1 58 // ===============================================================================
jsobiecki 0:719ea21609f1 59 while(1){
jsobiecki 0:719ea21609f1 60 getCountsAndReset();
jsobiecki 0:719ea21609f1 61 pc.printf("Speeds: Left=%lf Right=%lf\n", w[1], w[2]);
shut 6:2cd6ae395c0f 62 pc.printf("OBJECTIVE X: %lf OBJECTIVE Y: %lf\n", p_obj[0], p_obj[1]);
shut 6:2cd6ae395c0f 63 pc.printf("Position: X=%lf Y=%lf Theta=%lf\n\n", p[0], p[1], p[2]);
shut 6:2cd6ae395c0f 64 pc.printf("Force (X): X=%lf Force(Y)=%lf\n", fX, fY);
shut 2:c507076bfd93 65
jsobiecki 0:719ea21609f1 66 //Path calculation
shut 2:c507076bfd93 67 poseEst(p, radius, enc_res, b); //Pose estimation
shut 6:2cd6ae395c0f 68 updateActive(p[0], p[1]);
shut 6:2cd6ae395c0f 69 p_obj[0] = p[0]+k_f*fX; // add parameter to relate chosen direction (VFH) to the point nearby of the robot
shut 6:2cd6ae395c0f 70 p_obj[1] = p[1]+k_f*fY;
shut 2:c507076bfd93 71 //Control Law
shut 2:c507076bfd93 72 err = sqrt(pow((p_obj[0]-p[0]),2)+pow((p_obj[1]-p[1]),2)) - d_stalker; //distance to the point
jsobiecki 0:719ea21609f1 73 theta = atan2(p_obj[1]-p[1],p_obj[0]-p[0]);
jsobiecki 0:719ea21609f1 74 theta = atan2(sin(theta),cos(theta));
jsobiecki 0:719ea21609f1 75 p[2] = atan2(sin(p[2]),cos(p[2]));
jsobiecki 0:719ea21609f1 76 theta_error = theta-p[2];
shut 2:c507076bfd93 77 w[0] = k_s*(theta_error); //direction gain
jsobiecki 4:5a892f5ab5a8 78 integral += err;
shut 6:2cd6ae395c0f 79 v = k_v*err+k_i*integral; //Speed calculation
jsobiecki 0:719ea21609f1 80 w[1] = (v-(b/2)*w[0])/radius;
jsobiecki 0:719ea21609f1 81 w[2] = (v+(b/2)*w[0])/radius;
jsobiecki 0:719ea21609f1 82 SpeedLim(w);
shut 6:2cd6ae395c0f 83 //if((fabs(p[0]-p_final[0])+fabs(p[1]-p_final[1])) < 70) k_i = -0.005;
shut 6:2cd6ae395c0f 84 if((fabs(p[0]-p_final[0])+fabs(p[1]-p_final[1])) < 4){
jsobiecki 0:719ea21609f1 85 setSpeeds(0,0);
jsobiecki 0:719ea21609f1 86 }
jsobiecki 0:719ea21609f1 87 else{
jsobiecki 0:719ea21609f1 88 setSpeeds(w[1], w[2]);
jsobiecki 0:719ea21609f1 89 }
jsobiecki 0:719ea21609f1 90 wait(sample_time);
jsobiecki 0:719ea21609f1 91 }
jsobiecki 0:719ea21609f1 92 }
jsobiecki 0:719ea21609f1 93 // ===============================================================================
jsobiecki 0:719ea21609f1 94 // =================================== FUNCTIONS =================================
jsobiecki 0:719ea21609f1 95 // ===============================================================================
jsobiecki 0:719ea21609f1 96 //Pose Estimation function
jsobiecki 0:719ea21609f1 97 void poseEst(float p[], float radius, float enc_res, float b){
jsobiecki 0:719ea21609f1 98 float deltaDl, deltaDr, deltaD, deltaT;
jsobiecki 0:719ea21609f1 99 deltaDl = ((float)countsLeft)*(2.0*M_PI*radius/enc_res);
jsobiecki 0:719ea21609f1 100 deltaDr = ((float)countsRight)*(2.0*M_PI*radius/enc_res);
jsobiecki 0:719ea21609f1 101 deltaD = (deltaDr + deltaDl)/2;
jsobiecki 0:719ea21609f1 102 deltaT = (deltaDr - deltaDl)/b;
jsobiecki 0:719ea21609f1 103 if(fabs(deltaT) == 0){
jsobiecki 0:719ea21609f1 104 p[0] = p[0] + deltaD*cos(p[2]) + deltaT/2;
jsobiecki 0:719ea21609f1 105 p[1] = p[1] + deltaD*sin(p[2]) + deltaT/2;
jsobiecki 0:719ea21609f1 106 return;
jsobiecki 0:719ea21609f1 107 }
jsobiecki 0:719ea21609f1 108 p[0] = p[0] + deltaD*(sin(deltaT/2.0f)/(deltaT/2.0f))*cos(p[2]+deltaT/2.0f);
jsobiecki 0:719ea21609f1 109 p[1] = p[1] + deltaD*(sin(deltaT/2.0f)/(deltaT/2.0f))*sin(p[2]+deltaT/2.0f);
jsobiecki 0:719ea21609f1 110 p[2] = p[2] + deltaT;
jsobiecki 0:719ea21609f1 111 }
jsobiecki 0:719ea21609f1 112 //Speed limiter function
jsobiecki 0:719ea21609f1 113 void SpeedLim(float w[]){
jsobiecki 0:719ea21609f1 114 float wratio;
shut 6:2cd6ae395c0f 115 wratio = fabs(w[2]/w[1]);
jsobiecki 0:719ea21609f1 116 if(w[2] > 150 || w[1] > 150){
jsobiecki 0:719ea21609f1 117 if(wratio < 1){
jsobiecki 0:719ea21609f1 118 w[1] = 150;
jsobiecki 0:719ea21609f1 119 w[2] = w[1]*wratio;
jsobiecki 0:719ea21609f1 120 }
jsobiecki 0:719ea21609f1 121 else if(wratio > 1){
jsobiecki 0:719ea21609f1 122 w[2] = 150;
jsobiecki 0:719ea21609f1 123 w[1] = w[2]/wratio;
jsobiecki 0:719ea21609f1 124 }
jsobiecki 0:719ea21609f1 125 else{
jsobiecki 0:719ea21609f1 126 w[2] = 150;
jsobiecki 0:719ea21609f1 127 w[1] = 150;
jsobiecki 0:719ea21609f1 128 }
jsobiecki 0:719ea21609f1 129 }
jsobiecki 0:719ea21609f1 130 if(w[2] < 50 || w[1] < 50){
jsobiecki 0:719ea21609f1 131 if(wratio < 1){
jsobiecki 0:719ea21609f1 132 w[1] = 50;
jsobiecki 0:719ea21609f1 133 w[2] = w[1]*wratio;
jsobiecki 0:719ea21609f1 134 }
jsobiecki 0:719ea21609f1 135 else if(wratio > 1){
jsobiecki 0:719ea21609f1 136 w[2] = 50;
jsobiecki 0:719ea21609f1 137 w[1] = w[2]/wratio;
jsobiecki 0:719ea21609f1 138 }
jsobiecki 0:719ea21609f1 139 else{
jsobiecki 0:719ea21609f1 140 w[2] = 50;
jsobiecki 0:719ea21609f1 141 w[1] = 50;
jsobiecki 0:719ea21609f1 142 }
jsobiecki 0:719ea21609f1 143 }
jsobiecki 0:719ea21609f1 144 }
jsobiecki 4:5a892f5ab5a8 145
jsobiecki 4:5a892f5ab5a8 146 void initializeArrays() {
jsobiecki 4:5a892f5ab5a8 147 for (int i = 0; i < hSize; i++) {
jsobiecki 4:5a892f5ab5a8 148 for (int j = 0; j < hSize; j++) {
jsobiecki 4:5a892f5ab5a8 149 histogram[i][j].calculate(i, j);
shut 6:2cd6ae395c0f 150 if(i>15 && i<25 && j > 15 && j<25)
shut 6:2cd6ae395c0f 151 histogram[i][j].cellVal=5;
jsobiecki 4:5a892f5ab5a8 152 }
jsobiecki 4:5a892f5ab5a8 153 }
jsobiecki 4:5a892f5ab5a8 154 for (int i = 0; i < aSize; i++) {
jsobiecki 4:5a892f5ab5a8 155 for (int j = 0; j < aSize; j++) {
jsobiecki 4:5a892f5ab5a8 156 activeReg[i][j].calDist(i, j);
jsobiecki 4:5a892f5ab5a8 157 }
jsobiecki 4:5a892f5ab5a8 158 }
jsobiecki 4:5a892f5ab5a8 159 }
jsobiecki 7:3a755ebe4eaf 160
jsobiecki 5:1649f59c37de 161 //every time robot changes position we need to call this function to update active region and calculate forces
jsobiecki 4:5a892f5ab5a8 162 //xR, yR - robots position in coordinates system
shut 6:2cd6ae395c0f 163 void updateActive(float xR, float yR) {
jsobiecki 4:5a892f5ab5a8 164 int idXr = 0;
jsobiecki 4:5a892f5ab5a8 165 int idYr = 0;
jsobiecki 4:5a892f5ab5a8 166 for (int i = 0; i < hSize; i++) {
jsobiecki 4:5a892f5ab5a8 167 for (int j = 0; j < hSize; j++) {
shut 6:2cd6ae395c0f 168 if (xR > histogram[i][j].x - 2.5f && xR < histogram[i][j].x + 2.5f && yR > histogram[i][j].y - 2.5f &&
shut 6:2cd6ae395c0f 169 yR < histogram[i][j].y + 2.5f) {
jsobiecki 4:5a892f5ab5a8 170 idXr = i;
jsobiecki 4:5a892f5ab5a8 171 idYr = j;
jsobiecki 4:5a892f5ab5a8 172 break;
shut 2:c507076bfd93 173 }
shut 2:c507076bfd93 174 }
shut 2:c507076bfd93 175 }
jsobiecki 4:5a892f5ab5a8 176 int m = idXr - aSize / 2;
jsobiecki 4:5a892f5ab5a8 177 for (int k = 0; k < aSize; k++) {
jsobiecki 4:5a892f5ab5a8 178 int n = idYr - aSize / 2;
jsobiecki 4:5a892f5ab5a8 179 for (int l = 0; l < aSize; l++) {
shut 6:2cd6ae395c0f 180 if(m >= 0 && n >= 0 && m < hSize && n < hSize) {
jsobiecki 4:5a892f5ab5a8 181 activeReg[k][l].cellVal = histogram[m][n].cellVal;
jsobiecki 4:5a892f5ab5a8 182 }
jsobiecki 4:5a892f5ab5a8 183 n++;
jsobiecki 4:5a892f5ab5a8 184 }
jsobiecki 4:5a892f5ab5a8 185 m++;
jsobiecki 4:5a892f5ab5a8 186 }
jsobiecki 7:3a755ebe4eaf 187
jsobiecki 7:3a755ebe4eaf 188 for (int i = 0; i < aSize; i++) {
jsobiecki 7:3a755ebe4eaf 189 for (int j = 0; j < aSize; j++) {
jsobiecki 7:3a755ebe4eaf 190 activeReg[i][j].calForce();
jsobiecki 5:1649f59c37de 191 }
jsobiecki 5:1649f59c37de 192 }
jsobiecki 7:3a755ebe4eaf 193 activeReg[5][5].amplitude=0;
jsobiecki 7:3a755ebe4eaf 194 activeReg[5][5].amplitude=0;
jsobiecki 7:3a755ebe4eaf 195
jsobiecki 7:3a755ebe4eaf 196 calcSectors();
jsobiecki 7:3a755ebe4eaf 197 }
jsobiecki 7:3a755ebe4eaf 198 void calcSectors(){
jsobiecki 7:3a755ebe4eaf 199 for (int k = 0; k < 30; ++k) {
jsobiecki 7:3a755ebe4eaf 200 for (int i = 0; i < aSize; ++i) {
jsobiecki 7:3a755ebe4eaf 201 for (int j = 0; j < aSize; ++j) {
jsobiecki 7:3a755ebe4eaf 202 if(activeReg[i][j].sectorK==k)
jsobiecki 7:3a755ebe4eaf 203 secVal[k]+=activeReg[i][j].amplitude;
jsobiecki 7:3a755ebe4eaf 204 }
jsobiecki 7:3a755ebe4eaf 205 }
jsobiecki 7:3a755ebe4eaf 206 }
jsobiecki 7:3a755ebe4eaf 207
jsobiecki 7:3a755ebe4eaf 208 smooth[0]=(secVal[28]+2*secVal[29]+2*secVal[0]+2*secVal[1]+secVal[2])/5;
jsobiecki 7:3a755ebe4eaf 209 smooth[1]=(secVal[29]+2*secVal[0]+2*secVal[1]+2*secVal[2]+secVal[3])/5;
jsobiecki 7:3a755ebe4eaf 210 smooth[28]=(secVal[26]+2*secVal[27]+2*secVal[28]+2*secVal[29]+secVal[0])/5;
jsobiecki 7:3a755ebe4eaf 211 smooth[29]=(secVal[27]+2*secVal[28]+2*secVal[29]+2*secVal[0]+secVal[1])/5;
jsobiecki 7:3a755ebe4eaf 212 for (int i = 2; i < 28; ++i) {
jsobiecki 7:3a755ebe4eaf 213 smooth[i]=(secVal[i-L]+2*secVal[i-L+1]+2*secVal[i]+2*secVal[i+L-1]+secVal[i+L])/5;
jsobiecki 7:3a755ebe4eaf 214 }
shut 2:c507076bfd93 215 }