Diff: main.cpp

Revision:

1:6e374a9b1bf5

Parent:

0:749a196e6ebe

Child:

2:5bf1990918aa

--- a/main.cpp	Sat Oct 26 00:55:35 2019 +0000
+++ b/main.cpp	Tue Oct 29 22:44:00 2019 +0000
@@ -10,9 +10,12 @@
 AnalogIn opticLeftIn(PTB2);
 AnalogIn opticRigthIn(PTB3);
 DigitalOut ultrasonicTrigger(PTA1);
-DigitalIn ultrasonicEcho(PTB9);
-DigitalIn speedLeft(PTA2);
-DigitalIn speedRigth(PTB23);
+DigitalIn ultrasonicEcho(PTC0);
+DigitalIn speedLeft(PTC7);
+DigitalIn speedRigth(PTC5);
+InterruptIn speedLeftInterrupt(PTA2);
+InterruptIn speedRigthInterrupt(PTB23);
+
 
 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 // Timers y relacionados
@@ -29,14 +32,15 @@
 void sendOpticData();
 void sendUltrasonicData();
 void sendSpeedData();
-
+void riseSpeedLeftInterrupt();
+void riseSpeedRigthInterrupt();
 
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 //   Constantes locales
 
 // Version del FW
-const uint8_t FIRMWARE_VERSION[] = "V0.03";
+const uint8_t FIRMWARE_VERSION[] = "V0.10";
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 // Definiciones locales
@@ -93,8 +97,12 @@
     setup(&rigthOpticSensor, &opticRigthIn);
     setup(&leftOpticSensor, &opticLeftIn);
     setup(&ultrasonicSensor, &ultrasonicTrigger, &ultrasonicEcho);
-    setup(&rigthSpeedSensor, &speedRigth);
-    setup(&leftSpeedSensor, &speedLeft);
+    
+    setup(&rigthSpeedSensor, &speedRigth, 32);
+    setup(&leftSpeedSensor, &speedLeft, 32);
+    speedLeftInterrupt.rise(&riseSpeedLeftInterrupt);
+    speedRigthInterrupt.rise(&riseSpeedRigthInterrupt);
+
 
     opticTimeInterval = 0;
     ultrasonicTimeInterval = 0;
@@ -150,16 +158,27 @@
 void serialReceivePc(){
     serialReceive(&pserial);
 }
-void serialRecevePc(_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++){
@@ -214,35 +233,72 @@
  */
 void sendUltrasonicData(){
   static uint32_t stime = timer1ms.read_ms();
-  static bool inProgress = false;
-  if(inProgress){
+  
+  // 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){ // Ya tengo la distancia, la envio
+    if(distance > 0){ // Ya tengo la distancia, la envio
       work.u16[0] = ultrasonicTimeInterval;
-      serialEnqueueHeader(&pserial, 12);
+      serialEnqueueHeader(&pserial, 6);
       serialEnqueueData(&pserial, 0xA1); // Comando
       serialEnqueueData(&pserial, work.u8[0]); // Intervalo actual (2 bytes)
       serialEnqueueData(&pserial, work.u8[1]);
   
-      work.u16[0] = ultrasonicTimeInterval;
+      work.u16[0] = distance;
       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)){
+  if(timer1ms.read_ms() > stime+(ultrasonicTimeInterval*10)){
     reset(&ultrasonicSensor);
-    inProgress = init(&ultrasonicSensor);
+    init(&ultrasonicSensor);
     stime = timer1ms.read_ms();
   }
 }
 
 
 
-void sendSpeedData(){}
+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