Raj Patel / Mbed 2 deprecated RoboticMappingControl

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 mbed-rtos Motor LSM9DS1_Library_cal X_NUCLEO_53L0A1

RobotController.cpp@14:0e6b26c1a7c5, 2019-04-25 (annotated)

Committer:
rpatelpj
Date:
Thu Apr 25 03:21:17 2019 +0000
Revision:
14:0e6b26c1a7c5
Parent:
11:531208aca075
Child:
16:5c38e24db603
Added rotation error correction and lidar functionality to RobotController

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rpatelpj 5:77c6821ae418 1 #include "RobotController.h"
rpatelpj 5:77c6821ae418 2
rpatelpj 5:77c6821ae418 3 RobotController::RobotController(PinName leftWheelPwm, PinName leftWheelFwd,
rpatelpj 5:77c6821ae418 4 PinName leftWheelRev, PinName rightWheelPwm, PinName rightWheelFwd,
rpatelpj 11:531208aca075 5 PinName rightWheelRev, PinName leftEncoder, PinName rightEncoder,
rpatelpj 14:0e6b26c1a7c5 6 PinName lidarShdn, PinName sda, PinName scl) :
rpatelpj 5:77c6821ae418 7 leftWheel(leftWheelPwm, leftWheelFwd, leftWheelRev),
rpatelpj 5:77c6821ae418 8 rightWheel(rightWheelPwm, rightWheelFwd, rightWheelRev),
rpatelpj 14:0e6b26c1a7c5 9 _leftEncoder(leftEncoder), _rightEncoder(rightEncoder), _lidarShdn(lidarShdn),
rpatelpj 14:0e6b26c1a7c5 10 imu(sda, scl, 0xD6, 0x3C) {
rpatelpj 14:0e6b26c1a7c5 11 lidarDevice = new DevI2C(sda, scl);
rpatelpj 14:0e6b26c1a7c5 12 lidarBoard = XNucleo53L0A1::instance(lidarDevice, A2, D8, D2);
rpatelpj 14:0e6b26c1a7c5 13 _lidarShdn = 0;
rpatelpj 14:0e6b26c1a7c5 14 wait(0.1);
rpatelpj 14:0e6b26c1a7c5 15 _lidarShdn = 1;
rpatelpj 14:0e6b26c1a7c5 16 wait(0.1);
rpatelpj 14:0e6b26c1a7c5 17 int status = lidarBoard->init_board();
rpatelpj 14:0e6b26c1a7c5 18 while(status) {
rpatelpj 14:0e6b26c1a7c5 19 status = lidarBoard->init_board();
rpatelpj 14:0e6b26c1a7c5 20 }
rpatelpj 14:0e6b26c1a7c5 21 lidarDistance = 0;
rpatelpj 11:531208aca075 22 imu.begin();
rpatelpj 11:531208aca075 23 imu.calibrate();
rpatelpj 11:531208aca075 24 w1 = 0.0;
rpatelpj 11:531208aca075 25 w2 = 0.0;
rpatelpj 11:531208aca075 26 t1 = 0.0;
rpatelpj 11:531208aca075 27 t2 = 0.0;
rpatelpj 11:531208aca075 28 }
rpatelpj 11:531208aca075 29
rpatelpj 11:531208aca075 30 RobotController::~RobotController() {
rpatelpj 11:531208aca075 31 delete &leftWheel;
rpatelpj 11:531208aca075 32 delete &rightWheel;
rpatelpj 11:531208aca075 33 delete &leftEncoder;
rpatelpj 11:531208aca075 34 delete &rightEncoder;
rpatelpj 11:531208aca075 35 delete &imu;
rpatelpj 11:531208aca075 36 delete &t;
rpatelpj 8:bfa4bf23522c 37 }
rpatelpj 8:bfa4bf23522c 38
rpatelpj 8:bfa4bf23522c 39 void RobotController::detectObstacles() {
rpatelpj 8:bfa4bf23522c 40 led = 0b0001;
rpatelpj 8:bfa4bf23522c 41 for (int i = 0; i < 360; i++) {
rpatelpj 14:0e6b26c1a7c5 42 _leftEncoder.reset();
rpatelpj 14:0e6b26c1a7c5 43 _rightEncoder.reset();
rpatelpj 14:0e6b26c1a7c5 44 leftWheel.speed(1.8);
rpatelpj 14:0e6b26c1a7c5 45 rightWheel.speed(-1.8);
rpatelpj 14:0e6b26c1a7c5 46 while((_leftEncoder.read() < 1) && (_rightEncoder.read() < 1));
rpatelpj 8:bfa4bf23522c 47 leftWheel.speed(0);
rpatelpj 8:bfa4bf23522c 48 rightWheel.speed(0);
rpatelpj 14:0e6b26c1a7c5 49 lidarBoard->sensor_centre->get_distance(&lidarDistance);
rpatelpj 14:0e6b26c1a7c5 50 obstacles[i] = (int)(lidarDistance*MMTOIN);
rpatelpj 8:bfa4bf23522c 51 }
rpatelpj 8:bfa4bf23522c 52 led = 0;
rpatelpj 5:77c6821ae418 53 }
rpatelpj 5:77c6821ae418 54
rpatelpj 8:bfa4bf23522c 55 void RobotController::followTrajectory() {
rpatelpj 8:bfa4bf23522c 56 led = 0b1000;
rpatelpj 8:bfa4bf23522c 57 while(!pb);
rpatelpj 8:bfa4bf23522c 58 for (int i = 0; i < trajectoryLength; i = i + 2) {
rpatelpj 14:0e6b26c1a7c5 59 t.reset();
rpatelpj 14:0e6b26c1a7c5 60 yaw = 0.0;
rpatelpj 14:0e6b26c1a7c5 61 w1 = 0.0;
rpatelpj 14:0e6b26c1a7c5 62 w2 = 0.0;
rpatelpj 14:0e6b26c1a7c5 63 t1 = 0.0;
rpatelpj 14:0e6b26c1a7c5 64 t2 = 0.0;
rpatelpj 14:0e6b26c1a7c5 65 int angle = trajectory[i];
rpatelpj 14:0e6b26c1a7c5 66 if ((trajectory[i] >= 0) && (trajectory[i] < 90)) {
rpatelpj 14:0e6b26c1a7c5 67 angle = angle*ROTERRI;
rpatelpj 14:0e6b26c1a7c5 68 } else if (trajectory[i] < 180) {
rpatelpj 14:0e6b26c1a7c5 69 angle = angle*ROTERRII;
rpatelpj 14:0e6b26c1a7c5 70 } else if (trajectory[i] < 360) {
rpatelpj 14:0e6b26c1a7c5 71 angle = angle*ROTERRIII;
rpatelpj 14:0e6b26c1a7c5 72 }
rpatelpj 14:0e6b26c1a7c5 73 t.start();
rpatelpj 8:bfa4bf23522c 74 leftWheel.speed(0.2);
rpatelpj 8:bfa4bf23522c 75 rightWheel.speed(-0.2);
rpatelpj 14:0e6b26c1a7c5 76 while(yaw < angle) {
rpatelpj 14:0e6b26c1a7c5 77 yaw = yaw + (((w2+w1)/2.0)*(t2-t1));
rpatelpj 14:0e6b26c1a7c5 78 while(!imu.gyroAvailable());
rpatelpj 14:0e6b26c1a7c5 79 imu.readGyro();
rpatelpj 14:0e6b26c1a7c5 80 w1 = w2;
rpatelpj 14:0e6b26c1a7c5 81 w2 = imu.calcGyro(imu.gz);
rpatelpj 14:0e6b26c1a7c5 82 t1 = t2;
rpatelpj 14:0e6b26c1a7c5 83 t2 = t.read();
rpatelpj 14:0e6b26c1a7c5 84 }
rpatelpj 8:bfa4bf23522c 85 leftWheel.speed(0);
rpatelpj 8:bfa4bf23522c 86 rightWheel.speed(0);
rpatelpj 14:0e6b26c1a7c5 87 t.stop();
rpatelpj 8:bfa4bf23522c 88 leftEncoder.reset();
rpatelpj 8:bfa4bf23522c 89 rightEncoder.reset();
rpatelpj 14:0e6b26c1a7c5 90 int distance = (int)(trajectory[i + 1]*COUNTPERIN);
rpatelpj 8:bfa4bf23522c 91 leftWheel.speed(0.2);
rpatelpj 8:bfa4bf23522c 92 rightWheel.speed(0.2);
rpatelpj 14:0e6b26c1a7c5 93 while((leftEncoder.read() < distance) && (rightEncoder.read() < distance));
rpatelpj 8:bfa4bf23522c 94 leftWheel.speed(0);
rpatelpj 8:bfa4bf23522c 95 rightWheel.speed(0);
rpatelpj 8:bfa4bf23522c 96 }
rpatelpj 8:bfa4bf23522c 97 delete []trajectory;
rpatelpj 8:bfa4bf23522c 98 led = 0;
rpatelpj 5:77c6821ae418 99 }