Lab_interaccio/2021/DRON-screen/DRON_screen_v3/DRON_screen_v3.ino

#include <SPI.h>
#include "nRF24L01.h"
#include "RF24.h"
#include "printf.h"
#include "Adafruit_NeoPixel.h"

#define RADIOSEND     false

#define CE_PIN   12
#define CSN_PIN  2

#define DEBUG  0
#define TEST   0

#define SYMETRIC  0
#define SMOOTH  0
#define ALFA 0.3

#define RED 11
#define GREEN 10
#define BLUE 9
#define WHITE 6

#define SDO1           4
#define SDO2           8
#define SDO3           3

//#define NUMPIXELS1      240  
//#define NUMPIXELS2      240
//#define NUMPIXELS3      240


int NUM_LEDS = 600;
int TotalSteps = NUM_LEDS;
int Index = TotalSteps-1;

int intensity = 25;
int delaylight = 250;
int buttonState = 0;         // current state of the button
int lastButtonState = 0;     // previous state of the button
long pressDuration = 0;
int paloState = 0;

volatile int amplilight = 0;
volatile float ampliSmooth = 0;
volatile float ampliSmoothLast = 0;
volatile float random1, random2;

int sinus = 0;
int j=0;

int funcion = 1;
volatile float angle_rad = 0;
volatile float sin_result = 0;
volatile float angulo = 0;
volatile float incAngle = 0.1;
volatile float freqSinus =1;
volatile float escalado = 1.;
volatile float umbral = 1.;
volatile float randomDesviacion;
volatile float noise_random = 0;
unsigned long currentMillis = 0;        // will store last time LED was updated
unsigned long previousMillis = 0;        // will store last time LED was updated
unsigned long pressedTime  = 0;
unsigned long releasedTime = 0;

long intervalOn = 20;  
long intervalOff = 100; 
long pulseWidth = 100;
int pwm = 20;

uint8_t dibujar;
uint8_t selectedScene = 1;
uint8_t mode = 0; // uso de millis para calcular tiempos

uint8_t rojo = 25;
uint8_t verde = 25;
uint8_t azul = 25;
uint8_t rojoFinal = 25;
uint8_t verdeFinal = 25;
uint8_t azulFinal = 25;

//int idpin[8]  = { 5, 13, A4, A3, A2, A1, A0, A5};
int idpin[6]  = { A5, A0, A1, A2, A3, A4};
int chpin[2] = { 13, 5 }; // no se va a usar en esta App

Adafruit_NeoPixel pixels1 = Adafruit_NeoPixel(NUM_LEDS, SDO1, NEO_RGB + NEO_KHZ800);
Adafruit_NeoPixel pixels2 = Adafruit_NeoPixel(NUM_LEDS, SDO2, NEO_RGB + NEO_KHZ800);
Adafruit_NeoPixel pixels3 = Adafruit_NeoPixel(NUM_LEDS, SDO3, NEO_RGB + NEO_KHZ800);


byte neopix_gamma[] = {
    0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
    0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  1,  1,  1,  1,
    1,  1,  1,  1,  1,  1,  1,  1,  1,  2,  2,  2,  2,  2,  2,  2,
    2,  3,  3,  3,  3,  3,  3,  3,  4,  4,  4,  4,  4,  5,  5,  5,
    5,  6,  6,  6,  6,  7,  7,  7,  7,  8,  8,  8,  9,  9,  9, 10,
   10, 10, 11, 11, 11, 12, 12, 13, 13, 13, 14, 14, 15, 15, 16, 16,
   17, 17, 18, 18, 19, 19, 20, 20, 21, 21, 22, 22, 23, 24, 24, 25,
   25, 26, 27, 27, 28, 29, 29, 30, 31, 32, 32, 33, 34, 35, 35, 36,
   37, 38, 39, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 50,
   51, 52, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 66, 67, 68,
   69, 70, 72, 73, 74, 75, 77, 78, 79, 81, 82, 83, 85, 86, 87, 89,
   90, 92, 93, 95, 96, 98, 99,101,102,104,105,107,109,110,112,114,
  115,117,119,120,122,124,126,127,129,131,133,135,137,138,140,142,
  144,146,148,150,152,154,156,158,160,162,164,167,169,171,173,175,
  177,180,182,184,186,189,191,193,196,198,200,203,205,208,210,213,
  215,218,220,223,225,228,231,233,236,239,241,244,247,249,252,255 
};




