File content as of revision 2:5bf1990918aa:

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

#define WHEEL_ENCODER_SLOTS 32
#define WHEEL_PERIM 105

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


///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Conf IO
DigitalOut led1(PTB21); // HeartBeat - LedBlue

PwmOut servoPwm(PTA2);
DigitalOut servoDigitalPin(PTC12); // No logro estabilidad en PWM a periodos tan largos como requiere el servo, lo emulo con este pin

AnalogIn opticLeftIn(PTB2);
AnalogIn opticRigthIn(PTB3);

DigitalOut ultrasonicTrigger(PTB18);
DigitalIn ultrasonicEcho(PTC0);

DigitalIn speedLeft(PTC7);
DigitalIn speedRigth(PTC5);
InterruptIn speedLeftInterrupt(PTC7);  // Interrupciones para los pines de las horquillas
InterruptIn speedRigthInterrupt(PTC5);

DigitalOut motorLeftA(PTB19);
DigitalOut motorLeftB(PTC1);
PwmOut motorLeftPwm(PTC2);
DigitalOut motorRigthA(PTC8);
DigitalOut motorRigthB(PTC9);
PwmOut motorRigthPwm(PTC3);

/* Conf IO  Blue Pill
DigitalOut led1(LED1); // HeartBeat
PwmOut cg90(PB_5);
AnalogIn opticLeftIn(PA_0);
AnalogIn opticRigthIn(PA_1);
DigitalOut ultrasonicTrigger(PB_4);
DigitalIn ultrasonicEcho(PB_3);
DigitalIn speedLeft(PA_15);
DigitalIn speedRigth(PA_11);
InterruptIn speedLeftInterrupt(PA_15);   // Interrupciones para los pines de las horquillas
InterruptIn speedRigthInterrupt(PA_12);
PwmOut motorLeft(PA_11);
PwmOut motorRigth(PA_8);
*/


///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Timers y relacionados
Ticker ticker100ms;
Ticker ticker20ms;
Timer timer1ms;
uint32_t maxUsTicker;

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



////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// 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;

// Auxiliar
_work work;

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Variables 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;

// Indica si el servo está en la posicion a la que fue configurado
bool servoIsInPos;
volatile bool flipServoDigitalOut;
volatile uint32_t flipTimeServoDigitalOut;

// Velocidad, direccion y timeout solicitada en los motores
uint16_t leftMotorRqSpeed;
uint16_t rigthMotorRqSpeed;
uint32_t leftMotorRqTimeOut;
uint32_t rigthMotorRqTimeOut;
uint8_t leftMotorRqType;
uint8_t rigthMotorRqType;


// Flag del heartBeat()
bool heartBeatFlag;


///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////   MAIN

int main(){
    maxUsTicker = us_ticker_read();
    ticker100ms.attach(&heartBeatSetFlag, 0.1);
    ticker20ms.attach(&pwmServo, 0.02);
    heartBeatFlag = false;
    timer1ms.start();
    
    // Setup de puerto serie
    pc.attach(&serialReceivePc, Serial::RxIrq);
    serialSetup(&pserial, &pc);
    
    // setup de los sensores opticos
    setup(&rigthOpticSensor, &opticRigthIn);
    setup(&leftOpticSensor, &opticLeftIn);
    
    // setup del sensor ultrasonico
    setup(&ultrasonicSensor, &ultrasonicTrigger, &ultrasonicEcho);
    
    // setup de los sensores de horquilla
    setup(&leftSpeedSensor, &speedLeft, WHEEL_ENCODER_SLOTS, WHEEL_PERIM);
    setup(&rigthSpeedSensor, &speedRigth, WHEEL_ENCODER_SLOTS, WHEEL_PERIM);
    speedLeftInterrupt.rise(&riseSpeedLeftInterrupt);
    speedRigthInterrupt.rise(&riseSpeedRigthInterrupt);

    // setup del servo
    setup(&servo, &servoPwm);
    setMinPulseWidth_us(&servo, 500);
    setMaxPulseWidth_us(&servo, 2500);

    
    // setup de los driver de motor
    setup(&motorLeft, &leftSpeedSensor, &motorLeftPwm, &motorLeftA, &motorLeftB);
    setup(&motorRigth, &rigthSpeedSensor, &motorRigthPwm, &motorRigthA, &motorRigthB);

    // Variables de control
    opticTimeInterval = 0;
    ultrasonicTimeInterval = 0;
    motorSpeedTimeInterval = 0;
    
    servoIsInPos = true;
    flipServoDigitalOut = false;
        
    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);
            reset(&ultrasonicSensor);
            reset(&rigthSpeedSensor);
            reset(&leftSpeedSensor);
        }
        
        // 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 && servoIsInPos){
            sendUltrasonicData();
        }
          
        if(motorSpeedTimeInterval){
           sendSpeedData();
        }

        // Ejecuto tareas con actuadores
        if(inPos(&servo) && !servoIsInPos){
            servoIsInPos = true;
            // Notifico fin de movimiento
            work.u16[0] = servo.speed;
            serialEnqueueHeader(&pserial, 6);
            serialEnqueueData(&pserial, 0xA2);
            serialEnqueueData(&pserial, servo.currentPos);
            serialEnqueueData(&pserial, work.u8[0]);
            serialEnqueueData(&pserial, work.u8[1]);
            serialEnqueueData(&pserial, 0x0A);
            serialEnqueueChksum(&pserial);
        }
        
        // pseudo PWM del servo
        if(flipServoDigitalOut && us_ticker_read() > (flipTimeServoDigitalOut+servo.currentPulseWidth)){
            flipServoDigitalOut = false;
            servoDigitalPin.write(0);
        }
    }
}



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

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

/*
 * pwm a manopla para manejar el faquing servo. Periodo es 20ms, pulsos entre 1 y 2ms. el rango es 180, esta func se ejecuta cada 10us y me da una definicion de 100 llamados cada 1ms
 */
void pwmServo(){
    flipServoDigitalOut = true;
    flipTimeServoDigitalOut = us_ticker_read();
    servoDigitalPin.write(1);
}

/*
 * 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 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 ok no se setea a true, el comando se vuelve a prosesar
 * Por ejemplo para responder el alive, si no hay lugar en el buffer no seteo ok y por lo tanto
 * el comando se vuelve a procesar. No es indefinido, 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;
    }
    // Servo
    case 0xA2:{
      work.u8[0] = pserial.RX.buf[pserial.RX.cmdIndex+2];
      work.u8[1] = pserial.RX.buf[pserial.RX.cmdIndex+3];
      setPos(&servo, pserial.RX.buf[pserial.RX.cmdIndex+1], work.u16[0]);
      servoIsInPos = false;
      ok = serialEnqueueHeader(&pserial, 3) && 
        serialEnqueueData(&pserial, 0xA2) && 
        serialEnqueueData(&pserial, 0x0D) && 
        serialEnqueueChksum(&pserial);
      break;
    }
    // Control de motores
    case 0xA3:{
      rigthMotorRqType = pserial.RX.buf[pserial.RX.cmdIndex+1];
      leftMotorRqType = pserial.RX.buf[pserial.RX.cmdIndex+2];
      
      work.u8[0] = pserial.RX.buf[pserial.RX.cmdIndex+3];
      work.u8[1] = pserial.RX.buf[pserial.RX.cmdIndex+4];
      work.u8[2] = pserial.RX.buf[pserial.RX.cmdIndex+5];
      work.u8[3] = pserial.RX.buf[pserial.RX.cmdIndex+6];
      rigthMotorRqTimeOut = work.u32;
      
      work.u8[0] = pserial.RX.buf[pserial.RX.cmdIndex+7];
      work.u8[1] = pserial.RX.buf[pserial.RX.cmdIndex+8];
      work.u8[2] = pserial.RX.buf[pserial.RX.cmdIndex+9];
      work.u8[3] = pserial.RX.buf[pserial.RX.cmdIndex+10];
      leftMotorRqTimeOut = work.u32;;

      work.u8[0] = pserial.RX.buf[pserial.RX.cmdIndex+11];
      work.u8[1] = pserial.RX.buf[pserial.RX.cmdIndex+12];
      work.u8[2] = pserial.RX.buf[pserial.RX.cmdIndex+13];
      work.u8[3] = pserial.RX.buf[pserial.RX.cmdIndex+14];      
      rigthMotorRqSpeed = work.u16[0];
      leftMotorRqSpeed = work.u16[1];
      ok = serialEnqueueHeader(&pserial, 5) && 
        serialEnqueueData(&pserial, 0xA3) && 
        serialEnqueueData(&pserial, rigthMotorRqType) && 
        serialEnqueueData(&pserial, leftMotorRqType) && 
        serialEnqueueData(&pserial, 0x0D) && 
        serialEnqueueChksum(&pserial);
      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;
    }
    // Comandos fuera de protocolo
    // Setear period y pulseWidth del servo (ui16 period, ui16 min, ui16 max)
    case 0xE0:{
        work.u8[0] = pserial.RX.buf[pserial.RX.cmdIndex+1];
        work.u8[1] = pserial.RX.buf[pserial.RX.cmdIndex+2];
        setPeriod_us(&servo, work.u16[0]);
        work.u8[0] = pserial.RX.buf[pserial.RX.cmdIndex+3];
        work.u8[1] = pserial.RX.buf[pserial.RX.cmdIndex+4];
        work.u8[2] = pserial.RX.buf[pserial.RX.cmdIndex+5];
        work.u8[3] = pserial.RX.buf[pserial.RX.cmdIndex+6];
        setMinPulseWidth_us(&servo, work.u16[0]);
        setMaxPulseWidth_us(&servo, work.u16[1]);
        ok = serialEnqueueHeader(&pserial, 3) && 
            serialEnqueueData(&pserial, 0xE0) && 
            serialEnqueueData(&pserial, 0x0D) && 
            serialEnqueueChksum(&pserial);
        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;
  }
}