Hello, guys. So, this is the project that I am working on. Supposedly, I want to run this one and maintain its straight line movement using MPU6050, but I am really having a hard time coding it, and I badly your assistance.
This is my code for it
// SERVOT based on Hacked Roomba
// Delivers food along straight path to tables 3m or 5m away from Kitchen
// Far Tables 1 2
// Near Tables 3 4
// Kitchen
// Turns to customer
// Then goes back to the kitchen
// Adjusts course if too near a left or right wall
#include<AFMotor.h>
#include "SR04.h"
// usonic 30deg to right
#define TRIG_PIN1 2
#define ECHO_PIN1 10
// usonic 30deg to left
#define TRIG_PIN2 13
#define ECHO_PIN2 9
// time delay in mS
// 1 meter ==> 42 steps
#define MAXSTEP_NEAR 126 //3m
#define MAXSTEP_FAR 210 //5m
#define TURNTIME 100
#define TURN90TIME 900
#define TURN180TIME 1800
#define FWDTIME 50
#define TOONEAR 15 //cm
SR04 sr1 = SR04(ECHO_PIN1,TRIG_PIN1);
SR04 sr2 = SR04(ECHO_PIN2,TRIG_PIN2);
long usonic_right,usonic_left;
int fwdstep,turning;
const int buttonPin = 1;
int buttonState = 0;
// Table selector 4bit DIP switch connected to analog pins
// analog pin is GND if DIP switch is pressed
int sensorPin1 = A0; // select the input pin for table 1
int sensorValue1 = 0; // variable to store the value coming from the sensor
int sensorPin2 = A1; // select the input pin for table 2
int sensorValue2 = 0; // variable to store the value coming from the sensor
int sensorPin3 = A2; // select the input pin for table 3
int sensorValue3 = 0; // variable to store the value coming from the sensor
int sensorPin4 = A3; // select the input pin for table 4
int sensorValue4 = 0; // variable to store the value coming from the sensor
int tablenumber = 1;
int maxstep =0;
AF_DCMotor motor1(1); // left motor
AF_DCMotor motor2(2); // right motor
void setup() {
// run once at startup
Serial.begin(9600);
delay(1000);
motor1.setSpeed(255);
motor2.setSpeed(255);
// initialize the pushbutton pin as an input:
pinMode(buttonPin, INPUT);
}
void loop() {
fwdstep=0;
turning=0;
// check if the pushbutton is pressed. If it is, the buttonState is LOW:
do
{
buttonState = digitalRead(buttonPin);
}
while (buttonState == HIGH);
// read the value from the sensor and assign table number
sensorValue1 = analogRead(sensorPin1);
if (sensorValue1<100) tablenumber=1;
sensorValue2 = analogRead(sensorPin2);
if (sensorValue2<100) tablenumber=2;
sensorValue3 = analogRead(sensorPin3);
if (sensorValue3<100) tablenumber=3;
sensorValue4 = analogRead(sensorPin4);
if (sensorValue4<100) tablenumber=4;
if (tablenumber<3)
maxstep = MAXSTEP_FAR;
else
maxstep = MAXSTEP_NEAR;
// Deliver to table
do {
if (turning==0)
{
motor1.run(FORWARD);
motor2.run(FORWARD);
delay(FWDTIME);
}
usonic_right=sr1.Distance();
usonic_left=sr2.Distance();
if ((usonic_right>TOONEAR)&&(usonic_left>TOONEAR))
{
turning=0;
}
if (usonic_right<=TOONEAR)
{
Serial.print(usonic_right);
Serial.println("cm ");
// CCW
motor1.run(BACKWARD);
motor2.run(FORWARD);
delay(TURNTIME);
turning=1;
}
if (usonic_left<=TOONEAR)
{
Serial.print(usonic_left);
Serial.println("cm");
// CW
motor1.run(FORWARD);
motor2.run(BACKWARD);
delay(TURNTIME);
turning=1;
}
fwdstep++;
} while (fwdstep<maxstep);
// turn to face customer
if ((tablenumber==2)||(tablenumber==4))
{
// turn 90deg to table CW
motor1.run(FORWARD);
motor2.run(BACKWARD);
delay(TURN90TIME);
motor1.run(RELEASE);
motor2.run(RELEASE);
delay(100);
}
else
{
// turn 90deg to table CCW
motor1.run(BACKWARD);
motor2.run(FORWARD);
delay(TURN90TIME);
motor1.run(RELEASE);
motor2.run(RELEASE);
delay(100);
}
// wait for customer to get food and press button
// check if the pushbutton is pressed. If it is, the buttonState is LOW:
do
{
buttonState = digitalRead(buttonPin);
}
while (buttonState == HIGH);
// turn 90deg to kitchen
if ((tablenumber==2)||(tablenumber==4))
{
// turn 90deg to table CW
motor1.run(FORWARD);
motor2.run(BACKWARD);
delay(TURN90TIME);
motor1.run(RELEASE);
motor2.run(RELEASE);
delay(100);
}
else
{
// turn 90deg to table CCW
motor1.run(BACKWARD);
motor2.run(FORWARD);
delay(TURN90TIME);
motor1.run(RELEASE);
motor2.run(RELEASE);
delay(100);
}
//-----------------------------------------
// Return to Kitchen
fwdstep=0;
turning=0;
do {
if (turning==0)
{
motor1.run(FORWARD);
motor2.run(FORWARD);
delay(FWDTIME);
}
usonic_right=sr1.Distance();
usonic_left=sr2.Distance();
if ((usonic_right>TOONEAR)&&(usonic_left>TOONEAR))
{
turning=0;
}
if (usonic_right<=TOONEAR)
{
Serial.print(usonic_right);
Serial.println("cm ");
// CCW
motor1.run(BACKWARD);
motor2.run(FORWARD);
delay(TURNTIME);
turning=1;
}
if (usonic_left<=TOONEAR)
{
Serial.print(usonic_left);
Serial.println("cm");
// CW
motor1.run(FORWARD);
motor2.run(BACKWARD);
delay(TURNTIME);
turning=1;
}
fwdstep++;
} while (fwdstep<maxstep);
// turn 180deg to table
// CW
motor1.run(FORWARD);
motor2.run(BACKWARD);
delay(TURN180TIME);
motor1.run(RELEASE);
motor2.run(RELEASE);
delay(100);
}