Updated motor controlling system

new functions and some fixes

hover-controller/PinConfig.h

@@ -27,8 +27,22 @@
 
 #define MAIN_PROP_GPIO      RPI_V2_GPIO_P1_13
 
-#define MAIN_PROP_START_PPM 1060
-#define MAIN_PROP_STOP_PPM  2400
+#define MAIN_PROP_START_PPM     1065
+#define MAIN_PROP_IDLE_PPM      1065
+#define MAIN_PROP_STOP_PPM      2400
+#define MAIN_PROP_MAX_PPM       2020
+
+#define LEFT_PROP_START_PPM     1065
+#define LEFT_PROP_IDLE_PPM      1065
+#define LEFT_PROP_STOP_PPM      2400
+#define LEFT_PROP_MAX_PPM       2020
+
+#define RIGHT_PROP_START_PPM    1065
+#define RIGHT_PROP_IDLE_PPM     1065
+#define RIGHT_PROP_STOP_PPM     2400
+#define RIGHT_PROP_MAX_PPM      2020
+
+#define ESC_WAIT_PPM        1005
 
 
 #endif // __PIN_CONFIG_H__

hover-controller/hover-controller.cpp

@@ -1,15 +1,16 @@
 /*
 TMRh20 2014
+Angel garcía 2019
 
  This program is free software; you can redistribute it and/or
  modify it under the terms of the GNU General Public License
  version 2 as published by the Free Software Foundation.
  */
 
-/** General Data Transfer Rate Test
- * This example demonstrates basic data transfer functionality with the
- updated library. This example will display the transfer rates acheived using
- the slower form of high-speed transfer using blocking-writes.
+/** Hover Controller
+ * This program manage all the RF communications and electronic elements
+ * communication.
+ * The code here manage rf intereactions and pass messages to the dispatcher.
  */
 
 #include "../rfudp/TxTunnel.h"
