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@11:531208aca075, 2019-04-23 (annotated)

Committer:
rpatelpj
Date:
Tue Apr 23 19:56:39 2019 +0000
Revision:
11:531208aca075
Parent:
8:bfa4bf23522c
Child:
14:0e6b26c1a7c5
Remove HALLFX_ENCODER lib, create and implement Encoder class, include Imu lib, implement RobotController angular position tracking and destructor

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 11:531208aca075 6 PinName imuSda, PinName imuScl) :
rpatelpj 5:77c6821ae418 7 leftWheel(leftWheelPwm, leftWheelFwd, leftWheelRev),
rpatelpj 5:77c6821ae418 8 rightWheel(rightWheelPwm, rightWheelFwd, rightWheelRev),
rpatelpj 11:531208aca075 9 leftEncoder(leftEncoder), rightEncoder(rightEncoder),
rpatelpj 11:531208aca075 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 11:531208aca075 22 delete &leftEncoder;
rpatelpj 11:531208aca075 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 8:bfa4bf23522c 31 leftEncoder.reset();
rpatelpj 8:bfa4bf23522c 32 rightEncoder.reset();
rpatelpj 11:531208aca075 33 t.start();
rpatelpj 8:bfa4bf23522c 34 leftWheel.speed(0.2);
rpatelpj 8:bfa4bf23522c 35 rightWheel.speed(-0.2);
rpatelpj 11:531208aca075 36 // while((leftEncoder.read() < COUNTPERDEG) && (rightEncoder.read() < COUNTPERDEG));
rpatelpj 11:531208aca075 37 while((((w2+w1)/2.0)*(t2-t1)) < 1.0) {
rpatelpj 11:531208aca075 38 while(!imu.gyroAvailable());
rpatelpj 11:531208aca075 39 imu.readGyro();
rpatelpj 11:531208aca075 40 w1 = w2;
rpatelpj 11:531208aca075 41 w2 = imu.calcGyro(imu.gz);
rpatelpj 11:531208aca075 42 t1 = t2;
rpatelpj 11:531208aca075 43 t2 = t.read();
rpatelpj 11:531208aca075 44 }
rpatelpj 11:531208aca075 45 t.stop();
rpatelpj 8:bfa4bf23522c 46 leftWheel.speed(0);
rpatelpj 8:bfa4bf23522c 47 rightWheel.speed(0);
rpatelpj 8:bfa4bf23522c 48 obstacles[i] = 0; // TODO ping lidar
rpatelpj 8:bfa4bf23522c 49 }
rpatelpj 8:bfa4bf23522c 50 led = 0;
rpatelpj 5:77c6821ae418 51 }
rpatelpj 5:77c6821ae418 52
rpatelpj 8:bfa4bf23522c 53 void RobotController::followTrajectory() {
rpatelpj 8:bfa4bf23522c 54 led = 0b1000;
rpatelpj 8:bfa4bf23522c 55 while(!pb);
rpatelpj 8:bfa4bf23522c 56 for (int i = 0; i < trajectoryLength; i = i + 2) {
rpatelpj 8:bfa4bf23522c 57 leftEncoder.reset();
rpatelpj 8:bfa4bf23522c 58 rightEncoder.reset();
rpatelpj 8:bfa4bf23522c 59 leftWheel.speed(0.2);
rpatelpj 8:bfa4bf23522c 60 rightWheel.speed(-0.2);
rpatelpj 8:bfa4bf23522c 61 int angleCount = (int)(trajectory[i]*COUNTPERDEG);
rpatelpj 8:bfa4bf23522c 62 while((leftEncoder.read() < angleCount) && (rightEncoder.read() < angleCount));
rpatelpj 8:bfa4bf23522c 63 leftWheel.speed(0);
rpatelpj 8:bfa4bf23522c 64 rightWheel.speed(0);
rpatelpj 8:bfa4bf23522c 65 leftEncoder.reset();
rpatelpj 8:bfa4bf23522c 66 rightEncoder.reset();
rpatelpj 8:bfa4bf23522c 67 int distanceCount = (int)(trajectory[i + 1]*COUNTPERIN);
rpatelpj 8:bfa4bf23522c 68 leftWheel.speed(0.2);
rpatelpj 8:bfa4bf23522c 69 rightWheel.speed(0.2);
rpatelpj 8:bfa4bf23522c 70 pc.putc(distanceCount);
rpatelpj 8:bfa4bf23522c 71 while((leftEncoder.read() < distanceCount) && (rightEncoder.read() < distanceCount));
rpatelpj 8:bfa4bf23522c 72 leftWheel.speed(0);
rpatelpj 8:bfa4bf23522c 73 rightWheel.speed(0);
rpatelpj 8:bfa4bf23522c 74 }
rpatelpj 8:bfa4bf23522c 75 delete []trajectory;
rpatelpj 8:bfa4bf23522c 76 led = 0;
rpatelpj 5:77c6821ae418 77 }