mirror of
https://github.com/eddyem/eddys_snippets.git
synced 2026-03-20 08:41:02 +03:00
first commit
This commit is contained in:
Eddy
22
Trinamic/client/Makefile
Normal file
22
Trinamic/client/Makefile
Normal file
@@ -0,0 +1,22 @@
|
||||
PROGRAM = client
|
||||
LDFLAGS =
|
||||
SRCS = client.c
|
||||
CC = gcc
|
||||
DEFINES = -D_XOPEN_SOURCE=501
|
||||
CXX = gcc
|
||||
CFLAGS = -Wall -Werror $(DEFINES)
|
||||
OBJS = $(SRCS:.c=.o)
|
||||
all : $(PROGRAM) clean
|
||||
$(PROGRAM) : $(OBJS)
|
||||
$(CC) $(CFLAGS) $(OBJS) $(LDFLAGS) -o $(PROGRAM)
|
||||
|
||||
# some addition dependencies
|
||||
# %.o: %.c
|
||||
# $(CC) $(LDFLAGS) $(CFLAGS) $< -o $@
|
||||
#$(SRCS) : %.c : %.h $(INDEPENDENT_HEADERS)
|
||||
# @touch $@
|
||||
|
||||
clean:
|
||||
/bin/rm -f *.o *~
|
||||
depend:
|
||||
$(CXX) -MM $(CXX.SRCS)
|
||||
495
Trinamic/client/client.c
Normal file
495
Trinamic/client/client.c
Normal file
@@ -0,0 +1,495 @@
|
||||
/*
|
||||
* client.c - simple terminal client
|
||||
*
|
||||
* 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
|
||||
#include <stdint.h> // int types
|
||||
#include <sys/time.h> // gettimeofday
|
||||
|
||||
#define BUFLEN 1024
|
||||
|
||||
double t0; // start time
|
||||
|
||||
uint32_t motor_speed = 10; // motor speed
|
||||
|
||||
FILE *fout = NULL; // file for messages duplicating
|
||||
char *comdev = "/dev/ttyUSB0";
|
||||
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);
|
||||
if(fout) fclose(fout);
|
||||
printf("Exit! (%d)\n", ex_stat);
|
||||
exit(ex_stat);
|
||||
}
|
||||
|
||||
int send_command(uint8_t *ninebytes){
|
||||
uint8_t crc = 0;
|
||||
int i;
|
||||
printf("send: ");
|
||||
for(i = 0; i < 8; crc += ninebytes[i++])
|
||||
printf("%u,", ninebytes[i]);
|
||||
ninebytes[8] = crc;
|
||||
printf("%u\n",ninebytes[8]);
|
||||
if(9 != write(comfd, ninebytes, 9)){
|
||||
perror("Can't write to Trinamic");
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* 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(comdev,O_RDWR|O_NOCTTY|O_NONBLOCK)) < 0){
|
||||
fprintf(stderr,"Can't use port %s\n",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; // 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");
|
||||
tcsetattr(STDIN_FILENO, TCSANOW, &newt);
|
||||
}
|
||||
|
||||
/**
|
||||
* getchar() without echo
|
||||
* wait until at least one character pressed
|
||||
* @return character readed
|
||||
*
|
||||
int mygetchar(){
|
||||
int ret;
|
||||
do ret = read_console();
|
||||
while(ret == 0);
|
||||
return ret;
|
||||
}*/
|
||||
|
||||
/**
|
||||
* read both tty & console
|
||||
* @param buff (o) - buffer for messages readed from tty or NULL (to omit readed info)
|
||||
* @param length (io) - buff's length (return readed len or 0)
|
||||
* @param rb (io) - byte readed from console, -1 if nothing read, NULL to omit console reading
|
||||
* @return 1 if something was readed here or there
|
||||
*/
|
||||
int read_tty_and_console(uint8_t *buff, size_t *length, int *rb){
|
||||
ssize_t L;
|
||||
ssize_t l;
|
||||
uint8_t buff1[BUFLEN+1];
|
||||
size_t buffsz;
|
||||
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 = 10000;
|
||||
sel = select(comfd + 1, &rfds, NULL, NULL, &tv);
|
||||
if(!buff){
|
||||
buffsz = BUFLEN;
|
||||
buff = buff1;
|
||||
}else{
|
||||
buffsz = *length;
|
||||
}
|
||||
if(sel > 0){
|
||||
if(rb){
|
||||
if(FD_ISSET(STDIN_FILENO, &rfds)){
|
||||
*rb = getchar();
|
||||
retval = 1;
|
||||
}else{
|
||||
*rb = -1;
|
||||
}
|
||||
}
|
||||
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
|
||||
//DBG("readed %zd bytes, try more.. ", L);
|
||||
buffsz -= L;
|
||||
usleep(10000);
|
||||
while(buffsz > 0 && (l = read(comfd, buff+L, buffsz)) > 0){
|
||||
L += l;
|
||||
buffsz -= l;
|
||||
usleep(10000);
|
||||
}
|
||||
//DBG("full len: %zd\n", L);
|
||||
|
||||
if(length) *length = (size_t) L;
|
||||
retval = 1;
|
||||
}
|
||||
}else{
|
||||
if(length) *length = 0;
|
||||
}
|
||||
}
|
||||
return retval;
|
||||
}
|
||||
|
||||
int32_t get_integer(uint8_t *buff){
|
||||
int32_t val;
|
||||
int ii;
|
||||
uint8_t *valptr = (uint8_t*) &val + 3;
|
||||
for(ii = 4; ii < 8; ii++)
|
||||
*valptr-- = buff[ii];
|
||||
return val;
|
||||
}
|
||||
|
||||
void help(){
|
||||
printf("Use this commands:\n"
|
||||
"h\tShow this help\n"
|
||||
"+/-\tIncrease/decrease speed by 10%% or 1\n"
|
||||
"M/m\tMove to relative position\n"
|
||||
"R/L\tRotate motor 1 right/left\n"
|
||||
"r/l\tRotate motor 2 right/left\n"
|
||||
"S/s\tStop 1st or 2nd motor\n"
|
||||
"W/w\tSet ustepping to 1/16\n"
|
||||
"E/e\tSet ustepping to 1/4\n"
|
||||
"Z/z\tSet to zero current position\n"
|
||||
"G/g\tGet current position\n"
|
||||
"A/a\tGet ALL information about\n"
|
||||
"q\tQuit\n"
|
||||
);
|
||||
}
|
||||
|
||||
typedef struct{
|
||||
char *name; // command name
|
||||
uint8_t cmd; // command number
|
||||
} custom_command;
|
||||
|
||||
void get_information(uint8_t *com){
|
||||
int i;
|
||||
uint8_t buff[9];
|
||||
size_t L;
|
||||
custom_command info[] = {
|
||||
{"target position", 0},
|
||||
{"actual position", 1},
|
||||
{"target speed", 2},
|
||||
{"actual speed", 3},
|
||||
{"max positioning speed", 4},
|
||||
{"max acceleration", 5},
|
||||
{"abs. max current", 6},
|
||||
{"standby current", 7},
|
||||
{"target pos. reached", 8},
|
||||
{"ref. switch status", 9},
|
||||
{"right limit switch status", 10},
|
||||
{"left limit switch status", 11},
|
||||
{"right limit switch disable", 12},
|
||||
{"left limit switch disable", 13},
|
||||
{"minimum speed", 130},
|
||||
{"actual acceleration", 135},
|
||||
{"ramp mode", 138},
|
||||
{"microstep resolution", 140},
|
||||
{"soft stop flag", 149},
|
||||
{"ramp divisor", 153},
|
||||
{"pulse divisor", 154},
|
||||
{"referencing mode", 193},
|
||||
{"referencing search speed", 194},
|
||||
{"referencing switch speed", 195},
|
||||
{"distance end switches", 196},
|
||||
{"mixed decay threshold", 203},
|
||||
{"freewheeling", 204},
|
||||
{"stall detection threshold", 205},
|
||||
{"actual load value", 206},
|
||||
{"driver error flags", 208},
|
||||
{"fullstep threshold", 211},
|
||||
{"power down delay", 214},
|
||||
{NULL, 0}
|
||||
};
|
||||
printf("\n");
|
||||
for(i = 0; info[i].name; i++){
|
||||
printf("%s (%u) ", info[i].name, info[i].cmd);
|
||||
com[2] = info[i].cmd;
|
||||
if(send_command(com)){ // command send OK
|
||||
L = 9;
|
||||
if(read_tty_and_console(buff, &L, NULL)){ // get answer
|
||||
if(L == 9 && buff[2] > 6){ // answer's length is right && no error in status
|
||||
int32_t val = get_integer(buff);
|
||||
printf("= %d\n", val);
|
||||
//for(ii = 0; ii < 9; ii++) printf("%u,",buff[ii]);
|
||||
}else printf("L=%zd (stat: %u)\n", L, buff[2]);
|
||||
}}
|
||||
printf("\n");
|
||||
}
|
||||
}
|
||||
|
||||
void con_sig(int rb){
|
||||
if(rb < 1) return;
|
||||
uint32_t value = motor_speed; // here is the value to send
|
||||
/*
|
||||
* trinamic binary format (bytes):
|
||||
* 0 - address
|
||||
* 1 - command
|
||||
* 2 - type
|
||||
* 3 - motor/bank number
|
||||
* 4 \
|
||||
* 5 | value (msb first!)
|
||||
* 6 |
|
||||
* 7 /
|
||||
* 8 - checksum (sum of bytes 0..8)
|
||||
*/
|
||||
uint8_t command[9] = {1,0,0,0,0,0,0,0,0}; // command to sent
|
||||
int i, V;
|
||||
uint8_t *cmd = &command[1]; // byte 1 of message is command itself
|
||||
if(rb < 1) return;
|
||||
if(rb == 'q' || rb == 'Q') quit(0);
|
||||
if(rb == 'h' || rb == 'H'){
|
||||
help();
|
||||
return;
|
||||
}
|
||||
void change_motor_speed(int dir){
|
||||
uint32_t Delta = motor_speed / 10, newspd;
|
||||
int i;
|
||||
if(Delta == 0) Delta = 1;
|
||||
if(dir > 0){
|
||||
newspd = motor_speed + Delta;
|
||||
if(newspd > motor_speed) motor_speed = newspd;
|
||||
else motor_speed = (uint32_t)0xffffffffUL;
|
||||
}else{
|
||||
newspd = motor_speed - Delta;
|
||||
if(newspd < motor_speed && newspd) motor_speed = newspd;
|
||||
else motor_speed = 1;
|
||||
}
|
||||
printf("\nspeed changed to %u\n", motor_speed);
|
||||
*cmd = 5; // SAP
|
||||
command[2] = 4; // max pos speed
|
||||
uint8_t *spd = (uint8_t*) &motor_speed + 3;
|
||||
for(i = 4; i < 8; i++)
|
||||
command[i] = *spd--;
|
||||
send_command(command); // store new speed
|
||||
read_tty_and_console(NULL, NULL, NULL);
|
||||
command[3] = 2; // and for second motor
|
||||
send_command(command);
|
||||
read_tty_and_console(NULL, NULL, NULL);
|
||||
}
|
||||
if(rb == '+'){
|
||||
change_motor_speed(1);
|
||||
}else if(rb == '-'){
|
||||
change_motor_speed(-1);
|
||||
}
|
||||
/*
|
||||
* Now we must analize commands: all letters exept Q/q & H/h are for motors commands
|
||||
*/
|
||||
if(rb > 'a'-1 && rb < 'z'+1){ // small letter -> change motor number to 2
|
||||
command[3] = 2;
|
||||
}else if(rb > 'A'-1 && rb < 'Z'+1){ // capital letter -> change to small
|
||||
rb += 'a' - 'A';
|
||||
}else return; // wrong command
|
||||
switch(rb){
|
||||
case 'r': // move motor right
|
||||
*cmd = 1; // ROR
|
||||
break;
|
||||
case 'l': // move motor left
|
||||
*cmd = 2; // ROL
|
||||
break;
|
||||
case 's': // stop motor
|
||||
*cmd = 3; // STP
|
||||
break;
|
||||
case 'w': // 1/16
|
||||
*cmd = 5; // SAP
|
||||
command[2] = 140; // ustep resolution
|
||||
value = 4;
|
||||
break;
|
||||
case 'e': // 1/4
|
||||
*cmd = 5; // SAP
|
||||
command[2] = 140; // ustep resolution
|
||||
value = 2;
|
||||
break;
|
||||
case 'g': // get current position
|
||||
*cmd = 6; // GAP
|
||||
command[2] = 1; // actual position
|
||||
break;
|
||||
case 'a': // get everything!
|
||||
*cmd = 6; // GAP
|
||||
get_information(command);
|
||||
return;
|
||||
break;
|
||||
case 'z': // set to zero current position
|
||||
*cmd = 5; // SAP
|
||||
command[2] = 1; // actual position
|
||||
value = 0;
|
||||
break;
|
||||
case 'm': // move to relative position
|
||||
tcsetattr(STDIN_FILENO, TCSANOW, &oldt); // return terminal to original state
|
||||
if(scanf("%d", &V) > 0 && V){
|
||||
*cmd = 4; // MVP
|
||||
command[2] = 1; // REL
|
||||
value = (uint32_t) V;
|
||||
//if(V > 0) value = (uint32_t) V;
|
||||
//else value =
|
||||
}
|
||||
tcsetattr(STDIN_FILENO, TCSANOW, &newt); // omit echo
|
||||
break;
|
||||
}
|
||||
if(*cmd){
|
||||
// copy param to buffer
|
||||
uint8_t *spd = (uint8_t*) &value + 3;
|
||||
for(i = 4; i < 8; i++)
|
||||
command[i] = *spd--;
|
||||
send_command(command);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Get integer value from buffer
|
||||
* @param buff (i) - buffer with int
|
||||
* @param len - length of data in buffer (could be 2 or 4)
|
||||
* @return
|
||||
*
|
||||
uint32_t get_int(uint8_t *buff, size_t len){
|
||||
if(len != 2 && len != 4){
|
||||
fprintf(stdout, "Bad data length!\n");
|
||||
return 0xffffffff;
|
||||
}
|
||||
uint32_t data = 0;
|
||||
uint8_t *i8 = (uint8_t*) &data;
|
||||
if(len == 2) memcpy(i8, buff, 2);
|
||||
else memcpy(i8, buff, 4);
|
||||
return data;
|
||||
}*/
|
||||
|
||||
/**
|
||||
* Copy line by line buffer buff to file removing cmd starting from newline
|
||||
* @param buffer - data to put into file
|
||||
* @param cmd - symbol to remove from line startint (if found, change *cmd to (-1)
|
||||
* or NULL, (-1) if no command to remove
|
||||
*/
|
||||
void copy_buf_to_file(uint8_t *buffer, int *cmd){
|
||||
uint8_t *buff, *line, *ptr;
|
||||
if(!cmd || *cmd < 0){
|
||||
fprintf(fout, "%s", buffer);
|
||||
return;
|
||||
}
|
||||
buff = (uint8_t*)strdup((char*)buffer), ptr = buff;
|
||||
do{
|
||||
if(!*ptr) break;
|
||||
if(ptr[0] == (char)*cmd){
|
||||
*cmd = -1;
|
||||
ptr++;
|
||||
if(ptr[0] == '\n') ptr++;
|
||||
if(!*ptr) break;
|
||||
}
|
||||
line = ptr;
|
||||
ptr = (uint8_t*)strchr((char*)buff, '\n');
|
||||
if(ptr){
|
||||
*ptr++ = 0;
|
||||
//fprintf(fout, "%s\n", line);
|
||||
}//else
|
||||
//fprintf(fout, "%s", line); // no newline found in buffer
|
||||
fprintf(fout, "%s\n", line);
|
||||
}while(ptr);
|
||||
free(buff);
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, char *argv[]){
|
||||
int rb, oldcmd = -1;
|
||||
uint8_t buff[BUFLEN+1];
|
||||
size_t L;
|
||||
if(argc == 2){
|
||||
fout = fopen(argv[1], "a");
|
||||
if(!fout){
|
||||
perror("Can't open output file");
|
||||
exit(-1);
|
||||
}
|
||||
setbuf(fout, NULL);
|
||||
}
|
||||
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);
|
||||
//t0 = dtime();
|
||||
while(1){
|
||||
L = BUFLEN;
|
||||
if(read_tty_and_console(buff, &L, &rb)){
|
||||
if(rb > 0){
|
||||
con_sig(rb);
|
||||
oldcmd = rb;
|
||||
}
|
||||
if(L > 0){
|
||||
uint8_t *ptr = buff;
|
||||
int ii;
|
||||
int32_t val = get_integer(buff);
|
||||
printf("value = %d (full answer: ", val);
|
||||
for(ii = L - 1; ii > 0; ii--){
|
||||
uint8_t C = *ptr++;
|
||||
printf("%u", C);
|
||||
//if(C > 31) printf("(%c)", C);
|
||||
printf(",");
|
||||
}
|
||||
printf("%u)\n", *ptr);
|
||||
if(fout){
|
||||
copy_buf_to_file(buff, &oldcmd);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user