SRM / Mbed 2 deprecated LabWork2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@3:a0b0e4acf5e9, 2019-03-22 (annotated)

Committer:
shut
Date:
Fri Mar 22 16:00:45 2019 +0000
Revision:
3:a0b0e4acf5e9
Parent:
2:c507076bfd93 
TESTTTTT

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 0:719ea21609f1 4 #define M_PI 3.14159265358979323846
jsobiecki 0:719ea21609f1 5 //EXERCICIO 1
jsobiecki 0:719ea21609f1 6 //Luis Cruz N2011164454
jsobiecki 0:719ea21609f1 7 //Jacek Sobecki
jsobiecki 0:719ea21609f1 8 Serial pc(SERIAL_TX, SERIAL_RX, 115200);
jsobiecki 0:719ea21609f1 9 DigitalIn button(PC_13);
jsobiecki 0:719ea21609f1 10 void poseEst(float p[], float radius, float enc_res, float b);
jsobiecki 0:719ea21609f1 11 void SpeedLim(float w[]);
shut 2:c507076bfd93 12 //int ReadSensors();
shut 3:a0b0e4acf5e9 13 const int n = 200, activeSize = 11;
shut 3:a0b0e4acf5e9 14 void updateHist(int hist[][n], int active[][activeSize],int x,int y);
jsobiecki 0:719ea21609f1 15 int main(){
jsobiecki 0:719ea21609f1 16
jsobiecki 0:719ea21609f1 17 button.mode(PullUp);
jsobiecki 0:719ea21609f1 18 getCountsAndReset();
jsobiecki 0:719ea21609f1 19 setSpeeds(0, 0);
jsobiecki 0:719ea21609f1 20 while(button==1);
jsobiecki 0:719ea21609f1 21
jsobiecki 0:719ea21609f1 22 //w[0] = Omega | w[1] = X | w[2] = Y
jsobiecki 0:719ea21609f1 23 //p[0] = X | p[1] = Y | p[2] = Theta
jsobiecki 0:719ea21609f1 24 //p_obj[0] = X | p_obj[1] = Y | p_obj[2] = Theta
jsobiecki 0:719ea21609f1 25 //b = Distance between wheels, enc_res = Encoder Resolution, v = Calculated speed
jsobiecki 0:719ea21609f1 26 //k_v = Speed gain, k_s = Curvature gain, wratio = Angular speed ratio control command
shut 2:c507076bfd93 27 //Cells dim: 5x5cm |
shut 3:a0b0e4acf5e9 28 const int m = 200;
shut 2:c507076bfd93 29 int hist[m][n] , active[activeSize][activeSize];
shut 2:c507076bfd93 30 float w[3], v, p[3], p_obj[3], theta, theta_error, err, integral = 0.0;
shut 2:c507076bfd93 31 const float radius = 3.5, b = 13.3, enc_res = 1440, k_v = 7,
shut 3:a0b0e4acf5e9 32 k_s = 60, k_i = 0.0, sample_time = 0.05, Fcr = 1.0, d_stalker = 3.0;
jsobiecki 0:719ea21609f1 33 // ===============================================================================
jsobiecki 0:719ea21609f1 34 // =================================== COORDS ====================================
shut 2:c507076bfd93 35 // ===============================================================================
jsobiecki 0:719ea21609f1 36 //Target coordinates
shut 3:a0b0e4acf5e9 37 p_obj[0] = 200, p_obj[1] = 200, p_obj[2] = 0;
jsobiecki 0:719ea21609f1 38 //Initial coordinates:
shut 2:c507076bfd93 39 p[0] = 100, p[1] = 100, p[2] = 0;
jsobiecki 0:719ea21609f1 40 // ===============================================================================
jsobiecki 0:719ea21609f1 41 // =================================== EXECUTION =================================
jsobiecki 0:719ea21609f1 42 // ===============================================================================
jsobiecki 0:719ea21609f1 43 while(1){
jsobiecki 0:719ea21609f1 44 getCountsAndReset();
jsobiecki 0:719ea21609f1 45 pc.printf("Speeds: Left=%lf Right=%lf\n", w[1], w[2]);
shut 3:a0b0e4acf5e9 46 //pc.printf("Odometer: X=%lf Y=%lf Theta=%lf\n", p[0], p[1], p[2]);
shut 3:a0b0e4acf5e9 47 pc.printf("Position: X=%lf Y=%lf Theta=%lf\n\n", p[0], p[1], p[2]);
shut 2:c507076bfd93 48
jsobiecki 0:719ea21609f1 49 //Path calculation
shut 2:c507076bfd93 50 poseEst(p, radius, enc_res, b); //Pose estimation
shut 2:c507076bfd93 51 //Control Law
shut 2:c507076bfd93 52 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 53 theta = atan2(p_obj[1]-p[1],p_obj[0]-p[0]);
jsobiecki 0:719ea21609f1 54 theta = atan2(sin(theta),cos(theta));
jsobiecki 0:719ea21609f1 55 p[2] = atan2(sin(p[2]),cos(p[2]));
jsobiecki 0:719ea21609f1 56 theta_error = theta-p[2];
shut 2:c507076bfd93 57 w[0] = k_s*(theta_error); //direction gain
shut 3:a0b0e4acf5e9 58 integral += err;
shut 2:c507076bfd93 59 v = k_v*err+k_i*integral; //Speed gain
jsobiecki 0:719ea21609f1 60 w[1] = (v-(b/2)*w[0])/radius;
jsobiecki 0:719ea21609f1 61 w[2] = (v+(b/2)*w[0])/radius;
jsobiecki 0:719ea21609f1 62 SpeedLim(w);
shut 3:a0b0e4acf5e9 63 if((fabs(p[0]-p_obj[0])+fabs(p[1]-p_obj[1])) < 5){
shut 3:a0b0e4acf5e9 64 pc.printf("\n YOU REACHED YOUR DESTINATION, THANK YOU FOR CHOOSING EMIRATES\n");
jsobiecki 0:719ea21609f1 65 setSpeeds(0,0);
jsobiecki 0:719ea21609f1 66 }
jsobiecki 0:719ea21609f1 67 else{
jsobiecki 0:719ea21609f1 68 setSpeeds(w[1], w[2]);
jsobiecki 0:719ea21609f1 69 }
jsobiecki 0:719ea21609f1 70 wait(sample_time);
jsobiecki 0:719ea21609f1 71 }
jsobiecki 0:719ea21609f1 72 }
jsobiecki 0:719ea21609f1 73 // ===============================================================================
jsobiecki 0:719ea21609f1 74 // =================================== FUNCTIONS =================================
jsobiecki 0:719ea21609f1 75 // ===============================================================================
jsobiecki 0:719ea21609f1 76 //Pose Estimation function
jsobiecki 0:719ea21609f1 77 void poseEst(float p[], float radius, float enc_res, float b){
jsobiecki 0:719ea21609f1 78 float deltaDl, deltaDr, deltaD, deltaT;
jsobiecki 0:719ea21609f1 79 deltaDl = ((float)countsLeft)*(2.0*M_PI*radius/enc_res);
jsobiecki 0:719ea21609f1 80 deltaDr = ((float)countsRight)*(2.0*M_PI*radius/enc_res);
jsobiecki 0:719ea21609f1 81 deltaD = (deltaDr + deltaDl)/2;
jsobiecki 0:719ea21609f1 82 deltaT = (deltaDr - deltaDl)/b;
jsobiecki 0:719ea21609f1 83 if(fabs(deltaT) == 0){
jsobiecki 0:719ea21609f1 84 p[0] = p[0] + deltaD*cos(p[2]) + deltaT/2;
jsobiecki 0:719ea21609f1 85 p[1] = p[1] + deltaD*sin(p[2]) + deltaT/2;
jsobiecki 0:719ea21609f1 86 return;
jsobiecki 0:719ea21609f1 87 }
jsobiecki 0:719ea21609f1 88 p[0] = p[0] + deltaD*(sin(deltaT/2.0f)/(deltaT/2.0f))*cos(p[2]+deltaT/2.0f);
jsobiecki 0:719ea21609f1 89 p[1] = p[1] + deltaD*(sin(deltaT/2.0f)/(deltaT/2.0f))*sin(p[2]+deltaT/2.0f);
jsobiecki 0:719ea21609f1 90 p[2] = p[2] + deltaT;
jsobiecki 0:719ea21609f1 91 }
jsobiecki 0:719ea21609f1 92 //Speed limiter function
jsobiecki 0:719ea21609f1 93 void SpeedLim(float w[]){
jsobiecki 0:719ea21609f1 94 float wratio;
jsobiecki 0:719ea21609f1 95 wratio = w[2]/w[1];
jsobiecki 0:719ea21609f1 96 if(w[2] > 150 || w[1] > 150){
jsobiecki 0:719ea21609f1 97 if(wratio < 1){
jsobiecki 0:719ea21609f1 98 w[1] = 150;
jsobiecki 0:719ea21609f1 99 w[2] = w[1]*wratio;
jsobiecki 0:719ea21609f1 100 }
jsobiecki 0:719ea21609f1 101 else if(wratio > 1){
jsobiecki 0:719ea21609f1 102 w[2] = 150;
jsobiecki 0:719ea21609f1 103 w[1] = w[2]/wratio;
jsobiecki 0:719ea21609f1 104 }
jsobiecki 0:719ea21609f1 105 else{
jsobiecki 0:719ea21609f1 106 w[2] = 150;
jsobiecki 0:719ea21609f1 107 w[1] = 150;
jsobiecki 0:719ea21609f1 108 }
jsobiecki 0:719ea21609f1 109 }
shut 3:a0b0e4acf5e9 110 if(w[2] < 35 || w[1] < 35){
jsobiecki 0:719ea21609f1 111 if(wratio < 1){
shut 3:a0b0e4acf5e9 112 w[1] = 35;
jsobiecki 0:719ea21609f1 113 w[2] = w[1]*wratio;
jsobiecki 0:719ea21609f1 114 }
jsobiecki 0:719ea21609f1 115 else if(wratio > 1){
shut 3:a0b0e4acf5e9 116 w[2] = 35;
jsobiecki 0:719ea21609f1 117 w[1] = w[2]/wratio;
jsobiecki 0:719ea21609f1 118 }
jsobiecki 0:719ea21609f1 119 else{
shut 3:a0b0e4acf5e9 120 w[2] = 35;
shut 3:a0b0e4acf5e9 121 w[1] = 35;
jsobiecki 0:719ea21609f1 122 }
jsobiecki 0:719ea21609f1 123 }
jsobiecki 0:719ea21609f1 124 }
shut 3:a0b0e4acf5e9 125 void updateHist(int hist[][n], int active[][activeSize],int x,int y){
shut 2:c507076bfd93 126 int aX=0;
shut 2:c507076bfd93 127 for(int i=x-(activeSize/2);i<x+(activeSize/2+1);i++){
shut 2:c507076bfd93 128 int aY=0;
shut 2:c507076bfd93 129 for(int j=y-(activeSize/2);j<y+(activeSize/2+1);j++){
shut 2:c507076bfd93 130 if(i>=0 && j>=0){
shut 2:c507076bfd93 131 hist[i][j]=active[aX][aY];
shut 2:c507076bfd93 132 aY++;
shut 2:c507076bfd93 133 }
shut 2:c507076bfd93 134 }
shut 2:c507076bfd93 135 aX++;
shut 2:c507076bfd93 136 }
shut 2:c507076bfd93 137 }