From 66f80908e3138d7a346a51e3bc014a4ae2cdac6a Mon Sep 17 00:00:00 2001 From: Lucas Costa Date: Mon, 8 May 2023 09:41:41 -0300 Subject: [PATCH] Update robo_limpeza.ino --- robo_limpeza/robo_limpeza.ino | 174 ++++++++++++++++++++++++---------- 1 file changed, 125 insertions(+), 49 deletions(-) diff --git a/robo_limpeza/robo_limpeza.ino b/robo_limpeza/robo_limpeza.ino index 61eb9c3..d898ae9 100644 --- a/robo_limpeza/robo_limpeza.ino +++ b/robo_limpeza/robo_limpeza.ino @@ -1,94 +1,92 @@ // Programação de um robô de limpeza // Feito na aula de Programação para Engenharia Elétrica +#include + #define INTERRUPT_PIN 2 #define BLUE_LED 3 #define RED_LED 4 - #define R_MOTOR_PIN_A 6 #define R_MOTOR_PIN_B 5 #define L_MOTOR_PIN_A 10 #define L_MOTOR_PIN_B 11 - #define FORWARD 0 #define BACKWARD 1 #define STOP -1 +int interrupt_state = HIGH; + void setup() { + // MPU 6050 + mpu6050_init(); + // MOTORS // LOW and HIGH -> forward // HIGH and LOW -> backward pinMode(R_MOTOR_PIN_A, OUTPUT); pinMode(R_MOTOR_PIN_B, OUTPUT); - // LOW and HIGH -> forward // HIGH and LOW -> backward pinMode(L_MOTOR_PIN_A, OUTPUT); pinMode(L_MOTOR_PIN_B, OUTPUT); - // BLUE and RED LED + // BLUE and RED LEDs pinMode(BLUE_LED, OUTPUT); pinMode(RED_LED, OUTPUT); - // sensor digital input + // SENSORS INTERRUPTION - LOW HIGH CHANGE RISING FALLING pinMode(INTERRUPT_PIN, INPUT); + attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), interrupt_function, FALLING); - // interruption - // pinMode(INTERRUPT_PIN, INPUT_PULLUP); - // attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), interrupt_function, LOW); - // noInterrupts(); - // interrupts(); + // ROBOT ON STATE + digitalWrite(RED_LED, HIGH); - Serial.begin(9600); - // Serial.println(); + // SERIAL DEBUG + Serial.begin(115200); } void loop() { - // Robot ON - digitalWrite(RED_LED, HIGH); - - move(1000); - turn_left(); - move(1000); - turn_left(); - move(1000); - turn_left(); - move(1000); - turn_left(); - - delay(10); -} - -int left_is_clear(void){ - int state; - r_motor(FORWARD); - l_motor(STOP); - delay(250); - state = digitalRead(INTERRUPT_PIN); - r_motor(BACKWARD); - l_motor(STOP); - delay(250); - return state; + // SAFE State + r_motor_velocity(0); + l_motor_velocity(0); + delay(5000); } int front_is_clear(void){ - return digitalRead(INTERRUPT_PIN); + int front_state = HIGH; + if(!interrupt_state){ + front_state = LOW; + interrupt_state = HIGH; + } + return front_state; } void turn_left(void){ - r_motor(FORWARD); - l_motor(BACKWARD); - delay(1080); - r_motor(STOP); - l_motor(STOP); + for(int counter = 0; counter <= 127; counter++){ + r_motor_velocity(counter); + l_motor_velocity(-counter); + delay(4); + } + //while(int(mpu6050_yaw() < 90)); + for(int counter = 127; counter >= 0; counter--){ + r_motor_velocity(counter); + l_motor_velocity(-counter); + delay(4); + } } void move(int distance) { - r_motor(FORWARD); - l_motor(FORWARD); - delay(5 * distance); - r_motor(STOP); - l_motor(STOP); + for(int counter = 0; counter <= 127; counter++){ + r_motor_velocity(counter); + l_motor_velocity(counter); + delay(4); + } + delay(50*(distance-5)); + for(int counter = 127; counter >= 0; counter--){ + r_motor_velocity(counter); + l_motor_velocity(counter); + delay(4); + } } void r_motor(int direction){ @@ -144,5 +142,83 @@ void l_motor_velocity(int velocity){ } void interrupt_function() { - digitalWrite(BLUE_LED, HIGH); + interrupt_state = !interrupt_state; +} + +void mpu6050_init(void){ + Wire.begin(); + Wire.beginTransmission(0x68); + Wire.write(0x6B); + Wire.write(0x00); + Wire.endTransmission(); +} + +float mpu6050_offset(void){ + float gyro_Z_offset=0; + // Sum 1024 sensor reads + for(int counter = 0; counter < 4096; counter++) { + Wire.beginTransmission(0x68); + Wire.write(0x47); + Wire.endTransmission(); + Wire.requestFrom(0x68, 0x02); + if (Wire.available() >= 0x02) + gyro_Z_offset += (Wire.read() << 8) | Wire.read(); + } + // Return average offset + return gyro_Z_offset/4096.0; +} + +float mpu6050_yaw(void){ + static int fuse = 1; + static float yaw, gyro_Z, gyro_Z_offset; + static float current_time, previous_time, elapsed_time; + // Calculate offset + if(fuse){ + gyro_Z_offset = mpu6050_offset(); + fuse = 0; + } + // Average value filtering + gyro_Z = 0; + for(int counter = 0; counter < 16; counter++){ + // For a 250deg/s range divide the raw value by 131.0, according to the datasheet + Wire.beginTransmission(0x68); + Wire.write(0x47); + Wire.endTransmission(); + Wire.requestFrom(0x68, 0x02); + if (Wire.available() >= 0x02) + gyro_Z += (((Wire.read() << 8) | Wire.read()) - gyro_Z_offset ) / 131.0; + } + gyro_Z = gyro_Z/16.0; + + /*Wire.beginTransmission(0x68); + Wire.write(0x3B); + Wire.endTransmission(); + Wire.requestFrom(0x68, 0x06); + accel_X=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L) + accel_Y=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L) + accel_Z=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L) + + Wire.beginTransmission(0x68); + Wire.write(0x3B); + Wire.endTransmission(); + Wire.requestFrom(0x68, 0x02); + temp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L) + + Wire.beginTransmission(0x68); + Wire.write(0x43); + Wire.endTransmission(); + Wire.requestFrom(0x68, 0x06); + // For a 250deg/s range divide the raw value by 131.0, according to the datasheet + gyro_X = (Wire.read() << 8 | Wire.read()) / 131.0; // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L) + gyro_Y = (Wire.read() << 8 | Wire.read()) / 131.0; // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L) + gyro_Z = (Wire.read() << 8 | Wire.read()) / 131.0; // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)*/ + + // Calculate yaw angle from angular velocity + current_time = millis(); + elapsed_time = (current_time - previous_time) / 1000.0; + previous_time = current_time; + yaw += gyro_Z * elapsed_time; + + // Return YAW + return yaw; }