|
|
|
marto
Experienced Roboteer
Joined: 08 Jul 2004
Posts: 5459
Location: Brisbane, QLD
|
Well got it working. There is still some issue with sending data to the microcontroller. It could be my very very basic code on the micro or more likely I think it is how python handles writing data to USB.
Here is codes through.
AVR - Teensy
code:
#include <avr/io.h>
#include <avr/pgmspace.h>
#include <stdint.h>
#include <util/delay.h>
#include "usb_serial.h"
const float down=0.6;
const float max = 1.4/255;
const float min = 0.4;
#define debug 0
#define CPU_PRESCALE(n) (CLKPR = 0x80, CLKPR = (n))
#define CPU_16MHz 0x00
#define CPU_8MHz 0x01
#define CPU_4MHz 0x02
#define CPU_2MHz 0x03
#define CPU_1MHz 0x04
#define CPU_500kHz 0x05
#define CPU_250kHz 0x06
#define CPU_125kHz 0x07
#define CPU_62kHz 0x08
int main(void)
{
CPU_PRESCALE(CPU_16MHz);
DDRD |= (1<<6); //Set PORTD Pin 6 to output.
PORTD |= (1<<6); //Set High Initially
_delay_ms(10); //Startup Delay
int time[6] = {128,128,128,128,128,128};
short stage = -1;
short channel = 0;
short value = 0;
usb_init();
usb_serial_flush_input();
while(1) //Infinite Loop of
{
for(int i =0; i < 6; i++)
{
PORTD &= ~(1<<6);
_delay_ms(down); //Lead Pulse
PORTD |= (1<<6);
_delay_ms( (double) time[i] *max +min); //Time1
}
PORTD &= ~(1<<6);
_delay_ms(down); //Pause
PORTD |= (1<<6);
_delay_ms(22);
if (usb_serial_available() > 0)
{
char r = usb_serial_getchar();
if (r == '#')
{
stage = 0;
if(debug)
{
usb_serial_putchar('$');
}
}
else if (stage == 0 && r <= '6' && r > '0')
{
channel = r - '1';
stage = 1;
if(debug)
{
usb_serial_putchar('%');
usb_serial_putchar(r);
}
}
else if (stage == 1 && r == '&')
{
stage = 2;
if(debug)
{
usb_serial_putchar('2');
}
}
else if (stage == 2)
{
value = r;
stage = -1;
time[channel]=value;
if(debug)
{
usb_serial_putchar('~');
int x = value/100;
usb_serial_putchar(x + '0');
x = (value - x*100)/10;
usb_serial_putchar(x+'0');
x = (value - x*10);
usb_serial_putchar(x+'0');
}
}
}
}
}
Even simply PyGame controller
code:
#!/usr/bin/env python
import pygame
from pygame.locals import *
import time, serial
ch1mid = 85
ch2mid = 60
ch1low = 50
ch2low = 100
ch1high = 130
ch2high = 20
if __name__ == '__main__':
try:
# Initilise pygame
pygame.init()
screen = pygame.display.set_mode((100, 100))
pygame.display.set_caption('rcController')
pygame.mouse.set_visible(0)
port = serial.Serial('/dev/ttyACM0',115200,xonxoff=True, rtscts=True, dsrdtr=True)
ch1 = ch1mid;
ch2 = ch2mid;
done = False
while not done:
for event in pygame.event.get():
if event.type == KEYUP:
if (event.key == K_UP or event.key == K_DOWN):
ch1 = ch1mid
ch2 = ch2mid
port.write('#1&'+chr(ch1)+'#2&'+chr(ch2))
port.flush()
if (event.key == K_LEFT or event.key == K_RIGHT):
ch1 = ch1mid
ch2 = ch2mid
port.write('#1&'+chr(ch1)+'#2&'+chr(ch2))
port.flush()
if event.type == KEYDOWN:
if (event.key == K_UP):
ch1 = ch1high
ch2 = ch2high
port.write('#1&'+chr(ch1)+'#2&'+chr(ch2))
port.flush()
elif (event.key == K_DOWN):
ch1 = ch1low
ch2 = ch2low
port.write('#1&'+chr(ch1)+'#2&'+chr(ch2))
port.flush()
elif (event.key == K_LEFT):
ch1 = ch1low
ch2 = ch2high
port.write('#1&'+chr(ch1)+'#2&'+chr(ch2))
port.flush()
elif (event.key == K_RIGHT):
ch1 = ch1high
ch2 = ch2low
port.write('#1&'+chr(ch1)+'#2&'+chr(ch2))
port.flush()
elif (event.key == K_ESCAPE):
done = True
time.sleep(0.1)
print ch1, ch2
except KeyboardInterrupt: pass
Youtube vid is uploading _________________ Steven Martin
Twisted Constructions
http://www.botbitz.com
|
Sat Jul 16, 2011 11:13 pm |
|
|
|
marto
Experienced Roboteer
Joined: 08 Jul 2004
Posts: 5459
Location: Brisbane, QLD
|
If there was code police I might be shot for this terrible example of coding. But here is a joystick -> USB interface.
My lag was cause by an if statement instead of a while statement when reading in data. Works much better even with python code after that was replaced.
code:
//Code based on unknown source from the internets...
#include <stdio.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <string>
#include <iostream>
#include <sstream>
#include <algorithm>
#include <linux/joystick.h>
#include <cereal_port/CerealPort.h>
#define MAX_AXIS 16
#define MAX_BUTTON 16
const int ch1low = 50;
const int ch2low = 100;
const int ch1high = 130;
const int ch2high = 20;
struct padData {
unsigned char axisCount;
unsigned char buttonCount;
int fd;
int version;
char devName[80];
int aPos[MAX_AXIS];
int bPos[MAX_BUTTON];
bool changed;
js_event ev;
};
padData pad;
int main (void)
{
bool cereal_open = true;
cereal::CerealPort serial_port;
pad.fd = open("/dev/input/js0", O_RDONLY);
if (pad.fd > 0)
{
printf ("js0 opened OK\n");
std::string output;
// Get pad info ...
ioctl(pad.fd, JSIOCGAXES, &pad.axisCount);
ioctl(pad.fd, JSIOCGBUTTONS, &pad.buttonCount);
ioctl(pad.fd, JSIOCGVERSION, &pad.version);
ioctl(pad.fd, JSIOCGNAME(80), &pad.devName);
fcntl(pad.fd, F_SETFL, O_NONBLOCK);
printf ("axis : %d\n", pad.axisCount);
printf ("buttons : %d\n", pad.buttonCount);
printf ("version : %d\n", pad.version);
printf ("name : %s\n", pad.devName);
// set default values
pad.changed = false;
for (int i=0;i<pad.axisCount;i++) pad.aPos[i]=0;
for (int i=0;i<pad.buttonCount;i++) pad.bPos[i]=0;
try{ serial_port.open("/dev/ttyACM0",115200);}
catch (cereal::Exception& e){
std::cout << "Failed to open USB port" << std::endl;
cereal_open = false;
}
std::cout << "connected" << std::endl;
while(cereal_open)
{
int result = read(pad.fd, &pad.ev, sizeof(pad.ev));
while (result > 0)
{
switch (pad.ev.type)
{
case JS_EVENT_INIT:
case JS_EVENT_INIT | JS_EVENT_AXIS:
case JS_EVENT_INIT | JS_EVENT_BUTTON:
break;
case JS_EVENT_AXIS:
pad.aPos[pad.ev.number] = pad.ev.value;
pad.changed = true;
break;
case JS_EVENT_BUTTON:
pad.bPos[pad.ev.number] = pad.ev.value;
pad.changed = true;
break;
default:
printf ("Other event ? %d\nnumber=%d\nvalue=%d\n",
pad.ev.type,pad.ev.number, pad.ev.value);
break;
}
result = read(pad.fd, &pad.ev, sizeof(pad.ev));
} //else usleep(1);
if (pad.changed)
{
printf ("----------------------------------------------\n");
printf ("axis : %d\n", pad.axisCount);
printf ("buttons : %d\n", pad.buttonCount);
printf ("version : %d\n", pad.version);
printf ("name : %s\n", pad.devName);
printf ("----------------------------------------------\n");
printf ("last ev time : %d\n", pad.ev.time);
for (int i=0;i<pad.axisCount;i++) printf (" Axis %2d |",i);
printf ("\n");
for (int i=0;i<pad.axisCount;i++) printf (" %7d |",pad.aPos[i]);
printf ("\n\n");
for (int i=0;i<pad.buttonCount;i++) printf (" Btn.%2d |",i);
printf ("\n");
for (int i=0;i<pad.buttonCount;i++) printf (" %2d |",pad.bPos[i]);
printf ("\n");
pad.changed = false;
std::stringstream outputstream;
uint8_t ch1,ch2,ch3;
ch1 = ((pad.aPos[0]-pad.aPos[1])/(256.0*256.0))*(ch1high-ch1low)+(ch1low+ch1high)/2- 2;
ch2 = std::max(std::min(((-pad.aPos[1]-pad.aPos[0])/(256.0*256.0)),0.8),-1.0)*(ch2high-ch2low)+(ch2low+ch2high)/2- -4;
std::cout << (int)ch1 << " " << (int)ch2 << std::endl;
ch3 = 150*pad.bPos[0]+20;
outputstream << "#1&" << ch1 << "#2&" << ch2 << "#3&" << ch3;
output = outputstream.str();
std::cout << output.c_str() << std::endl;
serial_port.flush();
serial_port.write(output.c_str(),output.length());
}
}
close(pad.fd);
}
else {
printf ("failed to open /dev/input/js0\n");
}
return 0;
}
Youtube vid in a bit.
There is something awesome about driving a robot with a joystick
Steve _________________ Steven Martin
Twisted Constructions
http://www.botbitz.com
|
Wed Jul 20, 2011 10:55 pm |
|
|
|
|
|
|
|
|
|
|
|
|
|