Hi,
I have included all libraries needed, still got error:
CODE:
#include <Wire.h> // Library for the BNO055
#include <Adafruit_Sensor.h> // Library for the BNO055
#include <Adafruit_BNO055.h> // Library for the BNO055
#include <utility/imumaths.h> // Library for the BNO055
#include <NewPing.h>
#include <SimpleKalmanFilter.h>
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define OLED_RESET 4 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
#define SONAR_NUM 4 // Number of sensors.
#define MAX_DISTANCE 20 // Maximum distance (in cm) to ping.
#define PING_INTERVAL 33 //Looping the pings after 33 microseconds.
unsigned long pingTimer[SONAR_NUM]; // Holds the times when the next ping should happen for each sensor.
unsigned int cm[SONAR_NUM]; // Where the ping distances are stored.
uint8_t currentSensor = 0; // Keeps track of which sensor is active.
NewPing sonar[SONAR_NUM] = { // Sensor object array.
NewPing(2, 3, MAX_DISTANCE), // named as: cm0 Each sensor's trigger pin, echo pin, and max distance to ping. /// named as: cm0 [ULSFL (ultrasonic sensor front left)] Each sensor's trigger pin, echo pin, and max distance to ping.
NewPing(4, 5, MAX_DISTANCE), // named as: cm1 //// NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
NewPing(6, 7, MAX_DISTANCE), // named as: cm2 [ULSL (ultrasonic sensor right)] Each sensor's trigger pin, echo pin, and max distance to ping.
NewPing(8, 9, MAX_DISTANCE), // named as: cm3 [ULSR (ultrasonic sensor right)]
};
int ult_sensorT[] = {2, 4, 6, 8 };
int ult_sensorE[] = {3, 5, 7, 9 };
const uint8_t MOTOR_A1 = 10;
const uint8_t MOTOR_A2 = 11;
const uint8_t MOTOR_B1 = 12;
const uint8_t MOTOR_B2 = 13;
const uint8_t SWITCH = A0;
uint8_t AVOIDANCE_DELAY = 2;
void disableMotors(void);
void enableMotors(void);
void goForward(void);
void goLeft(void);
void goRight(void);
void leftAvoid(void);
void rightAvoid(void);
void setupPins(void);
// Headings
void calculateHeadingError(void);
void correctHeading(void);
// Ultrasonic sensors
void doPingRight(void);
void doPingRightCtr(void);
void doPingLeftCtr(void);
void doPingLeft(void);
void getDistances(void);
// IR sensor
void readIRSensor(void);
// Obstacle avoidance
void avoidObstacles(void);
/*--------Module Variables--------*/
bool crossed_the_tape = false;
bool done = false;
int desired_heading;
int current_heading;
int heading_threshold = 60; // 120 degree cone until stage 2
int heading_error;
int ir_reflect_previous;
int ir_reflect_current;
int threshold = 200;
int distance_right; //// cm3
int distance_rightctr; ///// cm2
int distance_leftctr; ///// cm1
int distance_left;///// cm0
int limit = 9; // Inches, try 5-10 depending on battery strength
bool right_or_left = false; // Switch
int sswitch; //// state of switch
// Keep track of the time in milliseconds
unsigned long start_time;
unsigned long time_elapsed_threshold = 2000;
uint8_t oldSensorReading[3]; //Store last valid value of the sensors.
Adafruit_BNO055 bno = Adafruit_BNO055(55);
void setup() {
Serial.begin(9600);
Serial.println("SETUP");
Serial.print("File : "), Serial.println(__FILE__);
Serial.print("Date : "), Serial.println(__DATE__);
display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS);
enableMotors();
ir_reflect_previous = digitalRead(SWITCH);
ir_reflect_current = ir_reflect_previous; //// for M1:
pinMode(MOTOR_A1, OUTPUT);
pinMode(MOTOR_A2, OUTPUT);
pinMode(MOTOR_B1, OUTPUT);
pinMode(MOTOR_B2, OUTPUT);
for (int i = 0; i < 4; i++) // use for Buttons, limt switches, Uls sensors pin setup
{
pinMode(ult_sensorT[i], OUTPUT); ////
digitalWrite(ult_sensorT[i], LOW);
}
for (int i = 0; i < 4; i++) // use for Buttons, limt switches, Uls sensors pin setup
{
pinMode(ult_sensorE[i], INPUT_PULLUP); ////too slow, changed : INPUT -- INPUT_PULLUP,
digitalWrite(ult_sensorE[i], LOW);
}
}
void AOACar_loop() {
Serial.println("switch = ");
sswitch = digitalRead (SWITCH);
Serial.println(SWITCH);
////// Ultrasonic();
// Stage 0 - before the robot enters the
// multi-obstacle environment
while (!crossed_the_tape) {
delay(50);
// Read desired heading
sensors_event_t event;
bno.getEvent(&event);
desired_heading = event.orientation.x;
goForward();
readIRSensor();
}
crossed_the_tape = false;
// Stage 1
while (!crossed_the_tape) {
// Read all four HC-SR04 sensors
getDistances();
// Avoid any obstacle along the way
avoidObstacles();
}
heading_threshold = 10;
// Capture the time
start_time = millis();
// Stage 2
while (!done) {
calculateHeadingError();
// Correct the heading if needed
if (abs(heading_error) <= abs(heading_threshold)) {
goForward();
}
else {
correctHeading();
goForward();
}
// Check to see if we are done
if (((millis()) - start_time) > time_elapsed_threshold) { //// beyonged a certain time, stop the car.
done = true;
}
}
while (done) {
disableMotors();
delay(1000);
}
}
void avoidObstacles() {
// Avoid any objects
if ((distance_leftctr < limit) && (distance_rightctr < limit)) {
// Switch back and forth
if (right_or_left) {
rightAvoid();
}
else {
leftAvoid();
}
right_or_left = !(right_or_left);
}
else if ((distance_left < limit) || (distance_leftctr < limit)) {
rightAvoid();
}
else if ((distance_right < limit) || (distance_rightctr < limit)) {
leftAvoid();
}
else {
calculateHeadingError();
// Correct the heading if needed
if (abs(heading_error) <= abs(heading_threshold)) {
goForward();
}
else {
correctHeading();
goForward();
}
// Check to see if we have crossed the tape
readIRSensor();
delay(50);
}
}
void calculateHeadingError() {
// Read the current heading
sensors_event_t event;
bno.getEvent(&event);
current_heading = event.orientation.x;
// Calculate the heading error
heading_error = current_heading - desired_heading;
if (heading_error > 180) {
heading_error -= 360;
}
if (heading_error < -180) {
heading_error += 360;
}
}
void correctHeading() {
// Turn the vehicle until it is facing the correct
// direction
if (heading_error < -heading_threshold) {
while (heading_error < -heading_threshold) {
goRight();
delay(4);
calculateHeadingError();
}
}
else {
while (heading_error > heading_threshold) {
goLeft();
delay(4);
calculateHeadingError();
}
}
}
void getDistances() {
// Take distance readings on the HC-SR04
doPingRight();
doPingLeftCtr();
doPingRightCtr();
doPingLeft();
}
void goForward() {
digitalWrite(MOTOR_A1, LOW);
digitalWrite(MOTOR_A2, HIGH);
digitalWrite(MOTOR_B1, LOW);
digitalWrite (MOTOR_B2, HIGH);
}
void goLeft() {
digitalWrite(MOTOR_A1, LOW);
digitalWrite(MOTOR_A2, HIGH);
digitalWrite(MOTOR_B1, HIGH);
digitalWrite (MOTOR_B2, LOW);
}
void goRight() {
digitalWrite(MOTOR_A1, HIGH);
digitalWrite(MOTOR_A2, LOW);
digitalWrite(MOTOR_B1, LOW);
digitalWrite (MOTOR_B2, HIGH);
}
void leftAvoid() {
// Go to the left when an object is detected
// on the right-side of the vehicle
while ((distance_right < limit) || (distance_rightctr < limit)) {
goLeft();
delay(4);
doPingRight();
doPingRightCtr();
}
goForward();
for (int i = 0; i < AVOIDANCE_DELAY; i++) {
// Read the reflectance sensor
//// ir_reflect_current = analogRead(IR_SENSOR); //// for M1:
// Check to see if we have crossed over the reflective tape
//// if ((ir_reflect_previous - ir_reflect_current) >= threshold) {
if (digitalRead(SWITCH) == HIGH) {
//// ir_reflect_previous = digitalRead(SWITCH);
// Update if we crossed the tape
crossed_the_tape = true; //// first
break;
}
// Update the previous reading
ir_reflect_previous = ir_reflect_current;
delay(50);
}
}
void readIRSensor() {
// Read the reflectance sensor
//// ir_reflect_current = analogRead(IR_SENSOR);
ir_reflect_current = digitalRead(SWITCH);
// Check to see if we have crossed over the reflective tape
if (digitalRead(SWITCH) == 1) {
// Update if we crossed the tape
crossed_the_tape = true; //// second
}
// Update the previous reading
ir_reflect_previous = ir_reflect_current;
}
void rightAvoid() {
// Go to the right when an object is detected
// on the left-side of the vehicle
while ((distance_left < limit) || (distance_leftctr < limit)) {
goRight();
delay(4);
doPingLeft();
doPingLeftCtr();
}
goForward();
for (int i = 0; i < AVOIDANCE_DELAY; i++) {
// Read the reflectance sensor
ir_reflect_current = digitalRead(SWITCH);
// Check to see if we have crossed over the reflective tape
if (digitalRead(SWITCH) == 1 ) {
// Update if we crossed the tape
crossed_the_tape = true; //// third
break;
}
// Update the previous reading
ir_reflect_previous = ir_reflect_current;
delay(50);
}
}
void Ultrasonic() {
for (uint8_t i = 0; i < SONAR_NUM; i++) { // Loop through all the sensors.
if (millis() >= pingTimer[i]) { // Is it this sensor's time to ping?
pingTimer[i] += PING_INTERVAL * SONAR_NUM; // Set next time this sensor will be pinged.
if (i == 0 && currentSensor == SONAR_NUM - 1) oneSensorCycle(); // Sensor ping cycle complete, do something with the results.
sonar[currentSensor].timer_stop(); // Make sure previous timer is canceled before starting a new ping (insurance).
currentSensor = i; // Sensor being accessed.
cm[currentSensor] = 0; // Make distance zero in case there's no ping echo for this sensor.
sonar[currentSensor].ping_timer(echoCheck); // Do the ping (processing continues, interrupt will call echoCheck to look for echo).
}
}
// The rest of your code would go here.
}
void echoCheck() { // If ping received, set the sensor distance to array.
if (sonar[currentSensor].check_timer())
cm[currentSensor] = sonar[currentSensor].ping_result / US_ROUNDTRIP_CM;
}
void setupPins() {
pinMode(MOTOR_A1, OUTPUT);
pinMode(MOTOR_A2, OUTPUT);
///// pinMode(ENABLE_B, OUTPUT);
pinMode(MOTOR_B1, OUTPUT);
pinMode(MOTOR_B2, OUTPUT);
}
void oneSensorCycle() {
distance_left = returnLastValidRead(0, cm[0]); ///// WAS: leftSensor = returnLastValidRead(0, cm[0]);
distance_leftctr = returnLastValidRead(1, cm[1]); ///// WAS: centerSensor = returnLastValidRead(1, cm[1]);
distance_rightctr = returnLastValidRead(2, cm[2]); ///// WAS: centerSensor = returnLastValidRead(1, cm[1]);
distance_right = returnLastValidRead(3, cm[3]); ///// WAS: rightSensor = returnLastValidRead(2, cm[2]);
}
//If sensor value is 0, then return the last stored value different than 0.
int returnLastValidRead(uint8_t sensorArray, uint8_t cm) {
if (cm != 0) {
return oldSensorReading[sensorArray] = cm;
} else {
return oldSensorReading[sensorArray];
}
}
ERROR:
Arduino: 1.8.3 (Windows 7), Board: "Arduino/Genuino Uno"
C:\Users\HUA~1.DEL\AppData\Local\Temp\cc8V1mHq.ltrans1.ltrans.o: In function `setup':
C:\Users\HUA.DELLV-PC\Documents\Arduino\AOACar_M1_post/AOACar_M1_post.ino:119: undefined reference to `enableMotors()'
C:\Users\HUA~1.DEL\AppData\Local\Temp\cc8V1mHq.ltrans1.ltrans.o: In function `main':
E:\ENGINEERING\DIY\Electronic\ARDUINO\arduino-1.8.3-windows\arduino-1.8.3\hardware\arduino\avr\cores\arduino/main.cpp:46: undefined reference to `loop'
collect2.exe: error: ld returned 1 exit status
exit status 1
Error compiling for board Arduino/Genuino Uno.
This report would have more information with
"Show verbose output during compilation"
option enabled in File -> Preferences.