 /////////////////////////////////////////////////////////////////////////////////////////
// Module Name: BobControl.cpp
// Project: Foucault Pendulum, Subsystem BobControl
// Target: Arduino MEGA + ETHernet Shield
// Last Mod: See .ino file
// jan@breem.nl,  janbee@hack42.nl, mail@foucaultpendulum.nl
// www.foucaultslinger.nl   www.foucaultpendulum.nl
//
/////////////////////////////////////////////////////////////////////////////////////////

#include "Globals.h"

t_State State; 
bool 
  HaveSync, HalfSwing, 
  DoUpdateAmplitudeControl,
  DidResetMyself, DidReSyncMyself,
  SeenCenter, MissedCenter, 
  SeenRim_1,  MissedRim_1, 
  SeenRim_2,  MissedRim_2,
  OneSecondPassed,
  DoReadBME;
unsigned int 
  PositionCounter, // counter for all timing.
  TStopDrive, TStartDrive, TPassCenter, 
  TPassRim_1, TPassRim_2, AmplitudeFromRim_mm;
long  
  MissedCenterTimeOutCounter;
byte 
  AdcCenter, AdcRim, 
  AdcHallNorth, AdcHallSouth, AdcHallEast, AdcHallWest,
  AdcV24, AdcVBattery,
  APeakCenter, APeakRim_1, APeakRim_2; 
int BatteryCurrentRaw;

//********************************************************************************************************
void CalculateAmplitudeFromRim (void) // takes around 1 millisecond
{
  const float pi = 3.1416;
  int Amplitude;
  static int PrevAmplitude;
  
  // Calculate Amplitude
  if ((TPassRim_1 != 0) && (TPassCenter != 0))
    Amplitude = round (float(RimCoilRadius_mm) / sin (pi * float (TPassRim_1) / float (TPassCenter)));
    
  // Average two succesive measurements to cover a full swing
  AmplitudeFromRim_mm = (Amplitude + PrevAmplitude) >> 1;  
  PrevAmplitude = Amplitude;
  //Serial.print ("CalculatedAmplitude "); Serial.println (AmplitudeFromRim_mm); 
  //Serial.println (); 
}

//********************************************************************************************************
void UpdateAmplitudeControl (void) 
{
  DoUpdateAmplitudeControl = false;
  //Serial.println ("Updating Amplitude Control");
  CalculateAmplitudeFromRim ();
  bool Max = (AmplitudeFromRim_mm < SetPoint_Amplitude_mm); // to small
  if (ForceMaximalDriveWidth) Max = true;
  if (ForceMinimalDriveWidth) Max = false;
  if (Max)
  {
    TStartDrive = TMidDrive - TMaximalDriveWidth;
    TStopDrive =  TMidDrive + TMaximalDriveWidth;
    UseMaximalDriveWidth = true;
    UseMinimalDriveWidth = false;
  }
  else
  {
    TStartDrive = TMidDrive - TMinimalDriveWidth;
    TStopDrive =  TMidDrive + TMinimalDriveWidth;
    UseMaximalDriveWidth = false;
    UseMinimalDriveWidth = true;
  }
}

