71 lines
1.7 KiB
C++
71 lines
1.7 KiB
C++
#include <stdio.h>
|
|
#include <string.h>
|
|
|
|
#include "PinConfig.h"
|
|
#include "servos.h"
|
|
|
|
|
|
void servo_mv_r(unsigned int pin, int lv) {
|
|
gpioSetMode(pin, PI_OUTPUT);
|
|
gpioServo(pin, lv);
|
|
}
|
|
|
|
int map(int input, int input_start, int input_end, int output_start, int output_end) {
|
|
return output_start + ((output_end - output_start) / (input_end - input_start)) * (input - input_start);
|
|
}
|
|
|
|
int conver_pc_raw(unsigned pin, int lv) {
|
|
int lv_new;
|
|
switch(pin) {
|
|
case LEFT_RUDDER_GPIO:
|
|
lv_new = map(lv, 0, 100, LEFT_RUDDER_MIN, LEFT_RUDDER_MAX);
|
|
break;
|
|
case RIGHT_RUDDER_GPIO:
|
|
lv_new=map(lv, 0, 100, RIGHT_RUDDER_MIN, RIGHT_RUDDER_MAX);
|
|
break;
|
|
default:
|
|
return -1;
|
|
}
|
|
return lv_new;
|
|
}
|
|
|
|
void servo_mv(char*msg, char* mode, unsigned int pin, int *lv) {
|
|
if (*mode == ' ')
|
|
*mode = ( *lv < 500 ) ? '%':'R';
|
|
|
|
if (*mode == '%') {
|
|
*lv=conver_pc_raw(pin, *lv);
|
|
} else
|
|
if (*mode == 'R') {
|
|
} else {
|
|
eprintf("[servos] mode not valid (%c)\n", *mode);
|
|
strcpy(msg, "SER_MODE_NOK");
|
|
return;
|
|
}
|
|
|
|
servo_mv_r(pin, *lv);
|
|
|
|
}
|
|
|
|
void servo_process(char*msg) {
|
|
char servo_index = msg[4];
|
|
char servo_mode = msg[5];
|
|
int lv;
|
|
|
|
sscanf(&msg[6], " %i ", &lv );
|
|
|
|
if (servo_index == 'L') {
|
|
servo_mv(msg, &servo_mode, LEFT_RUDDER_GPIO, &lv);
|
|
} else
|
|
if (servo_index == 'R') {
|
|
servo_mv(msg, &servo_mode, RIGHT_RUDDER_GPIO, &lv);
|
|
} else {
|
|
eprintf("[servos] index not valid (%c)\n", servo_index);
|
|
strcpy(msg, "SER_INDX_NOK");
|
|
return;
|
|
}
|
|
sprintf(msg, "SER %c%c %i", servo_index, servo_mode, lv);
|
|
}
|
|
|
|
|