Skip to content

Instantly share code, notes, and snippets.

@jake-sl
Created July 5, 2016 16:16
Show Gist options
  • Save jake-sl/e7064d2ccfcc2c4e1426949d99b02819 to your computer and use it in GitHub Desktop.
Save jake-sl/e7064d2ccfcc2c4e1426949d99b02819 to your computer and use it in GitHub Desktop.
#include <sys/ioctl.h>
#include <sys/types.h>
#include <stdlib.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdio.h>
#include <errno.h>
#include <string.h>
#include <termios.h>
#include <linux/joystick.h>
int main (int argc, char **argv)
{
int fd;
if (argc < 2) {
printf("Usage: %s <device>\n\n", argv[0]);
exit(1);
}
if ((fd = open(argv[argc - 1], O_RDONLY)) < 0) {
perror("failed to open device");
exit(1);
}
else {
unsigned char axes = 2;
unsigned char buttons = 2;
int *axis;
int *button;
int i;
struct js_event js;
float clutch;
float steering;
float throttle;
float brake;
float throttle_cruise;
float throttle_cur = 0.0f;
int cruise = 0;
int cruise_inc = 0;
int cruise_dec = 0;
int shifter;
ioctl(fd, JSIOCGAXES, &axes);
ioctl(fd, JSIOCGBUTTONS, &buttons);
fcntl(fd, F_SETFL, O_NONBLOCK);
axis = calloc(axes, sizeof(int));
button = calloc(buttons, sizeof(char));
while (1) {
while (read(fd, &js, sizeof(struct js_event)) == sizeof(struct js_event)) {
switch(js.type & ~JS_EVENT_INIT) {
case JS_EVENT_BUTTON:
button[js.number] = js.value;
break;
case JS_EVENT_AXIS:
axis[js.number] = js.value;
break;
}
}
if (errno != EAGAIN) {
perror("\nerror reading");
exit (1);
}
printf("\r");
steering = axis[0] / 32767.0f;
clutch = (axis[1] + 32767) / 65534.0f;
throttle = (32767 - axis[2]) / 65534.0f;
brake = (32767 - axis[3]) / 65534.0f;
if (cruise) {
if (button[18] || !(button[12] || button[13]) || brake > 0.25f) {
cruise = 0;
}
else {
if (button[7] && !cruise_inc) {
throttle_cruise += 0.05f;
}
if (button[20] && !cruise_dec) {
throttle_cruise -= 0.05f;
}
if (throttle_cruise < 0.0f) {
throttle_cruise = 0.0f;
}
else if (throttle_cruise > 1.0f) {
throttle_cruise = 1.0f;
}
if (throttle < throttle_cruise) {
throttle = throttle_cruise;
}
throttle_cur += 0.05f * (throttle - throttle_cur);
cruise_inc = button[7];
cruise_dec = button[20];
}
}
else {
if ((button[12] || button[13]) && button[6]) {
throttle_cruise = throttle_cur;
cruise = 1;
}
else {
brake *= brake;
brake *= brake;
if ((button[12] || button[13] || button[22]) && throttle <= 0.1f) {
throttle = 0.1f; // idle
}
throttle *= clutch;
throttle *= button[22] ? -1.0f : 1.0f; // reverse
throttle_cur += 0.01f * (throttle - throttle_cur);
throttle_cur *= 1.0f - brake;
}
}
if (button[12]) { // first gear
shifter = 1;
}
else if (button[13]) { // second gear
shifter = 2;
}
else if (button[22]) { // reverse gear
shifter = 1;
}
else { // neutral
shifter = 0;
}
printf("Throttle: %f ", throttle_cur);
printf("Steering: %f ", steering);
printf("Shifter: %d ", shifter);
printf("Clutch: %f ", clutch);
printf("Cruise: %s ", cruise ? "on" : "off");
fflush(stdout);
usleep(10000);
}
}
}
int
set_interface_attribs (int fd, int speed, int parity)
{
struct termios tty;
memset (&tty, 0, sizeof tty);
if (tcgetattr (fd, &tty) != 0)
{
error_message ("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)
{
error_message ("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)
{
error_message ("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)
error_message ("error %d setting term attributes", errno);
}
...
char *portname = "/dev/ttyACM0"
...
int fd = open (portname, O_RDWR | O_NOCTTY | O_SYNC);
if (fd < 0)
{
error_message ("error %d opening %s: %s", errno, portname, strerror (errno));
return;
}
set_interface_attribs (fd, B115200, 0); // set speed to 115,200 bps, 8n1 (no parity)
set_blocking (fd, 0); // set no blocking
write (fd, "hello!\n", 7); // send 7 character greeting
usleep ((7 + 25) * 100); // sleep enough to transmit the 7 plus
// receive 25: approx 100 uS per char transmit
char buf [100];
int n = read (fd, buf, sizeof buf); // read up to 100 characters if ready to read
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment