[no ci] Sigmastar: update motor_foscam sample (#1113)

general/package/sigmastar-motors/src/motor_foscam.c

@@ -26,13 +26,13 @@ static int sequence[][4] = {
 	{1, 0, 0, 0},
 };
 
-static int write_gpio(int pin, int value) {
+static int write_gpio(int pin, int val) {
 	struct gpiohandle_request rq;
 	struct gpiohandle_data data;
 
 	int fd = open(DEV_NAME, O_RDONLY);
 	if (fd < 0) {
-		printf("Unabled to open chip: %s\n", strerror(errno));
+		printf("Unable to open chip: %s\n", strerror(errno));
 		return 1;
 	}
 
@@ -41,15 +41,15 @@ static int write_gpio(int pin, int value) {
 	rq.lines = 1;
 
 	if (ioctl(fd, GPIO_GET_LINEHANDLE_IOCTL, &rq) < 0) {
-		printf("Unable to request gpio: %s\n", strerror(errno));
+		printf("Unable to request gpio %d: %s\n", pin, strerror(errno));
 		return 1;
 	}
 
 	close(fd);
-	data.values[0] = value;
+	data.values[0] = val;
 
 	if (ioctl(rq.fd, GPIOHANDLE_SET_LINE_VALUES_IOCTL, &data) < 0) {
-		printf("Unable to set value: %s\n", strerror(errno));
+		printf("Unable to set value %d: %s\n", val, strerror(errno));
 		return 1;
 	}
 
@@ -69,10 +69,14 @@ static int motor_control(int dir, int step) {
 	return 0;
 }
 
-static void motor_reset(int dir) {
+static int motor_check(int dir, int check) {
 	for (int i = 0; i < 4; i++) {
-		write_gpio(motor_gpio[dir][i], 0);
+		if (write_gpio(motor_gpio[dir][i], 0) && check) {
+			return 1;
+		}
 	}
+
+	return 0;
 }
 
 static void usage(const char *name) {
@@ -120,6 +124,10 @@ int main(int argc, char **argv) {
 		cnt = -10;
 	}
 
+	if (motor_check(dir, 1)) {
+		return -1;
+	}
+
 	int c1 = (cnt < 0) ? cnt * 10 : 0;
 	int c2 = (cnt < 0) ? 0 : cnt * 10;
 	int d1 = (cnt < 0) ? 4 : 0;
@@ -128,12 +136,13 @@ int main(int argc, char **argv) {
 	for (int i = c1; i < c2; i++) {
 		for (int j = d1; j < d2; j++) {
 			if (motor_control(dir, j)) {
-				return -1;
+				goto reset;
 			}
 		}
 	}
 
-	motor_reset(dir);
+reset:
+	motor_check(dir, 0);
 
 	return 0;
 }