@@ -78,11 +79,12 @@ int main(int argc, char** argv) {
     if(gpioInitialise()<0) {
         perror("[pigpio] gpioInitialise error");
         exit(EXIT_FAILURE);
-    }else{
-        servo_mv_r(MAIN_PROP_GPIO, 0);
-        servo_mv_r(LEFT_PROP_GPIO, 0);
-        servo_mv_r(RIGHT_PROP_GPIO, 0);
     }
+    // initialize all motors
+    all_motors_zero();  // set PPM to 0
+    sleep(1);           // wait 1 sec
+    all_motors_stop();  // set PPM to stop
+
 
     // This opens two pipes for these two nodes to communicate
     // back and forth.

hover-controller/motors.cpp

@@ -8,16 +8,49 @@ void motor_esc_initialize(unsigned int pin) {
     //servo_mv_r(pin, 0);
     //sleep(0.5);
     servo_mv_r(pin, MAIN_PROP_STOP_PPM);
-    usleep(10000);
     //sleep(1);
     //servo_mv_r(pin, MAIN_PROP_START_PPM);
 }
 
+void all_motors_stop() {
+    servo_mv_r(MAIN_PROP_GPIO,  MAIN_PROP_STOP_PPM);
+    servo_mv_r(LEFT_PROP_GPIO,  LEFT_PROP_STOP_PPM);
+    servo_mv_r(RIGHT_PROP_GPIO, RIGHT_PROP_STOP_PPM);
+}
+void all_motors_zero() {
+    servo_mv_r(MAIN_PROP_GPIO,  0);
+    servo_mv_r(LEFT_PROP_GPIO,  0);
+    servo_mv_r(RIGHT_PROP_GPIO, 0);
+}
+void all_motors_start() {
+    servo_mv_r(MAIN_PROP_GPIO,  MAIN_PROP_START_PPM);
+    servo_mv_r(LEFT_PROP_GPIO,  LEFT_PROP_START_PPM);
+    servo_mv_r(RIGHT_PROP_GPIO, RIGHT_PROP_START_PPM);
+}
+
+int map_esc_per(unsigned int pin, int *lv) {
+    switch(pin) {
+        case MAIN_PROP_GPIO:
+            *lv = map(*lv, 0, 100, MAIN_PROP_IDLE_PPM, MAIN_PROP_MAX_PPM);
+            break;
+        case LEFT_PROP_GPIO:
+            *lv = map(*lv, 0, 100, LEFT_PROP_IDLE_PPM, LEFT_PROP_MAX_PPM);
+            break;
+        case RIGHT_PROP_GPIO:
+            *lv = map(*lv, 0, 100, RIGHT_PROP_IDLE_PPM, RIGHT_PROP_MAX_PPM);
+            break;
+        default:
+            *lv = map(*lv, 0, 100, ESC_MIN, ESC_MAX);
+    }
+    return *lv;
+}
+
 void motor_process(char*msg){
     char servo_index = msg[4];
     char servo_mode  = msg[5];
     int lv;
     char action[5];
+    unsigned int pin;
 
     static bool started=false;
 
@@ -25,30 +58,65 @@ void motor_process(char*msg){
     eprintf("Act: %s\n", action);
 
     if (strcmp(action, "START") == 0) {
-        if (!started){
-            eprintf("Estarting ESC\n");
-            motor_esc_initialize(MAIN_PROP_GPIO);
-            servo_mv_r(MAIN_PROP_GPIO, MAIN_PROP_START_PPM);
+        if (!started) {
+            eprintf("Estarting ESCs\n");
+            all_motors_stop();  // set all motors to stop position
+            usleep(10000);      // wait just a bit (10000 µs or 0,01 secs)
+            all_motors_start(); // set all to start speed
+            //servo_mv_r(MAIN_PROP_GPIO, MAIN_PROP_START_PPM);
             started=true;
-        }else
+
+            strcpy(msg, "ESC_STARTED");
+        } else {
             eprintf("ESC already started\n");
+
+            strcpy(msg, "ESC_IS_STARTED");
+        }
+        return;
     } else
     if (strcmp(action, "STOP") == 0) {
-        servo_mv_r(MAIN_PROP_GPIO, MAIN_PROP_STOP_PPM);
+        //servo_mv_r(MAIN_PROP_GPIO, MAIN_PROP_STOP_PPM);
+        all_motors_stop();  // Stops all ESCs
         started=false;
+
+        strcpy(msg, "ESC_STOPPED");
+        return;
     } else {
         sscanf(&msg[6], " %i ", &lv );
     }
 
-    if (servo_index == 'M') {
-        servo_mv_r(MAIN_PROP_GPIO, lv);
-    } else
-    if (servo_index == 'L') {
-        servo_mv_r(LEFT_PROP_GPIO, lv);
-
-    } else
-    if (servo_index == 'R') {
-        servo_mv_r(RIGHT_PROP_GPIO, lv);
-
+    if (servo_index == 'M')
+        pin = MAIN_PROP_GPIO;
+    else
+    if (servo_index == 'L')
+        pin = LEFT_PROP_GPIO;
+    else
+    if (servo_index == 'R')
+        pin = RIGHT_PROP_GPIO;
+    else {
+        eprintf("[motors] index not valid (%c)\n", servo_index);
+        strcpy(msg, "ESC_INDX_NOK");
+        return;
     }
+    
+    switch (servo_mode) {
+        case ' ':
+            if (lv < 500)
+                servo_mode = '%';
+            else
+                servo_mode = 'R';
+                break;
+        case '%':
+            map_esc_per(pin, &lv);
+            break;
+        case 'R':
+            break;
+        default:
+            eprintf("[motors] mode not valid (%c)\n", *mode);
+            strcpy(msg, "ESC_MODE_NOK");
+            return;
+    }
+
+    servo_mv_r(pin, lv);
+    sprintf(msg, "ESC %c%c %i", servo_index, servo_mode, lv);
 }

hover-controller/motors.h

@@ -3,5 +3,6 @@
 
 #include "servos.h"
 void motor_process(char*msg);
+void all_motors_stop();
 
 #endif // __MOTORS_H__

rfudp/TxTunnel.h

@@ -32,17 +32,18 @@
 // various functions and definitions
 #define eprintf(...) fprintf(stderr, __VA_ARGS__);
 
-#define FALSE (1 != 1)
-#define TRUE (!FALSE)
+#define FALSE   (1 != 1)
+#define TRUE    (!FALSE)
 
 
 // TxTunnel Congif
 #define SERVER_IP   "192.168.1.149"
 #define SERVER_PORT 8080
 #define MAX_MSG_SZ  32
-#define RF_CHANNEL 0
+#define RF_CHANNEL  0
+#define MILLIS_WAIT 200
 
-#define ACK_MODE FALSE
+#define ACK_MODE    FALSE
 
 // Radio pipe addresses for the 2 nodes to communicate.
 const uint8_t addresses[][5] = {"2RPi","0RPi"};

rfudp/TxTunnelSender.cpp

@@ -6,10 +6,10 @@ TMRh20 2014
  version 2 as published by the Free Software Foundation.
  */
 
-/** General Data Transfer Rate Test
- * This example demonstrates basic data transfer functionality with the
- updated library. This example will display the transfer rates acheived using
- the slower form of high-speed transfer using blocking-writes.
+/** TxTunnelSender
+ * This programm acts as a middle layer between clients and a nRF24 antenna.
+ * It sends to the other node what it receive on the udp socket and answer the sender
+ * with whatever the node sends back.
  */
 
 #include "TxTunnel.h"
@@ -183,10 +183,10 @@ int main(int argc, char** argv){
 
         loop_start = millis();
         radio.startListening();
-        while ( !radio.available() && (millis() - loop_start) < 200) {
+        while ( !radio.available() && (millis() - loop_start) < MILLIS_WAIT) {
             // wait till receive or timeout
         }
-        if (millis() - loop_start >= 100) {
+        if (millis() - loop_start >= MILLIS_WAIT) {
             eprintf("Not Response.\n\r");
             strcpy(buffer,"TxERR\0");
             n=strlen(buffer);