#include <Wire.h> #include <ZumoShield.h> // Definir el pin al que está conectado el sensor Sharp const int SHARP_PIN = A0; // Valor umbral para determinar si se ha detectado un obstáculo const int OBSTACLE_THRESHOLD = 700; // #define LOG_SERIAL // write log output to serial port #define LED 13 Pushbutton button(ZUMO_BUTTON); // pushbutton on pin 12 // Accelerometer Settings #define RA_SIZE 3 // number of readings to include in running average of accelerometer readings #define XY_ACCELERATION_THRESHOLD 2400 // for detection of contact (~16000 = magnitude of acceleration due to gravity) // Reflectance Sensor Settings #define NUM_SENSORS 6 unsigned int sensor_values[NUM_SENSORS]; // this might need to be tuned for different lighting conditions, surfaces, etc. #define QTR_THRESHOLD 1500 // microseconds ZumoReflectanceSensorArray sensors(QTR_NO_EMITTER_PIN); // Constantes de velocidad const int MAX_SPEED = 400; const int MIN_SPEED = -400; // Motor Settings ZumoMotors motors; // these might need to be tuned for different motor types #define REVERSE_SPEED 200 // 0 is stopped, 400 is full speed #define TURN_SPEED 200 #define SEARCH_SPEED 200 #define SUSTAINED_SPEED 400 // switches to SUSTAINED_SPEED from FULL_SPEED after FULL_SPEED_DURATION_LIMIT ms #define FULL_SPEED 400 #define STOP_DURATION 100 // ms #define REVERSE_DURATION 200 // ms #define TURN_DURATION 300 // ms #define RIGHT 1 #define LEFT -1 enum ForwardSpeed { SearchSpeed, SustainedSpeed, FullSpeed }; ForwardSpeed _forwardSpeed; // current forward speed setting unsigned long full_speed_start_time; #define FULL_SPEED_DURATION_LIMIT 250 // ms // Sound Effects ZumoBuzzer buzzer; const char sound_effect[] PROGMEM = "O4 T100 V15 L4 MS g12>c12>e12>G6>E12 ML>G2"; // "charge" melody // use V0 to suppress sound effect; v15 for max volume // Timing unsigned long loop_start_time; unsigned long last_turn_time; unsigned long contact_made_time; #define MIN_DELAY_AFTER_TURN 400 // ms = min delay before detecting contact event #define MIN_DELAY_BETWEEN_CONTACTS 1000 // ms = min delay between detecting new contact event // RunningAverage class // based on RunningAverage library for Arduino // source: https://playground.arduino.cc/Main/RunningAverage template <typename T> class RunningAverage { public: RunningAverage(void); RunningAverage(int); ~RunningAverage(); void clear(); void addValue(T); T getAverage() const; void fillValue(T, int); protected: int _size; int _cnt; int _idx; T _sum; T * _ar; static T zero; }; // Accelerometer Class -- extends the ZumoIMU class to support reading and averaging the x-y acceleration // vectors from the accelerometer class Accelerometer : public ZumoIMU { typedef struct acc_data_xy { unsigned long timestamp; int x; int y; float dir; } acc_data_xy; public: Accelerometer() : ra_x(RA_SIZE), ra_y(RA_SIZE) {}; ~Accelerometer() {}; void getLogHeader(void); void readAcceleration(unsigned long timestamp); float len_xy() const; float dir_xy() const; int x_avg(void) const; int y_avg(void) const; long ss_xy_avg(void) const; float dir_xy_avg(void) const; private: acc_data_xy last; RunningAverage<int> ra_x; RunningAverage<int> ra_y; }; Accelerometer acc; boolean in_contact; // set when accelerometer detects contact with opposing robot // forward declaration void setForwardSpeed(ForwardSpeed speed); // Sensor Sharp int readSharpSensor() { return analogRead(SHARP_PIN); } void setup() { // Initialize the Wire library and join the I2C bus as a master Wire.begin(); // Initialize accelerometer acc.init(); acc.enableDefault(); #ifdef LOG_SERIAL Serial.begin(9600); acc.getLogHeader(); #endif randomSeed((unsigned int) millis()); // uncomment if necessary to correct motor directions //motors.flipLeftMotor(true); //motors.flipRightMotor(true); pinMode(LED, HIGH); buzzer.playMode(PLAY_AUTOMATIC); waitForButtonAndCountDown(false); } void waitForButtonAndCountDown(bool restarting) { #ifdef LOG_SERIAL Serial.print(restarting ? "Restarting Countdown" : "Starting Countdown"); Serial.println(); #endif digitalWrite(LED, HIGH); button.waitForButton(); digitalWrite(LED, LOW); // play audible countdown for (int i = 0; i < 3; i++) { delay(1000); buzzer.playNote(NOTE_G(3), 50, 12); } delay(1000); buzzer.playFromProgramSpace(sound_effect); delay(1000); // reset loop variables in_contact = false; // 1 if contact made; 0 if no contact or contact lost contact_made_time = 0; last_turn_time = millis(); // prevents false contact detection on initial acceleration _forwardSpeed = SearchSpeed; full_speed_start_time = 0; } void loop() { // Leer el valor del sensor Sharp int sharpValue = readSharpSensor(); // Verificar si se ha detectado un obstáculo if (sharpValue < OBSTACLE_THRESHOLD) { // Acción: ir a chocar con el obstáculo a máxima velocidad motors.setSpeeds(MAX_SPEED, MAX_SPEED); } else { // Acción predeterminada: buscar motors.setSpeeds(SEARCH_SPEED, SEARCH_SPEED); } if (button.isPressed()) { // if button is pressed, stop and wait for another press to go again motors.setSpeeds(0, 0); button.waitForRelease(); waitForButtonAndCountDown(true); } loop_start_time = millis(); acc.readAcceleration(loop_start_time); sensors.read(sensor_values); if ((_forwardSpeed == FullSpeed) && (loop_start_time - full_speed_start_time > FULL_SPEED_DURATION_LIMIT)) { setForwardSpeed(SustainedSpeed); } if (sensor_values[0] < QTR_THRESHOLD) { // if leftmost sensor detects line, reverse and turn to the right turn(RIGHT, true); } else if (sensor_values[5] < QTR_THRESHOLD) { // if rightmost sensor detects line, reverse and turn to the left turn(LEFT, true); } else // otherwise, go straight { if (check_for_contact()) on_contact_made(); int speed = getForwardSpeed(); motors.setSpeeds(speed, speed); } } // execute turn // direction: RIGHT or LEFT // randomize: to improve searching void turn(char direction, bool randomize) { #ifdef LOG_SERIAL Serial.print("turning ..."); Serial.println(); #endif // assume contact lost on_contact_lost(); static unsigned int duration_increment = TURN_DURATION / 4; // motors.setSpeeds(0,0); // delay(STOP_DURATION); motors.setSpeeds(-REVERSE_SPEED, -REVERSE_SPEED); delay(REVERSE_DURATION); motors.setSpeeds(TURN_SPEED * direction, -TURN_SPEED * direction); delay(randomize ? TURN_DURATION + (random(8) - 2) * duration_increment : TURN_DURATION); int speed = getForwardSpeed(); motors.setSpeeds(speed, speed); last_turn_time = millis(); } void setForwardSpeed(ForwardSpeed speed) { _forwardSpeed = speed; if (speed == FullSpeed) full_speed_start_time = loop_start_time; } int getForwardSpeed() { int speed; switch (_forwardSpeed) { case FullSpeed: speed = FULL_SPEED; break; case SustainedSpeed: speed = SUSTAINED_SPEED; break; default: speed = SEARCH_SPEED; break; } return speed; } // check for contact, but ignore readings immediately after turning or losing contact bool check_for_contact() { static long threshold_squared = (long) XY_ACCELERATION_THRESHOLD * (long) XY_ACCELERATION_THRESHOLD; return (acc.ss_xy_avg() > threshold_squared) && \ (loop_start_time - last_turn_time > MIN_DELAY_AFTER_TURN) && \ (loop_start_time - contact_made_time > MIN_DELAY_BETWEEN_CONTACTS); } // sound horn and accelerate on contact -- fight or flight void on_contact_made() { #ifdef LOG_SERIAL Serial.print("contact made"); Serial.println(); #endif in_contact = true; contact_made_time = loop_start_time; setForwardSpeed(FullSpeed); buzzer.playFromProgramSpace(sound_effect); } // reset forward speed void on_contact_lost() { #ifdef LOG_SERIAL Serial.print("contact lost"); Serial.println(); #endif in_contact = false; setForwardSpeed(SearchSpeed); } // class Accelerometer -- member function definitions void Accelerometer::getLogHeader(void) { Serial.print("millis x y len dir | len_avg dir_avg | avg_len"); Serial.println(); } void Accelerometer::readAcceleration(unsigned long timestamp) { readAcc(); if (a.x == last.x && a.y == last.y) return; last.timestamp = timestamp; last.x = a.x; last.y = a.y; ra_x.addValue(last.x); ra_y.addValue(last.y); #ifdef LOG_SERIAL Serial.print(last.timestamp); Serial.print(" "); Serial.print(last.x); Serial.print(" "); Serial.print(last.y); Serial.print(" "); Serial.print(len_xy()); Serial.print(" "); Serial.print(dir_xy()); Serial.print(" | "); Serial.print(sqrt(static_cast<float>(ss_xy_avg()))); Serial.print(" "); Serial.print(dir_xy_avg()); Serial.println(); #endif } float Accelerometer::len_xy() const { return sqrt(last.x*a.x + last.y*a.y); } float Accelerometer::dir_xy() const { return atan2(last.x, last.y) * 180.0 / M_PI; } int Accelerometer::x_avg(void) const { return ra_x.getAverage(); } int Accelerometer::y_avg(void) const { return ra_y.getAverage(); } long Accelerometer::ss_xy_avg(void) const { long x_avg_long = static_cast<long>(x_avg()); long y_avg_long = static_cast<long>(y_avg()); return x_avg_long*x_avg_long + y_avg_long*y_avg_long; } float Accelerometer::dir_xy_avg(void) const { return atan2(static_cast<float>(x_avg()), static_cast<float>(y_avg())) * 180.0 / M_PI; } // RunningAverage class // based on RunningAverage library for Arduino // source: https://playground.arduino.cc/Main/RunningAverage // author: Rob.Tillart@gmail.com // Released to the public domain template <typename T> T RunningAverage<T>::zero = static_cast<T>(0); template <typename T> RunningAverage<T>::RunningAverage(int n) { _size = n; _ar = (T*) malloc(_size * sizeof(T)); clear(); } template <typename T> RunningAverage<T>::~RunningAverage() { free(_ar); } // resets all counters template <typename T> void RunningAverage<T>::clear() { _cnt = 0; _idx = 0; _sum = zero; for (int i = 0; i< _size; i++) _ar[i] = zero; // needed to keep addValue simple } // adds a new value to the data-set template <typename T> void RunningAverage<T>::addValue(T f) { _sum -= _ar[_idx]; _ar[_idx] = f; _sum += _ar[_idx]; _idx++; if (_idx == _size) _idx = 0; // faster than % if (_cnt < _size) _cnt++; } // returns the average of the data-set added so far template <typename T> T RunningAverage<T>::getAverage() const { if (_cnt == 0) return zero; // NaN ? math.h return _sum / _cnt; } // fill the average with a value // the param number determines how often value is added (weight) // number should preferably be between 1 and size template <typename T> void RunningAverage<T>::fillValue(T value, int number) { clear(); for (int i = 0; i < number; i++) { addValue(value); } } en este codigo arreglar la efectividad de detectar con el sensor sharp
++ void loop() { // Leer el valor del sensor Sharp int sharpValue = readSharpSensor(); // Verificar si se ha detectado un obstáculo if (sharpValue < OBSTACLE_THRESHOLD) { // Acción: ir a chocar con el obstáculo a máxima velocidad motors.setSpeeds(MAX_SPEED, MAX_SPEED); } else { // Acción predeterminada: buscar motors.setSpeeds(SEARCH_SPEED, SEARCH_SPEED); } if (button.isPressed()) { // if button is pressed, stop and wait for another press to go again motors.setSpeeds(0, 0); button.waitForRelease(); waitForButtonAndCountDown(true); } loop_start_time = millis(); acc.readAcceleration(loop_start_time); sensors.read(s
