Skip to content

Instantly share code, notes, and snippets.

@vivekanandRdhakane

vivekanandRdhakane/Area_meter_esp3.ino

Last active April 25, 2020 05:56
Show Gist options
  • Save vivekanandRdhakane/3c3c917400d968ae71d67b4a402e91c6 to your computer and use it in GitHub Desktop.
Save vivekanandRdhakane/3c3c917400d968ae71d67b4a402e91c6 to your computer and use it in GitHub Desktop.
Download ZIP
Raw
Area_meter_esp3.ino
// ESP8266 Code for StokeMapak
// Author : Vivekanand Dhakane
// Updated on: 25/4/2020
#include "I2Cdev.h"
#include <ESP8266WiFi.h>
#include <WiFiUdp.h>
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu;
#define OUTPUT_READABLE_YAWPITCHROLL
#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)
#define scroll_1 1
#define scroll_2 3
boolean pre_a, pre_b, main_pre_a, main_pre_b;
boolean a, b;
int time_diff, count;
float pre_time;
boolean fwd = true;
int angle = 180;
const char* comp_IP = "your PC IP address"; // your PC IP address
boolean mpu_set =false;
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
const char* ssid = "MotoG";
const char* password = "123456789";
WiFiUDP Udp;
unsigned int localUdpPort = 4210; // local port to listen on
// 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
int data_count;
// packet structure for InvenSense teapot demo
char teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, 'Y', 'Z' };
float pretime;
char angle_yaw_globle[4]={ 0, 0, 0,0 };
// ================================================================
// === 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(0,2);
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
/*_________Connecting to wifi____________________________*/
//Serial.printf("Connecting to %s ", ssid);
WiFi.begin(ssid, password);
while (WiFi.status() != WL_CONNECTED)
{
delay(500);
//Serial.print(".");
}
//Serial.println(" connected");
Udp.begin(localUdpPort);
char Started[3] = {'U','D','P'};
Udp.beginPacket(comp_IP, 4210);
Udp.write(Started, 3);
Udp.endPacket();
//Serial.printf("Now listening at IP %s, UDP port %d\n", WiFi.localIP().toString().c_str(), localUdpPort);
/*_____________________________________________________________________________________________________________*/
// 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();
//pinMode(INTERRUPT_PIN, INPUT);
// verify connection
//Serial.println(F("Testing device connections..."));
//Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 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();
// 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
// make sure it worked (returns 0 if so)
if (devStatus == 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);
// 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();
// 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();
char msg[3] = {'M','I','N'};
Udp.beginPacket(comp_IP, 4210);
Udp.write(msg, 3);
Udp.endPacket();
} 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(")"));
char msg[3] = {'M','E','R'};
Udp.beginPacket(comp_IP, 4210);
Udp.write(msg, 3);
Udp.endPacket();
}
// configure LED for output
}
// ================================================================
// === MAIN PROGRAM LOOP ===
// ================================================================
void loop() {
// if programming failed, don't try to do anything
if (!dmpReady) return;
//delay(3);
if(mpu_set)
{
read_scroll();
}
// wait for MPU interrupt or extra packet(s) available
int empty_count =0;
while (fifoCount < packetSize) {
if (fifoCount < packetSize) {
// try to get out of the infinite loop
fifoCount = mpu.getFIFOCount();
}
if(empty_count>10)
{
if(empty_count%100==0)
{
set_mpu();
//
}
}
empty_count++;
/* Serial.print("empty_count =");
Serial.println(empty_count);*/
// other program behavior stuff here
// .
// .
// .
// if you are really paranoid you can frequently test in between other
// stuff to see if mpuInterrupt is true, and if so, "break;" from the
// while() loop to immediately process the MPU data
// .
// .
// .
}
// reset interrupt flag and get INT_STATUS byte
// mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
/* Serial.print(" mpuIntStatus =");
Serial.println(mpuIntStatus);*/
// get current FIFO count
fifoCount = mpu.getFIFOCount();
if(fifoCount < packetSize){
//Lets go back and wait for another interrupt. We shouldn't be here, we got an interrupt from another event
// This is blocking so don't do it while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
/* Serial.print(" blocking fifoCount =");
Serial.println(fifoCount);*/
}
// fifoCount = mpu.getFIFOCount(); // will be zero after reset no need to ask
// check for overflow (this should never happen unless our code is too inefficient)
else if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || 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 & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) {
// read a packet from FIFO
while(fifoCount >= packetSize){ // Lets catch up to NOW, someone is using the dreaded delay()!
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_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);
#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);
int yaw = int (ypr[0] * 180/M_PI);
int pitch = int (ypr[1] * 180/M_PI);
int roll = int (ypr[2] * 180/M_PI);
/* Serial.print("ypr\t");
Serial.print(roll);
Serial.print("\t");
Serial.print(pitch);
Serial.print("\t");
Serial.println(yaw);*/
String roll_s = String(roll);
String pitch_s = String(pitch) ;
String yaw_s = String(yaw);
char start_char[1]={'#'};
char end_char[1]={'*'};
char angle_roll[4]={ 0, 0, 0,0 };
char angle_pitch[4]={ 0, 0, 0,0};
char angle_yaw[4]={ 0, 0, 0,0 };
int index = 0;
for(int i=0;i<roll_s.length();i++)
{
angle_roll[i]= roll_s[i];
index=i;
}
angle_roll[++index] = '!';
for(int i=0;i<pitch_s.length();i++)
{
angle_pitch[i]= pitch_s[i];
index=i;
}
angle_pitch[++index] = '@';
for(int i=0;i<yaw_s.length();i++)
{
angle_yaw[i]= yaw_s[i];
index=i;
}
for(int i=0;i<4;i++)
{
angle_yaw_globle[i] =angle_yaw[i];
}
mpu_set =true;
//Serial.write(start_char,1);
//Serial.write(angle_roll,4);
//Serial.write(angle_pitch,4);
//Serial.write(angle_yaw,4);
//Serial.write(end_char,1);
//Serial.println();
//Serial.println();
/* Udp.beginPacket("192.168.43.28", 4210);
Udp.write(start_char,1);
Udp.write(angle_roll,4);
Udp.write(angle_pitch,4);
Udp.write(angle_yaw,4);
Udp.write(end_char,1);
Udp.endPacket();*/
//
// if(data_count%100 ==0)
// {
//Serial.print(">>>>");
/*Serial.println(millis()- pretime);
pretime = millis();*/
mpu.resetFIFO();
// }
#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.print(data_count);
Serial.print("=");
Serial.write(teapotPacket, 14);
Serial.println();
/* Udp.beginPacket("192.168.43.28", 4210);
Udp.write(data_count);
Udp.endPacket();*/
/* Serial.println(".");
Udp.beginPacket("192.168.43.28", 4210);
Udp.write(teapotPacket,14);
Udp.endPacket();
data_count++;
teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
mpu.resetFIFO();
*/
/*
mpu.dmpGetQuaternion(&q, fifoBuffer);
Serial.print((float)q.w);
Serial.print("@");
Serial.print((float)q.x);
Serial.print("#");
Serial.print((float)q.y);
Serial.print("&");
Serial.println((float)q.z);**/
// #endif
}
//mpu.resetFIFO();
}
void set_mpu()
{
// initialize device
/* mpu.initialize();
// load and configure the DMP
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
mpu.CalibrateAccel(6);
mpu.CalibrateGyro(6);
mpu.PrintActiveOffsets();
*/
devStatus = mpu.dmpInitialize();
mpu.setDMPEnabled(true);
mpuIntStatus = mpu.getIntStatus();
mpu.resetFIFO();
//packetSize = mpu.dmpGetFIFOPacketSize();
}
void read_scroll() {
// read the input pin:
main_pre_a = a;
main_pre_b = b;
a = digitalRead(scroll_1);
b = digitalRead(scroll_2);
//byte cur = a,b;
/*Serial.print(a);
Serial.print(" ");
Serial.println(b); */
if ((a == 0 && b == 0) || (a == 1 && b == 1) )
{
if ((pre_a == a) && (pre_b == b))
{
//Serial.println("same");
}
else
{
time_diff = millis() - pre_time;
pre_time = millis();
/*Serial.print(a);
Serial.print(" ");
Serial.println(b);
Serial.print(pre_a);
Serial.print(" ");
Serial.println(pre_b);*/
if (time_diff > 20)
{
count++;
/*Serial.print(a);
Serial.print(" ");
Serial.println(b);
Serial.print(main_pre_a);
Serial.print(" ");
Serial.println(main_pre_b);*/
if ((a == 0 && b == 0) && (main_pre_a == 1 && main_pre_b == 1))
{
fwd = true;
}
else if ((a == 0 && b == 0) && (main_pre_a == 0 && main_pre_b == 1)) {
fwd = false;
}
else if ((a == 1 && b == 1) && (main_pre_a == 0 && main_pre_b == 1)) {
fwd = true; // Serial.println("-from3");
}
else if ((a == 1 && b == 1) && (main_pre_a == 0 && main_pre_b == 0)) {
fwd = false;
}
char angle_char[4] = {'0','0', '0', '0' };
angle--;
String angle_s = String(angle);
for(int i=angle_s.length()-1,j=3; j>=0; i--,j--)
{
angle_char[j]= angle_s[i];
}
char dir_char[2] = {'0','_'};
if(fwd)
{
dir_char[0]='1';
}
char seperator[1] = {'*'};
//Serial.print(fwd);
// Serial.print("_________");
// Serial.println(count);
Udp.beginPacket(comp_IP, 4210);
Udp.write(dir_char, 2);
Udp.write(angle_char, 4);
Udp.write(seperator, 1);
Udp.write(angle_yaw_globle, 4);
Udp.endPacket();
}
}
pre_a = a;
pre_b = b;
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment