File content as of revision 1:6e374a9b1bf5:

#include "mbed.h"
#include "lib/Pserial.h"
#include "lib/Servo.h"
#include "lib/Sensor.h"

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Conf IO
DigitalOut led1(LED1); // HeartBeat
PwmOut cg90(PTD1);
AnalogIn opticLeftIn(PTB2);
AnalogIn opticRigthIn(PTB3);
DigitalOut ultrasonicTrigger(PTA1);
DigitalIn ultrasonicEcho(PTC0);
DigitalIn speedLeft(PTC7);
DigitalIn speedRigth(PTC5);
InterruptIn speedLeftInterrupt(PTA2);
InterruptIn speedRigthInterrupt(PTB23);


///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Timers y relacionados
Ticker timer100ms;
LowPowerTimer timer1ms;
uint32_t maxUsTicker;

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//  Prototipos locales
void heartBeat();
void heartBeatSetFlag();
void serialReceivePc();
void cmdExec();
void sendOpticData();
void sendUltrasonicData();
void sendSpeedData();
void riseSpeedLeftInterrupt();
void riseSpeedRigthInterrupt();


//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//   Constantes locales

// Version del FW
const uint8_t FIRMWARE_VERSION[] = "V0.10";

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Definiciones locales

// Estructura para uso auxiliar. Arduino es little endian
typedef union{
  uint8_t u8[4];
  uint16_t u16[2];
  int8_t i8[2];
  uint32_t u32;
  int32_t i32;
}_work;


////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Variables y objetos Locales
Serial pc(USBTX, USBRX, 19200);
_Pserial pserial;

_Sensor rigthOpticSensor;
_Sensor leftOpticSensor;
_Sensor ultrasonicSensor;
_Sensor rigthSpeedSensor;
_Sensor leftSpeedSensor;

_Servo servo;

//_Motor motorLeft
//_Motor motorRigth

/*
 * Control de los sensores, tiempos de muestreo
 */
uint16_t opticTimeInterval;
uint16_t ultrasonicTimeInterval;
uint16_t motorSpeedTimeInterval;

// Auxiliar
_work work;

// Flag del heartBeat()
bool heartBeatFlag;

extern _txbuffer TX;
extern _rxbuffer RX;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////   MAIN

int main(){
    maxUsTicker = us_ticker_read();
    timer100ms.attach(&heartBeatSetFlag, 0.1);
    heartBeatFlag = false;
    timer1ms.start();
    setup(&rigthOpticSensor, &opticRigthIn);
    setup(&leftOpticSensor, &opticLeftIn);
    setup(&ultrasonicSensor, &ultrasonicTrigger, &ultrasonicEcho);
    
    setup(&rigthSpeedSensor, &speedRigth, 32);
    setup(&leftSpeedSensor, &speedLeft, 32);
    speedLeftInterrupt.rise(&riseSpeedLeftInterrupt);
    speedRigthInterrupt.rise(&riseSpeedRigthInterrupt);


    opticTimeInterval = 0;
    ultrasonicTimeInterval = 0;
    motorSpeedTimeInterval = 0;
    
    pc.attach(&serialReceivePc, Serial::RxIrq);
    serialSetup(&pserial, &pc);
    
    setup(&servo, &cg90);
    while(1){
        // heartBeat
        if(heartBeatFlag) heartBeat();
        
        // Overflow de us_ticker_read(), reseteo todo lo que utiliza este dato
        if(maxUsTicker > us_ticker_read()){
            maxUsTicker = us_ticker_read();
            serialCmdClean(&pserial);
        }
        
         // envio datos que pueda haber en el buffer de salida
          serialSubmit(&pserial);
                    
          // proceso comando presente en el buffer
          if(pserial.RX.cmd)
            cmdExec();
        
          // Busco si hay un nuevo header en el buffer
          serialGetFrame(&pserial);
        
          // Ejecuto tareas de sensores
          if(opticTimeInterval){
            sendOpticData();
          }
        
          if(ultrasonicTimeInterval){
            sendUltrasonicData();
          }
          
          if(motorSpeedTimeInterval){
            sendSpeedData();
          }
    }
}



///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///  Funciones locales

/*
 * Interrupcion, datos en el puerto serie con la PC
 */
void serialReceivePc(){
    serialReceive(&pserial);
}


/*
 * Señal de activo, dos parpadeos de 100ms cada dos segundos
 * El timer me fueza a pasar pr el wile(1) del main, sino no tiene mucho sentido un heartbeat por interrupciones
 */
