#include <Wire.h>
#include <I2Cdev.h>
#include <MPU6050.h>
MPU6050 mpu;
int16_t ax, ay, az;
int16_t gx, gy, gz;
struct MyData {
byte X;
byte Y;
byte Z;
}; //INITIALIZER
MyData data;
//DEFINE PINS
int Button1 = 2; // Microswitch 1
int Button2 = 3; // Microswitch 2
#define echoPin 5 // Define the HC-SE04 echo on pin 5 on the arduino
#define trigPin 6 // Define the HC-SE04 triger on pin 6 on the arduino
#define LED1 7 // Led 1 (Act as robot bow)
#define LED2 8 // Led 2 (Act as robot up)
#define In3 10
#define In4 11
int upbutton = 0;
int downbutton = 0;
void setup() {
pinMode(Button1, INPUT_PULLUP); // SETTING MICROSWITCH 1
pinMode(Button2, INPUT_PULLUP); // SETTING MICROSWITCH 2
pinMode(LED1, OUTPUT); // SETTING LED 1 (ACT AS ROBOT BOW)
pinMode(LED2, OUTPUT); // SETTING LED 2 (ACT AS ROBOT DOWN)
pinMode(trigPin, OUTPUT); //set the trigpin to output
pinMode(echoPin, INPUT); //set the echopin to input
pinMode (In3, OUTPUT);
pinMode (In4, OUTPUT);
Serial.begin (9600); //Start the serial monitor
Wire.begin();
mpu.initialize();
}
void loop() {
{
int duration, distance; //< ----- BACA ULTRASONIC SENSOR
digitalWrite(trigPin, HIGH);
delayMicroseconds(500);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = (duration/2) / 29.1;
{
if ((distance >= 1) && (distance <= 90) )
{
Serial.println(" ");
Serial.println(" <--- Dalam Range --->"); //in centimeters
Serial.print(distance);
Serial.println(" CM");
Bow();
Greeting();
Up();
}
else
{
Serial.println(" Out of Range "); //in centimeters
}
}
// **********************
delay(500);
}
}
// ***********************
void Bow() //Start the Light subroutine
{
Serial.println(" <--- Bow --->");
do
{
Serial.println(" <---Masih Bow --->");
// SINI NAK MASUKKAN MOTOR BOW
digitalWrite(LED1, HIGH); // Robot Bow continuously
//digitalWrite(LED2, LOW);
digitalWrite (In3, HIGH); // motor up
//digitalWrite (In4, LOW);
//while (digitalRead(Button1) == HIGH) { // keep moving until button1 is pressed
//delay(100); // small delay for responsiveness
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //< ----- baca gyro
data.Z = map(az, -17000, 17000, 0, 255); // Z axis data
Serial.print("Axis Z = ");
Serial.print(data.Z);
Serial.print(" ");
int Button1=digitalRead(2); // <----- BACA SUIS 1
{
if((data.Z>157)&&(data.Z<159)||(Button1==LOW)) // KAT SINI NAK MASUKAN SYARAT MICROSUIS 1 TU
upbutton = 0;
else
upbutton = 1;
}
}while(upbutton==1);
{
// stopkan motor
Serial.println(" <--- ROBOT STOP BOW --->");
digitalWrite(LED1, LOW); // stop moving
digitalWrite(LED2, LOW);
digitalWrite (In3, LOW); // motor stop
digitalWrite (In4, LOW);
delay(3000); // delay for 3 seconds
}
}
//***********************
void Greeting() //Start the Light subroutine
{
Serial.println(" <--- greeting --->");
digitalWrite (22, HIGH);
delay (5000); //wait 15 seconds
digitalWrite (23, LOW);
delay (500);
}
//************************
void Up() //Start the Light subroutine
{
Serial.println(" <--- Up --->");
do
{
Serial.println(" <---Masih UP --->");
// SINI NAK MASUKKAN ROBOT UP
//digitalWrite(LED1, LOW); // move motor down continuously
digitalWrite(LED2, HIGH);
//digitalWrite (In3, LOW); // motor down
digitalWrite (In4, HIGH);
mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //< ----- BACA GYRO
data.Z = map(az, -17000, 17000, 0, 255); // Z axis data
Serial.print("Axis Z = ");
Serial.print(data.Z);
Serial.print(" ");
int Button2=digitalRead(3); // <----- BACA SUIS 2
{
if((data.Z>125)&&(data.Z<127)||(Button2==LOW)) // KAT SINI NAK MASUKAN SYARAT MICROSUIS 2 TU
downbutton = 0;
else
downbutton = 1;
}
}while(downbutton==1);
{
// stopkan motor
Serial.println(" <--- ROBOT STOP UP --->");
digitalWrite(LED1, LOW); // stop moving
digitalWrite(LED2, LOW);
digitalWrite (In3, LOW); // motor stop
digitalWrite (In4, LOW);
delay(5000);
}
//******************
}
No comments:
Post a Comment