RF24 radio(CE_PIN, CSN_PIN); // Create a Radio
const uint64_t pipe = 0xE8E8F0F0E1LL; // Define the transmit pipe
uint8_t CMD[ 8 ]; // CMD + PARAM
//int ch = 0x4C ;  // ESTRELLA
int ch = 0x76 ; // JOGUINES
//int ch = 0x20 ; // GASPAR




void resetBoard()
{
  //Serial.println("reset");
  NVIC_SystemReset();       // esta funcion en teoria si funciona en SAMD
}


byte id()
{
  int id = 0;
  for (byte i = 0; i < 6; i++ ) bitWrite(id, i, digitalRead(idpin[i]));
  return id;
}


int channel()
{
  int value = code();
  switch (value)
  {
    case 0:
      value = 0x10; // 
      break;
    case 1:
      value = 0x4C; // 
      break;
    case 3:
      value = 0x64; // 
      break;
    case 4:
      value = 0x7F; // 
      break;
    default:
      value = 0; // 
      break;
  }
  return value;
}

int code()
{
  int value = 0;
  for (byte i = 0; i < 2; i++) bitWrite(value, i, digitalRead(chpin[i]));
  return value;
}




//////////////////////////////////////////////////////////////////
/////////////////////////  SETUP  ////////////////////////////////
//////////////////////////////////////////////////////////////////


void setup() {


  for (int i = 0; i < 6; i++)
  {
    pinMode(idpin[i], INPUT);
  }

  for (int i = 0; i < 2; i++)
  {
    pinMode(chpin[i], INPUT);
  }

  pixels1.begin();
  pixels2.begin();
  pixels3.begin();
  pixels1.show(); // Initialize all pixels to 'off'
  pixels2.show(); // Initialize all pixels to 'off'
  pixels3.show(); // Initialize all pixels to 'off'

  Serial.begin(512000);
  //while (!Serial) {
  //   ; // wait for serial port to connect. Needed for Leonardo only
  //}

  //Serial.println("Serial READY");
  //delay(1000);

  //printf_begin();
  radio.begin();
  radio.setDataRate( RF24_250KBPS );
  radio.setPALevel( RF24_PA_MAX );
  radio.setAutoAck(0);
  radio.setPayloadSize(8);
  radio.setChannel(ch);
//#if RADIOSEND
//  radio.openWritingPipe(pipe);
//#else
  radio.openReadingPipe(1, pipe);
  radio.startListening();
//#endif
  radio.printDetails();


}


//////////////////////////////////////////////////////////////////
/////////////////////////  LOOP  ////////////////////////////////
//////////////////////////////////////////////////////////////////


