Hello!
I am an EE student working on a project with a final goal of building a "cane" for the visually impaired that alerts them when they near obstacles in the forward left, right, or center. I've been having trouble getting my code to function correctly, it complies and loads so I know it's my logic. (I don't have much experience with Arduino.) Would you be able to take a look at it to see if any major issues pop out? I have attached the code and a screenshot of the serial monitor. I tried to follow the forum rules, so I apologize if I missed anything.
Materials
RPLidar A1 Sensor
Arduino Uno
Piezo Buzzer (resistor into anode)
Blue LED, Green LED, Yellow LED (resistors into anodes)
Simple vibrating motor
Intended Function
Each LED represents an angular range (Blue: 0-120, Green: 120-240, Yellow: 240-360). The sensor spins 360 deg clockwise continuously and should measure distance to the nearest object. It should also record the angle. If the object is within a specified distance range, the corresponding LED should turn on. If the object is too close, the vibrating motor and buzzer should come on until the condition is no longer met. Eventually, I want to be able to limit input data to a 90 or 120 deg range in front of the cane. 120 deg will be divided into three ranges (left, right, center 40 deg each). There will be a T-shaped handle with a vibrating motor on each end (of the handle). If there is an obstacle within a specified distance range within the left angle range, the motor on the left side of the handle will vibrate and the cane will beep as the person gets closer. If the obstacle is in the center range, both vibe motors will vibrate, and if the object is on the right, the right motor vibrates. Since I cannot limit the rotation of the sensor, I would build a blinder that would surround the rear and sides.
Problem
Every time I run it, the sensor outputs dist and angle, but regardless of where the object is, the values don't seem to make sense. The LEDs and vibe motor also don't do anything. I have attached a screenshot of the data in my serial monitor as well. I apologize, I know this is a lot, but I tried to keep it neat and I would really appreciate any help.
Thank You!
#include <RPLidar.h>
RPLidar lidar;
//defined all the outputs
#define RPLIDAR_MOTOR 3 //PWM motor speed controll
#define LED_B 9 //blue led
#define LED_G 10 //green led
#define LED_Y 11 //yellow led
#define VIBE1 5 //vibrating motor
#define BUZZ 6 //buzzer speaker
int sound = 250;
void setup() {
// put your setup code here, to run once:
lidar.begin(Serial); //connect to the sensor
pinMode(RPLIDAR_MOTOR, OUTPUT);
pinMode(LED_B, OUTPUT);
pinMode(LED_G, OUTPUT);
pinMode(LED_Y, OUTPUT);
pinMode(VIBE1, OUTPUT);
pinMode(BUZZ, OUTPUT);
//start with all leds off and vibe motor off
digitalWrite(LED_B, LOW);
digitalWrite(LED_G, LOW);
digitalWrite(LED_Y, LOW);
analogWrite(VIBE1, 0);
}
//define the minimum distance and set angle to zero at that point
float minDistance = 100000;
float angleAtMinDist = 0;
float maxDistance = 200000;
int j = 0;
void loop() {
// put your main code here, to run repeatedly:
if (IS_OK(lidar.waitPoint())) //wait for sensor to respond
{
float distance = lidar.getCurrentPoint().distance;
float angle = lidar.getCurrentPoint().angle;
if (lidar.getCurrentPoint().startBit) //start measurement
{
j++;
}
Serial.print("Distance in mm: ");
Serial.println(distance);
Serial.print("\nAngle in deg: ");
Serial.println(angle);
if (j > 2)
{
if (distance > 300 && distance < 15000) //if the distance of object is in this range, execute loop
{
sound = 0; //no sound if obstacle is within acceptable range
j = 0;
if (angle > 0 && angle < 120) { //blue light if object is between 0 and 120 degrees
digitalWrite(LED_B, HIGH);
Serial.println("Object between 0 and 120 deg");
}
// else {
// digitalWrite(LED_B, LOW);
// }
else if (angle > 120 && angle < 240) { //green light if object is between 120 and 240 degrees
digitalWrite(LED_G, HIGH);
Serial.println("Object between 120 and 240 deg");
}
// else {
// digitalWrite(LED_G, LOW);
// }
else if (angle > 240 && angle <= 360) { //yellow light if object is between 240 and 360 degrees
digitalWrite(LED_Y, HIGH);
Serial.println("Object between 240 and 360 deg");
}
else { //no led light if input is outside range
digitalWrite(LED_Y, LOW);
digitalWrite(LED_G, LOW);
digitalWrite(LED_B, LOW);
}
}
else if (distance < 300) //if object is too close, buzzer and vibe activate
{
sound = 250;
analogWrite(VIBE1, 200);
}
minDistance = 100000;
angleAtMinDist = 0;
}
else {
if (distance > 0 && distance < minDistance) {
minDistance = distance;
angleAtMinDist = angle;
}
}
}
else {
analogWrite(RPLIDAR_MOTOR, 0);
// try to detect RPLIDAR...
rplidar_response_device_info_t info;
if (IS_OK(lidar.getDeviceInfo(info, 100))) {
//detected...
lidar.startScan();
analogWrite(RPLIDAR_MOTOR, 255);
delay(1000);
}
}
}
