There it is. I have try first to IR-transmit.
// Tennisball machine 2016 first try
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high
#define OUTPUT_READABLE_QUATERNION
#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;
#include <IRremote.h> // IR -library
#include <LiquidCrystal.h>
// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
char inData[20]; // Allocate some space for the string
char inChar; // Where to store the character read
byte index = 0; // Index into array; where to store the character
// orientation/motion vars
Quaternion q;
VectorInt16 aa;
VectorInt16 aaReal;
VectorInt16 aaWorld;
VectorFloat gravity;
float euler[3];
float ypr[3];
// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '
, 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
// motor one
int enA = 3;
int in1 = 38;
int in2 = 40;
// motor two
int enB = 4;
int in3 = 42;
int in4 = 44;
// variables button
int button1=30; // ball motors button
int button2=32; // ball stirrer motor button
int button3=34; // ball selector motor button
int xPosition = 0;
int yPosition = 0;
int buttonState = 0;
// Relay variables
int Relay1 = 22; //ball selector motor
int Relay2 = 24; //ball stirrer motor
int Relay3 = 28; //x-horisontal motor
int Relay4 = 26; //y-horisontal motor
// Ball selector variables
int switch1 = 46;
int SwitchStatus =HIGH;
int SwitchState=0;
// IR variables
int IRpin = 11; // pin for the IR sensor
IRrecv irrecv(IRpin);
decode_results results;
int Relay1on = true; // initializing Relay1on as true
int Relay2on = true; // initializing Relay2on as true
int Relay3on = true; // initializing Relay3on as true, xPosition
int Relay4on = true; // initializing Relay4on as true, yPosition
int Motoron = true; // initializing Ballmotor on as true
int VerMotoron = true; // initializing Ballmotor on as true
// initialize the library with the numbers of the interface pins
LiquidCrystal lcd(5, 6, 7, 8, 9, 10 );
//DC motor variables
byte motorSpeed = 255;
void setup()
{
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
Serial.begin(38400);
while (!Serial); // wait for Leonardo enumeration, others continue immediately
// initialize device
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
// verify connection
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
// load and configure the DMP
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(-75);
mpu.setYGyroOffset(69);
mpu.setZGyroOffset(-242);
mpu.setZAccelOffset(1688); // 1688 factory default for my test chip
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// turn on the DMP, now that it's ready
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
Serial.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
// set all the motor control pins to outputs
digitalWrite(switch1, HIGH); //pull-up ball selector micro switch
pinMode(enA, OUTPUT);
pinMode(enB, OUTPUT);
pinMode(in1, OUTPUT);
pinMode(in2, OUTPUT);
pinMode(in3, OUTPUT);
pinMode(in4, OUTPUT);
pinMode(Relay1, OUTPUT); //Set Pin22, Output Selector Motor Relay
pinMode(Relay2, OUTPUT); //Set Pin24, Output Stirrel Motor Relay
pinMode(Relay3, OUTPUT); //Set Pin28, Output Vertical Relay
pinMode(Relay4, OUTPUT); //Set Pin26, Output Horisontal Relay
digitalWrite(Relay1, HIGH); // Set Relay1 off
digitalWrite(Relay2, HIGH); // Set Relay2 off
digitalWrite(Relay3, HIGH); // Set Relay3 off
digitalWrite(Relay4, HIGH); // Set Relay4 off
// IR - staff
//Serial.begin(9600);
irrecv.enableIRIn(); // Start the receiver
// set up the LCD's number of columns and rows:
lcd.begin(16, 2);
// Print a message to the LCD.
lcd.print(" Tennisblaster ");
lcd.setCursor(0, 1);
lcd.print(" ver. 1.0");
}
void loop()
{
// if programming failed, don't try to do anything
if (!dmpReady) return;
}
// reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// get current FIFO count
fifoCount = mpu.getFIFOCount();
// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
Serial.println(F("FIFO overflow!"));
// otherwise, check for DMP data ready interrupt (this should happen frequently)
} else if (mpuIntStatus & 0x02) {
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
fifoCount -= packetSize;
#ifdef OUTPUT_READABLE_QUATERNION
// display quaternion values in easy matrix form: w x y z
mpu.dmpGetQuaternion(&q, fifoBuffer);
Serial.print("quat\t");
Serial.print(q.w);
Serial.print("\t");
Serial.println(q.x);
#endif
}
if (irrecv.decode(&results))
{
irrecv.resume(); // Receive the next value
}
switch(results.value) // IR-code
{
case 16724175:
if (Relay1on == true) // is Relay1on equal to true? Start BallselectorMotor
{
Relay1on = false;
digitalWrite(Relay1, HIGH);
delay(100); // keeps the transistion smooth
}
else
{
Relay1on = true;
digitalWrite(Relay1, LOW);
delay(200);}
break;
case 16718055:
if (Relay2on == true) // is Relay2on equal to true? BallDropMotor
{
Relay2on = false;
digitalWrite(Relay2, HIGH);
delay(200); // keeps the transistion smooth
}
else
{
Relay2on = true;
digitalWrite(Relay2, LOW);
delay(200);}
break;
case 16720605: // In this case I need help.....Tennisdrills
{
// digitalWrite(Relay3, LOW); // xPositionMotorRelay
//
// digitalWrite(Relay3, LOW); // yPositionMotorRelay
}
break;
}
if (digitalRead(switch1)==LOW) {
delay(500);
digitalWrite(Relay1, HIGH); // Relay1 OFF
Relay1on = false;
}
if (xPosition == (q.w)) {
digitalWrite(Relay4, HIGH); // Relay1 OFF
}
}
void MotorForward()
{
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, HIGH);
analogWrite(enA, motorSpeed);
analogWrite(enB, motorSpeed);
}
void MotorStop()
{
digitalWrite(in1, LOW);
digitalWrite(in2, LOW);
digitalWrite(in3, LOW);
digitalWrite(in4, LOW);
analogWrite(enA, 0);
analogWrite(enB, 0);
}