void loop()
{

   ////////////////////////////////////////////////////////////////// 
  /////////////////////////  RADIO ENCUESTA  ////////////////////////
  //////////////////////////////////////////////////////////////////


 /*
 if(DEBUG)
 {
    Serial.print("id: ");
    Serial.println(id());
    Serial.print("code: ");
    Serial.println(code());
    Serial.print("buttonState: ");
    Serial.println(code());
    delay(30);
 }
*/


////////////////////////////////////////////////////////////
////////////////////////  BOTON  ///////////////////////////
////////////////////////////////////////////////////////////

//Comportamiento del boton: 
//Pulsación rápida cambio de modo, pulsación larga cambio de resolución y escala (20% del palo largo)

  buttonState = code();

  if (buttonState != lastButtonState) 
  {
    // if the state has changed, increment the counter
    if (buttonState==2) 
    {
      //Serial.println("on");
      pressedTime = millis(); 
      //Serial.println(pressedTime);
      selectedScene++;
      if(selectedScene > 4)  // definir aqui las escenas que se quieren cambiar
        selectedScene = 1;
      
    } 
    else 
    {
      releasedTime = millis();
      //Serial.println("off");
      //Serial.println(releasedTime);
    }

    pressDuration = releasedTime - pressedTime;
    if( pressDuration > 300 )
    {
      //Serial.println("A long press is detected");
      if (paloState == LOW) 
      {
        paloState = HIGH;
        NUM_LEDS = 600; 
      } else 
      {
        paloState = LOW;
        NUM_LEDS = 120; 
      }
      
    }
    
    //else
    //  Serial.println("A short press is detected");
      
    delay(50);
    
  }
  
  lastButtonState = buttonState;

////////////////////////////////////////////////////////////
////////////////////////  RADIO  ///////////////////////////
////////////////////////////////////////////////////////////
  

  if ( radio.available() )
  {
    //Serial.println("Radio packet");
    while (radio.available())
    {
      //done = radio.read( CMD, sizeof(CMD) );
      radio.read( CMD, sizeof(CMD) );

      if (DEBUG)
      {
          Serial.println("Got command");
          for (int i = 0 ; i < 8 ; i++)
          {
            Serial.print(CMD[i]);
            Serial.print(",");
          }
          Serial.println();
      }

      
      if ( CMD[0] == 0 )    //
      {
        
        //Serial.println("COMANDO 0"); // ESCENA COMPLETA CON CONJUNTO DE PARAMETROS PREESTABLECIDOS
        // ESCENA 1 = MODO DESPEGUE (todo blanco con centro y extremo en rojo
        // ESCENA 2 = MODO COLLSEROLA (barrido blanco y los últimos 10 en rojo).
        // ESCENA 2 = ONDA 1 - Sinus basico
        // ESCENA 3 = ONDA 2 - Sinus doble
        // ....
        
        selectedScene = CMD[1];

        
      }
      else if ( CMD[0] == 1 ) 
      {
        
        //Serial.println("COMANDO 1"); // FUNCIONES SINUS 
        
      }
      else if ( CMD[0] == 2 )
      {
        
        //Serial.println("COMANDO 2"); // COLOR
        
        rojo = CMD[5];
        verde = CMD[6];
        azul = CMD[7];

        rojoFinal = (int)((rojo*intensity)/255.);
        verdeFinal = (int)((verde*intensity)/255.);
        azulFinal = (int)((azul*intensity)/255.);
      
      }
      else if ( CMD[0] == 3 )
      {
        
        //Serial.println("COMANDO 3"); // INTENSIDAD DE LA ONDA
        
        intensity = CMD[1];
        rojoFinal = (int)((rojo*intensity)/255.);
        verdeFinal = (int)((verde*intensity)/255.);
        azulFinal = (int)((azul*intensity)/255.);

      }
      else if ( CMD[0] == 4 )
      {
        
        //Serial.println("COMANDO 4"); // FRECUENCIA DE LA ONDA

        freqSinus = CMD[1];
        incAngle = freqSinus/50.; // rango entre 0.02 y 2
      }
      else if ( CMD[0] == 5 )
      {
        
        //Serial.println("COMANDO 5"); // ANCHURA DE PULSO + PWM
        pulseWidth = CMD[1]*5;
        pwm = CMD[2];
        
        intervalOn = pulseWidth*pwm/100.;  
        intervalOff = pulseWidth - intervalOn; 

        //Serial.print("ON: ");
        //Serial.print(intervalOn);
        //Serial.print("  OFF: ");
        //Serial.println(intervalOff);
        
      }
      else if ( CMD[0] == 6 )
      {
        
        //Serial.println("COMANDO 6"); // ESCALA + OFFSET
        escalado = CMD[1]/100.; //entre 0-2 donde 1 es sin escalar
        umbral = CMD[2]/100.;   // entre 0 y 2 donde 1 es el centro

        //Serial.print("Escalado: ");
        //Serial.print(escalado);
        //Serial.print("  Umbral: ");
        //Serial.println(umbral);     
      }
      else if ( CMD[0] == 7 ) // FUNCTIONS
      {

        funcion = CMD[1];    
        //Serial.println("COMANDO 7");
        
      }
      else if ( CMD[0] == 8 ) // RESET ANGULO
      {
        noise_random = CMD[1];  ; // cantidad de ruido sobre la muestra de 0 a 100
        //Serial.println("COMANDO 8"); //
        
      }
      else if ( CMD[0] == 9 ) // RESET ANGULO
      {
        angulo = 0;
        //Serial.println("COMANDO 8"); //
        
      }
      
    }
    
  }


////////////////////////////////////////////////////////////
////////////////////////  DRAW  ///////////////////////////
////////////////////////////////////////////////////////////


        
   if(selectedScene == 1)  // aterrizaje/despegue
   {
        for(int i = 0; i< NUM_LEDS ; i++)
        {
              pixels2.setPixelColor( i, intensity, intensity, intensity, 0 );
              pixels3.setPixelColor( i, intensity, intensity, intensity, 0 );
        }
        
        for(int i = NUM_LEDS-5; i< NUM_LEDS+5 ; i++)
        {
              pixels2.setPixelColor( i, intensity, 0, 0, 0 );
              pixels3.setPixelColor( i, intensity, 0, 0, 0 );
        }
        
        for(int i = (NUM_LEDS/2)-5; i< (NUM_LEDS/2)+5 ; i++)
        {
              pixels2.setPixelColor( i, intensity, 0, 0, 0 );
              pixels3.setPixelColor( i, intensity, 0, 0, 0 );
        }
        
        for(int i = 0; i< 10 ; i++)
        {
              pixels2.setPixelColor( i, intensity, 0, 0, 0 );
              pixels3.setPixelColor( i, intensity, 0, 0, 0 );
        }
        
        showStrip();
        delay(15);
        
   }
   else if(selectedScene == 2)  // collserola
   {
          j-=5;
          if(j < 0)
            j = NUM_LEDS;

          for(int i=0; i< NUM_LEDS ; i++)
          {
                pixels2.setPixelColor( i, 0, 0, 0, 0 );
                pixels3.setPixelColor( i, 0, 0, 0, 0 );
          }
          
//          for(int i = j+1; i <= j+5 ; i++)
//          {
//                pixels2.setPixelColor( i, 0, 0, 0, 0 );
//                pixels3.setPixelColor( i, 0, 0, 0, 0 );
//          }
          
          for(int i = j-5; i < j ; i++)
          {
               pixels2.setPixelColor( i, intensity, intensity, intensity, 0 );
               pixels3.setPixelColor( i, intensity, intensity, intensity, 0 );
          }
               
          //pixels2.setPixelColor( j, intensity, intensity, intensity, 0 );
          //pixels3.setPixelColor( j, intensity, intensity, intensity, 0 );
          
          for(int i = 0; i< 10 ; i++)
          {
                pixels2.setPixelColor( i, intensity, 0, 0, 0 );
                pixels3.setPixelColor( i, intensity, 0, 0, 0 );
          }
          
          showStrip();
      
   }
   else if(selectedScene == 3)  // Functions: f(x) = sin(x) + sin(2x)
   {
      incAngle = freqSinus/50.; // restauramos valor del modo 3
      angulo = angulo + incAngle; 
      if(angulo > 360)
      {
        angulo = 0;
        random1 = random(1,6);  // parametros aletaorios cada 2pi para que sea diferente siempre
        random2 = random(1,6);
      }
      
      angle_rad = angulo * M_PI / 180.;

//        sin(x) + sin(2x)
//        sin(4x/4) + sin(6x/4) + cos(x/4)
//        sin(x/1.5) + sin(x/2) + cos(x)
//        (sin(x/1.5) + sin(x/2) + cos(x))*sin(x*2)
//        (sin(x/1.5) + sin(x/2) + cos(x))*sin(x*0.25)*4

     switch (funcion)
      {
        case 1:
          sin_result = (sin(angle_rad) + sin(2*angle_rad))/2.;  // FUNCTION 1
          break;
        case 2:
          sin_result = (sin(angle_rad) + sin(4*angle_rad))/2.;  // + cos(angle_rad))/3.;  // FUNCTION 2
          break;
        case 3:
          sin_result = (2*sin(angle_rad) + sin(3*angle_rad) - cos(angle_rad))*cos(5*angle_rad)/4.;  // FUNCTION 3
          break;
        case 4:
          sin_result = (sin(5*angle_rad) + cos(3*angle_rad))*cos(angle_rad)/2. ;  // FUNCTION 4
          break;
        case 5:
          sin_result = (sin(11*angle_rad) + cos(5*angle_rad))*sin(angle_rad)/2. ;  // FUNCTION 5
          break;
        case 6:
          sin_result = (sin(11*angle_rad) + cos(random1*angle_rad))*sin(random2*angle_rad)/2. ;  // FUNCTION ALEATORIA
          break;
      }

      
      //sin_result = (sin(angle_rad) + sin(2*angle_rad))/2.;  // FUNCTION
      //randomDesviacion = noise_random/random(-100,100); // random entre -1 y 1
      amplilight = sin_result*escalado*(NUM_LEDS/2)  + (NUM_LEDS/2)*umbral ;
      
    
        if(DEBUG)
        {
          Serial.print("sin(");
          Serial.print(angulo);
          Serial.print("°) = ");
          Serial.print(sin_result);
          Serial.print("   Amplitud: ");
          Serial.print(amplilight);
          Serial.print("   Intensity: ");
          Serial.println(intensity);
        }
    
        
        for(int i=0; i< NUM_LEDS ; i++)
        {
              pixels2.setPixelColor( i, 0, 0, 0, 0 );
              pixels3.setPixelColor( i, 0, 0, 0, 0 );
        }
          
        for(int i = 0; i< amplilight ; i++)
        {
              pixels2.setPixelColor( i, rojoFinal, verdeFinal, azulFinal, 0 );
              pixels3.setPixelColor( i, rojoFinal, verdeFinal, azulFinal, 0 );
        }
    
    
        currentMillis = millis();
    
        if (currentMillis - previousMillis > intervalOff + intervalOn) {
    
          previousMillis = currentMillis;
          //dibujar = 0;
    
        }
        else if(currentMillis - previousMillis <= intervalOn )
        {
          dibujar = 1;
        }
        else  
          dibujar = 0;
    
    
    //        if(DEBUG)
    //        {
    //          Serial.print("Dibujar: ");
    //          Serial.println(dibujar);
    //        }
    
        
        if(dibujar)
          showStrip();
        else
        {
           for(int i=0; i< NUM_LEDS ; i++)
           {
                  pixels2.setPixelColor( i, 0, 0, 0, 0 );
                  pixels3.setPixelColor( i, 0, 0, 0, 0 );
           }
           
           showStrip(); 
             
        }
    }
    else if(selectedScene == 4) // Functions: f(x) = ... CON DELAYS
    {
      
      incAngle = freqSinus/10.; // para compensar delays
      angulo = angulo + incAngle; 
      if(angulo > 360)
        angulo = 0;
      
      angle_rad = angulo * M_PI / 180.;

//        sin(x) + sin(2x)
//        sin(4x/4) + sin(6x/4) + cos(x/4)
//        sin(x/1.5) + sin(x/2) + cos(x)
//        (sin(x/1.5) + sin(x/2) + cos(x))*sin(x*2)
//        (sin(x/1.5) + sin(x/2) + cos(x))*sin(x*0.25)*4


     switch (funcion)
      {
        case 1:
          sin_result = (sin(angle_rad) + sin(2*angle_rad))/2.;  // FUNCTION 1
          break;
        case 2:
          sin_result = (sin(angle_rad) + sin(4*angle_rad))/2.;  // + cos(angle_rad))/3.;  // FUNCTION 2
          break;
        case 3:
          sin_result = (2*sin(angle_rad) + sin(3*angle_rad) - cos(angle_rad))*cos(5*angle_rad)/4.;  // FUNCTION 3
          break;
        case 4:
          sin_result = (sin(5*angle_rad) + cos(3*angle_rad))*cos(angle_rad)/2. ;  // FUNCTION 4
          break;
        case 5:
          sin_result = (sin(11*angle_rad) + cos(5*angle_rad))*sin(angle_rad)/2. ;  // FUNCTION 5
          break;
        case 6:
          sin_result = (sin(11*angle_rad) + cos(random1*angle_rad))*sin(random2*angle_rad)/2. ;  // FUNCTION ALEATORIA
          break;
      }

      
      //sin_result = (sin(angle_rad) + sin(2*angle_rad))/2.;  // FUNCTION
      //amplilight = sin_result*escalado*(NUM_LEDS/2) + (NUM_LEDS/2)*umbral ;
      randomDesviacion = noise_random*random(-100,100)/10000.; // random entre -1 y 1

      //Serial.print("Random: ");
      //Serial.println(randomDesviacion);
      
      amplilight = (sin_result + randomDesviacion)*escalado*(NUM_LEDS/2)  + (NUM_LEDS/2)*umbral ;
     

      
        if(DEBUG)
        {
          Serial.print("sin(");
          Serial.print(angulo);
          Serial.print("°) = ");
          Serial.print(sin_result);
          Serial.print("   Amplitud: ");
          Serial.print(amplilight);
          Serial.print("   Intensity: ");
          Serial.println(intensity);
        }
    
        
        for(int i=0; i< NUM_LEDS ; i++)
        {
              pixels2.setPixelColor( i, 0, 0, 0, 0 );
              pixels3.setPixelColor( i, 0, 0, 0, 0 );
        }
          
        for(int i = 0; i< amplilight ; i++)
        {
              pixels2.setPixelColor( i, rojoFinal, verdeFinal, azulFinal, 0 );
              pixels3.setPixelColor( i, rojoFinal, verdeFinal, azulFinal, 0 );
        }

        showStrip(); 
        
        delay(intervalOn);


        if(pwm<100)
        {
          for(int i=0; i< NUM_LEDS ; i++)
          {
                pixels2.setPixelColor( i, 0, 0, 0, 0 );
                pixels3.setPixelColor( i, 0, 0, 0, 0 );
          }
             
          showStrip(); 
          delay(intervalOff);
        }
      
    }
  
}


void splitStrip()
{
  //  CONVERSION DE UNA ANIMACION DE 5M A DOS TRAMOS

  for ( int i = 0; i < 150 ; i++) // numPixels/2
  {

    pixels2.setPixelColor( i, pixels1.getPixelColor(i) );
    pixels3.setPixelColor( i, pixels1.getPixelColor(i + 150) );

  }

}


void showStrip()
{

  //pixels1.show(); // tira entera de 5m
  pixels2.show(); // parte 1 de 2.5m
  pixels3.show(); // parte 2 de 2.5m

}

void setPixel(int Pixel, byte red, byte green, byte blue, byte white)
{

  pixels1.setPixelColor(Pixel, pixels1.Color(red, green, blue, white));

}