mirror of
https://github.com/PotentiaRobotics/engine-software.git
synced 2025-04-21 12:30:16 -04:00
Spi parsing works
This commit is contained in:
FluffyCube9343
parent
cf35546265
commit
2c635f770d
|
|
@ -46,15 +46,15 @@ THE SOFTWARE.
|
|||
|
||||
// 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 <I2Cdev.h>
|
||||
|
||||
#include "MPU6050_6Axis_MotionApps20.h"
|
||||
//#include "MPU6050.h" // not necessary if using MotionApps include file
|
||||
#include <MPU6050_6Axis_MotionApps20.h>
|
||||
#include <MPU6050.h> // not necessary if using MotionApps include file
|
||||
|
||||
// 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"
|
||||
#include <Wire.h>
|
||||
#endif
|
||||
|
||||
// class default I2C address is 0x68
|
||||
|
|
|
|||
|
|
@ -35,5 +35,5 @@ void loop(void)
|
|||
Serial.print("\t");
|
||||
|
||||
Serial.println((char) Mastereceive);
|
||||
delay(250);
|
||||
delay(25);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -12,8 +12,17 @@ volatile boolean received;
|
|||
volatile byte Slavereceived,Slavesend;
|
||||
int buttonvalue;
|
||||
int x = 65;
|
||||
int wai = -1;
|
||||
String t1 = "1.00 0.00 0.00 0.00";
|
||||
int wai = -1; //I forgot what WAI stood for, but basically counts what index the counter is at, if the string was a list or smth
|
||||
//-1 = at start
|
||||
//0 to t1.length()-1 = data
|
||||
//t1.length = at end
|
||||
String t0 = "1.00 0.00 0.00 0.00";
|
||||
String t1 = "1.00 0.00 0.00 0.00"; //will be changed to actual data in code.
|
||||
String t2 = "180.00 180.00 180.00 0.00";
|
||||
String t3 = "100.00 0.00 -4.00 0.07";
|
||||
String t4 = "thisisbaddatalmaothisshouldnotwork";
|
||||
|
||||
|
||||
char Buf[32];
|
||||
int onset = 1;
|
||||
void setup()
|
||||
|
|
@ -45,8 +54,11 @@ void loop()
|
|||
if (Slavereceived==8)
|
||||
{
|
||||
if(wai==-1 && t1.length()>18 && t1.length()<33){
|
||||
//accepting a new string of data
|
||||
t1.toCharArray(Buf,t1.length());
|
||||
|
||||
}
|
||||
//regular code -> bytewise.
|
||||
if(t1.length()>18 && t1.length()<33){
|
||||
|
||||
|
||||
|
|
@ -54,28 +66,29 @@ void loop()
|
|||
|
||||
|
||||
|
||||
|
||||
//detecting which byte the data reading is on the string.
|
||||
Serial.print(wai);
|
||||
if(wai==t1.length()){x = 'e'; wai = -2; t1 = "wqieuroqwueyroqwueyroiweuqyrioqwueyrioqweuryoiqweuryoiqweurywq";}
|
||||
if(wai==t1.length()){x = 'e'; wai = -2; }/*t1 = "wqieuroqwueyroqwueyroiweuqyrioqwueyrioqweuryoiqweuryoiqweurywq";}*/ //end data byte, -2 + 1 = -1, which accepts data.
|
||||
else{
|
||||
if(wai == -1){x = 's';}
|
||||
if(wai == -1){x = 's';} //start byte
|
||||
|
||||
else{
|
||||
x = t1[wai];}
|
||||
x = t1[wai];} //actual data byte
|
||||
}}
|
||||
|
||||
Slavesend=x;
|
||||
if(x==laste && x == (int) 'e'){
|
||||
laste = x;
|
||||
|
||||
x = 'b';
|
||||
//"bad" data byte
|
||||
x = 'b'; //"if there was an end byte show that there is no data"
|
||||
}
|
||||
if(onset==1){
|
||||
//"onset" byte (to avoid data offsets with actual bytes.
|
||||
x = 'o';
|
||||
wai--;
|
||||
onset = 0;
|
||||
}
|
||||
laste = x;
|
||||
laste = x; //"when was the last e"
|
||||
|
||||
SPDR = Slavesend;
|
||||
SPDR = x;//Sends the x value to master via SPDR
|
||||
|
|
|
|||
542
communication/SPI/v0.6_S_SPI_INTEGRATE/v0.6_S_SPI_INTEGRATE.ino
Normal file
542
communication/SPI/v0.6_S_SPI_INTEGRATE/v0.6_S_SPI_INTEGRATE.ino
Normal file
|
|
@ -0,0 +1,542 @@
|
|||
// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
|
||||
// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
|
||||
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
|
||||
//
|
||||
// Changelog:
|
||||
// 2019-07-08 - Added Auto Calibration and offset generator
|
||||
// - and altered FIFO retrieval sequence to avoid using blocking code
|
||||
// 2016-04-18 - Eliminated a potential infinite loop
|
||||
// 2013-05-08 - added seamless Fastwire support
|
||||
// - added note about gyro calibration
|
||||
// 2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error
|
||||
// 2012-06-20 - improved FIFO overflow handling and simplified read process
|
||||
// 2012-06-19 - completely rearranged DMP initialization code and simplification
|
||||
// 2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly
|
||||
// 2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING
|
||||
// 2012-06-05 - add gravity-compensated initial reference frame acceleration output
|
||||
// - add 3D math helper file to DMP6 example sketch
|
||||
// - add Euler output and Yaw/Pitch/Roll output formats
|
||||
// 2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee)
|
||||
// 2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250
|
||||
// 2012-05-30 - basic DMP initialization working
|
||||
|
||||
/* ============================================
|
||||
I2Cdev device library code is placed under the MIT license
|
||||
Copyright (c) 2012 Jeff Rowberg
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
copies of the Software, and to permit persons to whom the Software is
|
||||
furnished to do so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in
|
||||
all copies or substantial portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
THE SOFTWARE.
|
||||
===============================================
|
||||
*/
|
||||
|
||||
// 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
|
||||
int laste = 0;
|
||||
#include<SPI.h>
|
||||
#define LEDpin 7
|
||||
#define buttonpin 2
|
||||
volatile boolean received;
|
||||
volatile byte Slavereceived,Slavesend;
|
||||
int buttonvalue;
|
||||
int x = 65;
|
||||
int wai = -1; //I forgot what WAI stood for, but basically counts what index the counter is at, if the string was a list or smth
|
||||
//-1 = at start
|
||||
//0 to t1.length()-1 = data
|
||||
//t1.length = at end
|
||||
String t0 = "1.00 0.00 0.00 0.00";
|
||||
String t1 = "1.00 0.00 0.00 0.00"; //will be changed to actual data in code.
|
||||
#include "I2Cdev.h"
|
||||
|
||||
#include "MPU6050_6Axis_MotionApps20.h"
|
||||
//#include "MPU6050.h" // not necessary if using MotionApps include file
|
||||
|
||||
// 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 SparkFun breakout and InvenSense evaluation board)
|
||||
// AD0 high = 0x69
|
||||
MPU6050 mpu;
|
||||
MPU6050 mpu2(0x69); // <-- use for AD0 high
|
||||
|
||||
char Buf[32];
|
||||
int onset = 1;
|
||||
/* =========================================================================
|
||||
NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
|
||||
depends on the MPU-6050's INT pin being connected to the Arduino's
|
||||
external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
|
||||
digital I/O pin 2.
|
||||
* ========================================================================= */
|
||||
|
||||
/* =========================================================================
|
||||
NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
|
||||
when using Serial.write(buf, len). The Teapot output uses this method.
|
||||
The solution requires a modification to the Arduino USBAPI.h file, which
|
||||
is fortunately simple, but annoying. This will be fixed in the next IDE
|
||||
release. For more info, see these links:
|
||||
|
||||
http://arduino.cc/forum/index.php/topic,109987.0.html
|
||||
http://code.google.com/p/arduino/issues/detail?id=958
|
||||
* ========================================================================= */
|
||||
|
||||
|
||||
|
||||
// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
|
||||
// quaternion components in a [w, x, y, z] format (not best for parsing
|
||||
// on a remote host such as Processing or something though)
|
||||
#define OUTPUT_READABLE_QUATERNION
|
||||
|
||||
// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
|
||||
// (in degrees) calculated from the quaternions coming from the FIFO.
|
||||
// Note that Euler angles suffer from gimbal lock (for more info, see
|
||||
// http://en.wikipedia.org/wiki/Gimbal_lock)
|
||||
//#define OUTPUT_READABLE_EULER
|
||||
|
||||
// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
|
||||
// pitch/roll angles (in degrees) calculated from the quaternions coming
|
||||
// from the FIFO. Note this also requires gravity vector calculations.
|
||||
// Also note that yaw/pitch/roll angles suffer from gimbal lock (for
|
||||
// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
|
||||
//#define OUTPUT_READABLE_YAWPITCHROLL
|
||||
|
||||
// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
|
||||
// components with gravity removed. This acceleration reference frame is
|
||||
// not compensated for orientation, so +X is always +X according to the
|
||||
// sensor, just without the effects of gravity. If you want acceleration
|
||||
// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
|
||||
//#define OUTPUT_READABLE_REALACCEL
|
||||
|
||||
// uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
|
||||
// components with gravity removed and adjusted for the world frame of
|
||||
// reference (yaw is relative to initial orientation, since no magnetometer
|
||||
// is present in this case). Could be quite handy in some cases.
|
||||
//#define OUTPUT_READABLE_WORLDACCEL
|
||||
|
||||
// uncomment "OUTPUT_TEAPOT" if you want output that matches the
|
||||
// format used for the InvenSense teapot demo
|
||||
//#define OUTPUT_TEAPOT
|
||||
|
||||
|
||||
|
||||
#define INTERRUPT_PIN 2 // use pin 2 on Arduino Uno & most boards
|
||||
#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
|
||||
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 mpu2IntStatus; // holds actual interrupt status byte from MPU
|
||||
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
|
||||
|
||||
uint8_t devStatus2; // return status after each device operation (0 = success, !0 = error)
|
||||
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
|
||||
|
||||
uint16_t packetSize2; // 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
|
||||
|
||||
uint16_t fifoCount2; // count of all bytes currently in FIFO
|
||||
uint8_t fifoBuffer2[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
|
||||
|
||||
float ypr2[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
|
||||
// packet structure for InvenSense teapot demo
|
||||
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
|
||||
|
||||
|
||||
|
||||
// ================================================================
|
||||
// === INTERRUPT DETECTION ROUTINE ===
|
||||
// ================================================================
|
||||
|
||||
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
|
||||
void dmpDataReady() {
|
||||
mpuInterrupt = true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
// ================================================================
|
||||
// === INITIAL SETUP ===
|
||||
// ================================================================
|
||||
|
||||
void setup() {
|
||||
// join I2C bus (I2Cdev library doesn't do this automatically)
|
||||
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
|
||||
Wire.begin();
|
||||
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
|
||||
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
|
||||
Fastwire::setup(400, true);
|
||||
#endif
|
||||
|
||||
// initialize serial communication
|
||||
// (115200 chosen because it is required for Teapot Demo output, but it's
|
||||
// really up to you depending on your project)
|
||||
Serial.begin(115200);
|
||||
while (!Serial); // wait for Leonardo enumeration, others continue immediately
|
||||
|
||||
// NOTE: 8MHz or slower host processors, like the Teensy @ 3.3V or Arduino
|
||||
// Pro Mini running at 3.3V, cannot handle this baud rate reliably due to
|
||||
// the baud timing being too misaligned with processor ticks. You must use
|
||||
// 38400 or slower in these cases, or use some kind of external separate
|
||||
// crystal solution for the UART timer.
|
||||
|
||||
// initialize device
|
||||
Serial.println(F("Initializing I2C devices..."));
|
||||
mpu.initialize();
|
||||
|
||||
mpu2.initialize();
|
||||
pinMode(INTERRUPT_PIN, INPUT);
|
||||
|
||||
// verify connection
|
||||
Serial.println(F("Testing device connections..."));
|
||||
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
|
||||
|
||||
Serial.println(mpu2.testConnection() ? F("MPU6050 #2 connection successful") : F("MPU6050 #2 connection failed"));
|
||||
|
||||
// wait for ready
|
||||
Serial.println(F("\nSend any character to begin DMP programming and demo: "));
|
||||
while (Serial.available() && Serial.read()); // empty buffer
|
||||
while (!Serial.available()); // wait for data
|
||||
while (Serial.available() && Serial.read()); // empty buffer again
|
||||
|
||||
// load and configure the DMP
|
||||
Serial.println(F("Initializing DMP..."));
|
||||
devStatus = mpu.dmpInitialize();
|
||||
|
||||
devStatus = mpu2.dmpInitialize();
|
||||
|
||||
// supply your own gyro offsets here, scaled for min sensitivity
|
||||
mpu.setXGyroOffset(220);
|
||||
mpu.setYGyroOffset(76);
|
||||
mpu.setZGyroOffset(-85);
|
||||
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
|
||||
|
||||
mpu2.setXGyroOffset(220);
|
||||
mpu2.setYGyroOffset(76);
|
||||
mpu2.setZGyroOffset(-85);
|
||||
mpu2.setZAccelOffset(1788); // 1688 factory default for my test chip
|
||||
|
||||
// make sure it worked (returns 0 if so)
|
||||
if (devStatus == 0 and devStatus2 == 0) {
|
||||
// Calibration Time: generate offsets and calibrate our MPU6050
|
||||
mpu.CalibrateAccel(6);
|
||||
mpu.CalibrateGyro(6);
|
||||
mpu.PrintActiveOffsets();
|
||||
// turn on the DMP, now that it's ready
|
||||
Serial.println(F("Enabling DMP..."));
|
||||
mpu.setDMPEnabled(true);
|
||||
|
||||
mpu2.CalibrateAccel(6);
|
||||
mpu2.CalibrateGyro(6);
|
||||
mpu2.PrintActiveOffsets();
|
||||
// turn on the DMP, now that it's ready
|
||||
Serial.println(F("Enabling DMP on 0x69..."));
|
||||
mpu2.setDMPEnabled(true);
|
||||
|
||||
// enable Arduino interrupt detection
|
||||
Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
|
||||
Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
|
||||
Serial.println(F(")..."));
|
||||
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
|
||||
mpuIntStatus = mpu.getIntStatus();
|
||||
mpu2IntStatus = 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();
|
||||
|
||||
packetSize2 = 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);
|
||||
pinMode(buttonpin,INPUT); // Setting pin 2 as INPUT
|
||||
pinMode(LEDpin,OUTPUT); // Setting pin 7 as OUTPUT
|
||||
pinMode(MISO,OUTPUT); //Sets MISO as OUTPUT (Have to Send data to Master IN
|
||||
|
||||
SPCR |= _BV(SPE); //Turn on SPI in Slave Mode
|
||||
received = false;
|
||||
|
||||
SPI.attachInterrupt(); //Interuupt ON is set for SPI commnucation
|
||||
Serial.println("SPI set up!");
|
||||
}
|
||||
|
||||
ISR (SPI_STC_vect) //Inerrrput routine function
|
||||
{
|
||||
Slavereceived = SPDR; // Value received from master if store in variable slavereceived
|
||||
received = true; //Sets received as True
|
||||
}
|
||||
|
||||
// ================================================================
|
||||
// === MAIN PROGRAM LOOP ===
|
||||
// ================================================================
|
||||
|
||||
void loop() {
|
||||
// if programming failed, don't try to do anything
|
||||
if (!dmpReady) return;
|
||||
// read a packet from FIFO
|
||||
if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet
|
||||
#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.print(q.x);
|
||||
Serial.print("\t");
|
||||
Serial.print(q.y);
|
||||
Serial.print("\t");
|
||||
Serial.println(q.z);
|
||||
t1 = String(q.w)+" "+String(q.x)+" "+String(q.y)+" "+String(q.z);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_READABLE_EULER
|
||||
// display Euler angles in degrees
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
mpu.dmpGetEuler(euler, &q);
|
||||
Serial.print("euler\t");
|
||||
Serial.print(euler[0] * 180/M_PI);
|
||||
Serial.print("\t");
|
||||
Serial.print(euler[1] * 180/M_PI);
|
||||
Serial.print("\t");
|
||||
Serial.println(euler[2] * 180/M_PI);
|
||||
#endif
|
||||
|
||||
#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);
|
||||
Serial.print("\t");
|
||||
Serial.print(ypr[1] * 180/M_PI);
|
||||
Serial.print("\t");
|
||||
Serial.println(ypr[2] * 180/M_PI);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_READABLE_REALACCEL
|
||||
// display real acceleration, adjusted to remove gravity
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
mpu.dmpGetAccel(&aa, fifoBuffer);
|
||||
mpu.dmpGetGravity(&gravity, &q);
|
||||
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
|
||||
Serial.print("areal\t");
|
||||
Serial.print(aaReal.x);
|
||||
Serial.print("\t");
|
||||
Serial.print(aaReal.y);
|
||||
Serial.print("\t");
|
||||
Serial.println(aaReal.z);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_READABLE_WORLDACCEL
|
||||
// display initial world-frame acceleration, adjusted to remove gravity
|
||||
// and rotated based on known orientation from quaternion
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
mpu.dmpGetAccel(&aa, fifoBuffer);
|
||||
mpu.dmpGetGravity(&gravity, &q);
|
||||
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
|
||||
mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
|
||||
Serial.print("aworld\t");
|
||||
Serial.print(aaWorld.x);
|
||||
Serial.print("\t");
|
||||
Serial.print(aaWorld.y);
|
||||
Serial.print("\t");
|
||||
Serial.println(aaWorld.z);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_TEAPOT
|
||||
// display quaternion values in InvenSense Teapot demo format:
|
||||
teapotPacket[2] = fifoBuffer[0];
|
||||
teapotPacket[3] = fifoBuffer[1];
|
||||
teapotPacket[4] = fifoBuffer[4];
|
||||
teapotPacket[5] = fifoBuffer[5];
|
||||
teapotPacket[6] = fifoBuffer[8];
|
||||
teapotPacket[7] = fifoBuffer[9];
|
||||
teapotPacket[8] = fifoBuffer[12];
|
||||
teapotPacket[9] = fifoBuffer[13];
|
||||
Serial.write(teapotPacket, 14);
|
||||
teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
|
||||
#endif
|
||||
|
||||
// blink LED to indicate activity
|
||||
blinkState = !blinkState;
|
||||
digitalWrite(LED_PIN, blinkState);
|
||||
}
|
||||
// read a packet from 2nd mpu FIFO
|
||||
if (mpu2.dmpGetCurrentFIFOPacket(fifoBuffer2)) { // Get the Latest packet
|
||||
#ifdef OUTPUT_READABLE_QUATERNION
|
||||
// display quaternion values in easy matrix form: w x y z
|
||||
mpu2.dmpGetQuaternion(&q, fifoBuffer2);
|
||||
Serial.print("quat2\t");
|
||||
Serial.print(q.w);
|
||||
Serial.print("\t");
|
||||
Serial.print(q.x);
|
||||
Serial.print("\t");
|
||||
Serial.print(q.y);
|
||||
Serial.print("\t");
|
||||
Serial.println(q.z);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_READABLE_EULER
|
||||
// display Euler angles in degrees
|
||||
mpu2.dmpGetQuaternion(&q, fifoBuffer2);
|
||||
mpu2.dmpGetEuler(euler, &q);
|
||||
Serial.print("euler2\t");
|
||||
Serial.print(euler[0] * 180/M_PI);
|
||||
Serial.print("\t");
|
||||
Serial.print(euler[1] * 180/M_PI);
|
||||
Serial.print("\t");
|
||||
Serial.println(euler[2] * 180/M_PI);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_READABLE_YAWPITCHROLL
|
||||
// display Euler angles in degrees
|
||||
mpu2.dmpGetQuaternion(&q, fifoBuffer2);
|
||||
mpu2.dmpGetGravity(&gravity, &q);
|
||||
mpu2.dmpGetYawPitchRoll(ypr, &q, &gravity);
|
||||
Serial.print("ypr2\t");
|
||||
Serial.print(ypr[0] * 180/M_PI);
|
||||
Serial.print("\t");
|
||||
Serial.print(ypr[1] * 180/M_PI);
|
||||
Serial.print("\t");
|
||||
Serial.println(ypr[2] * 180/M_PI);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_READABLE_REALACCEL
|
||||
// display real acceleration, adjusted to remove gravity
|
||||
mpu2.dmpGetQuaternion(&q, fifoBuffer2);
|
||||
mpu2.dmpGetAccel(&aa, fifoBuffer2);
|
||||
mpu2.dmpGetGravity(&gravity, &q);
|
||||
mpu2.dmpGetLinearAccel(&aaReal, &aa, &gravity);
|
||||
Serial.print("areal2\t");
|
||||
Serial.print(aaReal.x);
|
||||
Serial.print("\t");
|
||||
Serial.print(aaReal.y);
|
||||
Serial.print("\t");
|
||||
Serial.println(aaReal.z);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_READABLE_WORLDACCEL
|
||||
// display initial world-frame acceleration, adjusted to remove gravity
|
||||
// and rotated based on known orientation from quaternion
|
||||
mpu2.dmpGetQuaternion(&q, fifoBuffer2);
|
||||
mpu2.dmpGetAccel(&aa, fifoBuffer2);
|
||||
mpu2.dmpGetGravity(&gravity, &q);
|
||||
mpu2.dmpGetLinearAccel(&aaReal, &aa, &gravity);
|
||||
mpu2.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
|
||||
Serial.print("aworld2\t");
|
||||
Serial.print(aaWorld.x);
|
||||
Serial.print("\t");
|
||||
Serial.print(aaWorld.y);
|
||||
Serial.print("\t");
|
||||
Serial.println(aaWorld.z);
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_TEAPOT
|
||||
// display quaternion values in InvenSense Teapot demo format:
|
||||
teapotPacket[2] = fifoBuffer2[0];
|
||||
teapotPacket[3] = fifoBuffer2[1];
|
||||
teapotPacket[4] = fifoBuffer2[4];
|
||||
teapotPacket[5] = fifoBuffer2[5];
|
||||
teapotPacket[6] = fifoBuffer2[8];
|
||||
teapotPacket[7] = fifoBuffer2[9];
|
||||
teapotPacket[8] = fifoBuffer2[12];
|
||||
teapotPacket[9] = fifoBuffer2[13];
|
||||
Serial.write(teapotPacket, 14);
|
||||
teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
|
||||
#endif
|
||||
|
||||
// blink LED to indicate activity
|
||||
blinkState = !blinkState;
|
||||
digitalWrite(LED_PIN, blinkState);
|
||||
}
|
||||
if(received) //Logic to SET LED ON OR OFF depending upon the value recerived from master
|
||||
{
|
||||
if (Slavereceived==8)
|
||||
{
|
||||
if(wai==-1 && t1.length()>18 && t1.length()<33){
|
||||
//accepting a new string of data
|
||||
t1.toCharArray(Buf,t1.length());
|
||||
|
||||
}
|
||||
//regular code -> bytewise.
|
||||
if(t1.length()>18 && t1.length()<33){
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
//detecting which byte the data reading is on the string.
|
||||
Serial.print(wai);
|
||||
if(wai==t1.length()){x = 'e'; wai = -2; }/*t1 = "wqieuroqwueyroqwueyroiweuqyrioqwueyrioqweuryoiqweuryoiqweurywq";}*/ //end data byte, -2 + 1 = -1, which accepts data.
|
||||
else{
|
||||
if(wai == -1){x = 's';} //start byte
|
||||
|
||||
else{
|
||||
x = t1[wai];} //actual data byte
|
||||
}}
|
||||
|
||||
Slavesend=x;
|
||||
if(x==laste && x == (int) 'e'){
|
||||
laste = x;
|
||||
//"bad" data byte
|
||||
x = 'b'; //"if there was an end byte show that there is no data"
|
||||
}
|
||||
if(onset==1){
|
||||
//"onset" byte (to avoid data offsets with actual bytes.
|
||||
x = 'o';
|
||||
wai--;
|
||||
onset = 0;
|
||||
}
|
||||
laste = x; //"when was the last e"
|
||||
|
||||
SPDR = Slavesend;
|
||||
SPDR = x;//Sends the x value to master via SPDR
|
||||
wai++;
|
||||
|
||||
}
|
||||
delay(15);
|
||||
}
|
||||
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user