| 1 | pkg load matgeom |
| 2 | pkg load instrument-control |
| 3 | |
| 4 | graphics_toolkit("qt") |
| 5 | |
| 6 | tmtry_serial = serial("/dev/ttyUSB0", 9600); |
| 7 | srl_flush(tmtry_serial); |
| 8 | |
| 9 | center = [0 0 0]; |
| 10 | % specify lengths (x y z) |
| 11 | sizes = [4 2 1]; |
| 12 | % specify yaw pitch and roll |
| 13 | ypr = [0 0 0]; |
| 14 | |
| 15 | figure; |
| 16 | view (3); |
| 17 | axis equal; |
| 18 | hold off; |
| 19 | |
| 20 | x = [1 0 0]; |
| 21 | y = [0 1 0]; |
| 22 | z = [0 0 1]; |
| 23 | |
| 24 | while true |
| 25 | data = srl_read(tmtry_serial, 12); |
| 26 | |
| 27 | ## convert [u8; 4] array to float |
| 28 | ## y = swapbytes (typecast(uint8(data(1:4)), "single")) |
| 29 | ## p = swapbytes (typecast(uint8(data(5:8)), "single")) |
| 30 | ## r = swapbytes (typecast(uint8(data(9:12)), "single")) |
| 31 | y = typecast(uint8(data(1:4)), "single"); |
| 32 | p = typecast(uint8(data(5:8)), "single"); |
| 33 | r = typecast(uint8(data(9:12)), "single") |
| 34 | |
| 35 | ## rotate(c, x, r); |
| 36 | ## rotate(c, y, p); |
| 37 | ## rotate(c, z ,y); |
| 38 | ypr = [y p r]; |
| 39 | clf |
| 40 | drawCuboid ([center sizes ypr]); |
| 41 | view (3); |
| 42 | ## axis([0 5 0 5]); |
| 43 | axis equal; |
| 44 | drawnow; |
| 45 | |
| 46 | ## pause(0.5); |
| 47 | end |
| 48 | |
| 49 | close |
| 50 | |
