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@19:41d0cc14edc5, 2019-04-25 (annotated)

Committer:
abh15
Date:
Thu Apr 25 18:53:13 2019 +0000
Revision:
19:41d0cc14edc5
Parent:
18:7ed985584ec7
Child:
20:d9a373fecd42
Fixed bugs in 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 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;
abh15 19:41d0cc14edc5 30 while(!pb);
abh15 19:41d0cc14edc5 31 Thread::wait(500);
rpatelpj 8:bfa4bf23522c 32 for (int i = 0; i < 360; i++) {
rpatelpj 14:0e6b26c1a7c5 33 _leftEncoder.reset();
rpatelpj 14:0e6b26c1a7c5 34 _rightEncoder.reset();
abh15 19:41d0cc14edc5 35 leftWheel.speed(0.2);
abh15 19:41d0cc14edc5 36 rightWheel.speed(-0.2);
abh15 19:41d0cc14edc5 37 Thread::wait(10);
rpatelpj 14:0e6b26c1a7c5 38 while((_leftEncoder.read() < 1) && (_rightEncoder.read() < 1));
rpatelpj 8:bfa4bf23522c 39 leftWheel.speed(0);
rpatelpj 8:bfa4bf23522c 40 rightWheel.speed(0);
rpatelpj 18:7ed985584ec7 41 obstacles[i] = (int)(lidarDistance / 10);
rpatelpj 8:bfa4bf23522c 42 }
rpatelpj 8:bfa4bf23522c 43 led = 0;
rpatelpj 5:77c6821ae418 44 }
rpatelpj 5:77c6821ae418 45
rpatelpj 8:bfa4bf23522c 46 void RobotController::followTrajectory() {
rpatelpj 8:bfa4bf23522c 47 led = 0b1000;
rpatelpj 8:bfa4bf23522c 48 while(!pb);
abh15 19:41d0cc14edc5 49 Thread::wait(500);
abh15 19:41d0cc14edc5 50 for (int i = 0; i < trajectoryLength * 2; i = i + 2) {
rpatelpj 14:0e6b26c1a7c5 51 t.reset();
rpatelpj 14:0e6b26c1a7c5 52 yaw = 0.0;
rpatelpj 14:0e6b26c1a7c5 53 w1 = 0.0;
rpatelpj 14:0e6b26c1a7c5 54 w2 = 0.0;
rpatelpj 14:0e6b26c1a7c5 55 t1 = 0.0;
rpatelpj 14:0e6b26c1a7c5 56 t2 = 0.0;
rpatelpj 16:5c38e24db603 57 int angle = trajectory[i] % 360;
rpatelpj 16:5c38e24db603 58 if ((trajectory[i] >= 0) && (trajectory[i] <= 90)) {
rpatelpj 14:0e6b26c1a7c5 59 angle = angle*ROTERRI;
rpatelpj 16:5c38e24db603 60 } else if (trajectory[i] <= 180) {
rpatelpj 14:0e6b26c1a7c5 61 angle = angle*ROTERRII;
rpatelpj 14:0e6b26c1a7c5 62 } else if (trajectory[i] < 360) {
rpatelpj 14:0e6b26c1a7c5 63 angle = angle*ROTERRIII;
rpatelpj 14:0e6b26c1a7c5 64 }
rpatelpj 17:e1a5d25218de 65 useImu = true;
rpatelpj 14:0e6b26c1a7c5 66 t.start();
rpatelpj 8:bfa4bf23522c 67 leftWheel.speed(0.2);
rpatelpj 8:bfa4bf23522c 68 rightWheel.speed(-0.2);
rpatelpj 18:7ed985584ec7 69 while(yaw > -angle) {
rpatelpj 14:0e6b26c1a7c5 70 yaw = yaw + (((w2+w1)/2.0)*(t2-t1));
rpatelpj 14:0e6b26c1a7c5 71 while(!imu.gyroAvailable());
rpatelpj 14:0e6b26c1a7c5 72 imu.readGyro();
rpatelpj 14:0e6b26c1a7c5 73 w1 = w2;
rpatelpj 14:0e6b26c1a7c5 74 w2 = imu.calcGyro(imu.gz);
rpatelpj 14:0e6b26c1a7c5 75 t1 = t2;
rpatelpj 14:0e6b26c1a7c5 76 t2 = t.read();
rpatelpj 14:0e6b26c1a7c5 77 }
rpatelpj 8:bfa4bf23522c 78 leftWheel.speed(0);
rpatelpj 8:bfa4bf23522c 79 rightWheel.speed(0);
rpatelpj 14:0e6b26c1a7c5 80 t.stop();
rpatelpj 17:e1a5d25218de 81 useImu = false;
rpatelpj 16:5c38e24db603 82 _leftEncoder.reset();
rpatelpj 16:5c38e24db603 83 _rightEncoder.reset();
abh15 19:41d0cc14edc5 84 int distance = (int)(trajectory[i + 1] * COUNTPERCM / 3);
rpatelpj 8:bfa4bf23522c 85 leftWheel.speed(0.2);
rpatelpj 8:bfa4bf23522c 86 rightWheel.speed(0.2);
rpatelpj 16:5c38e24db603 87 while((_leftEncoder.read() < distance) && (_rightEncoder.read() < distance));
rpatelpj 8:bfa4bf23522c 88 leftWheel.speed(0);
rpatelpj 8:bfa4bf23522c 89 rightWheel.speed(0);
rpatelpj 8:bfa4bf23522c 90 }
rpatelpj 8:bfa4bf23522c 91 delete []trajectory;
rpatelpj 8:bfa4bf23522c 92 led = 0;
rpatelpj 5:77c6821ae418 93 }