Diff: main.cpp

Revision:

0:749a196e6ebe

Child:

1:6e374a9b1bf5

diff -r 000000000000 -r 749a196e6ebe main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Oct 26 00:55:35 2019 +0000
@@ -0,0 +1,306 @@
+#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(PTB9);
+DigitalIn speedLeft(PTA2);
+DigitalIn speedRigth(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();
+
+
+
+//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+//   Constantes locales
+
+// Version del FW
+const uint8_t FIRMWARE_VERSION[] = "V0.03";
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+// 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);
+    setup(&leftSpeedSensor, &speedLeft);
+
+    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);
+}
+void serialRecevePc(_Pserial *);
+
+
+/*
+ * Señal de activo, dos parpadeos de 100ms cada dos segundos
+ */
+void heartBeatSetFlag(){
+    heartBeatFlag = true;
+}
+
+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();
+  static bool inProgress = false;
+  if(inProgress){
+    uint16_t distance = isReady(&ultrasonicSensor);
+    if(distance){ // Ya tengo la distancia, la envio
+      work.u16[0] = ultrasonicTimeInterval;
+      serialEnqueueHeader(&pserial, 12);
+      serialEnqueueData(&pserial, 0xA1); // Comando
+      serialEnqueueData(&pserial, work.u8[0]); // Intervalo actual (2 bytes)
+      serialEnqueueData(&pserial, work.u8[1]);
+  
+      work.u16[0] = ultrasonicTimeInterval;
+      serialEnqueueData(&pserial, work.u8[0]); // Distancia medida (2 bytes)
+      serialEnqueueData(&pserial, work.u8[1]);
+  
+      serialEnqueueChksum(&pserial);
+      inProgress = false;
+    }
+  }
+  
+  if(timer1ms.read_ms() > stime+(opticTimeInterval*10)){
+    reset(&ultrasonicSensor);
+    inProgress = init(&ultrasonicSensor);
+    stime = timer1ms.read_ms();
+  }
+}
+
+
+
+void sendSpeedData(){}
+
+/*
+ * 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;
+  }
+}