#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_MotorShield.h>
#include <Wire.h>
Adafruit_MPU6050 mpu;
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *myMotor = AFMS.getMotor(1);
Adafruit_DCMotor *myMotor2 = AFMS.getMotor(2);
int datacollect = 0;
int accelX[60];
int accelY[60];
int smallestX = 1000;
int biggestX = -1000;
int smallestY = 1000;
int biggestY = -1000;
void setup(void) {
AFMS.begin();
Serial.begin(115200);
while (!Serial) {delay(10);}
Serial.println("Adafruit MPU6050 test!");
// Try to initialize!
if (!mpu.begin()) {
Serial.println("Failed to find MPU6050 chip");
while (1) { delay(10); } }
Serial.println("MPU6050 Found!");
//setupt motion detection
mpu.setHighPassFilter(MPU6050_HIGHPASS_0_63_HZ);
mpu.setMotionDetectionThreshold(1);
mpu.setMotionDetectionDuration(20);
mpu.setInterruptPinLatch(true);
// Keep it latched. Will turn off when reinitialized.
mpu.setInterruptPinPolarity(true);
mpu.setMotionInterrupt(true);
Serial.println("");
delay(10);
}
void loop() {
Serial.print("test");
if (datacollect == 0) {
for (int count = 0; count <=59; count++){
if (mpu.getMotionInterruptStatus()) {
/* Get new sensor events with the readings */
sensors_event_t a, g, temp;
mpu.getEvent(&a, &g, &temp);
accelX[count] = a.acceleration.x;
accelY[count] = a.acceleration.y;
Serial.print("AccelX:");
Serial.print(a.acceleration.x);
Serial.print(",");
Serial.print("AccelY:");
Serial.print(a.acceleration.y);
Serial.print(",");
Serial.print("AccelZ:");
Serial.print(a.acceleration.z);
Serial.print(", ");
Serial.print("GyroX:");
Serial.print(g.gyro.x);
Serial.print(",");
Serial.print("GyroY:");
Serial.print(g.gyro.y);
Serial.print(",");
Serial.print("GyroZ:");
Serial.print(g.gyro.z);
Serial.print("");
Serial.println("-----------");
Serial.println(a.acceleration.x*10);
}
}
for(int i=0;i<59;i++){
if(accelX[i]<smallestX){smallestX = accelX[i];}
if(accelX[i]>biggestX){biggestX = accelX[i];}
if(accelY[i]<smallestY){smallestY = accelY[i];}
if(accelY[i]>biggestY){biggestY = accelY[i];}
}
Serial.println("");
Serial.print("smallestX: ");
Serial.print(smallestX);
Serial.println("");
Serial.print("biggestX: ");
Serial.print(biggestX);
Serial.println("");
Serial.println("");
Serial.print("smallestY: ");
Serial.print(smallestY);
Serial.println("");
Serial.print("biggestY: ");
Serial.print(biggestY);
Serial.println("");
for(int i=0;i<59;i++){
accelX[i] = map(accelX[i], smallestX, biggestX, 60, 255);
accelY[i] = map(accelY[i],smallestY,biggestY,60,255);
}
Serial.print(accelX[23]);
Serial.print("");
Serial.print(accelY[23]);
Serial.print("");
delay(1000);
}
/*
if(count >= 60){
datacollect = 1;
}
}
if (datacollect == 1){
for(int count = 0; count<60;count++){
myMotor->setSpeed(ceil(accelX[count]));
myMotor2->setSpeed(ceil(accelY[count]));
myMotor->run(BACKWARD);
myMotor2->run(BACKWARD);
}
}
*/
}