mirror of
https://github.com/eddyem/eddys_snippets.git
synced 2026-03-20 08:41:02 +03:00
Added SMSD non-interactive command line client for Shack-Hartmann control system
This commit is contained in:
eddyem
405
Trinamic/SHA_client_cmdline/client.c
Normal file
405
Trinamic/SHA_client_cmdline/client.c
Normal file
@@ -0,0 +1,405 @@
|
||||
/*
|
||||
* client.c - simple terminal client for operationg with
|
||||
* Standa's 8MT175-150 translator by SMSD-1.5 driver
|
||||
*
|
||||
* Hardware operates in microsterpping mode (1/16),
|
||||
* max current = 1.2A
|
||||
* voltage = 12V
|
||||
* "0" of driver connected to end-switch at opposite from motor side
|
||||
* switch of motor's side connected to "IN1"
|
||||
*
|
||||
* Copyright 2013 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, strcmp etc
|
||||
#include <strings.h> // strcasecmp
|
||||
#include <stdint.h> // int types
|
||||
#include <sys/time.h> // gettimeofday
|
||||
#include "cmdlnopts.h"
|
||||
|
||||
#define DBG(...) do{fprintf(stderr, __VA_ARGS__); }while(0)
|
||||
|
||||
//double t0; // start time
|
||||
static int bus_error = 0; // last error of data output
|
||||
enum{
|
||||
NO_ERROR = 0, // normal execution
|
||||
CODE_ERR, // error of exexuted program code
|
||||
BUS_ERR, // data transmission error
|
||||
COMMAND_ERR, // wrong command
|
||||
CMD_DATA_ERR, // wrong data of command
|
||||
UNDEFINED_ERR // something else wrong
|
||||
};
|
||||
|
||||
int BAUD_RATE = B9600;
|
||||
uint16_t step_spd = 900; // stepper speed: 225 steps per second in 1/1 mode
|
||||
struct termio oldtty, tty; // TTY flags
|
||||
int comfd = -1; // TTY fd
|
||||
|
||||
int erase_ctrlr();
|
||||
|
||||
/**
|
||||
* Exit & return terminal to old state
|
||||
* @param ex_stat - status (return code)
|
||||
*/
|
||||
void quit(int ex_stat){
|
||||
if(comfd > 0){
|
||||
erase_ctrlr();
|
||||
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(){
|
||||
printf("\nOpen port...\n");
|
||||
if ((comfd = open(G.comdev,O_RDWR|O_NOCTTY|O_NONBLOCK)) < 0){
|
||||
fprintf(stderr,"Can't use port %s\n", G.comdev);
|
||||
quit(1);
|
||||
}
|
||||
printf(" OK\nGet current settings...\n");
|
||||
if(ioctl(comfd,TCGETA,&oldtty) < 0) quit(-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 | PARENB; // 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) quit(-1); // set new mode
|
||||
printf(" OK\n");
|
||||
}
|
||||
|
||||
/**
|
||||
* Read data from TTY
|
||||
* @param buff (o) - buffer for data read
|
||||
* @param length - buffer len
|
||||
* @return amount of readed bytes
|
||||
*/
|
||||
size_t read_tty(char *buff, size_t length){
|
||||
ssize_t L = 0;
|
||||
fd_set rfds;
|
||||
struct timeval tv;
|
||||
int retval;
|
||||
FD_ZERO(&rfds);
|
||||
FD_SET(comfd, &rfds);
|
||||
tv.tv_sec = 0; tv.tv_usec = 50000;
|
||||
retval = select(comfd + 1, &rfds, NULL, NULL, &tv);
|
||||
if(retval < 1) return 0;
|
||||
if(FD_ISSET(comfd, &rfds)){
|
||||
if((L = read(comfd, buff, length)) < 1){
|
||||
fprintf(stderr, "ERROR on bus, exit!\n");
|
||||
quit(-4);
|
||||
}
|
||||
}
|
||||
return (size_t)L;
|
||||
}
|
||||
|
||||
void write_tty(char *str, int L){
|
||||
ssize_t D = write(comfd, str, L);
|
||||
if(D != L){
|
||||
fprintf(stderr, "ERROR on bus, exit!\n");
|
||||
quit(-3);
|
||||
}
|
||||
}
|
||||
|
||||
size_t read_ctrl_command(char *buf, size_t L){ // read data from controller to buffer buf
|
||||
int i, j;
|
||||
char *ptr = buf;
|
||||
size_t R;
|
||||
memset(buf, 0, L);
|
||||
for(j = 0; j < L; j++, ptr++){
|
||||
R = 0;
|
||||
for(i = 0; i < 10 && !R; i++){
|
||||
R = read_tty(ptr, 1);
|
||||
}
|
||||
if(!R){j--; break;} // nothing to read
|
||||
if(*ptr == '*') // read only one command
|
||||
break;
|
||||
if(*ptr < ' '){ // omit spaces & non-characters
|
||||
j--; ptr--;
|
||||
}
|
||||
}
|
||||
return (size_t) j + 1;
|
||||
}
|
||||
|
||||
int parse_ctrlr_ans(char *ans){
|
||||
char *E = NULL, *Star = NULL;
|
||||
if(!ans || !*ans) return 1;
|
||||
bus_error = NO_ERROR;
|
||||
if(!(E = strchr(ans, 'E')) || !(Star = strchr(ans, '*')) || E[1] != '1'){
|
||||
fprintf(stderr, "Answer format error (got: %s)\n", ans);
|
||||
bus_error = UNDEFINED_ERR;
|
||||
return 0;
|
||||
}
|
||||
switch (E[2]){ // E = "E1x"
|
||||
case '0': // 10 - normal execution
|
||||
break;
|
||||
case '4': // 14 - program end
|
||||
printf("Last command exectuted normally\n");
|
||||
break;
|
||||
case '2': // command interrupt by other signal
|
||||
fprintf(stderr, "Last command terminated\n");
|
||||
break;
|
||||
case '3':
|
||||
bus_error = CODE_ERR;
|
||||
fprintf(stderr, "runtime");
|
||||
break;
|
||||
case '5':
|
||||
bus_error = BUS_ERR;
|
||||
fprintf(stderr, "data bus");
|
||||
break;
|
||||
case '6':
|
||||
bus_error = COMMAND_ERR;
|
||||
fprintf(stderr, "command");
|
||||
break;
|
||||
case '9':
|
||||
bus_error = CMD_DATA_ERR;
|
||||
fprintf(stderr, "command data");
|
||||
break;
|
||||
default:
|
||||
bus_error = UNDEFINED_ERR;
|
||||
fprintf(stderr, "undefined (%s)", ans);
|
||||
}
|
||||
if(bus_error != NO_ERROR){
|
||||
fprintf(stderr, " error in controller\n");
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
int send_command(char *cmd){
|
||||
int L = strlen(cmd);
|
||||
size_t R = 0;
|
||||
char ans[256];
|
||||
write_tty(cmd, L);
|
||||
R = read_ctrl_command(ans, 255);
|
||||
// DBG("readed: %s (cmd: %s, R = %zd, L = %d)\n", ans, cmd, R, L);
|
||||
if(!R || (strncmp(ans, cmd, L) != 0)){
|
||||
fprintf(stderr, "Error: controller doesn't respond (answer: %s)\n", ans);
|
||||
return 0;
|
||||
}
|
||||
R = read_ctrl_command(ans, 255);
|
||||
// DBG("readed: %s\n", ans);
|
||||
if(!R){ // controller is running or error
|
||||
fprintf(stderr, "Controller doesn't answer!\n");
|
||||
return 0;
|
||||
}
|
||||
return parse_ctrlr_ans(ans);
|
||||
}
|
||||
|
||||
int erase_ctrlr(){
|
||||
char *errmsg = "\n\nCan't erase controller's memory: some errors occured!\n\n";
|
||||
printf("Erasing old program\n");
|
||||
#define safely_send(x) do{ if(bus_error != NO_ERROR){ \
|
||||
fprintf(stderr, errmsg); return 0;} send_command(x); }while(0)
|
||||
if(!send_command("LD1*")){ // start writing a program
|
||||
//if(!send_command("LB*")){ // start writing a program into op-buffer
|
||||
if(bus_error == COMMAND_ERR){ // motor is moving
|
||||
printf("Found running program, stop it\n");
|
||||
if(!send_command("ST1*"))
|
||||
//if(!send_command("ST*"))
|
||||
send_command("SP*");
|
||||
send_command("LD1*");
|
||||
//send_command("LB*");
|
||||
}else{
|
||||
fprintf(stderr, "Controller doesn't answer: maybe no power?\n");
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
safely_send("BG*"); // move address pointer to beginning
|
||||
safely_send("DS*"); // turn off motor
|
||||
safely_send("ED*"); // end of program
|
||||
if(bus_error != NO_ERROR){
|
||||
fprintf(stderr, errmsg);
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
int con_sig(int rb, int stepsN){
|
||||
int got_command = 0;
|
||||
char command[256];
|
||||
char buf[13];
|
||||
#define Die_on_error(arg) do{if(!send_command(arg)) goto erase_;}while(0)
|
||||
if(strchr("-+01Rr", rb)){ // command to execute
|
||||
got_command = 1;
|
||||
if(!send_command("LD1*")){ // start writing a program
|
||||
//if(!send_command("LB*")){ // start writing a program into op-buffer
|
||||
fprintf(stderr, "Error: previous program is running!\n");
|
||||
return 0;
|
||||
}
|
||||
Die_on_error("BG*"); // move address pointer to beginning
|
||||
if(strchr("-+01", rb)){
|
||||
Die_on_error("EN*"); // enable power
|
||||
//Die_on_error("SD10000*"); // set speed to max (156.25 steps per second with 1/16)
|
||||
snprintf(buf, 12, "SD%u*", step_spd);
|
||||
Die_on_error(buf);
|
||||
}
|
||||
}
|
||||
switch(rb){
|
||||
case '0':
|
||||
Die_on_error("DL*");
|
||||
Die_on_error("HM*");
|
||||
break;
|
||||
case '1':
|
||||
Die_on_error("DR*");
|
||||
Die_on_error("ML*");
|
||||
break;
|
||||
case '+':
|
||||
Die_on_error("DR*");
|
||||
if(stepsN)
|
||||
sprintf(command, "MV%d*", stepsN);
|
||||
else
|
||||
sprintf(command, "MV*");
|
||||
Die_on_error(command);
|
||||
break;
|
||||
case '-':
|
||||
Die_on_error("DL*");
|
||||
if(stepsN)
|
||||
sprintf(command, "MV%d*", stepsN);
|
||||
else
|
||||
sprintf(command, "MV*");
|
||||
Die_on_error(command);
|
||||
break;
|
||||
case 'S':
|
||||
Die_on_error("SP*");
|
||||
break;
|
||||
case 'A':
|
||||
Die_on_error("ST1*");
|
||||
//Die_on_error("ST*");
|
||||
break;
|
||||
case 'E':
|
||||
erase_ctrlr();
|
||||
break;
|
||||
case 'R':
|
||||
Die_on_error("SF*");
|
||||
break;
|
||||
case 'r':
|
||||
Die_on_error("CF*");
|
||||
break;
|
||||
case '>': // increase speed for 25 pulses
|
||||
step_spd += 25;
|
||||
printf("\nCurrent speed: %u pulses per sec\n", step_spd);
|
||||
//snprintf(buf, 12, "SD%u*", step_spd);
|
||||
//Die_on_error(buf);
|
||||
break;
|
||||
case '<': // decrease speed for 25 pulses
|
||||
if(step_spd > 25){
|
||||
step_spd -= 25;
|
||||
printf("\nCurrent speed: %u pulses per sec\n", step_spd);
|
||||
//snprintf(buf, 12, "SD%u*", step_spd);
|
||||
//Die_on_error(buf);
|
||||
}else
|
||||
printf("\nSpeed is too low\n");
|
||||
break;
|
||||
|
||||
}
|
||||
if(got_command){ // there was some command: write ending words
|
||||
Die_on_error("DS*"); // turn off power from motor at end
|
||||
Die_on_error("ED*"); // signal about command end
|
||||
Die_on_error("ST1*");// start program
|
||||
return 1;
|
||||
}
|
||||
return 0;
|
||||
erase_:
|
||||
erase_ctrlr();
|
||||
return 0;
|
||||
}
|
||||
|
||||
void wait_for_answer(){
|
||||
char buff[128], *bufptr = buff;
|
||||
size_t L;
|
||||
while(1){
|
||||
L = read_tty(bufptr, 127);
|
||||
if(L){
|
||||
bufptr += L;
|
||||
if(bufptr - buff > 127){
|
||||
fprintf(stderr, "Error: input buffer overflow!\n");
|
||||
bufptr = buff;
|
||||
}
|
||||
if(bufptr[-1] == '*'){ // end of input command
|
||||
*bufptr = 0;
|
||||
parse_ctrlr_ans(buff);
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[]){
|
||||
parce_args(argc, argv);
|
||||
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);
|
||||
erase_ctrlr();
|
||||
if(G.erasecmd) return 0;
|
||||
if(G.relaycmd == -1 && G.gotopos == NULL){
|
||||
printf("No commands given!\n");
|
||||
return -1;
|
||||
}
|
||||
if(G.relaycmd != -1){
|
||||
int ans;
|
||||
if(G.relaycmd) // turn on
|
||||
ans = con_sig('R',0);
|
||||
else // turn off
|
||||
ans = con_sig('r',0);
|
||||
if(ans)
|
||||
wait_for_answer();
|
||||
else
|
||||
return -1;
|
||||
}
|
||||
if(G.gotopos){
|
||||
if(strcasecmp(G.gotopos, "refmir") == 0){
|
||||
if(!con_sig('1',0)) return -1;
|
||||
printf("Go to last end-switch\n");
|
||||
wait_for_answer();
|
||||
if(!con_sig('-',500)) return -1;
|
||||
}else if(strcasecmp(G.gotopos, "diagmir") == 0){
|
||||
if(!con_sig('0',0)) return -1;
|
||||
printf("Go to zero's end-switch\n");
|
||||
wait_for_answer();
|
||||
if(!con_sig('+',2500)) return -1;
|
||||
}else if(strcasecmp(G.gotopos, "shack") == 0){
|
||||
if(!con_sig('1',0)) return -1;
|
||||
printf("Go to last end-switch\n");
|
||||
wait_for_answer();
|
||||
if(!con_sig('-',30000)) return -1;
|
||||
}else{
|
||||
printf("Wrong goto command, should be one of refmir/diagmir/shack\n");
|
||||
return -1;
|
||||
}
|
||||
printf("Go to position\n");
|
||||
wait_for_answer();
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user