SynchroRead

#define COARSE 1
#define FINE 4
#define REF 0 
#define WINDINGA 0
#define WINDINGB 1
#define WINDINGC 2
#define MIDRANGE 512
#define LEAKAGE 0.9
#define MICROPERIOD 2048
#define MICROMASK 0x000007ff
#define READTHRESHOLD 50
#define FINESCALE 6.00000
#define COARSESCALE 0.1666667
#define RATIO 36
#define COARSEOFFSET 152
#define FINEOFFSET 120
#define HALFFINE 30


const int drive = 9; //used to make the sine waveform
const int coarseA = A1;
const int coarseB = A2;
const int coarseC = A3;
const int fineA = A4;
const int fineB = A5;
const int fineC = A6;
const int ref = A7;//A1;
const unsigned long millisbit = 2;
const unsigned long milliswritebit = 512;//32;
const float thetaDecay = 0.8;
const float cos30 = sqrt(3)/2;
const float cos60 = 0.5;

int lastref=1;
int thisref=1;
int readings[] = { 0 , 0 , 0 , 0 , 0 , 0 , 0};
int maximums[] = { 0 , 0 , 0 , 0 , 0 , 0 , 0};
int minimums[] = { 0 , 0 , 0 , 0 , 0 , 0 , 0};
int medians[] = { MIDRANGE , MIDRANGE , MIDRANGE , MIDRANGE , MIDRANGE , MIDRANGE , MIDRANGE };
unsigned long medianslong[]= { MIDRANGE*256 , MIDRANGE*256 , MIDRANGE*256 , MIDRANGE*256 , MIDRANGE*256 , MIDRANGE*256 , MIDRANGE*256 };
int scaled[] = { 0 , 0 , 0 , 0 , 0 , 0 }; //B/A C/A Coarse then fine
float smoothscaled[]= { 0 , 0 , 0 , 0 , 0 , 0 };
unsigned long lastdecrement = millisbit; //2=match or 0=nomatch, 2nd bit in millis (~4 milliseconds)
unsigned long lastwrite = milliswritebit;
int refscale = MIDRANGE;
int i=0;
int millisresult = 0;
unsigned int wave[MICROPERIOD];
unsigned int thisWave = 0;
float k=0;
float opposite[] = { 0 , 0 };
float adjacent[] = { 0 , 0 };
float thetaSynchro[] = { 0 , 0 };
float theta = 0;
unsigned long microtime=0;
float fine=0;
float coarse=0;
float calculated=0;
float halfangle=0;
float peg=0;
float testpeg=0;
boolean readcoarse=true;

void setup() {
  // put your setup code here, to run once:
  SerialUSB.begin(57600);  //ignores speed and runs FAST
  //delay(10000);
  //SerialUSB.print("microperiod is ");
  //SerialUSB.print(MICROPERIOD);
  //SerialUSB.print("Starting Loop  \n");
  
  for (i=0;i<MICROPERIOD;i++) {
    k=2048+2047*sin(2*PI*i/MICROPERIOD);
    wave[i]=(unsigned int) k;
  }
  //SerialUSB.print("Starting Loop\n");
  
  analogWriteResolution(12);
  pinMode(DAC0, OUTPUT);  //actually make waveform from teensy 3.2
  pinMode(coarseA, INPUT);
  pinMode(coarseB, INPUT);
  pinMode(coarseC, INPUT);
  pinMode(fineA, INPUT);
  pinMode(fineB, INPUT);
  pinMode(fineC, INPUT);
  pinMode(ref, INPUT);
  
}

