Mercurial > imath
changeset 0:0714c9003e6c draft
Initial commit
| author | Radomir Dopieralski <sheep-devel@sheep.art.pl> |
|---|---|
| date | Wed, 21 Dec 2016 22:16:40 +0100 |
| parents | |
| children | a56bbe3b03df |
| files | .hgignore Makefile asin.py atan2.py imath.c imath.h main.c |
| diffstat | 7 files changed, 352 insertions(+), 0 deletions(-) [+] |
line wrap: on
line diff
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/.hgignore Wed Dec 21 22:16:40 2016 +0100 @@ -0,0 +1,17 @@ +syntax: glob +*.pyc +*~ +.*.swp +*.o +*.elf +*.hex +docs/* +build/* +dist/* +bin/* +lib/* +include/* +local +*.egg-info +cache/* +*.DS_Store
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Makefile Wed Dec 21 22:16:40 2016 +0100 @@ -0,0 +1,72 @@ +DEVICE = atmega328p +CLOCK = 16000000 +PROGRAMMER = -c usbasp +OBJECTS = main.o imath.o + +# for ATTiny85 +# see http://www.engbedded.com/fusecalc/ +#FUSES = -U lfuse:w:0xe2:m -U hfuse:w:0xdf:m -U efuse:w:0xff:m +#-U lfuse:w:0x62:m -U hfuse:w:0xdf:m -U efuse:w:0xff:m + + +# Tune the lines below only if you know what you are doing: +AVRDUDE = avrdude $(PROGRAMMER) -p $(DEVICE) +COMPILE = avr-gcc -std=gnu99 -Wall -Os -DF_CPU=$(CLOCK) -mmcu=$(DEVICE) + +# symbolic targets: +all: main.hex + +.c.o: + $(COMPILE) -c $< -o $@ + +.S.o: + $(COMPILE) -x assembler-with-cpp -c $< -o $@ + +.c.s: + $(COMPILE) -S $< -o $@ + +flash: all + $(AVRDUDE) -U flash:w:main.hex:i + +fuse: + $(AVRDUDE) $(FUSES) + +# Xcode uses the Makefile targets "", "clean" and "install" +install: flash fuse + +# if you use a bootloader, change the command below appropriately: +load: all + bootloadHID main.hex + +clean: + rm -f main.hex main.elf $(OBJECTS) + +# file targets: +main.elf: $(OBJECTS) + $(COMPILE) -o main.elf $(OBJECTS) + +main.hex: main.elf + rm -f main.hex + avr-objcopy -j .text -j .data -O ihex main.elf main.hex + avr-size --format=avr --mcu=$(DEVICE) main.elf +# If you have an EEPROM section, you must also create a hex file for the +# EEPROM and add it to the "flash" target. + +# Targets for code debugging and analysis: +disasm: main.elf + avr-objdump -d main.elf + +cpp: + $(COMPILE) -E main.c`gg + +deps: + $(COMPILE) -M main.c pwm.c >> Makefile +imath.o: imath.c imath.h /usr/lib/gcc/avr/4.9.2/include/stdint.h \ + /usr/lib/avr/include/stdint.h /usr/lib/avr/include/avr/pgmspace.h \ + /usr/lib/avr/include/inttypes.h /usr/lib/gcc/avr/4.9.2/include/stddef.h \ + /usr/lib/avr/include/avr/io.h /usr/lib/avr/include/avr/sfr_defs.h \ + /usr/lib/avr/include/avr/portpins.h /usr/lib/avr/include/avr/common.h \ + /usr/lib/avr/include/avr/version.h /usr/lib/avr/include/avr/fuse.h \ + /usr/lib/avr/include/avr/lock.h +main.o: main.c imath.h /usr/lib/gcc/avr/4.9.2/include/stdint.h \ + /usr/lib/avr/include/stdint.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/asin.py Wed Dec 21 22:16:40 2016 +0100 @@ -0,0 +1,71 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import math + +def ANGLE(a): + return int(a * 10000.0 + 0.5) + +ATAN = [ + ANGLE(45.000), # atan(1/1) + ANGLE(26.565), # atan(1/2) + ANGLE(14.036), # atan(1/4) + ANGLE( 7.125), # atan(1/8) + ANGLE( 3.576), # atan(1/16) + ANGLE( 1.790), # atan(1/32) + ANGLE( 0.895), # atan(1/64) + ANGLE( 0.448), # atan(1/128) + ANGLE( 0.224), # atan(1/256) + ANGLE( 0.112), # atan(1/512) + ANGLE( 0.056), # atan(1/1024) + ANGLE( 0.028), # atan(1/2048) +] + +scale = 1 << 14 +mx = 0 +my = 0 +nx = 0 +ny = 0 +mv = 0 +nv = 0 + +def asin(value): + global mx, my, nx, ny, mv, nv + result = 0 + x = scale + y = 0 + for i, atan in enumerate(ATAN): + if y < value: + for j in range(2): + tx = x + x -= y>>i + y += tx>>i + result += atan << 1 + else: + for j in range(2): + tx = x + x += y>>i + y -= tx>>i + result -= atan << 1 + mx = max(x, mx) + my = max(y, my) + nx = min(x, nx) + ny = min(y, ny) + value += value >> (i << 1) + mv = max(value, mv) + nv = min(value, nv) + return result + + +for y in xrange(-scale, scale, scale >> 5): + iasin = float(asin(y))/10000 + masin = math.degrees(math.asin(float(y)/scale)) + print "y: %d, isin: %f, masin: %f, diff: %f" % ( + y, + iasin, + masin, + iasin - masin, + ) +print "mx: %d, my: %d, log: %f" % (mx, my, math.log(max(mx, my), 2)) +print "nx: %d, ny: %d, log: %f" % (nx, ny, math.log(-min(nx, ny), 2)) +print "nv: %d, mv: %d" % (nv, mv)
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/atan2.py Wed Dec 21 22:16:40 2016 +0100 @@ -0,0 +1,60 @@ +import math + +def ANGLE(a): + return int(a * 10000.0 + 0.5) + +ATAN = [ + ANGLE(45.000), # atan(1/1) + ANGLE(26.565), # atan(1/2) + ANGLE(14.036), # atan(1/4) + ANGLE( 7.125), # atan(1/8) + ANGLE( 3.576), # atan(1/16) + ANGLE( 1.790), # atan(1/32) + ANGLE( 0.895), # atan(1/64) + ANGLE( 0.448), # atan(1/128) + ANGLE( 0.224), # atan(1/256) + ANGLE( 0.112), # atan(1/512) +# ANGLE( 0.056), # atan(1/1024) +# ANGLE( 0.028), # atan(1/2048) +] + +scale = 1 << 14 + + +def atan2(y, x): + result = 0 + if x == 0 and y == 0: + return 0 + if y < 0: + x, y = -x, -y + result -= ANGLE(180) + if x < 0: + x, y = y, -x + result += ANGLE(90) + while x < scale and y < scale: + x <<= 1 + y <<= 1 + for i, atan in enumerate(ATAN): + if y > x>>i: + tx = x + x += y>>i + y -= tx>>i + result += atan + result += ATAN[-1] >> 1 + return result + + +md = 0 +for y in xrange(-100, 100): + for x in xrange(-100, 100): + iatan = float(atan2(y, x))/10000 + matan = math.degrees(math.atan2(y, x)) + print "x: %d, y: %d, iatan: %f, matan: %f, diff: %f" % ( + x, + y, + iatan, + matan, + iatan - matan, + ) + md = max(md, abs(iatan - matan)) +print md
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/imath.c Wed Dec 21 22:16:40 2016 +0100 @@ -0,0 +1,115 @@ +#include "imath.h" +#include <avr/pgmspace.h> + + +#define ANGLE_SCALE 10 // how many units per degree +#define ANGLE(a) (int16_t)(((a) * ANGLE_SCALE) + 0.5) +#define ITERATIONS 10 +#define SCALE (1<<14) // how many units per 1 + + +static const PROGMEM int16_t ATAN[ITERATIONS] = { + ANGLE(45.000), // atan(1/1) + ANGLE(26.565), // atan(1/2) + ANGLE(14.036), // atan(1/4) + ANGLE(7.125), // atan(1/8) + ANGLE(3.576), // atan(1/16) + ANGLE(1.790), // atan(1/32) + ANGLE(0.895), // atan(1/64) + ANGLE(0.448), // atan(1/128) + ANGLE(0.224), // atan(1/256) + ANGLE(0.112), // atan(1/512) + // ANGLE(0.056), // atan(1/1024) + // ANGLE(0.028), // atan(1/2048) +}; + + +/* + * Takes two 16-bit signed integers and calculates atan2 for them. + * Returns a value from -1800 (for -180°) to 1800 (for 180°). + */ +int16_t iatan2(int16_t y, int16_t x) { + int16_t result = 0; + uint16_t tx, ux, uy; + uint8_t i = 0; + + if (x == 0 && y == 0) { + return 0; + } + if (y < 0) { + x = -x; + uy = -y; + result -= ANGLE(180); + } else { + uy = y; + } + if (x < 0) { + ux = uy; + uy = -x; + result += ANGLE(90); + } else { + ux = x; + } + while (ux < SCALE && uy < SCALE) { + ux <<= 1; + uy <<= 1; + } + while (i < ITERATIONS) { + if (uy > ux>>i) { + tx = ux; + ux += uy >> i; + uy -= tx >> i; + result += pgm_read_word(&ATAN[i]); + } + } + return result + (pgm_read_word(&ATAN[i - 1]) >> 1); +} + +/* + * Takes a number between -1<<14 and 1<<14 and returns an angle between + * -1800 (for -180°) and 1800 (for 180°). + */ +int16_t iasin(int16_t value) { + int16_t result = 0; + int16_t x = SCALE; + int16_t y = 0; + int16_t tx; + + for (uint8_t i = 0; i < ITERATIONS; ++i) { + for (uint8_t j = 0; j < 2; ++j) { + tx = x; + if (y < value) { + x -= y>>i; + y += tx>>i; + result += pgm_read_word(&ATAN[i]); + } else { + x += y>>i; + y -= tx>>i; + result -= pgm_read_word(&ATAN[i]); + } + } + value += value >> (i << 1); + } + return value; +} + +/* + * Takes a unsigned integer and returns nearest approximation of its square + * root. + */ +uint16_t isqrt(uint16_t value) { uint16_t result = 0; uint16_t bit = SCALE; + + while (bit > value) { + bit >>= 2; + } + while (bit != 0) { + if (value >= result + bit) { + value -= result + bit; + result = (result >> 1) + bit; + } else { + result >>= 1; + } + bit >>= 2; + } + return result; +}