restructuring

deprecated/F1/stepper_motion/client-p2/main.c

@@ -0,0 +1,284 @@
+/*
+ * main.c - main file
+ *
+ * Copyright 2015 Edward V. Emelianoff <eddy@sao.ru>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+ * MA 02110-1301, USA.
+ */
+#include <termios.h>		// tcsetattr
+#include <unistd.h>			// tcsetattr, close, read, write
+#include <sys/ioctl.h>		// ioctl
+#include <stdio.h>			// printf, getchar, fopen, perror
+#include <stdlib.h>			// exit
+#include <sys/stat.h>		// read
+#include <fcntl.h>			// read
+#include <signal.h>			// signal
+#include <time.h>			// time
+#include <string.h>			// memcpy
+#include <stdint.h>			// int types
+#include <sys/time.h>		// gettimeofday
+#include <assert.h>			// assert
+#include <pthread.h>		// threads
+
+#include "bta_shdata.h"
+#include "cmdlnopts.h"
+
+#ifndef _U_
+#define _U_    __attribute__((__unused__))
+#endif
+
+#ifndef fabs
+#define fabs(x)  ((x > 0.) ? x : -x)
+#endif
+
+// scale: how much steps there is in one angular second
+const double steps_per_second = 2.054;
+
+// end-switch at 8degr
+#define MIN_RESTRICT_ANGLE (29000.)
+// end-switch at 100degr
+#define MAX_RESTRICT_ANGLE (360000.)
+
+#define BUFLEN 1024
+
+glob_pars *Global_parameters = NULL;
+
+int is_running = 1; // ==0 to exit
+int BAUD_RATE = B115200;
+struct termio oldtty, tty; // TTY flags
+struct termios oldt, newt; // terminal flags
+int comfd; // TTY fd
+
+#define DBG(...) do{fprintf(stderr, __VA_ARGS__);}while(0)
+
+/**
+ * function for different purposes that need to know time intervals
+ * @return double value: time in seconds
+ */
+double dtime(){
+	double t;
+	struct timeval tv;
+	gettimeofday(&tv, NULL);
+	t = tv.tv_sec + ((double)tv.tv_usec)/1e6;
+	return t;
+}
+
+/**
+ * Exit & return terminal to old state
+ * @param ex_stat - status (return code)
+ */
+void quit(int ex_stat){
+	tcsetattr(STDIN_FILENO, TCSANOW, &oldt); // return terminal to previous state
+	ioctl(comfd, TCSANOW, &oldtty ); // return TTY to previous state
+	close(comfd);
+	printf("Exit! (%d)\n", ex_stat);
+	exit(ex_stat);
+}
+
+/**
+ * Open & setup TTY, terminal
+ */
+void tty_init(){
+	// terminal without echo
+	tcgetattr(STDIN_FILENO, &oldt);
+	newt = oldt;
+	newt.c_lflag &= ~(ICANON | ECHO);
+	if(tcsetattr(STDIN_FILENO, TCSANOW, &newt) < 0) quit(-2);
+	printf("\nOpen port...\n");
+	if ((comfd = open(Global_parameters->serialdev,O_RDWR|O_NOCTTY|O_NONBLOCK)) < 0){
+		fprintf(stderr,"Can't use port %s\n",Global_parameters->serialdev);
+		quit(1);
+	}
+	printf(" OK\nGet current settings...\n");
+	if(ioctl(comfd,TCGETA,&oldtty) < 0) exit(-1); // Get settings
+	tty = oldtty;
+	tty.c_lflag     = 0; // ~(ICANON | ECHO | ECHOE | ISIG)
+	tty.c_oflag     = 0;
+	tty.c_cflag     = BAUD_RATE|CS8|CREAD|CLOCAL; // 9.6k, 8N1, RW, ignore line ctrl
+	tty.c_cc[VMIN]  = 0;  // non-canonical mode
+	tty.c_cc[VTIME] = 5;
+	if(ioctl(comfd,TCSETA,&tty) < 0) exit(-1); // set new mode
+	printf(" OK\n");
+	tcsetattr(STDIN_FILENO, TCSANOW, &newt);
+}
+
+/**
+ * read tty
+ * @param buff (o)    - buffer for messages readed from tty
+ * @param length (io) - buff's length (return readed len or 0)
+ * @return 1 if something was readed here or there
+ */
+int read_tty(char *buff, size_t *length){
+	ssize_t L;
+	size_t buffsz = *length;
+	struct timeval tv;
+	int sel, retval = 0;
+	fd_set rfds;
+	FD_ZERO(&rfds);
+	FD_SET(STDIN_FILENO, &rfds);
+	FD_SET(comfd, &rfds);
+	tv.tv_sec = 0; tv.tv_usec = 100000;
+	sel = select(comfd + 1, &rfds, NULL, NULL, &tv);
+	if(sel > 0){
+		if(FD_ISSET(comfd, &rfds)){
+			if((L = read(comfd, buff, buffsz)) < 1){ // disconnect or other troubles
+				fprintf(stderr, "USB error or disconnected!\n");
+				quit(1);
+			}else{
+				if(L == 0){ // USB disconnected
+					fprintf(stderr, "USB disconnected!\n");
+					quit(1);
+				}
+				// all OK continue reading
+				*length = (size_t) L;
+				retval = 1;
+			}
+		}else{
+			*length = 0;
+		}
+	}
+	return retval;
+}
+
+void print_P2(){
+	int d, m;
+	double s = val_P;
+	d = ((int)s / 3600) % 360;
+	s -= 3600. * d;
+	m = s / 60;
+	s -= 60. * m;
+	printf("P2 value: %02d:%02d:%03.1f\n", d, m, s);
+}
+
+/*
+void help(){
+	P("G\tgo to N steps\n");
+	P("H\tshow this help\n");
+	P("P\tset stepper period (us)\n");
+	P("S\tstop motor\n");
+	P("T\tshow current approx. time\n");
+	P("W\tshow current steps value\n");
+	P("X\tshow current stepper period\n");
+}
+*/
+
+void write_tty(char *str){
+	while(*str){
+		write(comfd, str++, 1);
+	}
+}
+
+/**
+ * give command to move P2 on some angle
+ * @param rel  == 1 for relative moving
+ * @param angle - angle in seconds
+ */
+void move_P2(int rel, double angle){
+	double curP = val_P, targP;
+	char buf[256];
+	if(fabs(angle) < 1.) return; // don't move to such little degrees
+	if(curP > MIN_RESTRICT_ANGLE && curP < MAX_RESTRICT_ANGLE){
+		fprintf(stderr, "Error: motor is in restricted area!\n");
+		quit(-1);
+	}
+	if(rel) targP = curP + angle;
+	else targP = angle;
+	// check for restricted zone
+	if(targP > MIN_RESTRICT_ANGLE && targP < MAX_RESTRICT_ANGLE){
+		fprintf(stderr, "Error: you want to move into restricted area!\n");
+		quit(-10);
+	}
+	// now check preferred rotation direction
+	angle = targP - curP;
+	//printf("targP: %g, curP: %g, angle: %g\n", targP, curP, angle);
+	// 360degr = 1296000''; 180degr = 648000''
+	if(angle < 0.) angle += 1296000.;
+	if(angle > 648000.){ // it's better to move in negative direction
+		angle -= 1296000.;
+	}
+	// convert angle into steps
+	angle *= steps_per_second;
+	_U_ size_t L = snprintf(buf, 256, "G%d\n", ((int)angle));
+	//printf("BUF: %s\n", buf);
+	write_tty(buf);
+	//is_running = 0;
+	while(is_running);
+}
+
+void *moving_thread(_U_ void *buf){
+	if(Global_parameters->gotopos > 0.){
+//printf("gotoval: %g\n", Global_parameters->gotopos);
+		move_P2(0, Global_parameters->gotopos);
+	}else if(fabs(Global_parameters->relmove) > 1.){ // move relative current position
+//printf("relval: %g\n", Global_parameters->relmove);
+		move_P2(1, Global_parameters->relmove);
+	}
+//	sleep(1); // give a little time for receiving messages
+//	is_running = 0;
+	return NULL;
+}
+
+int main(int argc, char *argv[]){
+	char buff[BUFLEN+1];
+	pthread_t motor_thread;
+	size_t L;
+	Global_parameters = parse_args(argc, argv);
+	assert(Global_parameters != NULL);
+	if(!get_shm_block(&sdat, ClientSide) || !check_shm_block(&sdat)){
+		fprintf(stderr, "Can't get SHM block!");
+		return -1;
+	}
+	tty_init();
+	signal(SIGTERM, quit);		// kill (-15)
+	signal(SIGINT, quit);		// ctrl+C
+	signal(SIGQUIT, SIG_IGN);	// ctrl+\   .
+	signal(SIGTSTP, SIG_IGN);	// ctrl+Z
+	setbuf(stdout, NULL);
+	double t, t0 = 0;
+	double p_old = -400.; // impossible value for printing P2 val on first run
+
+	if(pthread_create(&motor_thread, NULL, moving_thread, NULL)){
+		/// "Не могу создать поток для захвата видео"
+		fprintf(stderr, "Can't create readout thread\n");
+		quit(-1);
+	}
+	while(is_running){
+		L = BUFLEN;
+		if((t = dtime()) - t0 > 1.){ // once per second (and on first run) check P2 position
+			if(fabs(val_P - p_old) > 0.1){
+				p_old = val_P;
+				print_P2();
+			}
+			t0 = t;
+		}
+		if(read_tty(buff, &L)){
+			if(L){
+				buff[L] = 0;
+				printf("GOT: %s", buff);
+				if(strncmp(buff, "Stop", 4) == 0){
+					is_running = 0;
+					sleep(1);
+				}
+			}
+		}
+		if(!check_shm_block(&sdat)){
+			fprintf(stderr, "Corruption in SHM block!");
+			quit(-2);
+		}
+	}
+	quit(0);
+	return 0;
+}