Potiental Field Robot
Team Members
- Daniel DeBord
- Frederick Erebor
- Trent Morgan
- Jing Xuan Wang
Description
The Potential Field Obstacle Avoiding Robot (PFoar) is designed to be act like a normal remote controlled robot except for when it detects a possible collision. The robot connects to the users phone using the Bluefruit app, the app is used to control the the speed and direction of the robot, as well as a the display onboard the PFoar. The robot is equipped with two sonar devices that implement the potential field. The potential field aids in steering around obstacles by changing the speed at which the robot is turning. With help from this potential field it is nearly impossible for the robot to experience front end collisions.
About Potential Fields
The purpose of a potential field in robotics is to create an artificial zone around the robot where detectable objects act with a virtual force against the robot to affect its steering and speed. In this application two sonar sensors are used to look in front of the robot to detect possible obstacles. If a possible obstruction is detected the mbed simulates a virtual force on the robot’s motor control in the form of a speed vector. The magnitude of the vector is determined by the distance between the robot in the object. This relationship is modeled by (10/(Relative_Distance)+1), wherethe relative distance is the distance between the object and sonar - 254 in mm. The 1/x relationship means that the virtual effect becomes exponentially greater as the robot approaches the object. In practice this allows the robot to steer around obstacles and stay close to walls without touching them.
Features
- Bluetooth controlled Robot
- RGB to indicate distance to obstacle
- Potential Field for obstacle avoidance
- Adjustable Speed control
- LCD displays
- Distance to obstacle
- Speed
- Current user input
Hardware Used
- 1x Mbed NXP LPC1768
- 2x HC-SR04 Sonar
- 1x Adafruit Bluefruit LE UART Friend -BLE
- 1x L298 Dual H-bridge
- 1x RGB LED
- 1x uLCD-144-G2 128 by 128 Smart Color LCD
- 2x Mini DC Motor
- 1x Micro USB adapter
- 1x 5V 2A Battery
- 1x Shadow Robot kit
Hardware Pinouts
mbed LPC1768 | Adafruit BLE |
---|---|
gnd | gnd |
VU(5V) | Vin(3.3-16V) |
nc | RTS |
gnd | CTS |
p27 (Serial RX) | TXO |
p28 (Serial TX) | RXI |
mbed LPC1768 | uLCD Header | uLCD cable |
---|---|---|
5V = VU | 5V | 5V |
gnd | gnd | gnd |
TX = p13 | RX | TX |
RX = p14 | TX | RX |
P15 | Reset | Reset |
mbed LPC1768 | HC-SR04 (Sonar 1) |
---|---|
VU(5V) | VCC |
gnd | gnd |
p9 | trig |
p10 | echo |
mbed LPC1768 | HC-SR04 (Sonar 2) |
---|---|
VU(5V) | VCC |
gnd | gnd |
p11 | trig |
p12 | echo |
mbed LPC1768 | H-Bridge | Motors |
---|---|---|
VU(5V) | VM | |
Vout(3.3V) | VCC | |
gnd | gnd | |
AO1 | Motor 1(+) | |
AO2 | Motor 1(-) | |
BO2 | Motor 2(-) | |
B01 | Motor 2(+) | |
p22 | PwMA | |
p5 | AI2 | |
p6 | AI1 | |
Vout (3.3V) | STBY | |
p8 | BI1 | |
p7 | BI2 | |
p21 | PwMB |
mbed LPC1768 | RBG Led |
---|---|
p24 | Red |
p23 | Green |
nc | Blue |
gnd | gnd |
Source Code
Libraries Used
Import library4DGL-uLCD-SE
Fork of 4DGL lib for uLCD-144-G2. Different command values needed. See https://mbed.org/users/4180_1/notebook/ulcd-144-g2-128-by-128-color-lcd/ for instructions and demo code.
Import librarymbed-rtos
Official mbed Real Time Operating System based on the RTX implementation of the CMSIS-RTOS API open standard.
Import libraryHC_SR04_Ultrasonic_Library
Works with interrupts
Import programMotor_HelloWorld
4180 H-Bridge motor
(The Motor Driver library is used from this program)
Code
main.cpp
#include "mbed.h" #include "Motor.h" #include "rtos.h" #include "ultrasonic.h" #include "math.h" #include "uLCD_4DGL.h" #include <time.h> /* time */ //the display actally has 8 led's, the last one is a dot BusOut myled(LED1,LED2,LED3,LED4); RawSerial tooth(p28,p27); // bluetooth serial ports Motor r_motor(p22, p5, p6); // pwm, fwd, rev Motor l_motor(p21, p7, p8); // pwm, fwd, rev PwmOut red(p24); PwmOut green(p23); Thread bluetooth_thread; Thread motor_thread; Thread sonar_thread; Thread rgb_thread; Thread lcd_thread; Mutex serial_mutex; Mutex usb_mutex; uLCD_4DGL uLCD(p13, p14, p15); //serial tx, serial rx, reset pin //For the motor's speed volatile float l_motor_speed; volatile float r_motor_speed; //Sonar Distances volatile int l_sonar_dis = 0; volatile int r_sonar_dis = 0; volatile float speed_scale = 5.0; //Potential Field Constants float particle_charge = 250.0; float sonar_angle = 45.0*3.14159/180.0; float distance_danger = 254; //The min turning radius, the distance maximum force occurs //LCD Display state volatile int user_input; volatile int LCD_Mode = 1; Mutex LCD_Mutex; //Gets the distance to the nearest obstacle in the view of the left sonar void l_dist(int distance) { l_sonar_dis = distance; //serial_mutex.lock(); //printf("Left Distance: %d \n Right Distance %d \n\n", l_sonar_dis, r_sonar_dis); //serial_mutex.unlock(); } //Gets the distance to the nearest obstacle in the view of the right sonar void r_dist(int distance) { r_sonar_dis = distance; //serial_mutex.lock(); //printf("Left Distance: %d \n Right Distance %d \n\n", l_sonar_dis, r_sonar_dis); //serial_mutex.unlock(); } ultrasonic l_sonar(p11, p12, .1, 1, &l_dist); //Set the trigger pin to p16 and the echo pin to p17 //have updates every .1 seconds and a timeout after 1 //second, and call dist when the distance changes ultrasonic r_sonar(p9, p10, .1, 1, &r_dist); //Set the trigger pin to p18 and the echo pin to p19 //have updates every .1 seconds and a timeout after 1 //second, and call dist when the distance changes //gets the closest distance float getClosest() { float closest; if(l_sonar_dis > r_sonar_dis) { if(r_sonar_dis > 500) { closest = 500; } else { closest = r_sonar_dis; } } else { if(l_sonar_dis > 500) { closest = 500; } else { closest = l_sonar_dis; } } return closest; } // Function for reading bluetooth //Arrows Control Direction, //Button b3 increases speed, b4 decreases it //Button b1 instantly stops the robot //Button b2 changes the state of the LCD display void bluetooth_func() { char bnum=0; char bhit=0; while(1) { if(tooth.readable()) { serial_mutex.lock(); if (tooth.getc()=='!') { if (tooth.getc()=='B') { //button data bnum = tooth.getc(); //button number bhit = tooth.getc(); if(bhit == '0') { continue; } if ((bnum>='1')&&(bnum<='8')) //is a number button 1..8 { // For LEDs if(bnum == '1') { user_input = 1; myled = 0b0001; l_motor_speed = 0.0; r_motor_speed = 0.0; } else if(bnum == '2') { if(LCD_Mode == 3){ LCD_Mode = 1; } else { LCD_Mode ++; } myled = 0b0010; } else if(bnum == '3') { if (speed_scale < 10) { speed_scale++; myled = 0b0100; } } else if(bnum == '4') { if(speed_scale > 2) { speed_scale--; myled = 0b1000; } } // For motors else if(bnum == '5') //up arrow { user_input = 5; l_motor_speed = 0.1*speed_scale; r_motor_speed = 0.1*speed_scale; myled = 0b0001; } else if(bnum == '6') //down arrow { user_input = 6; l_motor_speed = -0.1*speed_scale; r_motor_speed = -0.1*speed_scale; myled = 0b0010; } else if(bnum == '7') //left arrow { user_input = 7; l_motor_speed = 0.7/5.0*speed_scale; r_motor_speed = 0.3/5.0*speed_scale; myled = 0b0100; }else if(bnum == '8') //right arrow { user_input = 8; l_motor_speed = 0.3/5.0*speed_scale; r_motor_speed = 0.7/5.0*speed_scale; myled = 0b1000; } } } } serial_mutex.unlock(); } Thread::wait(200); } } //RGB display changes to red as objects approach, becomes more green as objects // gain distance void rgb_func() { float closest = 0.0; while(1) { closest = getClosest(); red = (-closest+500)/255; green = (closest-254)/255; Thread::wait(50); } } //Controls the motor function, //Controls the speed of the robot void motor_func() { while (1) { float rel_distance_r = (r_sonar_dis-distance_danger); float rel_distance_l = (l_sonar_dis-distance_danger); if(rel_distance_r < 0) { rel_distance_r = 0; } else if(rel_distance_r > 256) { rel_distance_r = 1000000; } if(rel_distance_l < 0) { rel_distance_l = 0; } else if(rel_distance_l > 256) { rel_distance_l = 1000000; } //float r_sonar_force = particle_charge/(rel_distance_r+0.01); //float l_sonar_force = particle_charge/(rel_distance_l+0.01); float resultant_l = -10.0/(rel_distance_r+1); float resultant_r = -10.0/(rel_distance_l+1); l_motor.speed(l_motor_speed+resultant_l); r_motor.speed(r_motor_speed+resultant_r); Thread::wait(10); } } //Controls the sonar thread and measures distances void sonar_func() { while(1) { l_sonar.checkDistance(); //call checkDistance() as much as possible, as this is where the distance gets updated r_sonar.checkDistance(); Thread::wait(20); } } void LCD_func() { float closest; while(1) { switch (LCD_Mode) { case 1: //display speed LCD_Mutex.lock(); uLCD.color(WHITE); uLCD.locate(0, 0); uLCD.printf("Speed \n\rDisplay: "); uLCD.locate(0, 3); uLCD.printf(" "); uLCD.locate(0, 3); if(speed_scale < 5) { uLCD.color(GREEN); } else if(speed_scale < 8) { uLCD.color(GREEN+BLUE); } else { uLCD.color(RED); } uLCD.printf("%2.2f", speed_scale); LCD_Mutex.unlock(); break; case 2: //displays nearest sonar distance closest = getClosest(); LCD_Mutex.lock(); uLCD.color(WHITE); uLCD.locate(0, 0); uLCD.printf("Sonar \n\rDisplay: "); uLCD.locate(0, 3); uLCD.printf(" "); uLCD.locate(0, 3); if(closest < 254) { uLCD.color(RED); } else if(closest < 499) { uLCD.color(GREEN+BLUE); } else { uLCD.color(GREEN); } if(closest >= 500) { uLCD.printf("%3.2f +", closest); } else { uLCD.printf("%3.2f", closest); } LCD_Mutex.unlock(); break; case 3: //displays current user input LCD_Mutex.lock(); uLCD.color(WHITE); uLCD.locate(0, 0); uLCD.printf("Input \n\rDisplay: "); uLCD.locate(0, 3); uLCD.printf(" "); uLCD.locate(0, 3); switch (user_input) { case 1: uLCD.color(RED); uLCD.printf("STOP"); break; case 5: uLCD.color(GREEN); uLCD.printf("FORWARD"); break; case 6: uLCD.color(GREEN); uLCD.printf("REVERSE"); break; case 7: uLCD.color(GREEN); uLCD.printf("LEFT"); break; case 8: uLCD.color(GREEN); uLCD.printf("RIGHT"); break; } LCD_Mutex.unlock(); break; } Thread::wait(500); } } int main() { //Configure uLCD LCD_Mutex.lock(); uLCD.cls(); uLCD.text_width(2); //2X size text uLCD.text_height(2); uLCD.baudrate(3000000); //jack up baud rate to max for fast display LCD_Mutex.unlock(); l_sonar.startUpdates(); r_sonar.startUpdates(); rgb_thread.start(rgb_func); bluetooth_thread.start(bluetooth_func); motor_thread.start(motor_func); sonar_thread.start(sonar_func); lcd_thread.start(LCD_func); while(1) { Thread::wait(5000); } }
Controls
Up Arrow: Move Forward
Down Arrow: Move Backwards
Right Arrow: Turn Right
Left Arrow: Turn Left
1 Buttton: Stop
2 Button: Change LCD
3 Button: Increase Speed
4 Button: Decrease Speed
Demo Video
Photo Gallery
Please log in to post comments.