void loop() {
  //thisWave=micros()&MICROMASK;
  //analogWrite(DAC0,wave[thisWave]);
  thisref=analogRead(ref)-medians[REF];
  lastref=thisref;
  //SerialUSB.print(lastref);
  //SerialUSB.print(" ");
  //SerialUSB.print(thisref);
  //SerialUSB.print("...\n");
  while (!((lastref*thisref)<0)) {
    lastref=thisref;
    thisref=analogRead(ref)-medians[REF];
    microtime=micros();
    //SerialUSB.print(lastref);
    //SerialUSB.print(" ");
    //SerialUSB.print(thisref);
    //SerialUSB.print("...");
    //delayMicroseconds(200);
  }
//  SerialUSB.print(microtime);
//  SerialUSB.print("\n");
  while ((micros()-microtime)<625) {  ;}
  //delayMicroseconds(600); //allow 625us, including read times after zero crossing to get ref
  readings[REF]=analogRead(ref);
//  SerialUSB.print(micros());
//  SerialUSB.print("Got Ref");
  while ((micros()-microtime)<1250) {  ;}
  //delayMicroseconds(600); //allow 625us, including read times after ref to get synchros quarter phase out
//  SerialUSB.print(micros());
//  SerialUSB.print("  Getting Windings ");
//  if (readcoarse) {
    readings[COARSE+WINDINGA]=analogRead(coarseA);
    readings[COARSE+WINDINGB]=analogRead(coarseB);
    readings[COARSE+WINDINGC]=analogRead(coarseC);
    readings[FINE+WINDINGA]=analogRead(fineA);
    readings[FINE+WINDINGB]=analogRead(fineB);
    readings[FINE+WINDINGC]=analogRead(fineC);
//  } else {

//  }
  readcoarse=!readcoarse;
//  SerialUSB.print(micros());
//  SerialUSB.print("  Got Windings \n");
//  readings[REF]=floor(analogRead(ref)+readings[REF])/2;
  //SerialUSB.print('.');
  for (i=0;i<7;i++) {
    if (readings[i]>maximums[i]) {
      maximums[i]=readings[i];
    }
  }
  for (i=0;i<7;i++) {
    if (readings[i]<minimums[i]) {
      minimums[i]=readings[i];
    }
  }

  /*signs[0]=signs[0]*LEAKAGE+(readings[0]-MIDRANGE)*(readings[1]-MIDRANGE);
  signs[1]=signs[1]*LEAKAGE+(readings[0]-MIDRANGE)*(readings[2]-MIDRANGE);
  signs[2]=signs[2]*LEAKAGE+(readings[3]-MIDRANGE)*(readings[4]-MIDRANGE);
  signs[3]=signs[3]*LEAKAGE+(readings[3]-MIDRANGE)*(readings[5]-MIDRANGE);*/
  //SerialUSB.print(readings[REF]);
  //SerialUSB.print(" : ");
  scaled[REF]=readings[REF]-medians[REF];
  //SerialUSB.print(scaled[REF]);
  //SerialUSB.print(" ");
  
  if ((scaled[REF]>READTHRESHOLD)||(scaled[REF]<-READTHRESHOLD)) {
/*  if (scaled[REF]>0) {
    if (scaled[REF]<10) {
      scaled[REF]+=2;
    }
  } else if (scaled[REF]<0) {
    if (scaled[REF]>-10) {
      scaled[REF]-=2;
    }
  }*/
    for (i=1;i<7;i++) { //skip REF, don't scale it by itself
      scaled[i]=(1000*(readings[i]-medians[i]))/scaled[REF];  
      smoothscaled[i]=smoothscaled[i]*0.98+scaled[i];  //0.95
    }
    for (i=0;i<2;i++) {
      //opposite[i] = smoothscaled[i*3+1] + smoothscaled[i*3+2]*cos60 + smoothscaled[i*3+3]*cos60;
      //adjacent[i] = (smoothscaled[i*3+2] - smoothscaled[i*3+3])*cos30;
      adjacent[i] = smoothscaled[i*3+1] - smoothscaled[i*3+2]*cos60 - smoothscaled[i*3+3]*cos60;
      opposite[i] = (smoothscaled[i*3+2] - smoothscaled[i*3+3])*cos30;
      thetaSynchro[i] = 180*atan2(opposite[i],adjacent[i])/PI;
    }
    millisresult = (millis()&millisbit);
    if (millisresult^lastdecrement) {
      lastdecrement=millisresult;
      for (i=0;i<7;i++) {
        maximums[i]--;
        minimums[i]++;
      }    
      //Only update medians slowly,
      for (i=0;i<7;i++) {
        if (medianslong[i]>(readings[i]*256)) {
          medianslong[i]--;
        }
        else if (medianslong[i]<(readings[i]*256)) {
          medianslong[i]++;
        }
        medians[i]=medianslong[i]/256;
      }
    }
    millisresult = (millis()&milliswritebit);
    if (millisresult^lastwrite) {
      lastwrite=millisresult;
      //SerialUSB.print("\n");
      SerialUSB.print(millis());
      SerialUSB.print(",");
      SerialUSB.print(milliswritebit);
      SerialUSB.print(",");
      SerialUSB.print(lastwrite);
      SerialUSB.print(",");
      SerialUSB.print(millisresult);
      SerialUSB.print(",");
      SerialUSB.print(wave[thisWave]);
      for (i=0;i<7;i++) {
        SerialUSB.print("  ,");
        SerialUSB.print(maximums[i]-medians[i]);
        SerialUSB.print(",");
        SerialUSB.print(minimums[i]-medians[i]);
        SerialUSB.print(",");
        SerialUSB.print(medians[i]);
      }
      SerialUSB.print("   ");
      for (i=0;i<7;i++) {
        SerialUSB.print(",");
        SerialUSB.print(scaled[i]);
      }
      for (i=0;i<2;i++) {
        SerialUSB.print("  ,  ");
        SerialUSB.print(opposite[i]);
        SerialUSB.print("  ,  ");
        SerialUSB.print(adjacent[i]);
        SerialUSB.print("  ,  ");
        SerialUSB.print(thetaSynchro[i]);
      }
 
      fine=thetaSynchro[1]-FINEOFFSET;
      coarse=thetaSynchro[0]-COARSEOFFSET;
      if (abs(fine)<=90) {
        halfangle=fine;
//        peg=round(coarse*RATIO/360.0)*(360/FINESCALE);
        testpeg=(coarse/10.0);
        peg=round(coarse/10.0)*(60);
      } else {
        if (fine<0) {
          halfangle=fine+180;
        } else {
          halfangle=fine-180;
        }
        //peg=round((coarse*RATIO/360.0)+0.5)*(360/FINESCALE)-HALFFINE;  
        testpeg=(coarse/10.0)+0.5;
        peg=round((coarse/10.0)+0.5)*(60)-HALFFINE;  
      }
      calculated=peg+halfangle/FINESCALE+180;
      SerialUSB.print(" c=");
      SerialUSB.print(coarse);
      SerialUSB.print(" f=");
      SerialUSB.print(fine);
      SerialUSB.print(" tp=");
      SerialUSB.print(testpeg);
      SerialUSB.print(" p=");
      SerialUSB.print(peg);
      SerialUSB.print(" ha=");
      SerialUSB.print(halfangle);
      SerialUSB.print(" , ");
      
      SerialUSB.print(" Arbitrary Degrees = ");
      SerialUSB.print(calculated);
      SerialUSB.print("\n");

    }
  }
 // delay(50);
} // put your main code here, to run repeatedly: