tdw

#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_HMC5883_U.h>

/* Assign a unique ID to this sensor at the same time */
Adafruit_HMC5883_Unified mag = Adafruit_HMC5883_Unified(12345);
int a0;
int a1;
int lastA0=0;
int lastA1=0;
int sumMomentum;
int aggressiveCounter=0;
int swimMode=0;
int passiveCounter=0;
char str[60]={0};
char strHeadingDegrees[10]={0};

void displaySensorDetails(void){
  sensor_t sensor;
  mag.getSensor(&sensor);
  Serial.println("------------------------------------");
  Serial.print  ("Sensor:       "); Serial.println(sensor.name);
  Serial.print  ("Driver Ver:   "); Serial.println(sensor.version);
  Serial.print  ("Unique ID:    "); Serial.println(sensor.sensor_id);
  Serial.print  ("Max Value:    "); Serial.print(sensor.max_value); Serial.println(" uT");
  Serial.print  ("Min Value:    "); Serial.print(sensor.min_value); Serial.println(" uT");
  Serial.print  ("Resolution:   "); Serial.print(sensor.resolution); Serial.println(" uT");  
  Serial.println("------------------------------------");
  Serial.println("");
  delay(500);
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  /* Initialise the sensor */
  
  if(!mag.begin()){
    Serial.println("Ooops, no HMC5883 detected ... Check your wiring!");
    while(1);
  }
  
  //displaySensorDetails();
  
}

void loop() {
  /* Get a new sensor event */ 
  
  sensors_event_t event; 
  mag.getEvent(&event);
  float heading = atan2(event.magnetic.y, event.magnetic.x);
  float declinationAngle = 0.22;
  heading += declinationAngle;
  // Correct for when signs are reversed.
  if(heading < 0)
    heading += 2*PI;
    
  // Check for wrap due to addition of declination.
  if(heading > 2*PI)
    heading -= 2*PI;
   
  // Convert radians to degrees for readability.
  float headingDegrees = heading * 180/M_PI; 
  dtostrf(headingDegrees, 3, 0, strHeadingDegrees); 
  
  a0=analogRead(0);
  a1=analogRead(1);
  /*
  Serial.print("a0 : ");
  Serial.println(a0);
  Serial.print("a1 : ");
  Serial.println(a1);
  */
  sumMomentum=abs(lastA0-a0)+abs(lastA1-a1);
  //Serial.print("sumMomentum : ");
  //Serial.println(sumMomentum);
  lastA0=a0;
  lastA1=a1;

  
  if(sumMomentum>80){
    aggressiveCounter++;
    if(aggressiveCounter>3){
      swimMode=1;
      passiveCounter=0;
     }
  }else{
    passiveCounter++;
    aggressiveCounter=0;
    if(passiveCounter<10){
      swimMode=1;
      aggressiveCounter=5;
    }else{
      swimMode=0;
    }
  }
  //sprintf(str,"{\"swimMomentum\":%d,\"swimMode\":%d,\"passiveCounter\":%d,\"aggressiveCounter\":%d}",sumMomentum,swimMode,passiveCounter,aggressiveCounter);
  sprintf(str,"{\"swimMode\":%d,\"headingDegrees\":%s}",swimMode,strHeadingDegrees);
  Serial.println(str);
  //Serial.println(headingDegrees);
  delay(100);
  
  
}