BrainForce

//Realizzato da Daniel Rossi
//5C Blaise Pascal - informatico
//Reggio Emilia 06-04-2017
#include <ads1292r.h>
#include <SPI.h>
#include <arduinoFFT.h>

ads1292r ADS1292 ;
arduinoFFT FFT = arduinoFFT();

const uint16_t samples = 32;//This value MUST ALWAYS be a power of 2
const int I_SottoCamp = 32, I_Camp = 128;

//imposta pacchetto da inviare via seriale
uint8_t DataPacketHeader[15], *PACKET;
volatile char DataPacketFooter[2];
volatile int datalen = 135;
uint8_t data_len = 8;

//Packet format
#define  CES_CMDIF_PKT_START_1   0x0A
#define CES_CMDIF_PKT_START_2   0xFA
#define CES_CMDIF_TYPE_DATA   0x02
#define CES_CMDIF_PKT_STOP    0x0B

char *SPI_RX_Buff_Ptr;
byte Campioni_Rosso[I_Camp], Campioni_Nero[I_Camp], Campioni_Blu[I_Camp],  SPI_RX_Buff[9];
double *DATA, *IMAG, *ALPHA, media, somma, soglia, mediatot, sommatot;
double SottoCamp_Rosso[I_SottoCamp], SottoCamp_Nero[I_SottoCamp], SottoCamp_Blu[I_SottoCamp], filtrato[I_SottoCamp], vImag[I_SottoCamp], Alpha[9], Power[9];
int i, k, conta, contamedie;

unsigned long utemp;
signed long stemp;

void ResetImag() {
  for (i = 0; i < I_SottoCamp; i++) {
    vImag[i] = 0;
  }
}

void setup() {

  // initalize the  data ready and chip select pins:
  pinMode(ADS1292_DRDY_PIN, INPUT);  //6
  pinMode(ADS1292_CS_PIN, OUTPUT);    //7
  pinMode(ADS1292_START_PIN, OUTPUT);  //5
  pinMode(ADS1292_PWDN_PIN, OUTPUT);  //4

  Serial.begin(56700);  // Baudrate for serial communica
  //initalize ADS1292 slave
  ADS1292.ads1292_Init();
  //ADS1292.ads1292_Reset();

  DataPacketHeader[0] = CES_CMDIF_PKT_START_1;
  DataPacketHeader[1] = CES_CMDIF_PKT_START_2;
  DataPacketHeader[2] = (datalen);
  DataPacketHeader[3] = (datalen >> 8);
  DataPacketHeader[4] = CES_CMDIF_TYPE_DATA;

  DataPacketFooter[0] = 0x00;
  DataPacketFooter[1] = CES_CMDIF_PKT_STOP;

  ResetImag();
}

signed long shifting(volatile byte val1, volatile byte val2, volatile byte val3) {
  utemp = (unsigned long) ((val1 << 16) | (val2 << 8) | val3);
  utemp = (unsigned long) (utemp << 8);
  stemp = (signed long) (utemp);
  stemp = (signed long) (stemp >> 8);
  return stemp;
}

void feedBodyPacchetto(uint8_t pacchetto[15], volatile double dato[9]) {
  for (i = 5; i < 15; i++) {
    pacchetto[i] = dato[i - 5];
  }
}




void loop() {
  /* creo 3 vettori contenenti 32 campioni ognuno
      campioni_Ref è il vettore che contiene i valori di riferimento per poi filtrare i segnali
      Campioni_Rosso è il vettore che contiene i valori del sensore rosso
      Campioni_Blu è il vettore che contiene i valori del sensore blu
  */

  for (i = 0; i < I_Camp; i++) {

    if ((digitalRead(ADS1292_DRDY_PIN)) == LOW)
    {
      SPI_RX_Buff_Ptr = ADS1292.ads1292_Read_Data();
      for (k = 0; k < 9; k++) {
        SPI_RX_Buff[k] =  *(SPI_RX_Buff_Ptr + k);
      }
    }
    //nero
    Campioni_Nero[i] = shifting(SPI_RX_Buff[0], SPI_RX_Buff[1], SPI_RX_Buff[2]);
    //blu
    Campioni_Blu[i] = shifting(SPI_RX_Buff[3], SPI_RX_Buff[4], SPI_RX_Buff[5]);
    //rosso
    Campioni_Rosso[i] = shifting(SPI_RX_Buff[6], SPI_RX_Buff[7] , SPI_RX_Buff[8]);

  }
  //sottocampiono il vettore della media del segnale

  k = 0;
  for (i = 0; i < I_Camp; i += 4) {
    SottoCamp_Rosso[k] = ((double)(Campioni_Rosso[i] )) ;
    SottoCamp_Blu[k] = ((double)(Campioni_Blu[i] )) ;
    SottoCamp_Nero[k] = ((double)(Campioni_Nero[i] )) ;
    filtrato[k] = SottoCamp_Rosso[k]  - SottoCamp_Blu[k];
    k++;
  }

  DATA = filtrato;
  IMAG = vImag;

  /*
     svolgo la trasformata di fourier per il vettore finale ottenuto
     in modo da passare il segnale dal dominio del tempo al dominio delle frequenze
  */

  FFT.Windowing(DATA, samples, FFT_WIN_TYP_HAMMING, FFT_FORWARD);
  FFT.Compute(DATA, IMAG, samples, FFT_FORWARD);


  k = 0;
  for (i = 0; i < I_SottoCamp; i++) {
    if ((vImag[i] > 150) || (vImag[i] < -150)) ResetImag();
    if ((i >= 16) && (i <= 24)) {
      Alpha[k] = filtrato[i];
      k++;
    }
  }

  for (i = 0; i < 9; i++) {
    Power[i] = (double) pow(Alpha[i], 2)  ;
  }
   ALPHA = Alpha;

     PACKET = DataPacketHeader;
     DataPacketHeader[0] = 0x0A;
     DataPacketHeader[1] = 0xFA;
     DataPacketHeader[2] = (uint8_t) (data_len);
     DataPacketHeader[3] = (uint8_t) (data_len >> 8);
     DataPacketHeader[4] = 0x02;

     feedBodyPacchetto(PACKET, ALPHA);

     DataPacketHeader[13] = 0x00;
     DataPacketHeader[14] = 0x0b;

     for (i = 0; i < 15; i++) // transmit the data
     {
      Serial.write(DataPacketHeader[i]);
     }



}   //chiusura void loop