João Pedro Castilho / Mbed OS DEEC_STM32_for_Romi_and_Rplidar

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.

main.cpp@5:22ad3005bc11, 2021-04-30 (annotated)

Committer:
ccpjboss
Date:
Fri Apr 30 12:23:33 2021 +0100
Revision:
5:22ad3005bc11
Parent:
4:272090bf74fe 
test

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fabiofaria 1:dc87724abce8 1 // Coded by Luís Afonso 11-04-2019
ccpjboss 5:22ad3005bc11 2 #include "BufferedSerial.h"
ccpjboss 5:22ad3005bc11 3 #include "Robot.h"
LuisRA 0:2b691d200d6f 4 #include "mbed.h"
LuisRA 0:2b691d200d6f 5 #include "rplidar.h"
ccpjboss 5:22ad3005bc11 6 #include <cstdio>
LuisRA 0:2b691d200d6f 7
ccpjboss 5:22ad3005bc11 8 BufferedSerial pc(USBTX, USBRX, 115200);
LuisRA 0:2b691d200d6f 9 RPLidar lidar;
LuisRA 0:2b691d200d6f 10 BufferedSerial se_lidar(PA_9, PA_10);
fabiofaria 3:0a718d139ed1 11 PwmOut rplidar_motor(D3);
LuisRA 0:2b691d200d6f 12
ccpjboss 5:22ad3005bc11 13 #define rodaDist 7.0
ccpjboss 5:22ad3005bc11 14 #define eixoDist 14.05
ccpjboss 5:22ad3005bc11 15 #define PI 3.14159265358979323846f
ccpjboss 5:22ad3005bc11 16
ccpjboss 5:22ad3005bc11 17 float distLeft = 0.0f;
ccpjboss 5:22ad3005bc11 18 float distRight = 0.0f;
ccpjboss 5:22ad3005bc11 19 float distCenter = 0.0f;
ccpjboss 5:22ad3005bc11 20 float deltaTheta = 0.0f;
ccpjboss 5:22ad3005bc11 21 double x = 0.0f;
ccpjboss 5:22ad3005bc11 22 double y = 0.0f;
ccpjboss 5:22ad3005bc11 23 double theta = 0.0f;
ccpjboss 5:22ad3005bc11 24 float speedL = 0.0f;
ccpjboss 5:22ad3005bc11 25 float speedR = 0.0f;
LuisRA 0:2b691d200d6f 26
ccpjboss 5:22ad3005bc11 27 float currentTime = 0.0f;
ccpjboss 5:22ad3005bc11 28 float lastTime = 0.0f;
ccpjboss 5:22ad3005bc11 29 float timeStep = 0.0f;
ccpjboss 5:22ad3005bc11 30
ccpjboss 5:22ad3005bc11 31 void updateOdometer(int countsLeft, int countsRight);
ccpjboss 5:22ad3005bc11 32 Timer t;
ccpjboss 5:22ad3005bc11 33 using namespace std::chrono;
ccpjboss 5:22ad3005bc11 34
ccpjboss 5:22ad3005bc11 35 int main() {
ccpjboss 5:22ad3005bc11 36 float odomX, odomY, odomTheta;
ccpjboss 5:22ad3005bc11 37 // Lidar initialization
ccpjboss 5:22ad3005bc11 38 setSpeeds(100, 100);
ccpjboss 5:22ad3005bc11 39
ccpjboss 5:22ad3005bc11 40 while (1) {
ccpjboss 5:22ad3005bc11 41 getCountsAndReset();
ccpjboss 5:22ad3005bc11 42 updateOdometer(countsLeft, countsRight);
LuisRA 0:2b691d200d6f 43
ccpjboss 5:22ad3005bc11 44 printf("X: %f\tY: %f\t T: %f\n", x, y, theta);
ccpjboss 5:22ad3005bc11 45 printf("SpeedL: %f\tSpeedR: %f\n", speedL, speedR);
ccpjboss 5:22ad3005bc11 46 }
ccpjboss 5:22ad3005bc11 47 }
ccpjboss 5:22ad3005bc11 48
ccpjboss 5:22ad3005bc11 49 void updateOdometer(int countsLeft, int countsRight) {
ccpjboss 5:22ad3005bc11 50 currentTime = t.read_us();
ccpjboss 5:22ad3005bc11 51 timeStep = currentTime - lastTime;
ccpjboss 5:22ad3005bc11 52
ccpjboss 5:22ad3005bc11 53 lastTime = currentTime;
ccpjboss 5:22ad3005bc11 54
ccpjboss 5:22ad3005bc11 55 printf("Left: %d\tRight: %d\n", countsLeft, countsRight);
ccpjboss 5:22ad3005bc11 56
ccpjboss 5:22ad3005bc11 57 distLeft = (double)countsLeft * ((rodaDist * PI) / 1440.0);
ccpjboss 5:22ad3005bc11 58 distRight = (double)countsRight * ((rodaDist * PI) / 1440.0);
ccpjboss 5:22ad3005bc11 59 distCenter = (distLeft + distRight) / 2;
ccpjboss 5:22ad3005bc11 60 deltaTheta = (distRight - distLeft) / eixoDist;
ccpjboss 5:22ad3005bc11 61 deltaTheta = atan2(sin(deltaTheta), cos(deltaTheta));
ccpjboss 5:22ad3005bc11 62
ccpjboss 5:22ad3005bc11 63 if (deltaTheta == 0) {
ccpjboss 5:22ad3005bc11 64 x = x + distCenter * cos(theta);
ccpjboss 5:22ad3005bc11 65 y = y + distCenter * sin(theta);
ccpjboss 5:22ad3005bc11 66 } else {
ccpjboss 5:22ad3005bc11 67 x = x + distCenter * sin(theta / 2.0) * cos(theta + deltaTheta / 2.0) /
ccpjboss 5:22ad3005bc11 68 (deltaTheta / 2.0);
ccpjboss 5:22ad3005bc11 69 y = y + distCenter * sin(theta / 2.0) * sin(theta + deltaTheta / 2.0) /
ccpjboss 5:22ad3005bc11 70 (deltaTheta / 2.0);
ccpjboss 5:22ad3005bc11 71 theta += deltaTheta;
ccpjboss 5:22ad3005bc11 72 theta = atan2(sin(theta), cos(theta));
ccpjboss 5:22ad3005bc11 73 }
ccpjboss 5:22ad3005bc11 74
ccpjboss 5:22ad3005bc11 75 speedL = distLeft / float(timeStep / 1E6);
ccpjboss 5:22ad3005bc11 76 speedR = distRight / float(timeStep / 1E6);
fabiofaria 1:dc87724abce8 77 }