SRM / Mbed 2 deprecated LabWork2VFH_valleytry

Dependencies:   mbed

main.cpp@5:1649f59c37de, 2019-03-28 (annotated)

Committer:
jsobiecki
Date:
Thu Mar 28 10:59:55 2019 +0000
Revision:
5:1649f59c37de
Parent:
4:5a892f5ab5a8
Child:
6:2cd6ae395c0f
Sum of forces done

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