diff options
-rw-r--r-- | README.md | 58 | ||||
-rw-r--r-- | client/Makefile | 9 | ||||
-rw-r--r-- | client/enginecontroller.vala | 237 | ||||
-rw-r--r-- | client/main.vala | 58 | ||||
-rw-r--r-- | doc/atmostripe-pinout.txt | 1 | ||||
-rw-r--r-- | doc/isp-plug.svg | 187 | ||||
-rw-r--r-- | firmware/Makefile | 32 | ||||
-rw-r--r-- | firmware/firmware.c | 170 |
8 files changed, 752 insertions, 0 deletions
diff --git a/README.md b/README.md new file mode 100644 index 0000000..6bdab4b --- /dev/null +++ b/README.md @@ -0,0 +1,58 @@ +ATmega Engine Controller +======================== + +The code from firmware/ is supposed to run on a ATmega48 +sending servo signals to cheap brushless controllers as used +in RC models. The ATmega48 gets its instructions via a serial +line from a host system. The code in client/ is a small test +binary for the host system. + +Connection +---------------- + + [Engine 1]--->[BLC 1]--->PB1 + [Engine 2]--->[BLC 2]--->PB2 + [Engine 3]--->[BLC 3]--->PB3 + [Engine 4]--->[BLC 4]--->PD3 + [Engine 5]--->[BLC 5]--->PD5 + [Engine 6]--->[BLC 6]--->PD6 + + [Host TX]--->PD0 + [Host RX]--->PD1 + +Serial Protocol +--------------- + +Each of the following commands will be acknowledged with +```0x2E```, which is the ASCII value for a dot. Available +commands are: + +- **no operation** + + This command simply does nothing. It consists of a + single byte: ```0x00``` + +- **enable** + + Sending the bytes ```0xCA```, ```0xFE```, ```0xBA```, ```0xBE``` activates + the engine controlling. Before sending this command + the controller does not activate any engine. + +- **disable** + + One can disable all engines again by sending the + bytes ```0xDE```, ```0xAD```, ```0xBA```, ```0xBE```. After disabling the + engines with this command all future engine settings + will be ignored. + +- **set speed (of a single engine)** + + To set the speed of a single engine on has to send + two bytes: first the engine number in the system + (```0x01``` - ```0x06```), the the desired speed (```0x00```-```0xFF```). + +- **set speed of all engines simultaneously** + + One can also set the speed of all engines simultaneously + to the same value by first sending ```0xFF``` and then sending + a second byte with the desired speed (```0x00```-```0xFF```). diff --git a/client/Makefile b/client/Makefile new file mode 100644 index 0000000..d274276 --- /dev/null +++ b/client/Makefile @@ -0,0 +1,9 @@ +all: engine + +engine: enginecontroller.vala main.vala + valac -o $@ --pkg posix --pkg linux $^ + +clean: + rm -f engine + +.PHONY: all clean diff --git a/client/enginecontroller.vala b/client/enginecontroller.vala new file mode 100644 index 0000000..23bd572 --- /dev/null +++ b/client/enginecontroller.vala @@ -0,0 +1,237 @@ +public class EngineController { + private Posix.termios newtio; + private Posix.termios restoretio; + private int fd=-1; + private int byterate; + + private void setup(string device, int rate, int bits, int stopbits) { + Posix.speed_t baudrate = Posix.B9600; + + fd = Posix.open(device, Posix.O_RDWR /*| Posix.O_NONBLOCK*/); + + if(fd < 0) { + fd = -1; + stderr.printf("Could not open device!\n"); + return; + } + + Posix.tcflush(fd, Posix.TCIOFLUSH); + + Posix.tcgetattr(fd, out restoretio); + + /* apply settings */ + switch(rate) { + case 300: + baudrate = Posix.B300; + break; + case 600: + baudrate = Posix.B600; + break; + case 1200: + baudrate = Posix.B1200; + break; + case 2400: + baudrate = Posix.B2400; + break; + case 4800: + baudrate = Posix.B4800; + break; + case 9600: + baudrate = Posix.B9600; + break; + case 19200: + baudrate = Posix.B19200; + break; + case 38400: + baudrate = Posix.B38400; + break; + case 57600: + baudrate = Posix.B57600; + break; + case 115200: + baudrate = Posix.B115200; + break; + case 230400: + baudrate = Posix.B230400; + break; + default: + /* not supported */ + rate = 9600; + break; + } + + Posix.cfsetospeed(ref newtio, baudrate); + Posix.cfsetispeed(ref newtio, baudrate); + + switch(bits) { + case 5: + newtio.c_cflag = (newtio.c_cflag & ~Posix.CSIZE) | Posix.CS5; + break; + case 6: + newtio.c_cflag = (newtio.c_cflag & ~Posix.CSIZE) | Posix.CS6; + break; + case 7: + newtio.c_cflag = (newtio.c_cflag & ~Posix.CSIZE) | Posix.CS7; + break; + case 8: + default: + newtio.c_cflag = (newtio.c_cflag & ~Posix.CSIZE) | Posix.CS8; + break; + } + + newtio.c_cflag |= Posix.CLOCAL | Posix.CREAD; + + newtio.c_cflag &= ~(Posix.PARENB | Posix.PARODD); + + /* TODO: parity */ + + newtio.c_cflag &= ~Linux.Termios.CRTSCTS; + + if(stopbits == 2) + newtio.c_cflag |= Posix.CSTOPB; + else + newtio.c_cflag &= ~Posix.CSTOPB; + + newtio.c_iflag = Posix.IGNBRK; + + newtio.c_lflag = 0; + newtio.c_oflag = 0; + + newtio.c_cc[Posix.VTIME]=1; + newtio.c_cc[Posix.VMIN]=1; + + newtio.c_lflag &= ~(Posix.ECHONL|Posix.NOFLSH); + + int mcs=0; + Posix.ioctl(fd, Linux.Termios.TIOCMGET, out mcs); + mcs |= Linux.Termios.TIOCM_RTS; + Posix.ioctl(fd, Linux.Termios.TIOCMSET, out mcs); + + Posix.tcsetattr(fd, Posix.TCSANOW, newtio); + + this.byterate = rate/bits; + } + + public EngineController(string device) { + setup(device, 9600, 8, 1); + } + + private ssize_t read(void *buf, size_t count) { + if(fd >= 0) + return Posix.read(fd, buf, count); + else + stderr.printf("not connected!\n"); + return 0; + } + + private ssize_t write(void *buf, size_t count) { + if(fd >= 0) { + ssize_t size = Posix.write(fd, buf, count); + return size; + } else { + stderr.printf("not connected!\n"); + return 0; + } + } + + public bool disable() { + uint8[4] buf = new uint8[4]; + buf[0] = 0xDE; + buf[1] = 0xAD; + buf[2] = 0xBA; + buf[3] = 0xBE; + + if(fd < 0) { + stderr.printf("not connected!\n"); + return false; + } + + write(buf, 4); + read(buf, 1); + + if(buf[0] == '.') + return true; + else + return false; + } + + public bool enable() { + uint8[4] buf = new uint8[4]; + buf[0] = 0xCA; + buf[1] = 0xFE; + buf[2] = 0xBA; + buf[3] = 0xBE; + + if(fd < 0) { + stderr.printf("not connected!\n"); + return false; + } + + write(buf, 4); + read(buf, 1); + + if(buf[0] == '.') + return true; + else + return false; + } + + public bool no_operation() { + uint8[1] buf = new uint8[1]; + buf[0] = 0x00; + + if(fd < 0) { + stderr.printf("not connected!\n"); + return false; + } + + write(buf, 1); + read(buf, 1); + + if(buf[0] == '.') + return true; + else + return false; + } + + public bool set_speed(uint8 engine, uint8 speed) { + uint8[2] buf = new uint8[2]; + buf[0] = engine+1; + buf[1] = speed; + + if(fd < 0) { + stderr.printf("not connected!\n"); + return false; + } + + if(engine > 5) + return false; + + write(buf, 2); + read(buf, 2); + + if(buf[0] == '.') + return true; + else + return false; + } + + public bool set_speed_all(uint8 speed) { + uint8[2] buf = new uint8[2]; + buf[0] = 0xFF; + buf[1] = speed; + + if(fd < 0) { + stderr.printf("not connected!\n"); + return false; + } + + write(buf, 2); + read(buf, 2); + + if(buf[0] == '.') + return true; + else + return false; + } +} diff --git a/client/main.vala b/client/main.vala new file mode 100644 index 0000000..2c676a5 --- /dev/null +++ b/client/main.vala @@ -0,0 +1,58 @@ +public static void main(string[] args) { + var engines = new EngineController("/dev/ttyAMA0"); + + if(args.length < 2) { + stderr.printf("%s <cmd>\n", args[0]); + stderr.printf("cmds: engine, enable, disable, nop\n"); + return; + } + + switch(args[1]) { + case "engine": + if(args.length < 4) { + stderr.printf("%s engine <engine> <speed>\n", args[0]); + return; + } + + uint8 engine = (uint8) int.parse(args[2]); + if("%u".printf(engine) != args[2]) + error("engine must be an integer"); + + uint8 speed = (uint8) int.parse(args[3]); + if("%u".printf(speed) != args[3]) + error("speed must be an integer"); + + if(engine == 6) + if(engines.set_speed_all(speed)) + stdout.printf("set speed of all engines to %u: ok\n", speed); + else + stdout.printf("set speed of all engines to %u: fail\n", speed); + if(engine <= 5) + if(engines.set_speed(engine, speed)) + stdout.printf("set speed of engine %u to %u: ok\n", engine, speed); + else + stdout.printf("set speed of engine %u to %u: fail\n", engine, speed); + break; + case "enable": + if(engines.enable()) + stdout.printf("enable motors: ok\n"); + else + stdout.printf("enable motors: fail\n"); + break; + case "disable": + if(engines.disable()) + stdout.printf("disable motors: ok\n"); + else + stdout.printf("disable motors: fail\n"); + break; + case "nop": + if(engines.no_operation()) + stdout.printf("no operation: ok\n"); + else + stdout.printf("no operation: fail\n"); + break; + default: + stderr.printf("cmd not supported!\n"); + break; + } +} diff --git a/doc/atmostripe-pinout.txt b/doc/atmostripe-pinout.txt new file mode 100644 index 0000000..6031357 --- /dev/null +++ b/doc/atmostripe-pinout.txt @@ -0,0 +1 @@ +Atmostripe ISP Pins: VCC, GND, RST, MOSI, MISO, SCK diff --git a/doc/isp-plug.svg b/doc/isp-plug.svg new file mode 100644 index 0000000..f68795d --- /dev/null +++ b/doc/isp-plug.svg @@ -0,0 +1,187 @@ +<?xml version="1.0" encoding="UTF-8" standalone="no"?> +<!-- Created with Inkscape (http://www.inkscape.org/) --> + +<svg + xmlns:dc="http://purl.org/dc/elements/1.1/" + xmlns:cc="http://creativecommons.org/ns#" + xmlns:rdf="http://www.w3.org/1999/02/22-rdf-syntax-ns#" + xmlns:svg="http://www.w3.org/2000/svg" + xmlns="http://www.w3.org/2000/svg" + xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd" + xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape" + id="svg2" + version="1.1" + inkscape:version="0.48.3.1 r9886" + width="240" + height="88" + sodipodi:docname="ISP 6 way pinout.png"> + <metadata + id="metadata8"> + <rdf:RDF> + <cc:Work + rdf:about=""> + <dc:format>image/svg+xml</dc:format> + <dc:type + rdf:resource="http://purl.org/dc/dcmitype/StillImage" /> + <dc:title></dc:title> + </cc:Work> + </rdf:RDF> + </metadata> + <defs + id="defs6" /> + <sodipodi:namedview + pagecolor="#ffffff" + bordercolor="#666666" + borderopacity="1" + objecttolerance="10" + gridtolerance="10" + guidetolerance="10" + inkscape:pageopacity="0" + inkscape:pageshadow="2" + inkscape:window-width="1179" + inkscape:window-height="678" + id="namedview4" + showgrid="false" + inkscape:zoom="3.4648232" + inkscape:cx="117.8808" + inkscape:cy="34.798019" + inkscape:window-x="0" + inkscape:window-y="0" + inkscape:window-maximized="0" + inkscape:current-layer="svg2" /> + <path + style="fill:none;stroke:#000000;stroke-width:1px;stroke-linecap:butt;stroke-linejoin:miter;stroke-opacity:1" + d="m 88,28 0,-24 62,0 0,81 -62,0 0,-23" + id="path2987" + inkscape:connector-curvature="0" + sodipodi:nodetypes="cccccc" /> + <path + sodipodi:type="arc" + style="fill:none;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0" + id="path2989" + sodipodi:cx="124.53738" + sodipodi:cy="21.185625" + sodipodi:rx="3.0304577" + sodipodi:ry="3.0304577" + d="m 127.56783,21.185625 a 3.0304577,3.0304577 0 1 1 -6.06091,0 3.0304577,3.0304577 0 1 1 6.06091,0 z" + transform="translate(4.928208,3.768891)" /> + <path + sodipodi:type="arc" + style="fill:none;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0" + id="path2989-4" + sodipodi:cx="124.53738" + sodipodi:cy="21.185625" + sodipodi:rx="3.0304577" + sodipodi:ry="3.0304577" + d="m 127.56783,21.185625 a 3.0304577,3.0304577 0 1 1 -6.06091,0 3.0304577,3.0304577 0 1 1 6.06091,0 z" + transform="translate(4.86284,24.364255)" /> + <path + sodipodi:type="arc" + style="fill:none;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0" + id="path2989-0" + sodipodi:cx="124.53738" + sodipodi:cy="21.185625" + sodipodi:rx="3.0304577" + sodipodi:ry="3.0304577" + d="m 127.56783,21.185625 a 3.0304577,3.0304577 0 1 1 -6.06091,0 3.0304577,3.0304577 0 1 1 6.06091,0 z" + transform="translate(4.717362,44.705716)" /> + <path + sodipodi:type="arc" + style="fill:none;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0" + id="path2989-0-1" + sodipodi:cx="124.53738" + sodipodi:cy="21.185625" + sodipodi:rx="3.0304577" + sodipodi:ry="3.0304577" + d="m 127.56783,21.185625 a 3.0304577,3.0304577 0 1 1 -6.06091,0 3.0304577,3.0304577 0 1 1 6.06091,0 z" + transform="translate(-15.690801,44.705716)" /> + <path + sodipodi:type="arc" + style="fill:none;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0" + id="path2989-0-3" + sodipodi:cx="124.53738" + sodipodi:cy="21.185625" + sodipodi:rx="3.0304577" + sodipodi:ry="3.0304577" + d="m 127.56783,21.185625 a 3.0304577,3.0304577 0 1 1 -6.06091,0 3.0304577,3.0304577 0 1 1 6.06091,0 z" + transform="translate(-15.690801,24.297552)" /> + <rect + style="fill:none;stroke:#000000;stroke-width:2;stroke-linecap:butt;stroke-miterlimit:4;stroke-opacity:1;stroke-dasharray:none;stroke-dashoffset:0" + id="rect3052" + width="7.3469386" + height="6.734694" + x="105.3772" + y="21.605625" /> + <text + xml:space="preserve" + style="font-size:9px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:end;line-height:125%;letter-spacing:0px;word-spacing:0px;text-anchor:end;fill:#000000;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans Mono;-inkscape-font-specification:Bitstream Vera Sans Mono" + x="82.087166" + y="29.972973" + id="text3054" + sodipodi:linespacing="125%"><tspan + sodipodi:role="line" + id="tspan3056" + x="82.087166" + y="29.972973" + style="font-size:14px;font-weight:bold;-inkscape-font-specification:Bitstream Vera Sans Mono Bold">MISO: 1</tspan></text> + <text + xml:space="preserve" + style="font-size:9px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:end;line-height:125%;letter-spacing:0px;word-spacing:0px;text-anchor:end;fill:#000000;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans Mono;-inkscape-font-specification:Bitstream Vera Sans Mono" + x="81.870941" + y="52.007938" + id="text3054-1" + sodipodi:linespacing="125%"><tspan + sodipodi:role="line" + id="tspan3056-4" + x="81.870941" + y="52.007938" + style="font-size:14px;font-weight:bold;-inkscape-font-specification:Bitstream Vera Sans Mono Bold">SCK: 3</tspan></text> + <text + xml:space="preserve" + style="font-size:9px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;text-align:end;line-height:125%;letter-spacing:0px;word-spacing:0px;text-anchor:end;fill:#000000;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans Mono;-inkscape-font-specification:Bitstream Vera Sans Mono" + x="82.693886" + y="70.987534" + id="text3054-1-9" + sodipodi:linespacing="125%"><tspan + sodipodi:role="line" + id="tspan3056-4-6" + x="82.693886" + y="70.987534" + style="font-size:14px;font-weight:bold;-inkscape-font-specification:Bitstream Vera Sans Mono Bold">RESET: 5</tspan></text> + <text + xml:space="preserve" + style="font-size:9px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans Mono;-inkscape-font-specification:Bitstream Vera Sans Mono" + x="157.418" + y="30.789299" + id="text3100" + sodipodi:linespacing="125%"><tspan + sodipodi:role="line" + id="tspan3102" + x="157.418" + y="30.789299" + style="font-size:14px;font-weight:bold;-inkscape-font-specification:Bitstream Vera Sans Mono Bold">2: VCC</tspan></text> + <text + xml:space="preserve" + style="font-size:9px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans Mono;-inkscape-font-specification:Bitstream Vera Sans Mono" + x="158.43869" + y="50.171204" + id="text3100-9" + sodipodi:linespacing="125%"><tspan + sodipodi:role="line" + id="tspan3102-8" + x="158.43869" + y="50.171204" + style="font-size:14px;font-weight:bold;-inkscape-font-specification:Bitstream Vera Sans Mono Bold">4: MOSI</tspan></text> + <text + xml:space="preserve" + style="font-size:9px;font-style:normal;font-variant:normal;font-weight:normal;font-stretch:normal;line-height:125%;letter-spacing:0px;word-spacing:0px;fill:#000000;fill-opacity:1;stroke:none;font-family:Bitstream Vera Sans Mono;-inkscape-font-specification:Bitstream Vera Sans Mono" + x="157.7283" + y="72.987534" + id="text3100-9-8" + sodipodi:linespacing="125%"><tspan + sodipodi:role="line" + id="tspan3102-8-6" + x="157.7283" + y="72.987534" + style="font-size:14px;font-weight:bold;-inkscape-font-specification:Bitstream Vera Sans Mono Bold">6: Ground</tspan></text> +</svg> diff --git a/firmware/Makefile b/firmware/Makefile new file mode 100644 index 0000000..d9b89e7 --- /dev/null +++ b/firmware/Makefile @@ -0,0 +1,32 @@ +SOURCES := firmware.c +OBJECTS := $(patsubst %.c, %.o, $(SOURCES)) +CFLAGS := -O2 -std=c99 -mmcu=atmega48 -Wall -DF_CPU=16000000UL -DDEBUG=1 +LDFLAGS := -mmcu=atmega48 -lm + +all: fw.hex + +%.o: %.c + @echo "[CC] $<" + @avr-gcc -o $@ $(CFLAGS) -c $< + +fw.hex: fw.elf + @echo "[HEX] $@" + @avr-objcopy $^ -O ihex -R .eeprom $@ + +fw.elf: ${OBJECTS} + @echo "[LD] $@" + @avr-gcc -o $@ $(LDFLAGS) $^ + +fw.S: fw.elf + @avr-objdump -d fw.elf -h -m avr > fw.S + +clean: + @rm -f ${OBJECTS} fw.hex fw.elf fw.S + +backup: + avrdude -c buspirate -P /dev/ttyUSB0 -p m48 -U flash:r:"backup.hex":i noreset + +flash: + avrdude -c buspirate -P /dev/ttyUSB0 -p m48 -U flash:w:"fw.hex":i reset + +.PHONY: all clean backup flash diff --git a/firmware/firmware.c b/firmware/firmware.c new file mode 100644 index 0000000..9a04d41 --- /dev/null +++ b/firmware/firmware.c @@ -0,0 +1,170 @@ +#include <avr/io.h> +#include <util/delay.h> +#include <util/delay.h> +#include <avr/interrupt.h> + +/* 500Hz -> 2ms */ +#define MAXPULSFREQ 500 + +/* timer1 value for a 2ms puls */ +#define TIMER_MAXPULS F_CPU/MAXPULSFREQ + +/* min/max values */ +#define MINPULS 13500 +#define MAXPULS 30000 + +/* baud rate for serial communication */ +#define BAUD 9600UL + +/* hardware settings for baud rate */ +#define UBRR_VAL ((F_CPU+BAUD*8)/(BAUD*16)-1) +#define BAUD_REAL (F_CPU/(16*(UBRR_VAL+1))) +#define BAUD_ERROR ((BAUD_REAL*1000)/BAUD) + +/* compile time check if baud rate is ok */ +#if ((BAUD_ERROR<990) || (BAUD_ERROR>1010)) + #error systematic error of baud rate is > 1% +#endif + + +static uint16_t servo_pulslength[20]; +static uint8_t activated = 0; + +/* init IO pins */ +static inline void init_io() { + /* PB1 = left:red, PB2 = left:green, PB3 = left:blue */ + DDRB = (1 << PB1) | (1 << PB2) | (1 << PB3); + + /* PD3 = right:red, PD5 = right:green, PD6 = right:blue */ + DDRD = (1 << PD3) | (1 << PD5) | (1 << PD6); + + /* These pins are broken, so enable them */ + DDRC = (1 << PC4) | (1 << PC5) | (1 << PC6); + PORTC = (1 << PC4) | (1 << PC5) | (1 << PC6); +} + +/* INTERRUPT: timer for software pwm to control servos */ +ISR(TIMER1_OVF_vect) { + static uint8_t servo_indexhalf = 0; + + switch(servo_indexhalf) { + case 0 : PORTB |= (1 << PB1); break; + case 1 : PORTB &= ~(1 << PB1); break; + case 2 : PORTB |= (1 << PB2); break; + case 3 : PORTB &= ~(1 << PB2); break; + case 4 : PORTB |= (1 << PB3); break; + case 5 : PORTB &= ~(1 << PB3); break; + case 6 : PORTD |= (1 << PD3); break; + case 7 : PORTD &= ~(1 << PD3); break; + case 8 : PORTD |= (1 << PD5); break; + case 9 : PORTD &= ~(1 << PD5); break; + case 10: PORTD |= (1 << PD6); break; + case 11: PORTD &= ~(1 << PD6); break; + } + + /* set time for the next interrupt */ + TCNT1 = servo_pulslength[servo_indexhalf]; + + servo_indexhalf++; + + if(servo_indexhalf == 20) + servo_indexhalf = 0; +} + +/* setup serial communication */ +static inline void init_uart() { + UBRR0 = UBRR_VAL; + UCSR0B |= (1 << TXEN0); + UCSR0B |= (1 << RXEN0); + + /* Frame Format: Asynchron 8N1 */ + UCSR0C = (1<<UCSZ01)|(1<<UCSZ00); +} + +/* set servo speed */ +static inline void servo_set(uint8_t index, uint8_t value) { + if(!activated) + value = 0; + + if (index < 6) { + uint16_t tmp = MINPULS+(MAXPULS-MINPULS)/256*value; + /* calculate hightime */ + servo_pulslength[index<<1]=(-1)*tmp; + /* sum of low and hightime for one servo is 2ms */ + servo_pulslength[(index<<1)+1] = (-1)*(TIMER_MAXPULS-tmp); + } +} + +/* init servos, reset speed */ +static inline void init_servo() { + uint8_t i; + + /* disable all motors */ + for(i=0;i<6;i++) + servo_set(i, 0); + + /* init timer */ + TCNT1 = 0 - 16000; + TCCR1A = 0; + TCCR1B = (1 << CS10); + TIMSK1 |= (1 << TOIE1); +} + +/* get a single byte from serial device */ +static inline uint8_t uart_getc() { + while (!(UCSR0A & (1<<RXC0))); + return UDR0; +} + +/* put a single byte to serial device */ +static inline void uart_putc(uint8_t c) { + while (!(UCSR0A & (1<<UDRE0))); + UDR0 = c; +} + +int main() { + /* initialize hardware */ + init_io(); + init_servo(); + init_uart(); + sei(); + + uint8_t c; + + while(1) { + c = uart_getc(); + + /* set speed for single engine */ + if(c >= 0x01 && c <= 0x06) { + uint8_t speed = uart_getc(); + servo_set(c-1, speed); + /* set speed for all engines */ + } else if(c == 0xFF) { + uint8_t speed = uart_getc(); + for(int i=0;i<6;i++) + servo_set(i, speed); + /* activate engines */ + } else if(c == 0xCA) { + if(uart_getc() != 0xFE) continue; + if(uart_getc() != 0xBA) continue; + if(uart_getc() != 0xBE) continue; + activated=1; + /* disable engines */ + } else if(c == 0xDE) { + if(uart_getc() != 0xAD) continue; + if(uart_getc() != 0xBA) continue; + if(uart_getc() != 0xBE) continue; + activated=0; + for(int i=0;i<6;i++) + servo_set(i, 0); + /* ignore unknown commands */ + } else if(c != 0x00) { + continue; + } + + /* acknowledge command */ + uart_putc('.'); + } + + return 0; +} |