void heartBeatSetFlag(){
    heartBeatFlag = true;
}

/*
 * Interrupciones de los pin digitales de los sensores
 */
void riseSpeedLeftInterrupt(){
    notify(&leftSpeedSensor);
}
void riseSpeedRigthInterrupt(){
    notify(&rigthSpeedSensor);
}


void heartBeat(){
  static int count = 0;
  switch (count++){
    case 0:
    case 2:
        led1 = 0;
        break;
    case 1:
    case 3:
        led1 = 1;
        break;
    case 20:
        count = 0;
        break;
    }
    heartBeatFlag = false;
}



/*
 * Envio datos del sensor optico a la velocidad especificada
 */
void sendOpticData(){
  static unsigned long stime = timer1ms.read_ms();
  if(timer1ms.read_ms() > stime+(opticTimeInterval*10)){
    work.u16[0] = opticTimeInterval;
    serialEnqueueHeader(&pserial, 12);
    serialEnqueueData(&pserial, 0xA0); // Comando
    serialEnqueueData(&pserial, work.u8[0]); // Intervalo actual (2 bytes)
    serialEnqueueData(&pserial, work.u8[1]);
    serialEnqueueData(&pserial, 0);  // Sensor exterior izquierdo (2 bytes)
    serialEnqueueData(&pserial, 0);
    work.u16[0] = getValue(&leftOpticSensor);
    work.u16[1] = getValue(&rigthOpticSensor);
    serialEnqueueData(&pserial, work.u8[0]); // Sensor interior izquierdo (2 bytes)
    serialEnqueueData(&pserial, work.u8[1]);
    serialEnqueueData(&pserial, work.u8[2]); // Sensor interior drecho (2 bytes)
    serialEnqueueData(&pserial, work.u8[3]);
    serialEnqueueData(&pserial, 0); // Sensor exterior derecho (2 bytes)
    serialEnqueueData(&pserial, 0);
    serialEnqueueChksum(&pserial);
    stime = timer1ms.read_ms();
  }
}



/*
 * Envio datos del sensor ultrasonico a la velocidad especificada
 * Los tiempos de llamada deberian estar espaciados al menos 50ms
 */
void sendUltrasonicData(){
  static uint32_t stime = timer1ms.read_ms();
  
  // Utilizando PING
  /*if(timer1ms.read_ms() > stime+(ultrasonicTimeInterval*10)){
      pserial.port->putc(0x1);
      uint16_t distance = ping(&ultrasonicSensor, 1000);
      pserial.port->putc(0xff);
      work.u16[0] = distance;
      pserial.port->putc(work.u8[1]);
      pserial.port->putc(work.u8[0]);
      serialEnqueueHeader(&pserial, 6);
      serialEnqueueData(&pserial, 0xA1); // Comando
      work.u16[0] = ultrasonicTimeInterval;
      serialEnqueueData(&pserial, work.u8[0]); // Intervalo actual (2 bytes)
      serialEnqueueData(&pserial, work.u8[1]);
      work.u16[0] = distance;
      serialEnqueueData(&pserial, work.u8[0]); // Distancia medida (2 bytes)
      serialEnqueueData(&pserial, work.u8[1]);
      serialEnqueueChksum(&pserial);
      stime = timer1ms.read_ms();
  }*/
  
  // Utilizando init y isReady
  if(ultrasonicSensor.stage){
    uint16_t distance = isReady(&ultrasonicSensor);
    if(distance > 0){ // Ya tengo la distancia, la envio
      work.u16[0] = ultrasonicTimeInterval;
      serialEnqueueHeader(&pserial, 6);
      serialEnqueueData(&pserial, 0xA1); // Comando
      serialEnqueueData(&pserial, work.u8[0]); // Intervalo actual (2 bytes)
      serialEnqueueData(&pserial, work.u8[1]);
  
      work.u16[0] = distance;
      serialEnqueueData(&pserial, work.u8[0]); // Distancia medida (2 bytes)
      serialEnqueueData(&pserial, work.u8[1]);
  
      serialEnqueueChksum(&pserial);
    }
  }
  
  if(timer1ms.read_ms() > stime+(ultrasonicTimeInterval*10)){
    reset(&ultrasonicSensor);
    init(&ultrasonicSensor);
    stime = timer1ms.read_ms();
  }
}



