/*
 * Program by Jose Lamarca Antena 2.0
 */

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

//
// Hardware configuration
//

// Set up nRF24L01 radio on SPI bus plus pins 8 & 9 
RF24 radio(7,8);
//
// Topology
//
// Four cars controlled
byte V1[7];
byte V2[7];
byte V3[7];
byte V4[7];

 
// Radio pipe addresses for the 2 nodes to communicate.
const uint64_t pipes[2] = { 0xF0F0F0F008LL, 0xF0F0F0F010LL };

int Connection=0;
int coche,xact,yact,thetaact,xdes,ydes,deb;

void setup(void)
{
  Serial.begin(9600);
  printf_begin();
 
  radio.begin();
  radio.setRetries(15,15);

  radio.openWritingPipe(pipes[1]);
  radio.openReadingPipe(1,pipes[0]);

  radio.printDetails();


  for (int i=0;i<7;i++){
        V1[i]=0;
        V2[i]=0; 
        V3[i]=0; 
        V4[i]=0; }

 }


void loop(void)
{


    radio.write( &V1, sizeof(V1) );
    radio.write( &V2, sizeof(V2) );
    radio.write( &V3, sizeof(V3) );
    radio.write( &V4, sizeof(V4) );
  }

void serialEvent(){

  if (Serial.available()) {
    coche=Serial.parseInt();
    xact=Serial.parseInt();
    yact=Serial.parseInt();
    thetaact=Serial.parseInt();
    xdes=Serial.parseInt();
    ydes=Serial.parseInt();
    deb=Serial.parseInt();
    }
   if (coche>0){
    V1[0]=coche;
    V1[1]=xact;
    V1[2]=yact;
    V1[3]=thetaact;  
    V1[4]=xdes;
    V1[5]=ydes;
    V1[6]=deb;
    Serial.println(coche);
    Serial.println(xact);
    Serial.println(yact); 
    Serial.println(thetaact);
    Serial.println(xdes);
    Serial.println(ydes);
    Serial.println(deb);}
/*      
  switch(coche)
  {
    case 1:
      V1[0]=coche;
      V1[1]=xact;
      V1[2]=yact;
      V1[3]=thetaact;  
      V1[4]=xdes;
      V1[5]=ydes;
      V1[6]=deb;
    case 2:
      V2[0]=coche;
      V2[1]=xact;
      V2[2]=yact;
      V2[3]=thetaact;  
      V2[4]=xdes;
      V2[5]=ydes;
      V2[6]=deb;
    case 3:
      V3[0]=coche;
      V3[1]=xact;
      V3[2]=yact;
      V3[3]=thetaact;  
      V3[4]=xdes;
      V3[5]=ydes;
      V3[6]=deb;
    case 4:
      V4[0]=coche;
      V4[1]=xact;
      V4[2]=yact;
      V4[3]=thetaact;  
      V4[4]=xdes;
      V4[5]=ydes;
      V4[6]=deb;
  }
   
*/
}