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

Committer:
rpatelpj
Date:
Thu Apr 25 22:13:21 2019 +0000
Revision:
20:d9a373fecd42
Parent:
19:41d0cc14edc5 
Added shortest rotation and conversion constants 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 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();
rpatelpj 20:d9a373fecd42 35 leftWheel.speed(0.205);
rpatelpj 20:d9a373fecd42 36 rightWheel.speed(-0.205);
rpatelpj 20:d9a373fecd42 37 Thread::wait(9);
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 20:d9a373fecd42 41 obstacles[i] = (int)(lidarDistance*MMTOCM);
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 20:d9a373fecd42 58 float leftWheelSpeed = 0.2;
rpatelpj 20:d9a373fecd42 59 float rightWheelSpeed = -0.2;
rpatelpj 16:5c38e24db603 60 if ((trajectory[i] >= 0) && (trajectory[i] <= 90)) {
rpatelpj 14:0e6b26c1a7c5 61 angle = angle*ROTERRI;
rpatelpj 16:5c38e24db603 62 } else if (trajectory[i] <= 180) {
rpatelpj 14:0e6b26c1a7c5 63 angle = angle*ROTERRII;
rpatelpj 20:d9a373fecd42 64 } else if (trajectory[i] <= 360) {
rpatelpj 20:d9a373fecd42 65 angle = (360 - angle)*ROTERRIII;
rpatelpj 20:d9a373fecd42 66 leftWheelSpeed = -leftWheelSpeed;
rpatelpj 20:d9a373fecd42 67 rightWheelSpeed = -rightWheelSpeed;
rpatelpj 14:0e6b26c1a7c5 68 }
rpatelpj 17:e1a5d25218de 69 useImu = true;
rpatelpj 14:0e6b26c1a7c5 70 t.start();
rpatelpj 20:d9a373fecd42 71 leftWheel.speed(leftWheelSpeed);
rpatelpj 20:d9a373fecd42 72 rightWheel.speed(rightWheelSpeed);
rpatelpj 20:d9a373fecd42 73 while(yaw < angle) {
rpatelpj 14:0e6b26c1a7c5 74 yaw = yaw + (((w2+w1)/2.0)*(t2-t1));
rpatelpj 14:0e6b26c1a7c5 75 while(!imu.gyroAvailable());
rpatelpj 14:0e6b26c1a7c5 76 imu.readGyro();
rpatelpj 14:0e6b26c1a7c5 77 w1 = w2;
rpatelpj 20:d9a373fecd42 78 w2 = abs(imu.calcGyro(imu.gz));
rpatelpj 14:0e6b26c1a7c5 79 t1 = t2;
rpatelpj 14:0e6b26c1a7c5 80 t2 = t.read();
rpatelpj 14:0e6b26c1a7c5 81 }
rpatelpj 8:bfa4bf23522c 82 leftWheel.speed(0);
rpatelpj 8:bfa4bf23522c 83 rightWheel.speed(0);
rpatelpj 14:0e6b26c1a7c5 84 t.stop();
rpatelpj 17:e1a5d25218de 85 useImu = false;
rpatelpj 16:5c38e24db603 86 _leftEncoder.reset();
rpatelpj 16:5c38e24db603 87 _rightEncoder.reset();
rpatelpj 20:d9a373fecd42 88 int distance = (int)(trajectory[i + 1] * COUNTTOMM);
rpatelpj 8:bfa4bf23522c 89 leftWheel.speed(0.2);
rpatelpj 8:bfa4bf23522c 90 rightWheel.speed(0.2);
rpatelpj 16:5c38e24db603 91 while((_leftEncoder.read() < distance) && (_rightEncoder.read() < distance));
rpatelpj 8:bfa4bf23522c 92 leftWheel.speed(0);
rpatelpj 8:bfa4bf23522c 93 rightWheel.speed(0);
rpatelpj 8:bfa4bf23522c 94 }
rpatelpj 8:bfa4bf23522c 95 delete []trajectory;
rpatelpj 8:bfa4bf23522c 96 led = 0;
rpatelpj 5:77c6821ae418 97 }