124 lines
3.2 KiB
C++
124 lines
3.2 KiB
C++
#include <stdio.h>
|
|
#include <string.h>
|
|
#include <unistd.h>
|
|
|
|
#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);
|
|
//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;
|
|
|
|
sscanf(&msg[3]," %s ", action);
|
|
eprintf("Act: %s\n", action);
|
|
|
|
if (strcmp(action, "START") == 0) {
|
|
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;
|
|
|
|
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);
|
|
all_motors_stop(); // Stops all ESCs
|
|
started=false;
|
|
|
|
strcpy(msg, "ESC_STOPPED");
|
|
return;
|
|
} else {
|
|
sscanf(&msg[6], " %i ", &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", *servo_mode);
|
|
strcpy(msg, "ESC_MODE_NOK");
|
|
return;
|
|
}
|
|
|
|
servo_mv_r(pin, lv);
|
|
sprintf(msg, "ESC %c%c %i", servo_index, servo_mode, lv);
|
|
}
|