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@17:e1a5d25218de, 2019-04-25 (annotated)

Committer:
rpatelpj
Date:
Thu Apr 25 05:56:18 2019 +0000
Revision:
17:e1a5d25218de
Parent:
16:5c38e24db603
Child:
18:7ed985584ec7
Include rtos lib and implement lidar in separate thread; Usage note: must remove mbed lib from imu lib to compile

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 17:e1a5d25218de 6 PinName imuSda, PinName imuScl) :
rpatelpj 5:77c6821ae418 7 leftWheel(leftWheelPwm, leftWheelFwd, leftWheelRev),
rpatelpj 5:77c6821ae418 8 rightWheel(rightWheelPwm, rightWheelFwd, rightWheelRev),
rpatelpj 17:e1a5d25218de 9 _leftEncoder(leftEncoder), _rightEncoder(rightEncoder),
rpatelpj 17:e1a5d25218de 10 imu(imuSda, imuScl, 0xD6, 0x3C) {
rpatelpj 11:531208aca075 11 imu.begin();
rpatelpj 11:531208aca075 12 imu.calibrate();
rpatelpj 11:531208aca075 13 w1 = 0.0;
rpatelpj 11:531208aca075 14 w2 = 0.0;
rpatelpj 11:531208aca075 15 t1 = 0.0;
rpatelpj 11:531208aca075 16 t2 = 0.0;
rpatelpj 11:531208aca075 17 }
rpatelpj 11:531208aca075 18
rpatelpj 11:531208aca075 19 RobotController::~RobotController() {
rpatelpj 11:531208aca075 20 delete &leftWheel;
rpatelpj 11:531208aca075 21 delete &rightWheel;
rpatelpj 16:5c38e24db603 22 delete &_leftEncoder;
rpatelpj 16:5c38e24db603 23 delete &_rightEncoder;
rpatelpj 11:531208aca075 24 delete &imu;
rpatelpj 11:531208aca075 25 delete &t;
rpatelpj 8:bfa4bf23522c 26 }
rpatelpj 8:bfa4bf23522c 27
rpatelpj 8:bfa4bf23522c 28 void RobotController::detectObstacles() {
rpatelpj 8:bfa4bf23522c 29 led = 0b0001;
rpatelpj 8:bfa4bf23522c 30 for (int i = 0; i < 360; i++) {
rpatelpj 14:0e6b26c1a7c5 31 _leftEncoder.reset();
rpatelpj 14:0e6b26c1a7c5 32 _rightEncoder.reset();
rpatelpj 16:5c38e24db603 33 leftWheel.speed(0.4);
rpatelpj 16:5c38e24db603 34 rightWheel.speed(-0.4);
rpatelpj 14:0e6b26c1a7c5 35 while((_leftEncoder.read() < 1) && (_rightEncoder.read() < 1));
rpatelpj 8:bfa4bf23522c 36 leftWheel.speed(0);
rpatelpj 8:bfa4bf23522c 37 rightWheel.speed(0);
rpatelpj 14:0e6b26c1a7c5 38 obstacles[i] = (int)(lidarDistance*MMTOIN);
rpatelpj 8:bfa4bf23522c 39 }
rpatelpj 8:bfa4bf23522c 40 led = 0;
rpatelpj 5:77c6821ae418 41 }
rpatelpj 5:77c6821ae418 42
rpatelpj 8:bfa4bf23522c 43 void RobotController::followTrajectory() {
rpatelpj 8:bfa4bf23522c 44 led = 0b1000;
rpatelpj 8:bfa4bf23522c 45 while(!pb);
rpatelpj 8:bfa4bf23522c 46 for (int i = 0; i < trajectoryLength; i = i + 2) {
rpatelpj 14:0e6b26c1a7c5 47 t.reset();
rpatelpj 14:0e6b26c1a7c5 48 yaw = 0.0;
rpatelpj 14:0e6b26c1a7c5 49 w1 = 0.0;
rpatelpj 14:0e6b26c1a7c5 50 w2 = 0.0;
rpatelpj 14:0e6b26c1a7c5 51 t1 = 0.0;
rpatelpj 14:0e6b26c1a7c5 52 t2 = 0.0;
rpatelpj 16:5c38e24db603 53 int angle = trajectory[i] % 360;
rpatelpj 16:5c38e24db603 54 if ((trajectory[i] >= 0) && (trajectory[i] <= 90)) {
rpatelpj 14:0e6b26c1a7c5 55 angle = angle*ROTERRI;
rpatelpj 16:5c38e24db603 56 } else if (trajectory[i] <= 180) {
rpatelpj 14:0e6b26c1a7c5 57 angle = angle*ROTERRII;
rpatelpj 14:0e6b26c1a7c5 58 } else if (trajectory[i] < 360) {
rpatelpj 14:0e6b26c1a7c5 59 angle = angle*ROTERRIII;
rpatelpj 14:0e6b26c1a7c5 60 }
rpatelpj 17:e1a5d25218de 61 useImu = true;
rpatelpj 14:0e6b26c1a7c5 62 t.start();
rpatelpj 8:bfa4bf23522c 63 leftWheel.speed(0.2);
rpatelpj 8:bfa4bf23522c 64 rightWheel.speed(-0.2);
rpatelpj 14:0e6b26c1a7c5 65 while(yaw < angle) {
rpatelpj 14:0e6b26c1a7c5 66 yaw = yaw + (((w2+w1)/2.0)*(t2-t1));
rpatelpj 14:0e6b26c1a7c5 67 while(!imu.gyroAvailable());
rpatelpj 14:0e6b26c1a7c5 68 imu.readGyro();
rpatelpj 14:0e6b26c1a7c5 69 w1 = w2;
rpatelpj 14:0e6b26c1a7c5 70 w2 = imu.calcGyro(imu.gz);
rpatelpj 14:0e6b26c1a7c5 71 t1 = t2;
rpatelpj 14:0e6b26c1a7c5 72 t2 = t.read();
rpatelpj 14:0e6b26c1a7c5 73 }
rpatelpj 8:bfa4bf23522c 74 leftWheel.speed(0);
rpatelpj 8:bfa4bf23522c 75 rightWheel.speed(0);
rpatelpj 14:0e6b26c1a7c5 76 t.stop();
rpatelpj 17:e1a5d25218de 77 useImu = false;
rpatelpj 16:5c38e24db603 78 _leftEncoder.reset();
rpatelpj 16:5c38e24db603 79 _rightEncoder.reset();
rpatelpj 14:0e6b26c1a7c5 80 int distance = (int)(trajectory[i + 1]*COUNTPERIN);
rpatelpj 8:bfa4bf23522c 81 leftWheel.speed(0.2);
rpatelpj 8:bfa4bf23522c 82 rightWheel.speed(0.2);
rpatelpj 16:5c38e24db603 83 while((_leftEncoder.read() < distance) && (_rightEncoder.read() < distance));
rpatelpj 8:bfa4bf23522c 84 leftWheel.speed(0);
rpatelpj 8:bfa4bf23522c 85 rightWheel.speed(0);
rpatelpj 8:bfa4bf23522c 86 }
rpatelpj 8:bfa4bf23522c 87 delete []trajectory;
rpatelpj 8:bfa4bf23522c 88 led = 0;
rpatelpj 5:77c6821ae418 89 }