Hi all, I'm trying to make a servo respond to data from an MPU6050. I'd like to be able to write a value to the servo based on 3 conditions:
- If the gyro is sensing acceleration then write a predefined value to the servo
- If the gyro is sensing deceleration then write a predefined value to the servo
- If the gyro is sensing neither (i.e traveling at a constant speed) then write a predefined value to the servo
Each circumstance has a different value written to the servo.
In my example below I'm trying to compare currentValue with previousValue.
Is there a better way to do this than what I have here? All I want is to be able to distinguish between the three states and react the servo accordingly.
I've reduced the ZAxGForce value from / 1000 to / 100 in order to have a larger value to play with
Any help is greatly appreciated!!
This is my first post so forgive me/let me know if I'm breaking any of the rules.
Thanks in advance!
// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"
#include "MPU6050.h"
#include <Servo.h>
Servo myservo;
Servo myservo2;// create servo object to control a servo
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
//MPU6050 accelgyro(0x69); // <-- use for AD0 high
int16_t ax, ay, az;
int16_t gx, gy, gz;
// uncomment "OUTPUT_READABLE_ACCELGYRO" if you want to see a tab-separated
// list of the accel X/Y/Z and then gyro X/Y/Z values in decimal. Easy to read,
// not so easy to parse, and slow(er) over UART.
#define OUTPUT_READABLE_ACCELGYRO
// uncomment "OUTPUT_BINARY_ACCELGYRO" to send all 6 axes of data as 16-bit
// binary, one right after the other. This is very fast (as fast as possible
// without compression or data loss), and easy to parse, but impossible to read
// for a human.
//#define OUTPUT_BINARY_ACCELGYRO
#define LED_PIN 13
bool blinkState = false;
int previousValue = 0;
int currentValue = 0;
void setup() {
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
myservo.attach(9); // attaches the servo on pin 9 to the servo object
myservo2.attach(8);
// initialize serial communication
// (38400 chosen because it works as well at 8MHz as it does at 16MHz, but
// it's really up to you depending on your project)
Serial.begin(38400);
// initialize device
Serial.println("Initializing I2C devices...");
accelgyro.initialize();
// verify connection
Serial.println("Testing device connections...");
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
// use the code below to change accel/gyro offset values
/*
Serial.println("Updating internal sensor offsets...");
// -76 -2359 1688 0 0 0
Serial.print(accelgyro.getXAccelOffset()); Serial.print("\t"); // -76
Serial.print(accelgyro.getYAccelOffset()); Serial.print("\t"); // -2359
Serial.print(accelgyro.getZAccelOffset()); Serial.print("\t"); // 1688
Serial.print(accelgyro.getXGyroOffset()); Serial.print("\t"); // 0
Serial.print(accelgyro.getYGyroOffset()); Serial.print("\t"); // 0
Serial.print(accelgyro.getZGyroOffset()); Serial.print("\t"); // 0
Serial.print("\n");
accelgyro.setXGyroOffset(220);
accelgyro.setYGyroOffset(76);
accelgyro.setZGyroOffset(-85);
Serial.print(accelgyro.getXAccelOffset()); Serial.print("\t"); // -76
Serial.print(accelgyro.getYAccelOffset()); Serial.print("\t"); // -2359
Serial.print(accelgyro.getZAccelOffset()); Serial.print("\t"); // 1688
Serial.print(accelgyro.getXGyroOffset()); Serial.print("\t"); // 0
Serial.print(accelgyro.getYGyroOffset()); Serial.print("\t"); // 0
Serial.print(accelgyro.getZGyroOffset()); Serial.print("\t"); // 0
Serial.print("\n");
*/
// configure Arduino LED for
pinMode(LED_PIN, OUTPUT);
}
int ZAxGForce = 0;
void loop() {
delay(500);
// read raw accel/gyro measurements from device
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
// these methods (and a few others) are also available
//accelgyro.getAcceleration(&ax, &ay, &az);
//accelgyro.getRotation(&gx, &gy, &gz);
#ifdef OUTPUT_READABLE_ACCELGYRO
// display tab-separated accel/gyro x/y/z values
Serial.print("a/g:\t");
// Serial.print(ax); Serial.print("\t");
// Serial.print(ay); Serial.print("\t");
Serial.print(az); Serial.print(" | ");Serial.print("G Force ZAx: "); Serial.println();
ZAxGForce = az / 100;
// Serial.print(gx); Serial.print("\t");
// Serial.print(gy); Serial.print("\t");
// Serial.println(gz);
#endif
#ifdef OUTPUT_BINARY_ACCELGYRO
Serial.write((uint8_t)(ax >> 8)); Serial.write((uint8_t)(ax & 0xFF));
Serial.write((uint8_t)(ay >> 8)); Serial.write((uint8_t)(ay & 0xFF));
Serial.write((uint8_t)(az >> 8)); Serial.write((uint8_t)(az & 0xFF));
Serial.write((uint8_t)(gx >> 8)); Serial.write((uint8_t)(gx & 0xFF));
Serial.write((uint8_t)(gy >> 8)); Serial.write((uint8_t)(gy & 0xFF));
Serial.write((uint8_t)(gz >> 8)); Serial.write((uint8_t)(gz & 0xFF));
#endif
// blink LED to indicate activity
blinkState = !blinkState;
digitalWrite(LED_PIN, blinkState);
int currentValue = ZAxGForce;
if (previousValue < currentValue){
Serial.println("AIRBRAKE");
myservo.write(130);
myservo2.write(130);
}
if (previousValue > currentValue){
Serial.println("DRS!");
myservo.write(80);
myservo2.write(80);
}
if (previousValue == currentValue){
Serial.println("ACTIVE!");
myservo.write(105);
myservo2.write(105);
}
Serial.print("Previous value ");
Serial.println(previousValue);
Serial.print("Current value ");
Serial.println(currentValue);
previousValue = currentValue;
}```