Good evening everyone,
I was wondering if anyone could help me with this code I wrote on Arduino IDE. I am working on a senior research design project and I am currently in testing phase. I am trying to build a robot that could go left and right based on sensor data from the mpu 6050 gyroscope. however, the problem is that on the first iteration of the for loop there is a function called MOVE_RIGHT where the mpu begins to read yaw data this works great and prints values in the serial monitor on the first instance that this function is called but doesn't work on the second and the function TURN_LEFT also reads data from the mpu module. attached is the
the code where the mpu is used and also the for loop statement with the turn left and right functions.
note: I am using this mpu library for the projecthttps://github.com/ElectronicCats/mpu6050
#include <RBD_LightSensor.h> // INCLUDING LIGHT SENSOR LIBRARY
#include <hcsr04.h> // INCLUDING ULTRASONIC SENSOR LIBRARY
#include <Servo.h> // INCLUDING SERVO LIBRARY IN ORDER TO DRIVE SERVO MOTORS FOR BOTH LEFT AND RIGHT TIRES
#include <SoftwareSerial.h> // INCLUDING SOFTWARE SERIAL LIBRARY IN ORDER TO INITIALIZE SOFTWARE SERIAL FUNCTION AND BLUTOOTH COMMUNICATIONS
#include <Wire.h> // INCLUDING WIRE LIBRARY FOR COMMUNICATIONS WITH SDA AND SCL OF ARDUINO BOARD
#include <MPU6050.h> // INCLUDING MPU6050 LIBRARY FOR GYROSCOPE FUNCTIONALITY
////////////////////////////////////////////////////////
#define LED_R 11 // RED LED GATE
#define LED_G A0 // GREEN LED GATE
#define LED_B 3 // BLUE LED GATE
#define TIRE_R 6 // RIGHT SERVO GATE
#define TIRE_L 5 // LEFT SERVO GATE
#define MOTOR_SPDR_AUGER A2 // DC HIGH TORQUE MOTOR GATE
#define LDR_SENSE A1 // PHOTO-RESISTOR PIN
#define ULTRASONIC_SENSE_TRIG 9 // ULTRA-SONIC SENSOR TRIG PIN
#define ULTRASONIC_SENSE_ECHO 10 // ULTRA-SONIC SENSOR ECHO PIN
////////////////////////////////////////////////////////
SoftwareSerial BLE_MOD (0,1); // SETTING UP SOFTWARE SERIAL FOR BLE AND ASSIGNING RX AND TX PINS TO 0 AND 1
MPU6050 GYRO_SENSE; // SETTING UP GYROSCOPE FOR MPU6050 OPERATIONS
HCSR04 hcsr04(ULTRASONIC_SENSE_TRIG, ULTRASONIC_SENSE_ECHO, 1, 1000); // SETTING UP ULTRASONIC SENSOR BY ASSIGNING TRIG INPUT TO PIN 9 AND ECHO OUTPUT TO PIN 10. ALSO ASSIGNING MIN RANGE TO 1MM AND MAX RANGE TO 1000MM
Servo RIGHT_TIRE; // SETTING UP RIGHT TIRE FOR SERVO MOVEMENTS
Servo LEFT_TIRE; // SETTING UP LEFT TIRE FOR SERVO MOVEMENTS
RBD::LightSensor LDR_SENSOR(LDR_SENSE); // SETTING UP LDR SENSOR LIBRARY BY ASSIGNING LDR SENSOR TO PIN A1
float RobotVelocity = 1.1; // INITLAIZES RV AND SETS IT EQUAL TO ROBOT VELOCITY WHICH IS 1.1 FT/S
float Length; // INITIALIZES LENGTH FOR INPUT FROM BLE DEVICE
float Width; // INITIALIZES WIDTH FOR INPUT FROM BLE DEVICE
float T3; // INITIALIZES AMW FOR AUTOMATED WIDTH MODE (FULL)
float T2; // INITIALIZES AMW FOR AUTOMATED WIDTH MODE
float T1; // INITIALIZES AML FOR AUTOMATED LENGTH MODE
unsigned long timer = 0; // INITAL TIMER SET TO 0MS
float timeStep = 0.01; // TIMESTEP SET TO 0.01MS
float yaw = 0; // INITIAL YAW SET TO 0
////////////////////////////////////////////////////////
void setup()
{
INITIALIZE(); // CALLS BACK SETUP_FUNCTION
}
void loop()
{
MAIN_PROGRAM(); // CALLS BACK INITIALIZE METHOD AFTER SETUP_FUNC
}
void INITIALIZE() // MAIN_PROCEDURE_FUNC METHOD
{
pinMode(LED_R,OUTPUT); // THIS PIN SET AS OUTPUT
pinMode(LED_G,OUTPUT); // THIS PIN SET AS OUTPUT
pinMode(LED_B,OUTPUT); // THIS PIN SET AS OUTPUT
pinMode(MOTOR_SPDR_AUGER,OUTPUT); // THIS PIN SET AS OUTPUT
pinMode(LDR_SENSE,INPUT); // THIS PIN SET AS INPUT
Serial.begin(115200);
BLE_MOD.begin(9600);// BEGINS THE SOFTWARE SERIAL TX AND RX PORTS TO START RECEIVING AND TRANSMITTING DATA
GYRO_SENSE.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G); // BEGINS GYROSCOPE OPERATIONS WITH APPLIED SETTINGS
GYRO_SENSE.calibrateGyro(); // CALIBRATES THE GYRO POSITION TO 0
GYRO_SENSE.setThreshold(3); //Sets threshold sensivty to 3
LED_SETUP(); // GOES TO LED SETUP PATTERN
}
void MAIN_PROGRAM() // MAIN_PROCEDURE_FUNC METHOD
{
BLE_MOD.listen(); // ALLOWS BLE DEVICE TO START LISTENING TO INCOMING DATA
while (BLE_MOD.available()>0) // WHILE BLE IS AVAILABLE DO THE FOLLOWING
{
Length = BLE_MOD.parseInt(); // STORES THE FIRST INPUT FROM THE MOBILE APP AND SETS IT TO LENGTH IN FT
BLE_MOD.setTimeout(10); // SETS A DELAY IN INCOMING INFO FROM THE BLE DEVICE TO PREVENT BUFFER
Width = BLE_MOD.parseInt(); // STORES THE SECOND INPUT FROM MOBILE APP AND SETS IT TO WIDTH IN FT
T1 = Length/RobotVelocity*1000*1; // SETS TIME FOR LENGTH / RV * 1000 *1
T2 = Width/RobotVelocity/3*1000*1; // SETS TIME FOR WIDTH / RV / 7 * 1000 *1
T3 = Width/RobotVelocity*1000*1; // SETS TIME FOR WHOLE WIDTH / RV *1000*1
float lightVal = LDR_SENSOR.getValue();// INITALIZES AND SETS LIGHTVAL TO GET INFORMATION FROM LDR SENSOR
if(lightVal > 20) // IF LIGHT VALUE IS GREATER THAN 20 (MEANING DAYLIGHT) DO THE FOLLOWING
{
DISTRIBUTE_SALT(); // CALLS DISTRIBUTE_SALT METHOD
}
else // IF LIGHT VALUE IS NOT GREATER THAN 20 (MEANING NIGHT TIME) DO THE FOLLOWING
{
NIGHT_LED(); // CALLS NIGHT_LED METHOD
DISTRIBUTE_SALT(); // CALLS DISTRIBUTE_SALT METHOD
}
}
}
////////////////////////////////////////////////////////
void DISTRIBUTE_SALT() // DISTRIBUTE_SALT METHOD
{
for(int Count = 0; Count<7; Count++) // SETS INTEGER PathPos TO 0 FOR INTIAL LOOP POSITION AND TELLS THE LOOP TO RUN 7 TIMES ADDING AN EXTRA LOOP AFTER EACH PREVIOUS LOOP
{
MOVE_FORWARD(T1);// CALLS MOVEMENT_FUNC_DAY METHOD AND SETS IT TO GO FORWARD
MOVE_RIGHT(); // CALLS MOVEMENT_FUNC_DAY METHOD AND SETS IT TO GO RIGHT
MOVE_FORWARD(T2); // CALLS MOVEMENT_FUNC_DAY METHOD AND SETS IT TO GO FORWARD
MOVE_RIGHT(); // CALLS MOVEMENT_FUNC_DAY METHOD AND SETS IT TO GO RIGHT
MOVE_FORWARD(T1); // CALLS MOVEMENT_FUNC_DAY METHOD AND SETS IT TO GO FORWARD
MOVE_LEFT(); // CALLS MOVEMENT_FUNC_DAY METHOD AND SETS IT TO GO LEFT
MOVE_FORWARD(T2); // CALLS MOVEMENT_FUNC_DAY METHOD AND SETS IT TO GO FORWARD
MOVE_LEFT(); // CALLS MOVEMENT_FUNC_DAY METHOD AND SETS IT TO GO LEFT
if (Count<7)
{
RETURN_TO_ORIGIN();
}
}
}
////////////////////////////////////////////////////////
void RETURN_TO_ORIGIN()
{
MOVE_LEFT(); // CALLS MOVEMENT_FUNC_DAY METHOD AND SETS IT TO GO LEFT
MOVE_FORWARD(T3); // CALLS MOVEMENT_FUNC_DAY METHOD AND SETS IT TO GO FORWARD
MOVE_RIGHT(); // CALLS MOVEMENT_FUNC_DAY METHOD AND SETS IT TO GO RIGHT
}
void MOVE_FORWARD(float timedelay)
{
GYRO_SENSE.calibrateGyro();// MPU6050 WILL SET ALL AXIS IN GYRO CHIP TO 0
RIGHT_TIRE.attach(TIRE_R); // ATTACHES RIGHT TIRE TO PIN 6
LEFT_TIRE.attach(TIRE_L); // ATTACHES LEFT TIRE TO PIN 5
RIGHT_TIRE.write(0); // WRITES 0 TO RIGHT TIRE
LEFT_TIRE.write(180); // WRITES 180 TO LEFT TIRE
analogWrite(MOTOR_SPDR_AUGER, 150); // WRITES ANALOG VALUE OF 200 TO SPREADER MOTOR
delay(timedelay);
}
////////////////////////////////////////////////////////
void MOVE_RIGHT()
{
RIGHT_TIRE.write(180); // WRITES 180 TO RIGHT TIRE
LEFT_TIRE.write(180); // WRITES 180 TO RIGHT TIRE
timer = millis(); // SETS TIMER EQUAL TO MILLIS
Vector normGyro = GYRO_SENSE.readNormalizeGyro();// MPU6050 WILL BEGIN READING IT'S OWN GYRO DATA
yaw = yaw + normGyro.ZAxis * timeStep; // SETS YAW EQUAL TO YAW + ZAXIS * TIMESTEP
Serial.println(yaw);
delay((timeStep * 1000) - (millis() - timer)); // CREATES DELAY BASED ON ABOVE VARIABLES
if (yaw <= -20)
{
}
else
{
MOVE_RIGHT();
}
}
////////////////////////////////////////////////////////
void MOVE_LEFT()
{
RIGHT_TIRE.write(0); // WRITES 180 TO RIGHT TIRE
LEFT_TIRE.write(0); // WRITES 180 TO LEFT TIRE
timer = millis(); // SETS TIMER EQUAL TO MILLIS
Vector normGyro = GYRO_SENSE.readNormalizeGyro();// MPU6050 WILL BEGIN READING IT'S OWN GYRO DATA
yaw = yaw + normGyro.ZAxis * timeStep; // SETS YAW EQUAL TO YAW + ZAXIS * TIMESTEP
Serial.println(yaw);
delay((timeStep * 1000) - (millis() - timer)); // CREATES DELAY BASED ON ABOVE VARIABLES
if (yaw >= 20)
{
}
else
{
MOVE_LEFT();
}
}
////////////////////////////////////////////////////////
void STOP()
{
RIGHT_TIRE.detach();
LEFT_TIRE.detach();
analogWrite(MOTOR_SPDR_AUGER, 0);
}