#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <nRF24L01.h>
#include <RF24.h>
Adafruit_MPU6050 mpu;
RF24 radio {4, 5};
int calibrate_pin = 34;
int en = 0;
float xcal,ycal,zcal;
float xval,yval,zval;
String message;
void setup(void) {
Serial.begin(115200);
radio.begin();
radio.setChannel(5);
radio.setDataRate(RF24_1MBPS);
radio.setPALevel(RF24_PA_HIGH);
radio.openWritingPipe(0x1234567890LL);
pinMode(calibrate_pin, INPUT);
while (!Serial)
delay(10); // will pause Zero, Leonardo, etc until serial console opens
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!");
mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
Serial.print("Accelerometer range set to: ");
switch (mpu.getAccelerometerRange()) {
case MPU6050_RANGE_2_G:
Serial.println("+-2G");
break;
case MPU6050_RANGE_4_G:
Serial.println("+-4G");
break;
case MPU6050_RANGE_8_G:
Serial.println("+-8G");
break;
case MPU6050_RANGE_16_G:
Serial.println("+-16G");
break;
}
mpu.setGyroRange(MPU6050_RANGE_500_DEG);
Serial.print("Gyro range set to: ");
switch (mpu.getGyroRange()) {
case MPU6050_RANGE_250_DEG:
Serial.println("+- 250 deg/s");
break;
case MPU6050_RANGE_500_DEG:
Serial.println("+- 500 deg/s");
break;
case MPU6050_RANGE_1000_DEG:
Serial.println("+- 1000 deg/s");
break;
case MPU6050_RANGE_2000_DEG:
Serial.println("+- 2000 deg/s");
break;
}
mpu.setFilterBandwidth(MPU6050_BAND_5_HZ);
Serial.print("Filter bandwidth set to: ");
switch (mpu.getFilterBandwidth()) {
case MPU6050_BAND_260_HZ:
Serial.println("260 Hz");
break;
case MPU6050_BAND_184_HZ:
Serial.println("184 Hz");
break;
case MPU6050_BAND_94_HZ:
Serial.println("94 Hz");
break;
case MPU6050_BAND_44_HZ:
Serial.println("44 Hz");
break;
case MPU6050_BAND_21_HZ:
Serial.println("21 Hz");
break;
case MPU6050_BAND_10_HZ:
Serial.println("10 Hz");
break;
case MPU6050_BAND_5_HZ:
Serial.println("5 Hz");
break;
}
Serial.println("");
delay(100);
}
void loop() {
/* Get new sensor events with the readings */
sensors_event_t a, g, temp;
mpu.getEvent(&a, &g, &temp);
if(digitalRead(calibrate_pin) == 1 and en == 0){
en = 1;
xcal = g.gyro.x;
ycal = g.gyro.y;
zcal = g.gyro.z;
}else if(digitalRead(calibrate_pin) == 0 and en == 1){
en = 0;
}
xval = g.gyro.x - xcal;
yval = g.gyro.y - ycal;
zval = g.gyro.z - zcal;
if (yval >= -2 and yval <= 2 and xval >= -2 and xval <= 2){
//back
message = "S";
radio.write(&message, sizeof(message));
}else if (xval >= 3){
//Right
message = "R";
radio.write(&message, sizeof(message));
}else if (xval <= -3){
//Left
message = "L";
radio.write(&message, sizeof(message));
}else if (yval >= 3){
//forward
message = "F";
radio.write(&message, sizeof(message));
}else if (yval <= -3){
//back
message = "B";
radio.write(&message, sizeof(message));
}
/* Print out the values */
// Serial.print("Acceleration X: ");
// Serial.print(a.acceleration.x);
// Serial.print(", Y: ");
// Serial.print(a.acceleration.y);
// Serial.print(", Z: ");
// Serial.print(a.acceleration.z);
// Serial.println(" m/s^2");
Serial.print("Rotation X: ");
Serial.print(xval);
Serial.print(", Y: ");
Serial.print(yval);
Serial.print(", Z: ");
Serial.print(zval);
Serial.println(" rad/s");
Serial.print("message:");
Serial.println(message);
// Serial.print("Temperature: ");
// Serial.print(temp.temperature);
// Serial.println(" degC");
Serial.println("");
delay(100);
}