Diff: main.cpp

Revision:

2:5bf1990918aa

Parent:

1:6e374a9b1bf5

--- a/main.cpp	Tue Oct 29 22:44:00 2019 +0000
+++ b/main.cpp	Tue Nov 05 20:07:52 2019 +0000
@@ -2,30 +2,67 @@
 #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(LED1); // HeartBeat
-PwmOut cg90(PTD1);
+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(PTA1);
+
+DigitalOut ultrasonicTrigger(PTB18);
 DigitalIn ultrasonicEcho(PTC0);
+
 DigitalIn speedLeft(PTC7);
 DigitalIn speedRigth(PTC5);
-InterruptIn speedLeftInterrupt(PTA2);
-InterruptIn speedRigthInterrupt(PTB23);
+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 timer100ms;
-LowPowerTimer timer1ms;
+Ticker ticker100ms;
+Ticker ticker20ms;
+Timer timer1ms;
 uint32_t maxUsTicker;
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 //  Prototipos locales
 void heartBeat();
+void pwmServo();
 void heartBeatSetFlag();
 void serialReceivePc();
 void cmdExec();
@@ -36,11 +73,6 @@
 void riseSpeedRigthInterrupt();
 
 
-//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-//   Constantes locales
-
-// Version del FW
-const uint8_t FIRMWARE_VERSION[] = "V0.10";
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 // Definiciones locales
@@ -54,9 +86,11 @@
   int32_t i32;
 }_work;
 
+// Auxiliar
+_work work;
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-// Variables y objetos Locales
+// Variables Locales
 Serial pc(USBTX, USBRX, 19200);
 _Pserial pserial;
 
@@ -68,8 +102,9 @@
 
 _Servo servo;
 
-//_Motor motorLeft
-//_Motor motorRigth
+_Motor motorLeft;
+_Motor motorRigth;
+
 
 /*
  * Control de los sensores, tiempos de muestreo
@@ -78,40 +113,69 @@
 uint16_t ultrasonicTimeInterval;
 uint16_t motorSpeedTimeInterval;
 
-// Auxiliar
-_work work;
+// 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;
 
-extern _txbuffer TX;
-extern _rxbuffer RX;
+
 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 /////////////////   MAIN
 
 int main(){
     maxUsTicker = us_ticker_read();
-    timer100ms.attach(&heartBeatSetFlag, 0.1);
+    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(&rigthSpeedSensor, &speedRigth, 32);
-    setup(&leftSpeedSensor, &speedLeft, 32);
+    // 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;
     
-    pc.attach(&serialReceivePc, Serial::RxIrq);
-    serialSetup(&pserial, &pc);
-    
-    setup(&servo, &cg90);
+    servoIsInPos = true;
+    flipServoDigitalOut = false;
+        
     while(1){
         // heartBeat
         if(heartBeatFlag) heartBeat();
@@ -120,30 +184,53 @@
         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);
+        // envio datos que pueda haber en el buffer de salida
+        serialSubmit(&pserial);
                     
-          // proceso comando presente en el buffer
-          if(pserial.RX.cmd)
+        // proceso comando presente en el buffer
+        if(pserial.RX.cmd)
             cmdExec();
         
-          // Busco si hay un nuevo header en el buffer
-          serialGetFrame(&pserial);
+        // Busco si hay un nuevo header en el buffer
+        serialGetFrame(&pserial);
         
-          // Ejecuto tareas de sensores
-          if(opticTimeInterval){
+        // Ejecuto tareas de sensores
+        if(opticTimeInterval){
             sendOpticData();
-          }
+        }
         
-          if(ultrasonicTimeInterval){
+        if(ultrasonicTimeInterval && servoIsInPos){
             sendUltrasonicData();
-          }
+        }
           
-          if(motorSpeedTimeInterval){
-            sendSpeedData();
-          }
+        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);
+        }
     }
 }
 
@@ -159,6 +246,14 @@
     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
@@ -234,26 +329,6 @@
 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);
@@ -302,7 +377,9 @@
 
 /*
  * Se ha recibido un comando por puerto serie, lo interpreto y respondo
- * Si no hay lugar en el buffer lo espero TXCMDTIME us
+ * 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;
@@ -342,6 +419,49 @@
       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];
@@ -350,6 +470,26 @@
       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);
     }