OpenIPC
/
firmware
mirror of https://github.com/OpenIPC/firmware.git
1
0
Fork 0
Code Activity

[no ci] Sigmastar: update foscam motor sample

pull/1153/head
Viktor 2023-11-18 13:36:59 +01:00
parent 38086dc9dd
commit 4ea34ba994
1 changed files with 28 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
general/package/sigmastar-motors/src/motor_foscam.c
View File

@ -10,35 +10,20 @@
#define DEV_NAME "/dev/gpiochip0" #define DEV_NAME "/dev/gpiochip0"
#define STEP_TIME 250 #define STEP_TIME 250
#define STEP_COUNT 4 #define STEP_COUNT 4
#define MAX_COUNT 10 #define MAX_COUNT 8
#define SEQ_COUNT 8 #define SEQ_COUNT 8
static int motor_gpio[][4] = { int gpio_x[] = {01, 02, 12, 13};
{01, 02, 12, 13}, int gpio_y[] = {62, 63, 64, 65};
{62, 63, 64, 65},
int sequence[][4] = {
{1, 0, 0, 0}, {1, 1, 0, 0}, {0, 1, 0, 0}, {0, 1, 1, 0},
{0, 0, 1, 0}, {0, 0, 1, 1}, {0, 0, 0, 1}, {1, 0, 0, 1},
{1, 0, 0, 1}, {0, 0, 0, 1}, {0, 0, 1, 1}, {0, 0, 1, 0},
{0, 1, 1, 0}, {0, 1, 0, 0}, {1, 1, 0, 0}, {1, 0, 0, 0},
}; };
static int sequence[][4] = { int write_gpio(int pin, int val) {
{1, 0, 0, 0},
{1, 1, 0, 0},
{0, 1, 0, 0},
{0, 1, 1, 0},
{0, 0, 1, 0},
{0, 0, 1, 1},
{0, 0, 0, 1},
{1, 0, 0, 1},
{1, 0, 0, 1},
{0, 0, 0, 1},
{0, 0, 1, 1},
{0, 0, 1, 0},
{0, 1, 1, 0},
{0, 1, 0, 0},
{1, 1, 0, 0},
{1, 0, 0, 0},
};
static int write_gpio(int pin, int val) {
struct gpiohandle_request rq; struct gpiohandle_request rq;
struct gpiohandle_data data; struct gpiohandle_data data;
@ -70,10 +55,10 @@ static int write_gpio(int pin, int val) {
return 0; return 0;
} }
static int motor_control(int dir, int seq) { int motor_control(int *gpio, int count) {
for (int i = seq; i < seq + SEQ_COUNT; i++) { for (int i = count; i < count + SEQ_COUNT; i++) {
for (int j = 0; j < 4; j++) { for (int j = 0; j < 4; j++) {
if (write_gpio(motor_gpio[dir][j], sequence[i][j])) { if (write_gpio(gpio[j], sequence[i][j])) {
return 1; return 1;
} }
usleep(STEP_TIME); usleep(STEP_TIME);
@ -83,9 +68,9 @@ static int motor_control(int dir, int seq) {
return 0; return 0;
} }
static int motor_check(int dir, int check) { int gpio_export(int *gpio) {
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
if (write_gpio(motor_gpio[dir][i], 0) && check) { if (write_gpio(gpio[i], 0)) {
return 1; return 1;
} }
} }
@ -93,28 +78,28 @@ static int motor_check(int dir, int check) {
return 0; return 0;
} }
static int limit_value(int x, int in_min, int in_max) { int limit_value(int x) {
if (x < in_min) { if (x < -MAX_COUNT) {
x = in_min; x = -MAX_COUNT;
} }
if (x > in_max) { if (x > MAX_COUNT) {
x = in_max; x = MAX_COUNT;
} }
return x; return x;
} }
int main(int argc, char **argv) { int main(int argc, char **argv) {
if (argc != 3) { if (argc < 2 || argc > 3) {
printf("Usage: %s [x_step] [y_step]\n", argv[0]); printf("Usage: %s [x_step] [y_step]\n", argv[0]);
return -1; return -1;
} }
int x = limit_value(atoi(argv[1]), -MAX_COUNT, MAX_COUNT); int x = limit_value(argv[1] ? atoi(argv[1]) : 0);
int y = limit_value(atoi(argv[2]), -MAX_COUNT, MAX_COUNT); int y = limit_value(argv[2] ? atoi(argv[2]) : 0);
if (motor_check(0, 1) || motor_check(1, 1)) { if (gpio_export(gpio_x) || gpio_export(gpio_y)) {
return -1; return -1;
} }
@ -122,20 +107,20 @@ int main(int argc, char **argv) {
int y_max = (y < 0) ? -y : y; int y_max = (y < 0) ? -y : y;
for (int i = 0; i < x_max * STEP_COUNT * 2; i++) { for (int i = 0; i < x_max * STEP_COUNT * 2; i++) {
if (motor_control(0, (x < 0) ? SEQ_COUNT : 0)) { if (motor_control(gpio_x, (x < 0) ? SEQ_COUNT : 0)) {
goto reset; goto reset;
} }
} }
for (int i = 0; i < y_max * STEP_COUNT; i++) { for (int i = 0; i < y_max * STEP_COUNT; i++) {
if (motor_control(1, (y < 0) ? 0 : SEQ_COUNT)) { if (motor_control(gpio_y, (y < 0) ? 0 : SEQ_COUNT)) {
goto reset; goto reset;
} }
} }
reset: reset:
motor_check(0, 0); gpio_export(gpio_x);
motor_check(1, 0); gpio_export(gpio_y);
return 0; return 0;
} }