//********************************************************************************************************
ISR(TIMER1_COMPA_vect) // interrupt at 10 kHz. 100 usec interval. Execution takes 9 .. 12 usec
{
  static byte Channel, DetectPeakCenter, DetectPeakRim, RimLed;
  static bool WaitCenter, HavePeakCenter, WaitRim_1, WaitRim_2;
  
  DIAGPIN_A15_H // Show frequency and duration of interrupt handler on pin A15
  
  static unsigned int OneSecondCounter;
  if (++OneSecondCounter >= 59999)
  {
    OneSecondCounter = 0;
    OneSecondPassed = true;
  }

  // Update analog inputs
  // In each TimeSlice we get the result of the previously started conversion
  // Analog channels 0..6 are sequentially sampled at 10 kHz
  // so each channel at a 1.66 kHz rate
 
  switch (Channel) // Channel is # of previously started conversion
  {
    // we freeze the Hall data from SeenCenter until sending that message
    // so the message with the SeenCenter flag has Hall data from the moment of the CenterPass
    case 1: 
      if (!SeenCenter) AdcHallNorth = ADCH; // result from channel 0
    break;
    
    case 2: 
      if (!SeenCenter) AdcHallSouth = ADCH; // result from channel 1
    break;
    
    case 3: 
      if (!SeenCenter) AdcHallEast =  ADCH; // result from channel 2
    break;
    
    case 4: 
      if (!SeenCenter) AdcHallWest =  ADCH; // result from channel 3
    break;
    
    case 5: 
      AdcCenter =    ADCH; // result from channel 4
    break;
    
    case 6: 
      AdcRim =       ADCH; // result from channel 5
    break;
    
    case 7: 
      AdcV24 =       ADCH; // result from channel 6
      Channel = 0;
    break;
  }
  ADMUX = Channel++ | 0x60; // include ADLAR for 8 bit conversion result and Ref = AVCC.
  // ADconverter is started at the end of this function to have more settle time

  // Drive Control
  if (PositionCounter == TStartDrive) if (EnableDrive) DriveON
  if (PositionCounter >= TStopDrive) DriveOFF

  if (RimLed > 0) // Timer for RimLed
  {
    RimLed--;
    RIMLEDON
  }
  else RIMLEDOFF

  // TimeOut for when we have no sync
  if (SeenCenter) MissedCenterTimeOutCounter = 60000; // 6 seconds
  if (MissedCenterTimeOutCounter > 0) MissedCenterTimeOutCounter--;
  else 
  {
    ForceReSync = true;
    DidReSyncMyself = true; // for status message
    MissedCenterTimeOutCounter = 60000;
    Serial.println ("TimeOut Forced Resync");
  }
       
  // Detect Bob Passes
  // The DC level of the central detection signal is midscale = 128
  // When the Bob passes we first get a positive change and when the Bob is exactly
  // over the detection coil the voltage crosses the midscale level in negative direction

  PositionCounter++; // for timing of Bob Pass detection and drive coil
 
  if (ForceReSync)
  {
    ForceReSync = false;
    HaveSync = false;
    State = st_Prepare;
  }
  
  switch (State)
  {
    case st_Prepare:
      HavePeakCenter = false;
      SeenCenter = 0;
      MissedCenter = 0;
      WaitRim_1 = false;
      SeenRim_1 = 0;
      MissedRim_1 = 0;
      WaitRim_2 = false;
      SeenRim_2 = 0;
      MissedRim_2 = 0;
      DetectPeakCenter = 0;
      DetectPeakRim = 0;
      //DoReadBatteryStatus = true;
      State = st_LookForCenterApproach;
    break;

    case st_LookForCenterApproach: 
      // When Bob is far away we have midscale value = ca. 128
      // Expect Bob to approach and do peak-hold
      if (AdcCenter >= DetectPeakCenter) DetectPeakCenter = AdcCenter; // track rising signal
      if (AdcCenter < (DetectPeakCenter -8))  // we are over the top
      {
        HavePeakCenter = true;
        APeakCenter = DetectPeakCenter; // Freeze peak amplitude
        State = st_LookForCenterCross;
      }
      if (PositionCounter > TMissedCenter) State = st_MissedCenter;
    break;

    case st_LookForCenterCross: // Bob is close to center, now wait for crossing 
      if (AdcCenter < 120)         // This is the zero crossing
      {
        DIAGPIN_A14_H
        SeenCenter = true; // Set to false at send outmessage
        HaveSync = true;       
        TPassCenter = PositionCounter;  // Freeze time of Center Pass
        PositionCounter = 0;            // Restart counting
        DoUpdateAmplitudeControl = true; // with data from previous swing
        HalfSwing = !HalfSwing;
        if (HalfSwing) ZeroLedON else ZeroLedOFF 
        State = st_DontLookNow;           
      }
      if (PositionCounter > TMissedCenter) State = st_MissedCenter;
    break;
        
    case st_MissedCenter:
      TPassCenter = PositionCounter;
      PositionCounter = 0; // Fire the Drive coil approximately correct
      MissedCenter = true;
      HaveSync = false;
      State = st_DontLookNow;
    break;

    case st_DontLookNow:
      // the cross talk from the drive pulse may be interpreted as a Rim Pass
      if (PositionCounter >= TStartLookForRim_1) 
      {
        State = st_LookForRim_1;
        // Now we can read the Dallas Temperature Sensors, because nothing else will happen for a while
        ReadaDallasSensor= true; // only once per pendulum period
      }
    break;  

    case st_LookForRim_1: // look for positive peak
      DIAGPIN_A14_L 
      if (AdcRim > DetectPeakRim) DetectPeakRim = AdcRim; // Track rising signal
      // when Bob is far away we have midscale value = ca. 128
      if (DetectPeakRim > 135) WaitRim_1 = true; // Bob is approaching the Rim coil
      if (WaitRim_1)
      {
        if (AdcRim < (DetectPeakRim - 5)) // We are over the top
        {
          RimLed = 100;
          SeenRim_1 = true; 
          TPassRim_1 = PositionCounter;
          APeakRim_1 = DetectPeakRim;
          DoUpdateAmplitudeControl = true;
          State = st_WaitRim_2; 
          DIAGPIN_A13_H         
          break;
        }
      }
      if (PositionCounter > TMissedRim_1)
      {
        MissedRim_1 = true;
        TPassRim_1 = PositionCounter;
        State = st_WaitRim_2;
      }
    break;

    case st_WaitRim_2:
      if (PositionCounter >= TStartLookForRim_2) 
      {
        DetectPeakRim = 255; // prepare for Rim_2 minmum search
        State = st_LookForRim_2;
        DIAGPIN_A13_L
      }
   
    case st_LookForRim_2: // Look for negative peak
      if (AdcRim < DetectPeakRim) DetectPeakRim = AdcRim; // Track falling signal
      // when Bob is far away we have midscale value = ca. 128
      if (AdcRim < 121) WaitRim_2 = true; // Bob is approaching the Rim coil
      if (WaitRim_2)
      {
        if (AdcRim > (DetectPeakRim + 5)) // We are across the dip
        {
          RimLed = 100;
          SeenRim_2 = true; 
          MissedRim_2 = false; 
          TPassRim_2 = PositionCounter;
          APeakRim_2 = DetectPeakRim;
          State = st_WaitToRepeatCycle;  
          DIAGPIN_A12_H        
          break;
        }
      }
      if (PositionCounter> TMissedRim_2)
      {
        MissedRim_2 = true;
        TPassRim_2 = PositionCounter;
        State = st_WaitToRepeatCycle;
      }
    break;

    case st_WaitToRepeatCycle:
      if (PositionCounter > TStartLookForCenter) 
      {
        DIAGPIN_A12_L
        DoReadBME= true;
        State = st_Prepare;
      }
    break;      

    default: State = st_Prepare;
  } // switch (State)

  ADCSRA |= 0x40; // start new A/D conversion
  DIAGPIN_A15_L  // end of interrupt handler
}

//********************************************************************************************************
void Init_BobControl (void)
{
  MissedCenterTimeOutCounter = 60000; // give it some time to find sync
  
  // Configure 16 bit Timer 1 for 10 kHz interrupts = 100 usec interval time
  TCCR1A = 0x00;  // WaveformMode 4 = CTC
  TCCR1B = 0x09;  // No prescaler, WaveformMode 4 = CTC, internal clock 16 MHz
  OCR1A = 1599;   // Div by 1600 for 10 kHz
  TIMSK1 = 0x02;  // Interrupt on OCRA match

  // A/D converter runs on 1 MHz clock
  // 1 conversion takes < 25 clocks or  < 25 usec
  ADCSRA = 0x84; // Enable A/D converter, clockdiv 16 for 1 MHz A/D clock
  ADCSRB = 0;    // channels 0..7 only
  ADMUX = 0x60;  // Ref = AVCC, ADLAR for 8-bit conversion result
  DIDR0 = 0xFF;  // Disable digital function of analog pins A0 .. A7

  DDRL = 0xFF;   // for Leds and Drive
  State = st_Prepare;
  EnableDrive = true;
}