void sendSpeedData(){
  static unsigned long stime = timer1ms.read_ms();
  if(timer1ms.read_ms() > stime+(motorSpeedTimeInterval*10)){
    work.u16[0] = motorSpeedTimeInterval;
    serialEnqueueHeader(&pserial, 8);
    serialEnqueueData(&pserial, 0xA4); // Comando
    serialEnqueueData(&pserial, work.u8[0]); // Intervalo actual (2 bytes)
    serialEnqueueData(&pserial, work.u8[1]);
    work.u16[0] = getRpm(&leftSpeedSensor);
    work.u16[1] = getRpm(&rigthSpeedSensor);
    serialEnqueueData(&pserial, work.u8[0]); // Sensor izquierdo (2 bytes)
    serialEnqueueData(&pserial, work.u8[1]);
    serialEnqueueData(&pserial, work.u8[2]); // Sensor drecho (2 bytes)
    serialEnqueueData(&pserial, work.u8[3]);
    serialEnqueueChksum(&pserial);
    stime = timer1ms.read_ms();
  }    
}

/*
 * Se ha recibido un comando por puerto serie, lo interpreto y respondo
 * Si no hay lugar en el buffer lo espero TXCMDTIME us
 */
void cmdExec(){
  bool ok = false;
  switch(pserial.RX.buf[pserial.RX.cmdIndex]){
    // ping
    case 0xF0:{
      ok = serialEnqueueHeader(&pserial, 3) && serialEnqueueData(&pserial, 0xF0) && serialEnqueueData(&pserial, 0x0D) && serialEnqueueChksum(&pserial);
      break;
    }
    // Version de Firmware
    case 0xF1:{
      ok = serialEnqueueHeader(&pserial, 13+sizeof(FIRMWARE_VERSION)+sizeof(__DATE__)+sizeof(__TIME__)) && serialEnqueueData(&pserial, 0xF1);
      for (int i = 0 ; i < sizeof(FIRMWARE_VERSION) ; i++)
        ok = ok && serialEnqueueData(&pserial, FIRMWARE_VERSION[i]);
      ok = ok && serialEnqueueData(&pserial, '_') && serialEnqueueData(&pserial, 'b') && serialEnqueueData(&pserial, 'u') && serialEnqueueData(&pserial, 'i') && serialEnqueueData(&pserial, 'l') && serialEnqueueData(&pserial, 'd') && serialEnqueueData(&pserial, '_');
      for (int i = 0 ; i < sizeof(__DATE__) ; i++)
        ok = ok && serialEnqueueData(&pserial, __DATE__[i]);
      ok = ok && serialEnqueueData(&pserial, '_') && serialEnqueueData(&pserial, 'a') && serialEnqueueData(&pserial, 't') && serialEnqueueData(&pserial, '_');
      for (int i = 0 ; i < sizeof(__TIME__) ; i++)
        ok = ok && serialEnqueueData(&pserial, __TIME__[i]);
      ok = ok && serialEnqueueChksum(&pserial);
      break;
    }
    // Sensores Opticos
    case 0xA0:{
      work.u8[0] = pserial.RX.buf[pserial.RX.cmdIndex+1];
      work.u8[1] = pserial.RX.buf[pserial.RX.cmdIndex+2];
      opticTimeInterval = work.u16[0];
      ok = true;
      break;
    }
    // Sensor Ultrasonico
    case 0xA1:{
      work.u8[0] = pserial.RX.buf[pserial.RX.cmdIndex+1];
      work.u8[1] = pserial.RX.buf[pserial.RX.cmdIndex+2];
      ultrasonicTimeInterval = work.u16[0];
      ok = true;
      break;
    }
    // Sensores de horquilla
    case 0xA4:{
      work.u8[0] = pserial.RX.buf[pserial.RX.cmdIndex+1];
      work.u8[1] = pserial.RX.buf[pserial.RX.cmdIndex+2];
      motorSpeedTimeInterval = work.u16[0];
      ok = true;
      break;
    }
    default:{
      ok = serialEnqueueHeader(&pserial, 2) && serialEnqueueData(&pserial, 0xFF) && serialEnqueueChksum(&pserial);
    }
  }
  if (ok || us_ticker_read() > pserial.TX.cmdTime + TXCMDTIME){
    pserial.RX.cmd = false;
    pserial.RX.indexR = pserial.RX.cmdIndex+pserial.RX.payloadSize;
    pserial.TX.cmdTime = 0;
  }
}