GT ECE 4180 Lab Team - Raj Madisetti and Arjun Sonti
Dependencies: mbed Servo Motordriver X_NUCLEO_53L0A1 HC_SR04_Ultrasonic_Library
Remote Control Car
Georgia Tech ECE 4180 Embedded Systems Design Final Project
Team Members
Raj Madisetti Arjun Sonti
main.cpp@3:4956cc0efdf3, 2020-11-19 (annotated)
- Committer:
- rmadisetti3
- Date:
- Thu Nov 19 17:23:54 2020 +0000
- Revision:
- 3:4956cc0efdf3
- Parent:
- 2:5c9526ccb055
- Child:
- 4:29839de66eae
Added Autopilot function
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
rmadisetti3 | 0:20a8dfc396cb | 1 | #include "mbed.h" |
rmadisetti3 | 0:20a8dfc396cb | 2 | #include "XNucleo53L0A1.h" |
rmadisetti3 | 0:20a8dfc396cb | 3 | #include "Servo.h" |
rmadisetti3 | 0:20a8dfc396cb | 4 | #include "motordriver.h" |
rmadisetti3 | 0:20a8dfc396cb | 5 | #include <stdio.h> |
rmadisetti3 | 1:1c1ad0c1c260 | 6 | Serial pc(USBTX,USBRX); |
rmadisetti3 | 2:5c9526ccb055 | 7 | Serial blue(p9,p10); |
rmadisetti3 | 1:1c1ad0c1c260 | 8 | DigitalOut shdn(p26); |
rmadisetti3 | 0:20a8dfc396cb | 9 | DigitalOut myled(LED1); |
rmadisetti3 | 0:20a8dfc396cb | 10 | |
rmadisetti3 | 1:1c1ad0c1c260 | 11 | #define VL53L0_I2C_SDA p28 |
rmadisetti3 | 1:1c1ad0c1c260 | 12 | #define VL53L0_I2C_SCL p27 |
rmadisetti3 | 0:20a8dfc396cb | 13 | |
rmadisetti3 | 3:4956cc0efdf3 | 14 | #define AUTOPILOT 10 |
rmadisetti3 | 3:4956cc0efdf3 | 15 | #define FORWARD 1 |
rmadisetti3 | 3:4956cc0efdf3 | 16 | #define REVERSE -1 |
rmadisetti3 | 3:4956cc0efdf3 | 17 | #define STOP 0 |
rmadisetti3 | 3:4956cc0efdf3 | 18 | |
rmadisetti3 | 1:1c1ad0c1c260 | 19 | static XNucleo53L0A1 *board=NULL; |
rmadisetti3 | 0:20a8dfc396cb | 20 | Motor M(p21, p22, p23, 1); // pwm, fwd, rev, can brake |
rmadisetti3 | 1:1c1ad0c1c260 | 21 | Servo S(p24); |
rmadisetti3 | 3:4956cc0efdf3 | 22 | int state = 0; //global variable stop state |
rmadisetti3 | 3:4956cc0efdf3 | 23 | int status; |
rmadisetti3 | 3:4956cc0efdf3 | 24 | uint32_t distanceCenter; |
rmadisetti3 | 3:4956cc0efdf3 | 25 | uint32_t distanceLeft; |
rmadisetti3 | 3:4956cc0efdf3 | 26 | uint32_t distanceRight; |
rmadisetti3 | 3:4956cc0efdf3 | 27 | |
rmadisetti3 | 3:4956cc0efdf3 | 28 | void getNewState() { |
rmadisetti3 | 3:4956cc0efdf3 | 29 | //Logic for AdaFruit App |
rmadisetti3 | 3:4956cc0efdf3 | 30 | char bnum=0; |
rmadisetti3 | 3:4956cc0efdf3 | 31 | char bhit=0; |
rmadisetti3 | 3:4956cc0efdf3 | 32 | if (blue.readable()) { |
rmadisetti3 | 3:4956cc0efdf3 | 33 | if (blue.getc()=='!') { |
rmadisetti3 | 3:4956cc0efdf3 | 34 | if (blue.getc()=='B') { //button data packet |
rmadisetti3 | 3:4956cc0efdf3 | 35 | bnum = blue.getc(); //button number |
rmadisetti3 | 3:4956cc0efdf3 | 36 | bhit = blue.getc(); //1=hit, 0=release |
rmadisetti3 | 3:4956cc0efdf3 | 37 | if (blue.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK? |
rmadisetti3 | 3:4956cc0efdf3 | 38 | myled = bnum - '0'; //current button number will appear on LEDs |
rmadisetti3 | 3:4956cc0efdf3 | 39 | switch (bnum) { |
rmadisetti3 | 3:4956cc0efdf3 | 40 | case '1': //AutoPilot Mode |
rmadisetti3 | 3:4956cc0efdf3 | 41 | if (bhit=='1') { |
rmadisetti3 | 3:4956cc0efdf3 | 42 | state = 10; //autopilot state |
rmadisetti3 | 3:4956cc0efdf3 | 43 | } |
rmadisetti3 | 3:4956cc0efdf3 | 44 | break; |
rmadisetti3 | 3:4956cc0efdf3 | 45 | case '5': //forward |
rmadisetti3 | 3:4956cc0efdf3 | 46 | if (bhit=='1') { |
rmadisetti3 | 3:4956cc0efdf3 | 47 | state = 1; //forward state |
rmadisetti3 | 3:4956cc0efdf3 | 48 | } else { |
rmadisetti3 | 3:4956cc0efdf3 | 49 | state = 0; //stop state |
rmadisetti3 | 3:4956cc0efdf3 | 50 | } |
rmadisetti3 | 3:4956cc0efdf3 | 51 | break; |
rmadisetti3 | 3:4956cc0efdf3 | 52 | case '6': //reverse |
rmadisetti3 | 3:4956cc0efdf3 | 53 | if (bhit=='1') { |
rmadisetti3 | 3:4956cc0efdf3 | 54 | state = -1; //reverse state |
rmadisetti3 | 3:4956cc0efdf3 | 55 | } else { |
rmadisetti3 | 3:4956cc0efdf3 | 56 | state = 0; //stop state |
rmadisetti3 | 3:4956cc0efdf3 | 57 | } |
rmadisetti3 | 3:4956cc0efdf3 | 58 | break; |
rmadisetti3 | 3:4956cc0efdf3 | 59 | case '7': //left |
rmadisetti3 | 3:4956cc0efdf3 | 60 | if (bhit=='1') { |
rmadisetti3 | 3:4956cc0efdf3 | 61 | S = S - 0.5; //turn left |
rmadisetti3 | 3:4956cc0efdf3 | 62 | } |
rmadisetti3 | 3:4956cc0efdf3 | 63 | break; |
rmadisetti3 | 3:4956cc0efdf3 | 64 | case '8': //right |
rmadisetti3 | 3:4956cc0efdf3 | 65 | if (bhit=='1') { |
rmadisetti3 | 3:4956cc0efdf3 | 66 | S = S + 0.5; //turn right |
rmadisetti3 | 3:4956cc0efdf3 | 67 | } |
rmadisetti3 | 3:4956cc0efdf3 | 68 | break; |
rmadisetti3 | 3:4956cc0efdf3 | 69 | } |
rmadisetti3 | 3:4956cc0efdf3 | 70 | } |
rmadisetti3 | 3:4956cc0efdf3 | 71 | } |
rmadisetti3 | 3:4956cc0efdf3 | 72 | } |
rmadisetti3 | 3:4956cc0efdf3 | 73 | } |
rmadisetti3 | 3:4956cc0efdf3 | 74 | } |
rmadisetti3 | 3:4956cc0efdf3 | 75 | |
rmadisetti3 | 3:4956cc0efdf3 | 76 | void autoPilot() { |
rmadisetti3 | 3:4956cc0efdf3 | 77 | if ((distanceCenter >= 250 || distanceCenter == 0)) { |
rmadisetti3 | 3:4956cc0efdf3 | 78 | M.speed(1.0); |
rmadisetti3 | 3:4956cc0efdf3 | 79 | S = 0.5; |
rmadisetti3 | 3:4956cc0efdf3 | 80 | } |
rmadisetti3 | 3:4956cc0efdf3 | 81 | else if (distanceRight > distanceLeft) { //More space on right so turn right |
rmadisetti3 | 3:4956cc0efdf3 | 82 | S = 1.0; |
rmadisetti3 | 3:4956cc0efdf3 | 83 | } |
rmadisetti3 | 3:4956cc0efdf3 | 84 | else { //or turn left |
rmadisetti3 | 3:4956cc0efdf3 | 85 | S = 0; |
rmadisetti3 | 3:4956cc0efdf3 | 86 | } |
rmadisetti3 | 3:4956cc0efdf3 | 87 | } |
rmadisetti3 | 0:20a8dfc396cb | 88 | |
rmadisetti3 | 0:20a8dfc396cb | 89 | |
rmadisetti3 | 0:20a8dfc396cb | 90 | int main() { |
rmadisetti3 | 3:4956cc0efdf3 | 91 | //LIDAR Initialization |
rmadisetti3 | 1:1c1ad0c1c260 | 92 | DevI2C *device_i2c = new DevI2C(VL53L0_I2C_SDA, VL53L0_I2C_SCL); |
rmadisetti3 | 1:1c1ad0c1c260 | 93 | /* creates the 53L0A1 expansion board singleton obj */ |
rmadisetti3 | 1:1c1ad0c1c260 | 94 | board = XNucleo53L0A1::instance(device_i2c, A2, D8, D2); |
rmadisetti3 | 1:1c1ad0c1c260 | 95 | shdn = 0; //must reset sensor for an mbed reset to work |
rmadisetti3 | 1:1c1ad0c1c260 | 96 | wait(0.1); |
rmadisetti3 | 1:1c1ad0c1c260 | 97 | shdn = 1; |
rmadisetti3 | 1:1c1ad0c1c260 | 98 | wait(0.1); |
rmadisetti3 | 1:1c1ad0c1c260 | 99 | /* init the 53L0A1 board with default values */ |
rmadisetti3 | 1:1c1ad0c1c260 | 100 | status = board->init_board(); |
rmadisetti3 | 1:1c1ad0c1c260 | 101 | while (status) { |
rmadisetti3 | 1:1c1ad0c1c260 | 102 | pc.printf("Failed to init board! \r\n"); |
rmadisetti3 | 1:1c1ad0c1c260 | 103 | status = board->init_board(); |
rmadisetti3 | 1:1c1ad0c1c260 | 104 | } |
rmadisetti3 | 1:1c1ad0c1c260 | 105 | |
rmadisetti3 | 3:4956cc0efdf3 | 106 | while(1) { //main loop for motor control |
rmadisetti3 | 3:4956cc0efdf3 | 107 | status = board->sensor_centre->get_distance(&distanceCenter); |
rmadisetti3 | 3:4956cc0efdf3 | 108 | pc.printf("D=%ld mm\r\n", distanceCenter); |
rmadisetti3 | 3:4956cc0efdf3 | 109 | getNewState(); |
rmadisetti3 | 3:4956cc0efdf3 | 110 | if ((distanceCenter >= 250 || distanceCenter == 0) && state == FORWARD) { |
rmadisetti3 | 3:4956cc0efdf3 | 111 | M.speed(1.0); |
rmadisetti3 | 3:4956cc0efdf3 | 112 | } |
rmadisetti3 | 3:4956cc0efdf3 | 113 | else if (state == REVERSE) { |
rmadisetti3 | 3:4956cc0efdf3 | 114 | M.speed(-1.0); |
rmadisetti3 | 3:4956cc0efdf3 | 115 | } |
rmadisetti3 | 3:4956cc0efdf3 | 116 | else if (state == AUTOPILOT) { |
rmadisetti3 | 3:4956cc0efdf3 | 117 | autoPilot(); |
rmadisetti3 | 3:4956cc0efdf3 | 118 | } |
rmadisetti3 | 3:4956cc0efdf3 | 119 | else { |
rmadisetti3 | 3:4956cc0efdf3 | 120 | M.speed(0); |
rmadisetti3 | 2:5c9526ccb055 | 121 | } |
rmadisetti3 | 2:5c9526ccb055 | 122 | } |
rmadisetti3 | 3:4956cc0efdf3 | 123 | |
rmadisetti3 | 0:20a8dfc396cb | 124 | } |