From ba2ce353b9c36952034a57b242408d3c22391801 Mon Sep 17 00:00:00 2001 From: Angel Garcia Date: Fri, 24 May 2019 03:16:35 +0200 Subject: [PATCH] Added motor query support Basic actions: start stop main propeller some minor fixes --- hover-controller/PinConfig.h | 37 +++++++++++------- hover-controller/dispatcher.cpp | 2 +- hover-controller/hover-controller.cpp | 4 ++ hover-controller/motors.cpp | 54 +++++++++++++++++++++++++++ hover-controller/motors.h | 1 + hover-controller/servos.h | 2 + rfudp/TxTunnelSender.cpp | 2 +- 7 files changed, 86 insertions(+), 16 deletions(-) diff --git a/hover-controller/PinConfig.h b/hover-controller/PinConfig.h index 7c47c73..11ec5dc 100644 --- a/hover-controller/PinConfig.h +++ b/hover-controller/PinConfig.h @@ -1,25 +1,34 @@ #ifndef __PIN_CONFIG_H__ #define __PIN_CONFIG_H__ -#define RF_CE_PIN RPI_V2_GPIO_P1_22 -#define SERVO_MAX 2400 //2300 -#define SERVO_MIN 500 +// RF pins +#define RF_CE_PIN RPI_V2_GPIO_P1_22 -#define LEFT_RUDDER_GPIO 23 -#define LEFT_RUDDER_MAX SERVO_MAX -#define LEFT_RUDDER_MIN SERVO_MIN +// Servo pins & config +#define SERVO_MAX 2400 //2300 +#define SERVO_MIN 500 -#define RIGHT_RUDDER_GPIO 24 -#define RIGHT_RUDDER_MAX SERVO_MAX -#define RIGHT_RUDDER_MIN SERVO_MIN +#define LEFT_RUDDER_GPIO 23 +#define LEFT_RUDDER_MAX SERVO_MAX +#define LEFT_RUDDER_MIN SERVO_MIN +#define RIGHT_RUDDER_GPIO 24 +#define RIGHT_RUDDER_MAX SERVO_MAX +#define RIGHT_RUDDER_MIN SERVO_MIN -#define ESC_MAX -#define ESC_MIN -#define ESC_REVERSE +// ESC pins & config +#define ESC_MAX 2020 +#define ESC_MIN 1000 +#define ESC_REVERSE + +#define RIGHT_PROP_GPIO RPI_V2_GPIO_P1_11 +#define LEFT_PROP_GPIO RPI_V2_GPIO_P1_15 + +#define MAIN_PROP_GPIO RPI_V2_GPIO_P1_13 + +#define MAIN_PROP_START_PPM 1060 +#define MAIN_PROP_STOP_PPM 2400 -#define RIGHT_PROP_GPIO -#define LEFT_PROP_GPIO #endif // __PIN_CONFIG_H__ diff --git a/hover-controller/dispatcher.cpp b/hover-controller/dispatcher.cpp index 30f8038..ff8d082 100644 --- a/hover-controller/dispatcher.cpp +++ b/hover-controller/dispatcher.cpp @@ -12,7 +12,7 @@ void process_msg(char*msg) { if (strcmp(msg_cmd, "ESC") == 0) { - + motor_process(msg); } else if (strcmp(msg_cmd, "SER") == 0) { servo_process(msg); diff --git a/hover-controller/hover-controller.cpp b/hover-controller/hover-controller.cpp index 9e5feac..675ff08 100644 --- a/hover-controller/hover-controller.cpp +++ b/hover-controller/hover-controller.cpp @@ -78,6 +78,10 @@ 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); } // This opens two pipes for these two nodes to communicate diff --git a/hover-controller/motors.cpp b/hover-controller/motors.cpp index e69de29..657fe35 100644 --- a/hover-controller/motors.cpp +++ b/hover-controller/motors.cpp @@ -0,0 +1,54 @@ +#include +#include +#include + +#include "motors.h" + +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 motor_process(char*msg){ + char servo_index = msg[4]; + char servo_mode = msg[5]; + int lv; + char action[5]; + + static bool started=false; + + sscanf(&msg[3]," %s ", action); + 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); + started=true; + }else + eprintf("ESC already started\n"); + } else + if (strcmp(action, "STOP") == 0) { + servo_mv_r(MAIN_PROP_GPIO, MAIN_PROP_STOP_PPM); + started=false; + } 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); + + } +} diff --git a/hover-controller/motors.h b/hover-controller/motors.h index b05e22a..9ed0147 100644 --- a/hover-controller/motors.h +++ b/hover-controller/motors.h @@ -2,5 +2,6 @@ #define __MOTORS_H__ #include "servos.h" +void motor_process(char*msg); #endif // __MOTORS_H__ diff --git a/hover-controller/servos.h b/hover-controller/servos.h index 6951de3..8bbf5ab 100644 --- a/hover-controller/servos.h +++ b/hover-controller/servos.h @@ -7,5 +7,7 @@ #include "PinConfig.h" void servo_process(char*msg); +void servo_mv_r(unsigned int pin, int lv); +int map(int input, int input_start, int input_end, int output_start, int output_end); #endif // __SERVOS_H__ diff --git a/rfudp/TxTunnelSender.cpp b/rfudp/TxTunnelSender.cpp index 3c9a18c..0795019 100644 --- a/rfudp/TxTunnelSender.cpp +++ b/rfudp/TxTunnelSender.cpp @@ -183,7 +183,7 @@ int main(int argc, char** argv){ loop_start = millis(); radio.startListening(); - while ( !radio.available() && (millis() - loop_start) < 100) { + while ( !radio.available() && (millis() - loop_start) < 200) { // wait till receive or timeout } if (millis() - loop_start >= 100) {