Permalink
Switch branches/tags
Nothing to show
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
295 lines (234 sloc) 8.46 KB
#include <stdio.h>
#include <sys/types.h>
#include <fcntl.h>
#include <unistd.h>
#include <errno.h>
#include <termios.h>
#include <string.h>
#include <math.h>
#include <time.h>
#include "generic_interface.hpp"
#include "multi_turn_angle_control_client.hpp"
#include "complex_motor_control_client.hpp"
int set_interface_attribs (int fd, int speed, int parity);
void set_blocking (int fd, int should_block);
void copyState();
double readAngle(int fd, GenericInterface *com);
void setAngle(int fd, GenericInterface *com, double angle, unsigned long time_ms);
const char *portname0 = "/dev/ttyUSB0";
const char *portname1 = "/dev/ttyUSB1";
int msg_count = 0;
int fd1 = -1; // fd for serial port
int fd2 = -1; // fd for serial port
// Make a communication interface object
GenericInterface com1;
GenericInterface com2;
// Make a Multi-turn Angle Controller object with obj_id 0
MultiTurnAngleControlClient angleIn(0);
MultiTurnAngleControlClient angleOut(0);
FILE *fp = NULL;
unsigned long elapsed_ms(struct timespec ref, struct timespec cur) {
return( (cur.tv_sec - ref.tv_sec) * 1000 + (cur.tv_nsec - ref.tv_nsec) / (1000 * 1000) );
}
#define UPDATE_RATE 40 // in ms
int main()
{
char c;
int n, tem;
int cnt = 0;
int spin_direction = -1;
int retangle;
struct timespec ts_ref;
struct timespec ts_cur;
double curangle = 0.0;
double curangle2 = 0.0;
unsigned long last_time_ms = 0;
unsigned long ft;
double fa, fa2;
unsigned long dtime;
double init_angle;
double init_angle2;
int init = 0;
/// setup console terminal prooperties
tem = fcntl(0, F_GETFL, 0);
fcntl (0, F_SETFL, (tem | O_NDELAY));
/// now setup motor controller terminal properties
fd1 = open (portname0, O_RDWR | O_NOCTTY | O_SYNC);
if (fd1 < 0) {
printf("error %d opening input controller %s: %s", errno, portname0, strerror (errno));
return -1;
}
set_interface_attribs (fd1, B115200, 0); // set speed to 115,200 bps, 8n1 (no parity)
fd2 = open (portname1, O_RDWR | O_NOCTTY | O_SYNC);
if (fd2 < 0) {
printf("error %d opening input controller %s: %s", errno, portname1, strerror (errno));
return -1;
}
set_interface_attribs (fd2, B115200, 0); // set speed to 115,200 bps, 8n1 (no parity)
// set_blocking (fd, 0); // set no blocking
fp = fopen( "motion.txt", "w+" );
if( fp == NULL ) {
printf( "Can't open motion.txt\n" );
return -1;
}
clock_gettime(CLOCK_MONOTONIC, &ts_ref); // initialize with the current time
// Insert code for interfacing with hardware here
printf( "Press enter to break.\n" );
while (1) {
// kbhit code
n = read(0, &c, 1);
if (n > 0) break;
cnt = cnt + 1;
// Send next message
curangle = readAngle(fd1, &com1);
curangle2 = readAngle(fd2, &com2);
usleep((useconds_t) UPDATE_RATE * 1000);
clock_gettime(CLOCK_MONOTONIC, &ts_cur);
printf( "elapsed time in ms: %ld, angle1: %lf angle2: %lf\n", elapsed_ms(ts_ref, ts_cur), curangle, curangle2);
if( !init ) {
init_angle = curangle;
init_angle2 = curangle2;
init = 1;
}
fprintf( fp, "%ld,%lf,%lf\n", elapsed_ms(ts_ref, ts_cur), curangle, curangle2 );
fflush(fp);
}
rewind(fp); // bring us back to the beginning
printf( "Press enter to playback.\n" );
while (1) {
// kbhit code
n = read(0, &c, 1);
if (n > 0) break;
}
setAngle( fd1, &com1, init_angle, 2.0 ); // home the arm
setAngle( fd2, &com2, init_angle2, 2.0 ); // home the arm
printf( "Press enter to break.\n" );
while (1 && !feof(fp) ) {
// kbhit code
n = read(0, &c, 1);
if (n > 0) break;
cnt = cnt + 1;
// Send next message
fscanf(fp, "%ld,%lf,%lf\n", &ft, &fa, &fa2 );
dtime = ft - last_time_ms;
// if( dtime > 1 ) {
// dtime = dtime - 1; // advance a ms
// }
usleep((useconds_t) (ft - last_time_ms) * 1000);
last_time_ms = ft;
setAngle( fd1, &com1, fa, dtime );
setAngle( fd2, &com2, fa2, dtime );
}
fcntl(0, F_SETFL, tem);
fclose(fp);
return 0;
}
#define BUFLEN 1024
double readAngle(int fd, GenericInterface *com) {
// This buffer is for passing around messages.
uint8_t communication_buffer_in[BUFLEN];
uint8_t communication_buffer_out[BUFLEN];
// Stores length of message to send or receive
uint8_t communication_length_in;
uint8_t communication_length_out;
float target_angle;
///////////// READ THE INPUT CONTROLLER
// Generate the set messages
angleIn.ctrl_coast_.set(*com); // put the input controller in "coast" mode
angleIn.obs_angular_displacement_.get(*com); // get the angular displacement
// Grab outbound messages in the com queue, store into buffer
// If it transferred something to communication_buffer...
if(com->GetTxBytes(communication_buffer_in, communication_length_in))
{
write(fd, communication_buffer_in, communication_length_in);
}
usleep((useconds_t) 1 * 1000); // delay 1ms for serial data to clear
// Reads however many bytes are currently available
communication_length_in = read(fd, communication_buffer_in, BUFLEN);
// Puts the recently read bytes into com's receive queue
com->SetRxBytes(communication_buffer_in, communication_length_in);
uint8_t *rx_data; // temporary pointer to received type+data bytes
uint8_t rx_length; // number of received type+data bytes
// while we have message packets to parse
while(com->PeekPacket(&rx_data,&rx_length)) {
// Share that packet with all client objects
angleIn.ReadMsg(*com,rx_data,rx_length);
// Once we're done with the message packet, drop it
com->DropPacket();
}
// Check if we have any fresh data
// Checking for fresh data is not required, it simply
// lets you know if you received a message that you
// have not yet read.
if(angleIn.obs_angular_displacement_.IsFresh()) {
target_angle = angleIn.obs_angular_displacement_.get_reply();
// printf( "angle: %3.2f\n", target_angle * 180.0 * M_PI / 10.0 );
}
return target_angle;
}
void setAngle(int fd, GenericInterface *com, double angle, unsigned long time_ms) {
// This buffer is for passing around messages.
uint8_t communication_buffer_in[BUFLEN];
uint8_t communication_buffer_out[BUFLEN];
// Stores length of message to send or receive
uint8_t communication_length_in;
uint8_t communication_length_out;
float target_angle;
/////////////// WRITE OUTPUT CONTROLLER
// Generate the set messages
angleOut.trajectory_angular_displacement_.set(*com,(float) angle);
angleOut.trajectory_duration_.set(*com, time_ms / 1000.0 );
angleOut.obs_angular_displacement_.get(*com);
// Grab outbound messages in the com queue, store into buffer
// If it transferred something to communication_buffer...
if(com->GetTxBytes(communication_buffer_out,communication_length_out))
{
write(fd, communication_buffer_out,communication_length_out);
}
printf( "command to angle: %lf in %ldms\n", angle, time_ms);
// printf( "message %d sent\n", ++msg_count );
}
int set_interface_attribs (int fd, int speed, int parity) {
struct termios tty;
memset (&tty, 0, sizeof tty);
if (tcgetattr (fd, &tty) != 0) {
printf ("error %d from tcgetattr", errno);
return -1;
}
cfsetospeed (&tty, speed);
cfsetispeed (&tty, speed);
tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8; // 8-bit chars
// disable IGNBRK for mismatched speed tests; otherwise receive break
// as \000 chars
tty.c_iflag &= ~IGNBRK; // disable break processing
tty.c_lflag = 0; // no signaling chars, no echo,
// no canonical processing
tty.c_oflag = 0; // no remapping, no delays
tty.c_cc[VMIN] = 0; // read doesn't block
tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout
tty.c_iflag &= ~(IXON | IXOFF | IXANY); // shut off xon/xoff ctrl
tty.c_cflag |= (CLOCAL | CREAD);// ignore modem controls,
// enable reading
tty.c_cflag &= ~(PARENB | PARODD); // shut off parity
tty.c_cflag |= parity;
tty.c_cflag &= ~CSTOPB;
tty.c_cflag &= ~CRTSCTS;
if (tcsetattr (fd, TCSANOW, &tty) != 0) {
printf ("error %d from tcsetattr", errno);
return -1;
}
return 0;
}
void set_blocking (int fd, int should_block) {
struct termios tty;
memset (&tty, 0, sizeof tty);
if (tcgetattr (fd, &tty) != 0)
{
printf ("error %d from tggetattr", errno);
return;
}
tty.c_cc[VMIN] = should_block ? 1 : 0;
tty.c_cc[VTIME] = 5; // 0.5 seconds read timeout
if (tcsetattr (fd, TCSANOW, &tty) != 0)
printf ("error %d setting term attributes", errno);
}