/*
Predator Shoulder Ca
nnon v2
by: Raymond Willis 8/1/17
r.willis_pi@yahoo.com
I edited serveral different pre-existing programs to
create this one. Too many to list all sources. If you
see your code, thank you.
This program uses 2 servos, 1 MPU6050 3 AXIS Module, 1 LED,
and 1 Laser Diode to have the servo mounted shoulder cannon
track your head position like in the Predator movies and
when a button is pressed the LED birghtens, fires the laser
diode for 0.5s, and then dims the LED (simulate a build up
of energy, firing, and then energy dissipation).
The I2CDev and MPU6050 libraries are required for this program.
The Servos follow the Y and Z axis of the MPU6050. The
module should be placed at the top-center of your head
or mask/helmet. When walking around, the Z axis will drift.
So at certain degrees for an extended period I assume the user
is walking around and go into scan mode, then when not walking
around I reset the MPU to re-zero the Z Axis.
I commented as much as I can to help anyone else use this.
*/
#include <Servo.h>
#include <string.h>
#include <stdio.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
Servo myservoY; // Roll
Servo myservoX; // Pitch
Servo myservoZ; // Yaw
int countA = 0;
int countB = 0;
int posY;
int posZ;
int rstZ;
int buttonState = LOW;
int oneFlash = 1; // variable to flash the LED Laser once per each button press.
const int buttonPin = 7; // Mom switch pin, fires laser diode
const int ledPin = 3; // LED
const int laserPin = 4; // Laser Diode
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu;
#define OUTPUT_READABLE_YAWPITCHROLL //Comment or uncomment to output data
#define LED_PIN 13
bool blinkState = false;
// 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
// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
// INTERRUPT DETECTION ROUTINE
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
void setup() {
pinMode(ledPin, OUTPUT);
pinMode(laserPin, OUTPUT);
pinMode(buttonPin, INPUT);
// 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
myservoY.attach(9); // Attach Y servo to pin 9
//myservoX.attach(10);// Attach X servo to pin 10, not used
myservoZ.attach(11); // Attach Z servo to pin 11
Serial.begin(115200);
while (!Serial); // wait for 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(2); //was 49
mpu.setYGyroOffset(-29); //was -26
mpu.setZGyroOffset(36); // was 13
mpu.setZAccelOffset(923); // 1688 factory default for my test chip 1459
// 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 {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
// configure LED for output
pinMode(LED_PIN, OUTPUT);
}
// Main Program Loop
void loop() {
// if programming failed, don't try to do anything
if (!dmpReady) return;
// wait for MPU interrupt or extra packet(s) available
while (!mpuInterrupt && fifoCount < packetSize) {
// other program behavior stuff here if oyu want
}
// 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) {
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;
#ifdef OUTPUT_READABLE_YAWPITCHROLL
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
Serial.print("ypr\t");
Serial.print(ypr[0] * 180 / M_PI);
myservoZ.write(int(ypr[0] * 180 / M_PI) + 90); // Rotation around Z
rstZ = (ypr[0] * 180 / M_PI); // Z axis into a variable to test
Serial.print("\t");
Serial.print(ypr[1] * 180 / M_PI);
myservoY.write(int(ypr[1] * 180 / M_PI) + 90); // Rotation around Y
Serial.print("\t");
Serial.println(ypr[2] * 180 / M_PI);
myservoX.write(int(ypr[2] * 180 / M_PI) + 90); // Rotation around X
#endif
/* If the Z axis servo is max left or right for so many seconds,
you are probably walking around and we need to re-orient the
Z Axis gyro's earth reference point to maintain correct orientation
for tracking head movement.
*/
if (rstZ > 100 || rstZ < (-100)) { // test if Z Axis is at max L or R
countA++; // count each time too far L or R
if (countA >= 3) { //if max L or R, 3 consecutive times..reset MPU
resetMode();
}
}
if (rstZ < 99 && rstZ > (-99)) { // when outside L or R max
countA = 0; // reset count to 0, you are likely stationary
}
buttonState = digitalRead(buttonPin); //read the firing button state
if (buttonState == HIGH) { // if the button reads High, the button is pressed
posY = (ypr[1] * 180 / M_PI); //read Y position
posZ = (ypr[0] * 180 / M_PI); //read Z position
myservoY.write(posY + 90); //stop moving and stay at last know positions
myservoZ.write(posZ + 90);
laserFire(); //perform the laser firing function
}
else if (buttonState == LOW) {
// If Im not holding the button, turn everything off:
analogWrite(ledPin, 0);
digitalWrite(laserPin, LOW);
oneFlash = 1;
}
}
Serial.print("Reset Count: ");
Serial.println(countA);
Serial.print("Laser Fire Button: ");
Serial.println(buttonState);
}
void resetMode() { // reset function
Serial.println("You must be walking around so let's reset Z Axis reference point!");
countA = 0;
mpu.reset(); // reset the Gyros to reset Z axis reference point
setup(); // re-initialize everything for new gyro data
}
void scanMode() { // scan mode function
Serial.println("We are in Auto Scan Mode.");
posY = 90; // set Y servo to 90 degrees
for (posZ = 45; posZ <= 115; posZ += 1) // start scanning Z servo L to R
{
myservoZ.write(posZ);
delay(60);
}
for (posZ = 115; posZ >= 45; posZ -= 1)
{
myservoZ.write(posZ);
delay(60);
}
}
void laserFire() { //Led & laser diode firing function
if (oneFlash >= 1) {
for (int fadeValue = 10 ; fadeValue <= 255; fadeValue += 5) {
delay(10);
analogWrite(ledPin, fadeValue); // LED glows bright, charging up
}
analogWrite(ledPin, 255); // after ramp up, keep on
//recreate mini recoil
digitalWrite(laserPin, HIGH); //energize laser diode
delay (500); // Laser Pointer ON time in millisecs after LED ramp up is done.
digitalWrite(laserPin, LOW); //laser diode off
for (int fadeValue = 255 ; fadeValue >= 0; fadeValue -= 10) {
delay(5);
analogWrite(ledPin, fadeValue); // LED dims down.
}
analogWrite(ledPin, 0); //led off
oneFlash = 0; //set button count to ensure it only fires once per button press
}
}