152 lines
4.0 KiB
C++
152 lines
4.0 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) {
|
|
int lv_new;
|
|
switch(pin) {
|
|
case MAIN_PROP_GPIO:
|
|
lv_new = map(lv, 0, 100, MAIN_PROP_IDLE_PPM, MAIN_PROP_MAX_PPM);
|
|
break;
|
|
case LEFT_PROP_GPIO:
|
|
lv_new = map(lv, 0, 100, LEFT_PROP_IDLE_PPM, LEFT_PROP_MAX_PPM);
|
|
break;
|
|
case RIGHT_PROP_GPIO:
|
|
lv_new = map(lv, 0, 100, RIGHT_PROP_IDLE_PPM, RIGHT_PROP_MAX_PPM);
|
|
break;
|
|
default:
|
|
lv_new = map(lv, 0, 100, ESC_MIN, ESC_MAX);
|
|
}
|
|
return lv_new;
|
|
}
|
|
void advance_both(char*msg, int *lv, int offset){
|
|
int lv_r, lv_l;
|
|
int offset_base = *lv/4; //get offset to ajust
|
|
|
|
lv_r = lv_l = *lv; // asign base level
|
|
eprintf("lv: %i,\tlv_r:%i\t,lv_l:%i\n", *lv, lv_r, lv_l);
|
|
|
|
// offset values if necesary
|
|
if( offset < 50)
|
|
lv_r = lv_r - offset_base;
|
|
if( offset < 50)
|
|
lv_l = lv_l - offset_base;
|
|
eprintf("lv: %i,\tlv_r:%i\t,lv_l:%i\n", *lv, lv_r, lv_l);
|
|
|
|
// map to real values
|
|
lv_r = map_esc_per(RIGHT_PROP_GPIO, lv_r);
|
|
lv_l = map_esc_per(LEFT_PROP_GPIO, lv_l);
|
|
|
|
// ajust motor speed
|
|
servo_mv_r(RIGHT_PROP_GPIO, lv_r);
|
|
servo_mv_r(LEFT_PROP_GPIO, lv_l);
|
|
}
|
|
|
|
void motor_process(char*msg){
|
|
char servo_index = msg[4];
|
|
char servo_mode = msg[5];
|
|
int lv, offset;
|
|
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 %i", &lv, &offset );
|
|
}
|
|
|
|
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
|
|
if (servo_index == 'P') {
|
|
advance_both(msg, &lv, offset);
|
|
return;
|
|
}
|
|
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 '%':
|
|
lv = 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);
|
|
}
|