├── .gitignore ├── LICENSE ├── README.md ├── dfpad ├── buttons.h ├── buttons.ino ├── com.h ├── com.ino └── dfpad.ino ├── docs ├── bom.md └── imgs │ ├── all-parts.jpg │ ├── bec-solder.jpg │ ├── body.jpg │ ├── elec.png │ ├── fixed-us.jpg │ ├── ids.jpg │ ├── kinematic.jpg │ ├── metabot.jpg │ ├── metabots.png │ ├── motion.png │ ├── motors-us.jpg │ ├── one-leg.jpg │ ├── opencm-fixations.jpg │ ├── opencm-fixed.jpg │ ├── opencmpp-board.jpg │ ├── opencmpp-mounted.jpg │ ├── opencmpp-robot.jpg │ ├── openscad.jpg │ ├── orientation.jpg │ ├── pulley-rivet.jpg │ ├── rivet-u-1.jpg │ ├── rivet-u-2.jpg │ ├── robot.jpg │ ├── side.jpg │ ├── side2.jpg │ ├── src │ ├── ids.xcf │ ├── motion.xcf │ └── orientation.xcf │ ├── us-2.jpg │ ├── wire.jpg │ └── zero-dot.jpg ├── electronics ├── .gitignore ├── README.md ├── xboard.brd ├── xboard.eps ├── xboard.pdf ├── xboard.pro ├── xboard.ps └── xboard.sch ├── firmware ├── README.md ├── metabot.bin ├── metabot.version ├── metabot2.bin ├── metabot2.version ├── robotis-loader.py └── src │ ├── .gitignore │ ├── Makefile │ ├── README.md │ ├── behavior.cpp │ ├── behavior.h │ ├── bt.cpp │ ├── bt.h │ ├── build-targets.mk │ ├── buzzer.cpp │ ├── buzzer.h │ ├── config.h │ ├── cubic.cpp │ ├── cubic.h │ ├── distance.cpp │ ├── distance.h │ ├── imu.cpp │ ├── imu.h │ ├── js │ └── CMakeLists.txt │ ├── kinematic.cpp │ ├── kinematic.h │ ├── leds.cpp │ ├── leds.h │ ├── main.cpp │ ├── mapping.cpp │ ├── mapping.h │ ├── motion.cpp │ ├── motion.h │ ├── motors.cpp │ ├── motors.h │ ├── prepare.sh │ ├── rhock-functions.cpp │ ├── rhock-functions.h │ ├── rhock-stream.cpp │ ├── rhock-stream.h │ ├── voltage.cpp │ └── voltage.h ├── mechanics ├── 3d │ ├── README.md │ ├── body_bottom.stl │ ├── body_top.stl │ ├── draws │ │ ├── body.pdf │ │ ├── leg.pdf │ │ ├── side.pdf │ │ └── u.pdf │ ├── head.stl │ ├── imgs │ │ ├── body.png │ │ ├── body_bottom.png │ │ ├── body_top.png │ │ ├── leg.png │ │ ├── parts │ │ │ ├── body_bottom.png │ │ │ ├── body_top.png │ │ │ ├── leg.png │ │ │ ├── side.png │ │ │ └── u.png │ │ ├── side.png │ │ └── u.png │ ├── leg.stl │ ├── plater.conf │ ├── scad │ │ ├── .gitignore │ │ ├── Makefile │ │ ├── README.md │ │ ├── joints │ │ │ ├── double_u.scad │ │ │ └── side_to_side.scad │ │ ├── metabot │ │ │ ├── body_bottom │ │ │ ├── config.scad │ │ │ ├── joints.scad │ │ │ ├── metabot.scad │ │ │ ├── parts.scad │ │ │ └── print │ │ │ │ ├── body_bottom.scad │ │ │ │ ├── body_projection.scad │ │ │ │ ├── body_top.scad │ │ │ │ ├── head.scad │ │ │ │ ├── leg.scad │ │ │ │ ├── leg_projection.scad │ │ │ │ ├── side.scad │ │ │ │ ├── side_projection.scad │ │ │ │ ├── u.scad │ │ │ │ └── u_projection.scad │ │ ├── models │ │ │ ├── arm.scad │ │ │ ├── motor.scad │ │ │ ├── motor_arm.scad │ │ │ └── ollo.scad │ │ ├── parts │ │ │ ├── body.scad │ │ │ ├── head.scad │ │ │ ├── leg.scad │ │ │ ├── side.scad │ │ │ └── u.scad │ │ └── util │ │ │ ├── bolt_trace.scad │ │ │ ├── hole3.scad │ │ │ ├── octopath.scad │ │ │ ├── repeat4.scad │ │ │ ├── rounded.scad │ │ │ ├── screws.scad │ │ │ └── thickLine.scad │ ├── side.stl │ └── u.stl ├── README.md └── laser │ ├── README.md │ ├── autodesk │ ├── body_bottom.ipt │ ├── body_top.ipt │ ├── entretoise.ipt │ ├── head.ipt │ ├── leg.ipt │ ├── models │ │ ├── arm.ipt │ │ ├── counter_arm.ipt │ │ ├── motor.ipt │ │ ├── motor_arm.iam │ │ ├── opencm.ipt │ │ └── x_board.ipt │ ├── parameters.ipt │ ├── pmma.styxml │ ├── robot.dwg │ ├── robot.iam │ ├── robot.pdf │ ├── side.ipt │ ├── spacer.ipt │ ├── tip.ipt │ ├── tip_up.ipt │ ├── u.ipt │ └── u_pattern │ │ ├── u_base.ipt │ │ ├── u_base.stl │ │ ├── u_base_hex.ipt │ │ ├── u_base_hex.stl │ │ ├── u_side.ipt │ │ └── u_side.stl │ ├── body_bottom.stl │ ├── body_top.stl │ ├── doc │ ├── body_bending.pdf │ ├── head_bending.pdf │ └── u_bending.pdf │ ├── head.stl │ ├── leg.stl │ ├── metabot.dxf │ ├── metabot.rld │ ├── metabot_concentric.dxf │ ├── parts │ ├── body_bottom.dxf │ ├── body_top.dxf │ ├── head.dxf │ ├── leg.dxf │ ├── side.dxf │ ├── spacer.dxf │ ├── tip.dxf │ ├── tip_up.dxf │ └── u.dxf │ ├── plater.conf │ ├── side.stl │ ├── spacer.stl │ ├── tip.stl │ ├── tip_up.stl │ ├── u.stl │ └── update.sh └── scripts └── nameBT.sh /.gitignore: -------------------------------------------------------------------------------- 1 | **/OldVersions 2 | **.lck 3 | **.swp -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | 2 | This project is under CC BY-NC license: 3 | 4 | http://creativecommons.org/licenses/by-nc/3.0/ 5 | 6 | The authors are: 7 | * Robot Campus 8 | * Rhoban System S.A.S 9 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # The Metabot open-source quadruped robot 2 | 3 | ![Metabot](docs/imgs/metabots.png) 4 | 5 | Metabot is an open-source DIY quadruped robotics platform. It can be 3D printed or 6 | laser cutted (3mm acrylic). 7 | 8 | Note that [the robot is now available as a kit](http://metabot.cc). 9 | In this repository you will find all the open-source material to make (or hack) your own. 10 | 11 | * [Bill of material](docs/bom.md) 12 | * Mechanics 13 | * [3D Parts](mechanics/3d) 14 | * [Laser parts](mechanics/laser) 15 | * [Electronics](electronics) 16 | * [Firmware](firmware) 17 | * [Joypad firmware](dfpad) 18 | 19 | You can control the robot using the Metabot app (available on the Play Store) 20 | 21 | [To get information about the previous robot version, see branch V1](https://github.com/Rhoban/Metabot/tree/v1) 22 | 23 | ## License 24 | 25 | This project is under CC BY-NC license: 26 | http://creativecommons.org/licenses/by-nc/3.0/ 27 | -------------------------------------------------------------------------------- /dfpad/buttons.h: -------------------------------------------------------------------------------- 1 | #ifndef _BUTTONS_H 2 | #define _BUTTONS_H 3 | #include "Arduino.h" 4 | 5 | // Buttons 6 | 7 | #define BTN_JLX A4 8 | #define BTN_JLY A5 9 | #define BTN_JRX A2 10 | #define BTN_JRY A3 11 | 12 | #define BTN_UP 5 13 | #define BTN_LEFT 6 14 | #define BTN_DOWN 7 15 | #define BTN_RIGHT 8 16 | 17 | #define BTN_R1 9 18 | #define BTN_R2 11 19 | #define BTN_R3 12 20 | #define BTN_R4 10 21 | 22 | #define BTN_LZ1 15 23 | #define BTN_LZ2 16 24 | #define BTN_RZ1 13 25 | #define BTN_RZ2 14 26 | 27 | #define BTN_START 4 28 | #define BTN_SELECT 3 29 | 30 | unsigned char allButtons[] = { 31 | BTN_JLX, BTN_JLY, BTN_JRX, BTN_JRY, 32 | BTN_UP, BTN_LEFT, BTN_DOWN, BTN_RIGHT, 33 | BTN_R1, BTN_R2, BTN_R3, BTN_R4, 34 | BTN_LZ1, BTN_LZ2, BTN_RZ1, BTN_RZ2, 35 | BTN_START, BTN_SELECT 36 | }; 37 | 38 | #define NB_BUTTONS (sizeof(allButtons)) 39 | 40 | // State of the buttons 41 | struct state { 42 | int joypad_left_x, joypad_left_y; 43 | int joypad_right_x, joypad_right_y; 44 | bool up, left, down, right; 45 | bool r1, r2, r3, r4; 46 | bool lz1, lz2, rz1, rz2; 47 | bool start, select; 48 | }; 49 | 50 | extern struct state last_btns; 51 | void initButtons(); 52 | struct state readButtons(); 53 | float normalize(int pad); 54 | 55 | #define ON_CHANGE(value) if (btns.value != last_btns.value) 56 | #define ON_PRESS(value) ON_CHANGE(value) if (btns.value) 57 | #define ON_RELEASE(value) ON_CHANGE(value) if (!btns.value) 58 | 59 | #endif 60 | -------------------------------------------------------------------------------- /dfpad/buttons.ino: -------------------------------------------------------------------------------- 1 | #include "buttons.h" 2 | 3 | struct state last_btns; 4 | 5 | void initButtons() 6 | { 7 | for (int i=0; i-4) v =0; 54 | return -v; 55 | } 56 | -------------------------------------------------------------------------------- /dfpad/com.h: -------------------------------------------------------------------------------- 1 | #ifndef _COM_H 2 | #define _COM_H 3 | 4 | #define COM_BAUD 115200 5 | 6 | void command(char *cmd); 7 | void command(char *cmd, float v); 8 | void initCom(); 9 | void comTick(); 10 | 11 | #endif 12 | -------------------------------------------------------------------------------- /dfpad/com.ino: -------------------------------------------------------------------------------- 1 | #include "com.h" 2 | 3 | #define COM_BAUD 115200 4 | 5 | void initCom() 6 | { 7 | Serial1.begin(COM_BAUD); 8 | } 9 | 10 | void comTick() 11 | { 12 | while (Serial1.available()) { 13 | char c = Serial1.read(); 14 | // if (Serial) Serial.write(c); 15 | } 16 | 17 | // Forwarding (Debug) 18 | while (Serial && Serial.available()) { 19 | Serial1.write(Serial.read()); 20 | } 21 | } 22 | 23 | void command(char *cmd) 24 | { 25 | Serial1.write(cmd); 26 | Serial1.write("\r\n"); 27 | 28 | } 29 | 30 | void command(char *cmd, float v) 31 | { 32 | Serial1.write(cmd); 33 | Serial1.write(" "); 34 | Serial1.print(v); 35 | Serial1.write("\r\n"); 36 | } 37 | 38 | -------------------------------------------------------------------------------- /dfpad/dfpad.ino: -------------------------------------------------------------------------------- 1 | #include "buttons.h" 2 | #include "com.h" 3 | 4 | // Height and front correction 5 | #define DEFAULT_H -70 6 | #define DEFAULT_FH 0 7 | 8 | int cheat = 0; 9 | int safe = 1; 10 | float h = DEFAULT_H; 11 | float frontH = DEFAULT_FH; 12 | 13 | // Remapping 14 | int remap = 0; 15 | 16 | // Is the robot started? 17 | bool started = false; 18 | 19 | // Crab mode? 20 | bool crab = false; 21 | 22 | // Using legs backward? 23 | bool backLegs = false; 24 | 25 | void setup() { 26 | initButtons(); 27 | initCom(); 28 | 29 | struct state btns = readButtons(); 30 | if (btns.select) { 31 | safe = 0; 32 | } 33 | } 34 | 35 | void loop() { 36 | comTick(); 37 | struct state btns = readButtons(); 38 | 39 | ON_RELEASE(start) { 40 | started = !started; 41 | if (started) { 42 | command("start"); 43 | } 44 | else { 45 | command("stop"); 46 | } 47 | } 48 | 49 | ON_CHANGE(joypad_left_x) { 50 | command("dx", -normalize(btns.joypad_left_x)); 51 | } 52 | 53 | ON_CHANGE(joypad_left_y) { 54 | command("dy", normalize(btns.joypad_left_y)); 55 | } 56 | 57 | ON_CHANGE(joypad_right_y) { 58 | command("turn", normalize(btns.joypad_right_y)/2.0); 59 | } 60 | 61 | 62 | if (safe == 0) { 63 | 64 | ON_RELEASE(r3) { 65 | crab = !crab; 66 | if (crab) { 67 | command("crab", 30); 68 | } else { 69 | command("crab", 0); 70 | } 71 | } 72 | 73 | ON_RELEASE(r2) { 74 | backLegs = !backLegs; 75 | if (backLegs) { 76 | command("backLegs", 1); 77 | } else { 78 | command("backLegs", 0); 79 | } 80 | } 81 | 82 | ON_RELEASE(select) { 83 | h = DEFAULT_H; 84 | frontH = DEFAULT_FH; 85 | command("h", h); 86 | command("frontH", frontH); 87 | } 88 | 89 | ON_RELEASE(up) { 90 | h -= 2; 91 | command("h", h); 92 | } 93 | ON_RELEASE(down) { 94 | h += 2; 95 | command("h", h); 96 | } 97 | ON_RELEASE(left) { 98 | frontH -= 2; 99 | command("frontH", frontH); 100 | } 101 | ON_RELEASE(right) { 102 | frontH += 2; 103 | command("frontH", frontH); 104 | } 105 | 106 | ON_PRESS(lz2) { 107 | command("alt", 30); 108 | } 109 | ON_RELEASE(lz2) { 110 | command("alt", 15); 111 | } 112 | 113 | ON_PRESS(rz2) { 114 | command("freq", 0.33); 115 | } 116 | ON_RELEASE(rz2) { 117 | command("freq", 2.0); 118 | } 119 | 120 | ON_RELEASE(lz1) { 121 | remap++; 122 | if (remap >= 4) remap = 0; 123 | command("remap", remap); 124 | } 125 | ON_RELEASE(rz1) { 126 | remap--; 127 | if (remap < 0) remap = 3; 128 | command("remap", remap); 129 | } 130 | } 131 | 132 | last_btns = btns; 133 | 134 | delay(3); 135 | } 136 | 137 | -------------------------------------------------------------------------------- /docs/bom.md: -------------------------------------------------------------------------------- 1 | # Bill of material 2 | 3 | Metabot V2 bill of materials 4 | 5 | ## ROBOTIS parts 6 | 7 | Name | Quantity | Description 8 | -----------------------|----------|---------------------------------- 9 | XL-320 | 12 | Robot servo-motors 10 | Short Rivets | ~300 | Short rivets 11 | Long Rivets | 48 | Long rivets 12 | Ollo Pulley | 12 | The pulley for the counter-axis 13 | Ollo Tool | 1 | A plastic tool that can be used to handle rivets 14 | 15 | ## Electronics 16 | 17 | Name | Quantity | Description 18 | -----------------------|----------|---------------------------------- 19 | XBoard | 1 | This is the custom [board](/electronics/) of the robot 20 | Maple Mini | 1 | A controller featuring a STM32F103CBT6, with sockets 21 | Mini USB cable | 1 | You'll need it to connect to the Maple Mini 22 | GY-85 IMU | 1 | 9DOF inertial measurement units (accelerometer, gyroscope and magnetometer) 23 | XBee Bluetooth | 1 | You can actually choose any bluetooth chip you want 24 | GP2Y0A21YK | 1 | 10 to 80 cm infrared distance sensor 25 | 26 | ## Batteries 27 | 28 | The XBoard features a JST-BEC female connector, which is compatible with many LiPo battery. 29 | You can actually use a lot of different power sources, but we recommend you the following references: 30 | 31 | Name | Quantity | Description 32 | -----------------------|----------|---------------------------------- 33 | 1000mAh 2S (7.4) LiPo | 1 | A battery, be careful about its geometry 34 | IMAX B3 AC charger | 1 | A cheap and simple to use LiPo charger 35 | 36 | ## Mechanics 37 | 38 | Name | Quantity | Description 39 | -----------------------|----------|---------------------------------- 40 | M4x40 | 12 | Tie the leg tips and the body 41 | M3x8 | 7 | 4 for the magnets to the head, 1 for the IMU and 2 for the GP-2 42 | M3x5 (countersunk) | 4 | Used to tie the magnets to the spacers 43 | M3 nylstop caps | 16 | We recommend nylstop caps 44 | M4 nylstop caps | 12 | We recommend nylstop caps 45 | M3 blind cap nut | 4 | Blind caps are prettiest for the top of the head 46 | Nylon M3 cap | 17 | 8 for the electronics, 1 for the IMU, 2 for the GP-2, 4 for the body spacers, 2 to replace magnets (to avoid magnetic interference) 47 | M3x12 male spacer | 1 | For the IMU 48 | M3x15ish male spacer | 4 | For the electronics 49 | M3x10 | 20 | 16 for the Us, 4 for the electronics 50 | Countersunk magnet (10x3mm, 3mm hole) | 6 | To plug and unplug the head 51 | Googly eyes | 2 | You can paste it on the front on the robot to get it nicer 52 | 53 | Note that you'll need allen 2 and 2.5, and a flat plier or an hex wrench for the nylstop caps 54 | 55 | ## Parts 56 | 57 | For mechanical parts, you have choice between 3D and laser. You can either print the 58 | parts yourself or cut it and bend it. 59 | 60 | ### The 3D option 61 | 62 | Name | Quantity 63 | -----------------------|---------- 64 | [U](/mechanics/3d/u.stl) | 8 65 | [side](/mechanics/3d/side.stl) | 8 66 | [leg](/mechanics/3d/leg.stl) | 4 67 | [body_bottom](/mechanics/3d/body_bottom.stl) | 1 68 | [body_top](/mechanics/3d/body_top.stl) | 1 69 | [head](/mechanics/3d/head.stl) | 1 70 | 71 | ### The laser option 72 | 73 | The parts are provided as DXF, the black layer (0) is cutting and the red one (1) 74 | is 1mm engraving. The target material is 3mm thick PMMA (acrylic). 75 | 76 | A packet version can also be [found here](/mechanics/laser/metabot.dxf). 77 | 78 | Name | Quantity | Comment 79 | -----------------------|----------|------------- 80 | [U](/mechanics/laser/parts/u.dxf) | 8 | [Requires bending](/mechanics/laser/doc/u_bending.pdf) 81 | [side](/mechanics/laser/parts/side.dxf) | 8 | 82 | [leg](/mechanics/laser/parts/leg.dxf) | 4 | 83 | [tip](/mechanics/laser/parts/tip.dxf) | 4 | 84 | [tip_up](/mechanics/laser/parts/tip_up.dxf) | 4 | 85 | [body_bottom](/mechanics/laser/parts/body_bottom.dxf) | 1 | [Requires bending](/mechanics/laser/doc/body_bending.pdf) 86 | [body_top](/mechanics/laser/parts/body_top.dxf) | 1 87 | [head](/mechanics/laser/parts/head.dxf) | 1 | [Required bending](/mechanics/laser/doc/head_bending.pdf) 88 | 89 | 90 | -------------------------------------------------------------------------------- /docs/imgs/all-parts.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/docs/imgs/all-parts.jpg -------------------------------------------------------------------------------- /docs/imgs/bec-solder.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/docs/imgs/bec-solder.jpg -------------------------------------------------------------------------------- /docs/imgs/body.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/docs/imgs/body.jpg -------------------------------------------------------------------------------- /docs/imgs/elec.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/docs/imgs/elec.png -------------------------------------------------------------------------------- /docs/imgs/fixed-us.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/docs/imgs/fixed-us.jpg -------------------------------------------------------------------------------- /docs/imgs/ids.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/docs/imgs/ids.jpg -------------------------------------------------------------------------------- /docs/imgs/kinematic.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/docs/imgs/kinematic.jpg -------------------------------------------------------------------------------- /docs/imgs/metabot.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/docs/imgs/metabot.jpg -------------------------------------------------------------------------------- /docs/imgs/metabots.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/docs/imgs/metabots.png -------------------------------------------------------------------------------- /docs/imgs/motion.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/docs/imgs/motion.png -------------------------------------------------------------------------------- /docs/imgs/motors-us.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/docs/imgs/motors-us.jpg -------------------------------------------------------------------------------- /docs/imgs/one-leg.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/docs/imgs/one-leg.jpg -------------------------------------------------------------------------------- /docs/imgs/opencm-fixations.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/docs/imgs/opencm-fixations.jpg -------------------------------------------------------------------------------- /docs/imgs/opencm-fixed.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/docs/imgs/opencm-fixed.jpg -------------------------------------------------------------------------------- /docs/imgs/opencmpp-board.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/docs/imgs/opencmpp-board.jpg -------------------------------------------------------------------------------- /docs/imgs/opencmpp-mounted.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/docs/imgs/opencmpp-mounted.jpg -------------------------------------------------------------------------------- /docs/imgs/opencmpp-robot.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/docs/imgs/opencmpp-robot.jpg -------------------------------------------------------------------------------- /docs/imgs/openscad.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/docs/imgs/openscad.jpg -------------------------------------------------------------------------------- /docs/imgs/orientation.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/docs/imgs/orientation.jpg -------------------------------------------------------------------------------- /docs/imgs/pulley-rivet.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/docs/imgs/pulley-rivet.jpg -------------------------------------------------------------------------------- /docs/imgs/rivet-u-1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/docs/imgs/rivet-u-1.jpg -------------------------------------------------------------------------------- /docs/imgs/rivet-u-2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/docs/imgs/rivet-u-2.jpg -------------------------------------------------------------------------------- /docs/imgs/robot.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/docs/imgs/robot.jpg -------------------------------------------------------------------------------- /docs/imgs/side.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/docs/imgs/side.jpg -------------------------------------------------------------------------------- /docs/imgs/side2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/docs/imgs/side2.jpg -------------------------------------------------------------------------------- /docs/imgs/src/ids.xcf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/docs/imgs/src/ids.xcf -------------------------------------------------------------------------------- /docs/imgs/src/motion.xcf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/docs/imgs/src/motion.xcf -------------------------------------------------------------------------------- /docs/imgs/src/orientation.xcf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/docs/imgs/src/orientation.xcf -------------------------------------------------------------------------------- /docs/imgs/us-2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/docs/imgs/us-2.jpg -------------------------------------------------------------------------------- /docs/imgs/wire.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/docs/imgs/wire.jpg -------------------------------------------------------------------------------- /docs/imgs/zero-dot.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/docs/imgs/zero-dot.jpg -------------------------------------------------------------------------------- /electronics/.gitignore: -------------------------------------------------------------------------------- 1 | **.*#* 2 | -------------------------------------------------------------------------------- /electronics/README.md: -------------------------------------------------------------------------------- 1 | # Metabot electronics 2 | 3 | ![Electronics](/docs/imgs/elec.png) 4 | 5 | Here are eagle schematics of the XBoard, the electronics inside the Metabot 6 | robot. 7 | 8 | ## BOM 9 | 10 | To assemble the board, you'll need the following components: 11 | 12 | Name | Farnell | Quantity | Description 13 | -----------|---------------|---------------|-------------------------------------- 14 | MOLEX-53253-0370 | 1756921 | 4 | Connector for the motors 15 | CD74HCT367M | 1739593 | 1 | Bus driver 16 | 1x20 0.1" header | 1593469 | 2 | Connector for the maple mini 17 | 1x8 0.1" header | 1593463 | 1 | Connector for the IMU 18 | 1x10 1mm header | 1109732 | 2 | Connector for the XBee 19 | CEM-1203 | - | 1 | Buzzer 20 | BC817 | 1081223RL | 1 | NPN transistor for the buzzer 21 | EEE1CA470SP | 9696946 | 1 | 47uF decoupling capacitor 22 | EEEFK1C221XP | 1850103 | 1 | 220uF decoupling capacitor 23 | TS2940CW-3.3RP | 7261403RL | 1 | 3.3V Regulator 24 | AP1117E50G-13 | 1825292RL | 1 | 5V Regulator 25 | LITTELFUSE 56000001009 | 1185363 | 1 | Fuse holder 26 | Radial Fuse, 3.15A 37013150410 | 1716619 | 1 | Fuse 27 | SUD50N04-8M8P-4GE3 | 1794799 | 1 | This big MOS can blow up the fuse programmatically 28 | JST-BEC connector | - | 1 | Connector for the battery 29 | R0805 22 ohms | - | 2 | 30 | R0805 470 ohms | - | 2 | 31 | R0805 1 Kohms | - | 1 | 32 | R0805 4 Kohms | - | 3 | 33 | R0805 10 Kohms | - | 1 | 34 | R0805 20 Kohms | - | 1 | 35 | C0805 100nF | - | 1 | 36 | 37 | -------------------------------------------------------------------------------- /electronics/xboard.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/electronics/xboard.pdf -------------------------------------------------------------------------------- /electronics/xboard.pro: -------------------------------------------------------------------------------- 1 | EAGLE AutoRouter Statistics: 2 | 3 | Job : /home/gregwar/Circuits/XBoard/xboard.brd 4 | 5 | Start at : 14:45:13 (11/02/16) 6 | End at : 14:45:17 (11/02/16) 7 | Elapsed time : 00:00:04 8 | 9 | Signals : 40 RoutingGrid: 7.87402 mil Layers: 2 10 | Connections : 78 predefined: 0 ( 0 Vias ) 11 | 12 | Router memory : 648016 13 | 14 | Passname : Busses Route Optimize1 Optimize2 Optimize3 Optimize4 15 | 16 | Time per pass : 00:00:00 00:00:01 00:00:01 00:00:00 00:00:01 00:00:01 17 | Number of Ripups : 0 0 0 0 0 0 18 | max. Level : 0 0 0 0 0 0 19 | max. Total : 0 0 0 0 0 0 20 | 21 | Routed : 3 78 78 78 78 78 22 | Vias : 0 114 45 26 19 19 23 | Resolution : 3.8 % 100.0 % 100.0 % 100.0 % 100.0 % 100.0 % 24 | 25 | Final : 100.0% finished 26 | -------------------------------------------------------------------------------- /firmware/README.md: -------------------------------------------------------------------------------- 1 | # Firmware 2 | 3 | The firmware is the code that is loaded on the robot and that generates the 4 | walking gait and talks to the motors. It is currently only adapted for the 5 | original 12-dof Metabot (4 legs, 3 motors per leg). 6 | 7 | ## Putting the firmware on the robot 8 | 9 | ### Linux 10 | 11 | You need to install the following packets: 12 | 13 | ``` 14 | $ apt-get install python python-serial 15 | ``` 16 | 17 | To put the firmware on the robot, simply cd into the `firmware/` repository and 18 | run the installation script: 19 | 20 | ``` 21 | sudo python robotis-loader.py /dev/ttyACM0 metabot2.bin 22 | ``` 23 | 24 | This should reset the board and then output some progress bar, your firmware is now 25 | on the robot! 26 | 27 | ### Windows 28 | 29 | You'll need python and pyserial. Then, be sure to have the drivers. You can install 30 | simply OpenCM IDE for that. 31 | 32 | Then, run: 33 | 34 | ``` 35 | sudo python robotis-loader.py COM4 metabot2.bin 36 | ``` 37 | 38 | Replace COM4 with the right serial port (you can see it in OpenCM ide or in the devices 39 | manager). 40 | 41 | ## The terminal 42 | 43 | ### Connecting to the on-board terminal 44 | 45 | #### Linux 46 | 47 | You can open a serial connection with the robot using `cu` or `screen`. You can use 48 | for instance: 49 | 50 | ``` 51 | you@yourmachine: cu -l /dev/ttyACM0 52 | Connected. 53 | 54 | $ 55 | ``` 56 | 57 | Type "~." to quit cu. 58 | 59 | #### Windows 60 | 61 | On Windows, you can use a serial client such as PuTTy. Just choose "Serial" as connection 62 | mode and enter "COMx", where x is the number of the device. 63 | 64 | You can find the number of the device in the devices manager. 65 | 66 | ## Commands 67 | 68 | ### Basics 69 | 70 | #### `start`: Enable the motors 71 | 72 | This will enable the motors. The LEDs will turn green and the torque will rise 73 | slowly until the robot reaches it default position. 74 | 75 | #### `stop`: Stop the motors 76 | 77 | Turns the LEDs red and shuts the torque down. 78 | 79 | #### `beep [freq] [duration]`: Emit a beep 80 | 81 | This will emit a beep using the internal buzzer, using `freq` Hz and `duration` 82 | ms. 83 | 84 | ### Controller 85 | 86 | Here are commands that can be used to control the motion of the robot 87 | 88 | #### `dx`, `dy`, `turn`: Controls the robot 89 | 90 | These are parameters that control the robot speed. Actually, `dx` and `dy` 91 | control the size of the each step (in mm) and `turn` the angle of each step 92 | (in °). 93 | 94 | #### `freq`: Steps per second 95 | 96 | This is the number of step the robot does on each second. 97 | 98 | #### `h`: Robot height 99 | 100 | This is the (z, in mm) height of the robot while it walks. 101 | 102 | #### `r`: Radius 103 | 104 | This is the radius of the legs (in mm), i.e the distance of the legs to the 105 | center of the body while walking. 106 | 107 | #### `alt`: Legs altitude 108 | 109 | This parameters controls the height the legs are lifted up (in mm) while 110 | walking. 111 | 112 | #### `crab`: Crab position 113 | 114 | This is an angle (in °) that can be positive or negative and allows to get a 115 | crab-like position while walking. 116 | 117 | ## Building, developing & hacking the firmware 118 | 119 | See the [firmware source README](src#firmware-source) 120 | -------------------------------------------------------------------------------- /firmware/metabot.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/firmware/metabot.bin -------------------------------------------------------------------------------- /firmware/metabot.version: -------------------------------------------------------------------------------- 1 | 1.1 2 | -------------------------------------------------------------------------------- /firmware/metabot2.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/firmware/metabot2.bin -------------------------------------------------------------------------------- /firmware/metabot2.version: -------------------------------------------------------------------------------- 1 | 1.0 2 | -------------------------------------------------------------------------------- /firmware/robotis-loader.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | # This script sends a program on a robotis board (OpenCM9.04 or CM900) 4 | # using the robotis bootloader (used in OpenCM IDE) 5 | # 6 | # Usage: 7 | # python robotis-loader.py 8 | # 9 | # Example: 10 | # python robotis-loader.py /dev/ttyACM0 firmware.bin 11 | # 12 | # https://github.com/Gregwar/robotis-loader 13 | 14 | import serial, sys, os, time 15 | 16 | print('~~ Robotis loader ~~') 17 | print('') 18 | 19 | # Reading command line 20 | if len(sys.argv) != 3: 21 | exit('! Usage: robotis-loader.py ') 22 | pgm, port, binary = sys.argv 23 | 24 | # Helper to prints a progress bar 25 | def progressBar(percent, precision=65): 26 | threshold=precision*percent/100.0 27 | sys.stdout.write('[ ') 28 | for x in xrange(0, precision): 29 | if x < threshold: sys.stdout.write('#') 30 | else: sys.stdout.write(' ') 31 | sys.stdout.write(' ] ') 32 | sys.stdout.flush() 33 | 34 | # Opening the firmware file 35 | try: 36 | stat = os.stat(binary) 37 | size = stat.st_size 38 | firmware = file(binary, 'rb') 39 | print('* Opening %s, size=%d' % (binary, size)) 40 | except: 41 | exit('! Unable to open file %s' % binary) 42 | 43 | # Opening serial port 44 | try: 45 | s = serial.Serial(port, baudrate=115200) 46 | except: 47 | exit('! Unable to open serial port %s' % port) 48 | 49 | print('* Resetting the board') 50 | s.setRTS(True) 51 | s.setDTR(False) 52 | time.sleep(0.1) 53 | s.setRTS(False) 54 | s.write('CM9X') 55 | s.close() 56 | time.sleep(1.0); 57 | 58 | print('* Connecting...') 59 | s = serial.Serial(port, baudrate=115200) 60 | s.write('AT&LD') 61 | print('* Download signal transmitted, waiting...') 62 | 63 | # Entering bootloader sequence 64 | while True: 65 | line = s.readline().strip() 66 | if line.endswith('Ready..'): 67 | print('* Board ready, sending data') 68 | cs = 0 69 | pos = 0 70 | while True: 71 | c = firmware.read(2048) 72 | if len(c): 73 | pos += len(c) 74 | sys.stdout.write("\r") 75 | progressBar(100*float(pos)/float(size)) 76 | s.write(c) 77 | for k in range(0,len(c)): 78 | cs = (cs+ord(c[k]))%256 79 | else: 80 | break 81 | print('') 82 | s.setDTR(True) 83 | print('* Checksum: %d' % (cs)) 84 | s.write(chr(cs)) 85 | print('* Firmware was sent') 86 | else: 87 | if line == 'Success..': 88 | print('* Success, running the code') 89 | print('') 90 | s.write('AT&RST') 91 | s.close() 92 | exit() 93 | else: 94 | print('Board -> '+line) 95 | -------------------------------------------------------------------------------- /firmware/src/.gitignore: -------------------------------------------------------------------------------- 1 | RobotsWar 2 | rhock 3 | build 4 | -------------------------------------------------------------------------------- /firmware/src/Makefile: -------------------------------------------------------------------------------- 1 | # Try "make help" first 2 | # 3 | MODEL="metabot2" 4 | VERSION="1.0" 5 | 6 | .DEFAULT_GOAL := sketch 7 | 8 | ## Local configuration 9 | LIB_MAPLE_HOME = ./Maple/LibMaple 10 | LIB_RHOBAN_HOME = ./Maple/LibRhoban 11 | LIB_RHOCK_HOME = ./Maple/LibRhock 12 | ENABLE_RHOCK := yes 13 | ENABLE_COMMANDS := yes 14 | ENABLE_DXL := yes 15 | 16 | ## 17 | ## Useful paths, constants, etc. 18 | ## 19 | 20 | ifeq ($(LIB_MAPLE_HOME),) 21 | SRCROOT := . 22 | else 23 | SRCROOT := $(LIB_MAPLE_HOME) 24 | endif 25 | BUILD_PATH = build 26 | LIBMAPLE_PATH := $(SRCROOT)/libmaple 27 | WIRISH_PATH := $(SRCROOT)/wirish 28 | SUPPORT_PATH := $(SRCROOT)/support 29 | LIBRARIES_PATH := $(SRCROOT)/libraries 30 | # Support files for linker 31 | LDDIR := $(SUPPORT_PATH)/ld 32 | # Support files for this Makefile 33 | MAKEDIR := $(SUPPORT_PATH)/make 34 | BOARD_INCLUDE_DIR := $(MAKEDIR)/board-includes 35 | 36 | ## 37 | ## Target-specific configuration. This determines some compiler and 38 | ## linker options/flags. 39 | ## 40 | 41 | # Try "make help" for more information on BOARD and MEMORY_TARGET; 42 | # these default to a Maple Flash build. 43 | BOARD ?= maple_mini 44 | MEMORY_TARGET ?= flash 45 | 46 | BOOTLOADER ?= robotis 47 | BOOTLOADER_PORT ?= /dev/ttyACM0 48 | 49 | # $(BOARD)- and $(MEMORY_TARGET)-specific configuration 50 | include $(MAKEDIR)/target-config.mk 51 | 52 | ## 53 | ## Build rules and useful templates 54 | ## 55 | 56 | include $(MAKEDIR)/build-rules.mk 57 | include $(MAKEDIR)/build-templates.mk 58 | 59 | ## 60 | ## Compilation flags 61 | ## 62 | 63 | # FIXME: the following allows for deprecated include style, e.g.: 64 | # #include "libmaple.h" 65 | # or 66 | # #include "wirish.h" 67 | # It slows compilation noticeably; remove after 1 release. 68 | TARGET_FLAGS += -I$(LIBMAPLE_PATH)/include/libmaple \ 69 | -I$(WIRISH_PATH)/include/wirish \ 70 | -I$(LIB_RHOBAN_HOME) \ 71 | -I$(LIB_RHOCK_HOME) 72 | TARGET_FLAGS += -I$(LIBRARIES_PATH) # for internal lib. includes, e.g. 73 | GLOBAL_CFLAGS := -Os -g3 -gdwarf-2 -nostdlib \ 74 | -ffunction-sections -fdata-sections \ 75 | -Wl,--gc-sections $(TARGET_FLAGS) \ 76 | -DBOOTLOADER_$(BOOTLOADER) \ 77 | -DMETABOT_VERSION="\"$(VERSION)\"" 78 | GLOBAL_CXXFLAGS := -fno-rtti -fno-exceptions -Wall $(TARGET_FLAGS) 79 | GLOBAL_ASFLAGS := -x assembler-with-cpp $(TARGET_FLAGS) 80 | LDFLAGS = $(TARGET_LDFLAGS) $(TOOLCHAIN_LDFLAGS) -mcpu=cortex-m3 -mthumb \ 81 | -Xlinker --gc-sections \ 82 | -Xassembler --march=armv7-m -Wall 83 | # -Xlinker --print-gc-sections \ 84 | 85 | ## 86 | ## Set all submodules here 87 | ## 88 | 89 | LIBMAPLE_MODULES += $(SRCROOT)/libmaple 90 | LIBMAPLE_MODULES += $(SRCROOT)/libmaple/usb # The USB module is kept separate 91 | LIBMAPLE_MODULES += $(LIBMAPLE_MODULE_SERIES) # STM32 series submodule in libmaple 92 | LIBMAPLE_MODULES += $(SRCROOT)/wirish 93 | 94 | # Official libraries: 95 | #LIBMAPLE_MODULES += $(SRCROOT)/libraries/Servo 96 | #LIBMAPLE_MODULES += $(SRCROOT)/libraries/LiquidCrystal 97 | #LIBMAPLE_MODULES += $(SRCROOT)/libraries/Wire 98 | # Experimental libraries: 99 | # LIBMAPLE_MODULES += $(SRCROOT)/libraries/FreeRTOS 100 | 101 | # User modules: 102 | ifneq ($(USER_MODULES),) 103 | LIBMAPLE_MODULES += $(USER_MODULES) 104 | endif 105 | 106 | # Rhoban lib 107 | LIBMAPLE_MODULES += $(LIB_RHOBAN_HOME) 108 | 109 | ifeq ($(ENABLE_RHOCK),yes) 110 | LIBMAPLE_MODULES += $(LIB_RHOCK_HOME) 111 | endif 112 | 113 | # Call each module's rules.mk: 114 | $(foreach m,$(LIBMAPLE_MODULES),$(eval $(call LIBMAPLE_MODULE_template,$(m)))) 115 | 116 | ## 117 | ## Targets 118 | ## 119 | 120 | # main target 121 | include build-targets.mk 122 | 123 | .PHONY: install sketch clean help cscope tags ctags ram flash jtag doxygen mrproper list-boards 124 | 125 | # Target upload commands 126 | # USB ID for DFU upload -- FIXME: do something smarter with this 127 | BOARD_USB_VENDOR_ID := 1eaf 128 | BOARD_USB_PRODUCT_ID := 0003 129 | 130 | ifeq ($(BOOTLOADER),maple) 131 | UPLOAD_ram := $(SUPPORT_PATH)/scripts/reset.py && \ 132 | sleep 1 && \ 133 | $(DFU) -a0 -d $(BOARD_USB_VENDOR_ID):$(BOARD_USB_PRODUCT_ID) -D $(BUILD_PATH)/$(BOARD).bin -R 134 | UPLOAD_flash := $(SUPPORT_PATH)/scripts/reset.py && \ 135 | sleep 1 && \ 136 | $(DFU) -a1 -d $(BOARD_USB_VENDOR_ID):$(BOARD_USB_PRODUCT_ID) -D $(BUILD_PATH)/$(BOARD).bin -R 137 | endif 138 | 139 | ifeq ($(BOOTLOADER),robotis) 140 | UPLOAD_flash := $(SUPPORT_PATH)/scripts/robotis-loader.py $(BOOTLOADER_PORT) $(BUILD_PATH)/$(BOARD).bin 141 | endif 142 | 143 | # Conditionally upload to whatever the last build was 144 | install: INSTALL_TARGET = $(shell cat $(BUILD_PATH)/build-type 2>/dev/null) 145 | install: $(BUILD_PATH)/$(BOARD).bin 146 | @echo "Install target:" $(INSTALL_TARGET) 147 | $(UPLOAD_$(INSTALL_TARGET)) 148 | 149 | release: $(BUILD_PATH)/$(BOARD).bin 150 | @echo "Copying to metabot.bin" 151 | @echo $(VERSION) > ../$(MODEL).version 152 | cp $(BUILD_PATH)/$(BOARD).bin ../$(MODEL).bin 153 | 154 | # Force a rebuild if the target changed 155 | PREV_BUILD_TYPE = $(shell cat $(BUILD_PATH)/build-type 2>/dev/null) 156 | build-check: 157 | ifneq ($(PREV_BUILD_TYPE), $(MEMORY_TARGET)) 158 | $(shell rm -rf $(BUILD_PATH);mkdir $(BUILD_PATH)) 159 | endif 160 | 161 | sketch: build-check MSG_INFO $(BUILD_PATH)/$(BOARD).bin 162 | 163 | clean: 164 | rm -rf build LibMaple LibRhoban LibRhock Rhock 165 | 166 | mrproper: clean 167 | rm -rf doxygen 168 | 169 | help: 170 | @echo "" 171 | @echo "Basic usage (BOARD defaults to maple_mini):" 172 | @echo " $$ cp your-main.cpp main.cpp" 173 | @echo " $$ make BOARD=your_board" 174 | @echo " $$ make BOARD=your_board install" 175 | @echo "" 176 | @echo "(Multiple source files? Link with libmaple.a (\`$$ make library')" 177 | @echo "or hack build-targets.mk appropriately.)" 178 | @echo "" 179 | @echo "Important targets:" 180 | @echo " sketch: Compile for BOARD to MEMORY_TARGET (default)." 181 | @echo " install: Compile and upload over USB using Maple bootloader" 182 | @echo "" 183 | @echo "You *must* set BOARD if not compiling for Maple (e.g." 184 | @echo "use BOARD=maple_mini for mini, etc.), and MEMORY_TARGET" 185 | @echo "if not compiling to Flash. Run \`$$ make list-boards' for" 186 | @echo "a list of all boards." 187 | @echo "" 188 | @echo "Valid MEMORY_TARGETs (default=flash):" 189 | @echo " ram: Compile to RAM (doesn't touch Flash)" 190 | @echo " flash: Compile to Flash (for Maple bootloader)" 191 | @echo " jtag: Compile for JTAG/SWD upload (overwrites bootloader)" 192 | @echo "" 193 | @echo "Other targets:" 194 | @echo " clean: Remove all build and object files" 195 | @echo " doxygen: Build Doxygen HTML and XML documentation" 196 | @echo " help: Show this message" 197 | @echo " mrproper: Remove all generated files" 198 | @echo "" 199 | 200 | cscope: 201 | rm -rf cscope.* 202 | find . -name '*.[hcS]' -o -name '*.cpp' | xargs cscope -b 203 | 204 | tags: 205 | etags `find . -name "*.c" -o -name "*.cpp" -o -name "*.h"` 206 | @echo "Made TAGS file for EMACS code browsing" 207 | 208 | ctags: 209 | ctags-exuberant -R . 210 | @echo "Made tags file for VIM code browsing" 211 | 212 | ram: 213 | @$(MAKE) MEMORY_TARGET=ram --no-print-directory sketch 214 | 215 | flash: 216 | @$(MAKE) MEMORY_TARGET=flash --no-print-directory sketch 217 | 218 | jtag: 219 | @$(MAKE) MEMORY_TARGET=jtag --no-print-directory sketch 220 | 221 | doxygen: 222 | doxygen $(SUPPORT_PATH)/doxygen/Doxyfile 223 | 224 | # This output is kind of ugly, but I don't understand make very well. 225 | list-boards: 226 | @echo " $(addsuffix "\\n",$(basename $(notdir $(wildcard $(BOARD_INCLUDE_DIR)/*.mk))))" 227 | -------------------------------------------------------------------------------- /firmware/src/README.md: -------------------------------------------------------------------------------- 1 | # Firmware source 2 | 3 | ## Building, developing & hacking the firmware 4 | 5 | See the [firmware source](src/README.md) 6 | 7 | You should install the arm cross-compilation toolchain: 8 | 9 | ``` 10 | sudo apt-get install libstdc++-arm-none-eabi-newlib gcc-arm-none-eabi libnewlib-arm-none-eabi binutils-arm-none-eabi 11 | ``` 12 | 13 | The firmware source is in the `src/` directory. To build it, you'll first have to run the 14 | prepare script to get the Maple framework: 15 | 16 | ``` 17 | ./prepare.sh 18 | ``` 19 | 20 | Then, run `make`: 21 | 22 | ``` 23 | make 24 | ``` 25 | 26 | You can use `make install` to send directly the firmware on the robot: 27 | 28 | ``` 29 | make install 30 | ``` 31 | 32 | ## Creating behaviors 33 | 34 | We already designed a simple architecture with few examples that can be found in the 35 | firmware in order to create behaviors for the robot. 36 | 37 | First, edit `behavior.h` and change the `BEHAVIOR_ENABLE` constant to `1`. Then, edit 38 | `behavior.cpp`, you can read the examples functions and uncomment the one(s) you like 39 | in the `behavior_tick` method. 40 | 41 | This will be executed when the robot is started (i.e when the `start` command is sent and 42 | after the motors are enabled). Feel free to edit this to create your own, custom behavior. 43 | -------------------------------------------------------------------------------- /firmware/src/behavior.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "imu.h" 4 | #include "distance.h" 5 | #include "motion.h" 6 | #include "buzzer.h" 7 | #include "behavior.h" 8 | #include "leds.h" 9 | 10 | TERMINAL_PARAMETER_INT(bhv, "Behavior", 0); 11 | 12 | void behavior_set(uint8_t b) 13 | { 14 | bhv = b; 15 | } 16 | 17 | /** 18 | * Here you can write down your own behaviour, the tick method (at the end of 19 | * this file) is called with a given dt (in s) that you can use for temporal 20 | * counters. 21 | * 22 | * You will find some examples that you can enable by uncommenting the method 23 | * calls below. The buzzer, leds, motors and sensors are used in theses examples, 24 | * allowing you to understand how to recover informations from the different 25 | * robot hardware features. 26 | */ 27 | 28 | /** 29 | * Behavior example 30 | * Simply blinks the led green and blue (0.5ms each) 31 | */ 32 | void behavior_blink_leds(float dt) 33 | { 34 | static float t = 0; 35 | t += dt; 36 | if (t > 1) t -= 1; 37 | 38 | if (t < 0.5) { 39 | led_set_all(LED_G, true); 40 | } else { 41 | led_set_all(LED_B, true); 42 | } 43 | } 44 | 45 | /** 46 | * Behavior example 47 | * Produce a +-30mm sinus on the height of the robot, it will 48 | * then appear to breath 49 | */ 50 | void behavior_sinus_height(float dt) 51 | { 52 | static float t = 0; 53 | t += dt; 54 | 55 | motion_set_h(30*sin(t)); 56 | } 57 | 58 | /** 59 | * Behavior example 60 | * The robot explore, it walks forward and turn around when there is an 61 | * obstacle in the front of it 62 | */ 63 | void behavior_explore(float dt) 64 | { 65 | static int state = 0; 66 | static float t = 0; 67 | t += dt; 68 | 69 | if (state == 0) { 70 | // Walking forward 71 | motion_set_x_speed(150); 72 | motion_set_turn_speed(0); 73 | 74 | // There is an obstacle, changing state 75 | if (distance_get() < 30) { 76 | t = 0; 77 | state = 1; 78 | // Produce a beep 79 | buzzer_beep(440, 100); 80 | } 81 | } else if (state == 1) { 82 | // Turning around 83 | motion_set_x_speed(0); 84 | motion_set_turn_speed(90); 85 | 86 | // 2s have elapsed (at 90°/s), going back to explore 87 | if (t > 2) { 88 | state = 0; 89 | } 90 | } 91 | } 92 | 93 | /** 94 | * Behavior example 95 | * The robot walks forward. If it is grabbed and tilted more than 15°, it will 96 | * stop walking and scream. 97 | */ 98 | void behavior_grab(float dt) 99 | { 100 | motion_set_x_speed(150); 101 | 102 | if (fabs(imu_pitch()) > 15 || fabs(imu_roll()) > 15) { 103 | buzzer_beep(200, 100); 104 | motion_set_x_speed(0); 105 | } 106 | } 107 | 108 | /** 109 | * Behavior example 110 | * The robot will turn around scanning for the longest distance, then it will 111 | * walk a little in this direction and start again. 112 | */ 113 | void behavior_scan_space(float dt) 114 | { 115 | static int state = 0; 116 | static float t = 0; 117 | static float best_yaw = 0; 118 | static float best_dist = 0; 119 | 120 | t += dt; 121 | 122 | if (state == 0) { 123 | // Scanning around 124 | motion_set_x_speed(0); 125 | motion_set_turn_speed(90); 126 | float dist = distance_get(); 127 | if (dist > best_dist) { 128 | buzzer_beep(800, 50); 129 | best_dist = dist; 130 | best_yaw = imu_yaw(); 131 | } 132 | if (t > 4) { 133 | state = 1; 134 | } 135 | } else if (state == 1) { 136 | // Aligning with the best seen yaw 137 | float err = imu_yaw()-best_yaw; 138 | float order = err; 139 | if (order > 90) order = 90; 140 | if (order > 0 && order < 30) order = 30; 141 | if (order < -90) order = -90; 142 | if (order < 0 && order > -30) order = -30; 143 | 144 | motion_set_x_speed(0); 145 | motion_set_turn_speed(order); 146 | if (fabs(err) < 6) { 147 | state = 2; 148 | t = 0; 149 | } 150 | } else if (state == 2) { 151 | // Walking a little 152 | motion_set_x_speed(189); 153 | motion_set_turn_speed(0); 154 | 155 | if (t > 2) { 156 | state = 0; 157 | t = 0; 158 | best_dist = 0; 159 | } 160 | } 161 | } 162 | 163 | void behavior_tick(float dt) 164 | { 165 | static int lastBhv = 0; 166 | 167 | if (bhv == 1) behavior_blink_leds(dt); 168 | if (bhv == 2) behavior_sinus_height(dt); 169 | if (bhv == 3) behavior_explore(dt); 170 | if (bhv == 4) behavior_grab(dt); 171 | if (bhv == 5) behavior_scan_space(dt); 172 | 173 | if (bhv == 0 && lastBhv != 0) { 174 | leds_decustom(); 175 | motion_set_x_speed(0); 176 | motion_set_y_speed(0); 177 | motion_set_turn_speed(0); 178 | motion_reset(); 179 | buzzer_stop(); 180 | } 181 | lastBhv = bhv; 182 | } 183 | -------------------------------------------------------------------------------- /firmware/src/behavior.h: -------------------------------------------------------------------------------- 1 | #ifndef _BEHAVIOR_H 2 | #define _BEHAVIOR_H 3 | 4 | #include 5 | 6 | void behavior_set(uint8_t b); 7 | void behavior_tick(float t); 8 | 9 | #endif 10 | -------------------------------------------------------------------------------- /firmware/src/bt.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "bt.h" 4 | 5 | #define BTCONF_PIN 20 6 | 7 | void bt_init() 8 | { 9 | pinMode(BTCONF_PIN, OUTPUT); 10 | digitalWrite(BTCONF_PIN, LOW); 11 | 12 | #ifdef BT_HC05 13 | RC.begin(38400); 14 | for (int k=0; k<5; k++) { 15 | RC.write("AT+RESET\r\n\r\n"); 16 | RC.write("AT+RESET\r\n"); 17 | } 18 | #endif 19 | 20 | #ifdef BT_HM12 21 | RC.write("AT+RESET"); delay(10); 22 | #endif 23 | 24 | RC.begin(BT_BAUD); 25 | } 26 | 27 | static void goToConf() 28 | { 29 | #ifdef BT_HC05 30 | digitalWrite(BTCONF_PIN, LOW); 31 | delay(100); 32 | digitalWrite(BTCONF_PIN, HIGH); 33 | #endif 34 | } 35 | 36 | static void bt_conf(char *name, char *pin) 37 | { 38 | #ifdef BT_HC05 39 | goToConf(); 40 | RC.write("AT\r\r"); 41 | delay(150); 42 | RC.write("AT+UART=921600,0,0\r\r"); 43 | delay(150); 44 | RC.write("AT+NAME="); 45 | RC.write(name); 46 | RC.write("\r\r"); 47 | delay(150); 48 | RC.write("AT+PSWD="); 49 | RC.write(pin); 50 | delay(150); 51 | RC.write("\r\r"); 52 | RC.write("AT+RESET\r\r"); 53 | delay(150); 54 | #endif 55 | 56 | #ifdef BT_HM12 57 | RC.write("AT+NAME"); 58 | RC.write(name); delay(100); 59 | RC.write("AT+NAMB"); 60 | RC.write(name); 61 | RC.write("-BLE"); delay(100); 62 | RC.write("AT+PINE"); 63 | RC.write(pin); delay(100); 64 | RC.write("AT+BAUD7"); delay(100); 65 | RC.write("AT+RESET"); delay(100); 66 | #endif 67 | } 68 | 69 | void bt_set_config(char *name, char *pin) 70 | { 71 | for (int k=0; k<3; k++) { 72 | RC.begin(9600); 73 | for (int n=0; n<3; n++) bt_conf(name, pin); 74 | #ifdef BT_HC05 75 | RC.begin(38400); 76 | for (int n=0; n<3; n++) bt_conf(name, pin); 77 | #endif 78 | RC.begin(BT_BAUD); 79 | for (int n=0; n<3; n++) bt_conf(name, pin); 80 | } 81 | } 82 | 83 | TERMINAL_COMMAND(btconf, "Bluetooth config") 84 | { 85 | if (argc != 2) { 86 | terminal_io()->println("Usage: btconf "); 87 | } else { 88 | char *name = argv[0]; 89 | char *pin = argv[1]; 90 | terminal_io()->println("Configuring bluetooth to name:"); 91 | terminal_io()->println(name); 92 | terminal_io()->println("And pin:"); 93 | terminal_io()->println(pin); 94 | 95 | bt_set_config(name, pin); 96 | } 97 | } 98 | 99 | TERMINAL_COMMAND(btpulse, "BT conf pulse") 100 | { 101 | goToConf(); 102 | } 103 | -------------------------------------------------------------------------------- /firmware/src/bt.h: -------------------------------------------------------------------------------- 1 | #ifndef _BT_H 2 | #define _BT_H 3 | 4 | #define BT_HC05 5 | // #define BT_HM12 6 | 7 | #if defined(BT_HC05) 8 | #define BT_BAUD 921600 9 | #elif defined(BT_HM12) 10 | #define BT_BAUD 230400 11 | #else 12 | #error "You must choose a BT chip" 13 | #endif 14 | 15 | /** 16 | * Initializes the bluetooth 17 | */ 18 | void bt_init(); 19 | 20 | /** 21 | * Configures the bluetooth (name=device name, pin=pair pin) 22 | */ 23 | void bt_set_config(char *name, char *pin); 24 | 25 | #endif 26 | -------------------------------------------------------------------------------- /firmware/src/build-targets.mk: -------------------------------------------------------------------------------- 1 | # Project targets 2 | # Defines here your cpp source files 3 | # Ex : main.cpp test.cpp ... 4 | SRC_FILES = main.cpp kinematic.cpp motion.cpp leds.cpp mapping.cpp motors.cpp \ 5 | buzzer.cpp imu.cpp distance.cpp voltage.cpp bt.cpp cubic.cpp behavior.cpp 6 | 7 | ifeq ($(ENABLE_RHOCK),yes) 8 | SRC_FILES += rhock-functions.cpp rhock-stream.cpp 9 | endif 10 | 11 | # Uncomment to disable robot campus commands 12 | CFLAGS += -DHAS_DXL -DHAS_TERMINAL -DDISABLE_SERVOS_COMMANDS 13 | # CFLAGS += -DDXL_VERSION_1 14 | 15 | OBJ_FILES_CPP = $(SRC_FILES:.cpp=.o) 16 | OBJ_FILES = $(addprefix $(BUILD_PATH)/,$(OBJ_FILES_CPP:.c=.o)) 17 | 18 | $(BUILD_PATH)/%.o: %.cpp 19 | $(SILENT_CXX) $(CXX) $(CFLAGS) $(CXXFLAGS) $(LIBMAPLE_INCLUDES) $(WIRISH_INCLUDES) -o $@ -c $< 20 | 21 | $(BUILD_PATH)/libmaple.a: $(BUILDDIRS) $(TGT_BIN) 22 | - rm -f $@ 23 | $(AR) crv $(BUILD_PATH)/libmaple.a $(TGT_BIN) 24 | 25 | library: $(BUILD_PATH)/libmaple.a 26 | 27 | .PHONY: library 28 | 29 | $(BUILD_PATH)/$(BOARD).elf: $(BUILDDIRS) $(TGT_BIN) $(OBJ_FILES) 30 | $(SILENT_LD) $(CXX) $(LDFLAGS) -o $@ $(TGT_BIN) $(OBJ_FILES) -Wl,-Map,$(BUILD_PATH)/$(BOARD).map 31 | 32 | $(BUILD_PATH)/$(BOARD).bin: $(BUILD_PATH)/$(BOARD).elf 33 | $(SILENT_OBJCOPY) $(OBJCOPY) -v -Obinary $(BUILD_PATH)/$(BOARD).elf $@ 1>/dev/null 34 | $(SILENT_DISAS) $(DISAS) -d $(BUILD_PATH)/$(BOARD).elf > $(BUILD_PATH)/$(BOARD).disas 35 | @echo " " 36 | @echo "Object file sizes:" 37 | @find $(BUILD_PATH) -iname "*.o" | xargs $(SIZE) -t > $(BUILD_PATH)/$(BOARD).sizes 38 | @cat $(BUILD_PATH)/$(BOARD).sizes 39 | @echo " " 40 | @echo "Final Size:" 41 | @$(SIZE) $< 42 | @echo $(MEMORY_TARGET) > $(BUILD_PATH)/build-type 43 | 44 | $(BUILDDIRS): 45 | @mkdir -p $@ 46 | 47 | MSG_INFO: 48 | @echo "================================================================================" 49 | @echo "" 50 | @echo " Build info:" 51 | @echo " BOARD: " $(BOARD) 52 | @echo " MCU: " $(MCU) 53 | @echo " MEMORY_TARGET: " $(MEMORY_TARGET) 54 | @echo "" 55 | @echo " See 'make help' for all possible targets" 56 | @echo "" 57 | @echo "================================================================================" 58 | @echo "" 59 | -------------------------------------------------------------------------------- /firmware/src/buzzer.cpp: -------------------------------------------------------------------------------- 1 | #include "buzzer.h" 2 | #include "voltage.h" 3 | #ifdef HAS_TERMINAL 4 | #include 5 | #endif 6 | 7 | // Config 8 | #ifndef __EMSCRIPTEN__ 9 | HardwareTimer timer(2); 10 | #endif 11 | 12 | #ifdef __EMSCRIPTEN__ 13 | #include 14 | #include 15 | unsigned int millis() 16 | { 17 | return EM_ASM_INT({ 18 | return (new Date()).getTime()%0xffffffff; 19 | }, 0); 20 | } 21 | #endif 22 | 23 | // Partitions 24 | struct buzzer_note { 25 | unsigned int freq; 26 | unsigned int duration; 27 | }; 28 | 29 | static struct buzzer_note melody_boot[] = { 30 | {523, 200/2}, 31 | {659, 350/2}, 32 | {523, 200/2}, 33 | {698, 300/2}, 34 | {659, 160/2}, 35 | {0, 0} 36 | }; 37 | 38 | static struct buzzer_note melody_alert[] = { 39 | {2000, 200}, 40 | {200, 200}, 41 | {2000, 200}, 42 | {200, 200}, 43 | {0, 0} 44 | }; 45 | 46 | static struct buzzer_note melody_alert_fast[] = { 47 | {2000, 100}, 48 | {200, 100}, 49 | {2000, 100}, 50 | {200, 100}, 51 | {0, 0} 52 | }; 53 | 54 | static struct buzzer_note melody_warning[] = { 55 | {800, 200}, 56 | {400, 200}, 57 | {200, 0}, 58 | {200, 400}, 59 | {0, 0} 60 | }; 61 | 62 | static struct buzzer_note melody_begin[] = { 63 | {800, 200}, 64 | {0, 200}, 65 | {800, 200}, 66 | {0, 0}, 67 | }; 68 | 69 | static struct buzzer_note melody_custom[] = { 70 | {0, 0}, 71 | {0, 0} 72 | }; 73 | 74 | // Status 75 | static struct buzzer_note *melody; 76 | static struct buzzer_note *melody_repeat; 77 | static int melody_st; 78 | 79 | #ifdef __EMSCRIPTEN__ 80 | static int playing_note = 0; 81 | 82 | int buzzer_get_frequency() 83 | { 84 | return playing_note; 85 | } 86 | 87 | EMSCRIPTEN_BINDINGS(buzzer) { 88 | emscripten::function("buzzer_get_frequency", &buzzer_get_frequency); 89 | } 90 | #endif 91 | 92 | void buzzer_init() 93 | { 94 | #ifndef __EMSCRIPTEN__ 95 | melody = NULL; 96 | pinMode(BUZZER_PIN, PWM); 97 | pwmWrite(BUZZER_PIN, 0); 98 | #endif 99 | } 100 | 101 | void buzzer_play_note(int note) 102 | { 103 | #ifdef __EMSCRIPTEN__ 104 | playing_note = note; 105 | #else 106 | timer.pause(); 107 | timer.setPrescaleFactor(72000000 / (note * 100)); 108 | timer.setOverflow(100); 109 | 110 | if (note == 0) { 111 | pinMode(BUZZER_PIN, OUTPUT); 112 | digitalWrite(BUZZER_PIN, LOW); 113 | } else { 114 | timer.refresh(); 115 | timer.resume(); 116 | pinMode(BUZZER_PIN, PWM); 117 | pwmWrite(BUZZER_PIN, 50); 118 | } 119 | #endif 120 | } 121 | 122 | static void buzzer_enter(struct buzzer_note *note) 123 | { 124 | buzzer_play_note(note->freq); 125 | melody = note; 126 | melody_st = millis(); 127 | 128 | if (note->freq == 0 && note->duration == 0) { 129 | if (melody_repeat != NULL) { 130 | buzzer_enter(melody_repeat); 131 | } else { 132 | melody = NULL; 133 | } 134 | } 135 | } 136 | 137 | void buzzer_play(unsigned int melody_num, bool repeat) 138 | { 139 | // Avoid playing another melody when there is a battery alert 140 | #ifndef __EMSCRIPTEN__ 141 | if (voltage_error() && melody_num != MELODY_ALERT && melody_num != MELODY_ALERT_FAST) { 142 | return; 143 | } 144 | #endif 145 | 146 | struct buzzer_note *to_play = NULL; 147 | 148 | if (melody_num == MELODY_BOOT) { 149 | to_play = &melody_boot[0]; 150 | } else if (melody_num == MELODY_ALERT) { 151 | to_play = &melody_alert[0]; 152 | } else if (melody_num == MELODY_ALERT_FAST) { 153 | to_play = &melody_alert_fast[0]; 154 | } else if (melody_num == MELODY_WARNING) { 155 | to_play = &melody_warning[0]; 156 | } else if (melody_num == MELODY_BEGIN) { 157 | to_play = &melody_begin[0]; 158 | } else if (melody_num == MELODY_CUSTOM) { 159 | to_play = &melody_custom[0]; 160 | } else { 161 | melody = NULL; 162 | } 163 | 164 | if (to_play) { 165 | melody_repeat = repeat ? to_play : NULL; 166 | buzzer_enter(to_play); 167 | } 168 | } 169 | 170 | void buzzer_tick() 171 | { 172 | if (melody != NULL) { 173 | if (millis()-melody_st > melody->duration) { 174 | buzzer_enter(melody+1); 175 | } 176 | } 177 | } 178 | 179 | void buzzer_stop() 180 | { 181 | if (voltage_error()) { 182 | return; 183 | } 184 | 185 | buzzer_play_note(0); 186 | melody = NULL; 187 | melody_repeat = NULL; 188 | } 189 | 190 | bool buzzer_is_playing() 191 | { 192 | return melody != NULL; 193 | } 194 | 195 | void buzzer_wait_play() 196 | { 197 | while (buzzer_is_playing()) { 198 | buzzer_tick(); 199 | } 200 | } 201 | 202 | void buzzer_beep(unsigned int freq, unsigned int duration) 203 | { 204 | melody_custom[0].freq = freq; 205 | melody_custom[0].duration = duration; 206 | buzzer_play(MELODY_CUSTOM); 207 | } 208 | 209 | #ifdef HAS_TERMINAL 210 | TERMINAL_COMMAND(play, "Play a melody") 211 | { 212 | int melnum = atoi(argv[0]); 213 | terminal_io()->print("Playing melody "); 214 | terminal_io()->print(melnum); 215 | terminal_io()->println(); 216 | buzzer_play(melnum); 217 | } 218 | 219 | TERMINAL_COMMAND(beep, "Plays a beep") 220 | { 221 | if (argc == 2) { 222 | buzzer_beep(atoi(argv[0]), atoi(argv[1])); 223 | } else if (argc == 1) { 224 | buzzer_beep(atoi(argv[0]), 1000); 225 | } else { 226 | terminal_io()->println("Usage: beep freq [duration]"); 227 | } 228 | } 229 | #endif 230 | -------------------------------------------------------------------------------- /firmware/src/buzzer.h: -------------------------------------------------------------------------------- 1 | #ifndef _BUZZER_H 2 | #define _BUZZER_H 3 | 4 | #define BUZZER_PIN 11 5 | 6 | // When the robot starts 7 | #define MELODY_BOOT 0 8 | // When the battery is low 9 | #define MELODY_ALERT 1 10 | #define MELODY_ALERT_FAST 2 11 | // When there is a warning 12 | #define MELODY_WARNING 3 13 | // When we start the ID of the motors 14 | #define MELODY_BEGIN 4 15 | // A custom melody used by beep 16 | #define MELODY_CUSTOM 5 17 | 18 | /** 19 | * Initializes the buzzer 20 | */ 21 | void buzzer_init(); 22 | 23 | /** 24 | * Plays a melody 25 | * @param melody The melody id (MELODY_*) 26 | * @param repeat Does the melody repeats continuously? 27 | */ 28 | void buzzer_play(unsigned int melody, bool repeat=false); 29 | 30 | /** 31 | * Stops playing any sound 32 | */ 33 | void buzzer_stop(); 34 | 35 | /** 36 | * Ticking the buzzer 37 | */ 38 | void buzzer_tick(); 39 | 40 | /** 41 | * Is the buzzer plaing? 42 | */ 43 | bool buzzer_is_playing(); 44 | 45 | /** 46 | * Wait the end of the play 47 | */ 48 | void buzzer_wait_play(); 49 | 50 | /** 51 | * Plays a beep 52 | * @param freq The frequency (Hz) 53 | * @param duration The duration (ms) 54 | */ 55 | void buzzer_beep(unsigned int freq, unsigned int duration); 56 | 57 | #endif 58 | -------------------------------------------------------------------------------- /firmware/src/config.h: -------------------------------------------------------------------------------- 1 | #ifdef HAS_DXL 2 | #include 3 | #endif 4 | 5 | // #define PROTOTYPE_LASER 6 | // #define PROTOTYPE_3D 7 | 8 | #if defined(PROTOTYPE_LASER) 9 | #define L0 62.5 10 | #define L1 46 11 | #define L2 56 12 | #define L3_1 90 13 | #define L3_2 44 14 | 15 | #elif defined(PROTOTYPE_3D) 16 | #define L0 61.5 17 | #define L1 46 18 | #define L2 59 19 | #define L3_1 85 20 | #define L3_2 44 21 | 22 | #else 23 | /** 24 | * This is the dimension of the joints, in mm 25 | */ 26 | #define L0 61 27 | #define L1 46 28 | #define L2 60 29 | #define L3_1 85 30 | #define L3_2 42 31 | #endif 32 | 33 | /** 34 | * Here we can tweak the robot odometry parameters, to adjust the speeds with 35 | * actually measured ones 36 | */ 37 | #ifdef __EMSCRIPTEN__ 38 | #define ODOMETRY_TRANSLATION 1.0 39 | #define ODOMETRY_ROTATION 1.0 40 | #else 41 | #define ODOMETRY_TRANSLATION 1.2 42 | #define ODOMETRY_ROTATION 1.0 43 | #endif 44 | 45 | /** 46 | * Servos are supposed to be like this: 47 | * 48 | * 2nd leg 3 49 | * | 2 <--- 1st leg 50 | * v 1 51 | * 6-5-4 10-11-11 52 | * 7 ^ 53 | * 3rd leg-> 8 | 54 | * 9 4th leg 55 | * 56 | * But you can change these IDs using the below array: 57 | */ 58 | static int servos_order[] = { 59 | // First leg 60 | 1, 2, 3, 61 | // Second leg 62 | 4, 5, 6, 63 | // Third leg 64 | 7, 8, 9, 65 | // Fourth leg 66 | 10, 11, 12 67 | }; 68 | 69 | /** 70 | * You can also change the signs of the servos if you mount 71 | * it backward for instance 72 | * 73 | * Change the 1 to -1 if the servos are backwards 74 | */ 75 | #define SIGN_A 1 76 | #define SIGN_B 1 77 | #define SIGN_C 1 78 | 79 | #ifdef HAS_DXL 80 | /** 81 | * This is the servos configuration, you can change the zero 82 | * or the min/max limits here 83 | */ 84 | static void config_init() __attribute__ ((unused)); 85 | 86 | static void config_init() 87 | { 88 | // First leg 89 | dxl_set_zero (servos_order[0], 0.00); 90 | dxl_set_min_max (servos_order[0], -90, 90); 91 | dxl_set_zero (servos_order[1], 0.00); 92 | dxl_set_min_max (servos_order[1], -100, 100); 93 | dxl_set_zero (servos_order[2], 0.00); 94 | dxl_set_min_max (servos_order[2], -145, 145); 95 | 96 | // Second leg 97 | dxl_set_zero (servos_order[3], 0.00); 98 | dxl_set_min_max (servos_order[3], -90, 90); 99 | dxl_set_zero (servos_order[4], 0.00); 100 | dxl_set_min_max (servos_order[4], -100, 100); 101 | dxl_set_zero (servos_order[5], 0.00); 102 | dxl_set_min_max (servos_order[5], -145, 145); 103 | 104 | // Third leg 105 | dxl_set_zero (servos_order[6], 0.00); 106 | dxl_set_min_max (servos_order[6], -90, 90); 107 | dxl_set_zero (servos_order[7], 0.00); 108 | dxl_set_min_max (servos_order[7], -100, 100); 109 | dxl_set_zero (servos_order[8], 0.00); 110 | dxl_set_min_max (servos_order[8], -145, 145); 111 | 112 | // Fourth leg 113 | dxl_set_zero (servos_order[9], 0.00); 114 | dxl_set_min_max (servos_order[9], -90, 90); 115 | dxl_set_zero (servos_order[10], 0.00); 116 | dxl_set_min_max (servos_order[10], -100, 100); 117 | dxl_set_zero (servos_order[11], 0); 118 | dxl_set_min_max (servos_order[11], -145, 145); 119 | } 120 | #endif 121 | -------------------------------------------------------------------------------- /firmware/src/cubic.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "cubic.h" 4 | 5 | Cubic::Cubic() : nbPoints(0) 6 | { 7 | size = CUBIC_DEFAULT_POINTS; 8 | points = (Point *)malloc(size*sizeof(Point)); 9 | polynoms = (Polynom *)malloc(size*sizeof(Polynom)); 10 | } 11 | 12 | Cubic::~Cubic() 13 | { 14 | free(points); 15 | free(polynoms); 16 | } 17 | 18 | void Cubic::checkSize() 19 | { 20 | if (nbPoints+1 >= size) { 21 | size *= 2; 22 | points = (Point *)realloc(points, size*sizeof(Point)); 23 | polynoms = (Polynom *)realloc(polynoms, size*sizeof(Polynom)); 24 | } 25 | } 26 | 27 | void Cubic::clear() 28 | { 29 | nbPoints = 0; 30 | } 31 | 32 | float Cubic::getXMax() 33 | { 34 | if (nbPoints == 0) { 35 | return 0.0; 36 | } 37 | 38 | return points[nbPoints-1].x; 39 | } 40 | 41 | void Cubic::addPoint(float x, float y, float yp) 42 | { 43 | checkSize(); 44 | 45 | Point point; 46 | point.x = x; 47 | point.y = y; 48 | point.yp = yp; 49 | points[nbPoints] = point; 50 | 51 | if (nbPoints > 0) { 52 | Point previous = points[nbPoints-1]; 53 | Polynom p; 54 | p.a = 2*previous.y + previous.yp - 2*y + yp; 55 | p.b = -3*previous.y - 2*previous.yp + 3*y - yp; 56 | p.c = previous.yp; 57 | p.d = previous.y; 58 | polynoms[nbPoints-1] = p; 59 | } 60 | 61 | nbPoints++; 62 | } 63 | 64 | float Cubic::get(float x) 65 | { 66 | int i; 67 | 68 | for (i=0; i= x) { 70 | break; 71 | } 72 | } 73 | 74 | if (i == 0 || i == nbPoints) { 75 | return 0.0; 76 | } 77 | 78 | Point A = points[i-1]; 79 | Point B = points[i]; 80 | Polynom f = polynoms[i-1]; 81 | x = (x-A.x)/(B.x-A.x); 82 | 83 | float tx = x; 84 | float result = f.d; 85 | result += f.c * tx; 86 | tx *= x; 87 | result += f.b * tx; 88 | tx *= x; 89 | result += f.a * tx; 90 | 91 | return result; 92 | } 93 | 94 | float Cubic::getMod(float x) 95 | { 96 | float maxX = getXMax(); 97 | 98 | if (x < 0.0 || x > maxX) { 99 | x -= maxX*(int)(x/maxX); 100 | } 101 | 102 | return get(x); 103 | } 104 | 105 | 106 | #undef POINT_X 107 | #undef POINT_Y 108 | -------------------------------------------------------------------------------- /firmware/src/cubic.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROBOTCAMPUS_CUBIC_H 2 | #define _ROBOTCAMPUS_CUBIC_H 3 | 4 | #ifndef CUBIC_DEFAULT_POINTS 5 | #define CUBIC_DEFAULT_POINTS 16 6 | #endif 7 | 8 | /** 9 | * Cubic splines 10 | */ 11 | class Cubic 12 | { 13 | public: 14 | Cubic(); 15 | ~Cubic(); 16 | 17 | /** 18 | * Add a point (x, y) to the function 19 | */ 20 | void addPoint(float x, float y, float yp); 21 | 22 | /** 23 | * Gets the xMax of the function 24 | */ 25 | float getXMax(); 26 | 27 | /** 28 | * Get the value for the given x 29 | */ 30 | float get(float x); 31 | 32 | /** 33 | * Gets the value for the given x, modulo the size of the function 34 | */ 35 | float getMod(float x); 36 | 37 | /** 38 | * Checks if the function can contain one more element 39 | */ 40 | void checkSize(); 41 | 42 | void clear(); 43 | 44 | protected: 45 | struct Point 46 | { 47 | float x, y, yp; 48 | }; 49 | 50 | /** 51 | * Cubic points 52 | */ 53 | Point *points; 54 | 55 | /** 56 | * Number of points 57 | */ 58 | int nbPoints; 59 | 60 | /** 61 | * Array size 62 | */ 63 | int size; 64 | 65 | struct Polynom 66 | { 67 | float a, b, c, d; 68 | }; 69 | 70 | /** 71 | * The nth item of this array is the slope between the nth point and the 72 | * n+1th point 73 | */ 74 | Polynom *polynoms; 75 | }; 76 | 77 | #endif 78 | -------------------------------------------------------------------------------- /firmware/src/distance.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "distance.h" 5 | #include 6 | 7 | #define DISTANCE_PIN 5 8 | 9 | Function volt_to_cm; 10 | static float value; 11 | static int lastMeasure; 12 | 13 | float distance_measure() 14 | { 15 | float voltage = analogRead(DISTANCE_PIN)*3.3/4096; 16 | return volt_to_cm.get(voltage); 17 | } 18 | 19 | void distance_init() 20 | { 21 | pinMode(DISTANCE_PIN, INPUT); 22 | 23 | // From the datasheet of GP2Y0A21YK 24 | // http://www.sharpsma.com/webfm_send/1208 25 | volt_to_cm.addPoint(0.0, 100); 26 | volt_to_cm.addPoint(0.4, 80); 27 | volt_to_cm.addPoint(0.45, 70); 28 | volt_to_cm.addPoint(0.5, 60); 29 | volt_to_cm.addPoint(0.6, 50); 30 | volt_to_cm.addPoint(0.75, 40); 31 | volt_to_cm.addPoint(0.92, 30); 32 | volt_to_cm.addPoint(1.3, 20); 33 | volt_to_cm.addPoint(1.65, 15); 34 | volt_to_cm.addPoint(2.3, 10); 35 | volt_to_cm.addPoint(2.9, 7); 36 | volt_to_cm.addPoint(3.15, 6.0); 37 | volt_to_cm.addPoint(3.3, 6.0); 38 | 39 | // From the datasheet of GP2Y0A41SK0F 40 | // http://www.sharp-world.com/products/device/lineup/data/pdf/datasheet/gp2y0a41sk_e.pdf 41 | // Yop, I measured pixels.... 42 | /* 43 | float PX2V = 3.0/402.0; 44 | volt_to_cm.addPoint(0.0, 50); 45 | volt_to_cm.addPoint(PX2V*42.5, 40); 46 | volt_to_cm.addPoint(PX2V*50, 35); 47 | volt_to_cm.addPoint(PX2V*60, 30); 48 | volt_to_cm.addPoint(PX2V*72.5, 25); 49 | volt_to_cm.addPoint(PX2V*90, 20); 50 | volt_to_cm.addPoint(PX2V*100, 18); 51 | volt_to_cm.addPoint(PX2V*110, 16); 52 | volt_to_cm.addPoint(PX2V*125, 14); 53 | volt_to_cm.addPoint(PX2V*142.5, 12); 54 | volt_to_cm.addPoint(PX2V*172.5, 10); 55 | volt_to_cm.addPoint(PX2V*188, 9); 56 | volt_to_cm.addPoint(PX2V*210, 8); 57 | volt_to_cm.addPoint(PX2V*238, 7); 58 | volt_to_cm.addPoint(PX2V*275, 6); 59 | volt_to_cm.addPoint(PX2V*315, 5); 60 | volt_to_cm.addPoint(PX2V*370, 4); 61 | volt_to_cm.addPoint(PX2V*408, 3); 62 | */ 63 | 64 | value = distance_measure(); 65 | } 66 | 67 | float distance_get() 68 | { 69 | return value; 70 | } 71 | 72 | void distance_tick() 73 | { 74 | if (millis()-lastMeasure > 10) { 75 | lastMeasure = millis(); 76 | value = 0.9*value + 0.1*distance_measure(); 77 | } 78 | } 79 | 80 | TERMINAL_COMMAND(dist, "Monitor distances") 81 | { 82 | bool continuous = (argc > 0); 83 | 84 | do { 85 | terminal_io()->println(distance_get()); 86 | distance_tick(); 87 | delay(10); 88 | } while (continuous && !SerialUSB.available()); 89 | } 90 | -------------------------------------------------------------------------------- /firmware/src/distance.h: -------------------------------------------------------------------------------- 1 | #ifndef __DISTANCE_H 2 | #define __DISTANCE_H 3 | 4 | /** 5 | * Initializes the distance sensor 6 | */ 7 | void distance_init(); 8 | 9 | /** 10 | * Reads the distance sensor (returns a value in cm) 11 | */ 12 | float distance_get(); 13 | 14 | /** 15 | * Updates the value 16 | */ 17 | void distance_tick(); 18 | 19 | #endif 20 | -------------------------------------------------------------------------------- /firmware/src/imu.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "imu.h" 6 | #include "motion.h" 7 | 8 | static bool initialized = false; 9 | 10 | #define I2C_TIMEOUT 2 11 | 12 | int32 i2c_master_xfer_reinit(i2c_dev *dev, 13 | i2c_msg *msgs, 14 | uint16 num, 15 | uint32 timeout) 16 | { 17 | int32 r = i2c_master_xfer(dev, msgs, num, timeout); 18 | if (r != 0 || dev->state != I2C_STATE_IDLE) { 19 | initialized = false; 20 | } 21 | return r; 22 | } 23 | 24 | static bool calibrated; 25 | static int last_update; 26 | float magn_x, magn_y, magn_z; 27 | float gyro_x, gyro_y, gyro_z; 28 | float acc_x, acc_y, acc_z; 29 | 30 | TERMINAL_PARAMETER_FLOAT(yaw, "Robot yaw", 0.0); 31 | TERMINAL_PARAMETER_FLOAT(pitch, "Robot pitch", 0.0); 32 | TERMINAL_PARAMETER_FLOAT(roll, "Robot roll", 0.0); 33 | TERMINAL_PARAMETER_FLOAT(gyro_yaw, "Robot gyro yaw", 0.0); 34 | 35 | TERMINAL_PARAMETER_BOOL(imudbg, "Debug the IMU", false); 36 | 37 | // Addresses 38 | #define MAGN_ADDR 0x1e 39 | #define GYRO_ADDR 0x68 40 | #define ACC_ADDR 0x53 41 | 42 | // Config 43 | float MAGN_X_MIN=-0.5; 44 | float MAGN_X_MAX=0.5; 45 | float MAGN_Y_MIN=-0.5; 46 | float MAGN_Y_MAX=0.5; 47 | float MAGN_Z_MIN=-0.5; 48 | float MAGN_Z_MAX=0.5; 49 | 50 | /* 51 | #define MAGN_X_MIN -180 52 | #define MAGN_X_MAX 0 53 | #define MAGN_Y_MIN -0.5 54 | #define MAGN_Y_MAX 0.5 55 | #define MAGN_Z_MIN -320 56 | #define MAGN_Z_MAX -105 57 | */ 58 | 59 | #define MAGN_X_CENTER ((MAGN_X_MIN+MAGN_X_MAX)/2.0) 60 | #define MAGN_X_AMP (MAGN_X_MAX-MAGN_X_MIN) 61 | #define MAGN_Y_CENTER ((MAGN_Y_MIN+MAGN_Y_MAX)/2.0) 62 | #define MAGN_Y_AMP (MAGN_Y_MAX-MAGN_Y_MIN) 63 | #define MAGN_Z_CENTER ((MAGN_Z_MIN+MAGN_Z_MAX)/2.0) 64 | #define MAGN_Z_AMP (MAGN_Z_MAX-MAGN_Z_MIN) 65 | 66 | #define GYRO_GAIN 0.06957 67 | 68 | #define DEG2RAD(x) ((x)*M_PI/180.0) 69 | #define RAD2DEG(x) ((x)*180.0/M_PI) 70 | 71 | // Signing 72 | #define VALUE_SIGN(value, length) \ 73 | ((value < (1<<(length-1))) ? \ 74 | (value) \ 75 | : (value-(1< 180) angle -= 360; 102 | while (angle < -180) angle += 360; 103 | 104 | return angle; 105 | } 106 | 107 | float weight_average(float a1, float w1, float a2, float w2) 108 | { 109 | float x = w1*cos(a1) + w2*cos(a2); 110 | float y = w1*sin(a1) + w2*sin(a2); 111 | 112 | return atan2(y, x); 113 | } 114 | 115 | void imu_init() 116 | { 117 | yaw = 0.0; 118 | last_update = millis(); 119 | 120 | // Initializing values 121 | magn_x = magn_y = magn_z = 0; 122 | 123 | // Initializing I2C bus 124 | i2c_init(I2C1); 125 | i2c_master_enable(I2C1, I2C_FAST_MODE); 126 | 127 | // Initializing magnetometer 128 | packet.addr = MAGN_ADDR; 129 | packet.flags = 0; 130 | packet.data = magn_continuous; 131 | packet.length = 2; 132 | if (i2c_master_xfer_reinit(I2C1, &packet, 1, I2C_TIMEOUT) != 0) goto init_error; 133 | 134 | packet.data = magn_50hz; 135 | if (i2c_master_xfer_reinit(I2C1, &packet, 1, I2C_TIMEOUT) != 0) goto init_error; 136 | 137 | packet.data = magn_sens; 138 | if (i2c_master_xfer_reinit(I2C1, &packet, 1, I2C_TIMEOUT) != 0) goto init_error; 139 | 140 | // Initializing accelerometer 141 | packet.addr = ACC_ADDR; 142 | packet.flags = 0; 143 | packet.data = acc_measure; 144 | packet.length = 2; 145 | if (i2c_master_xfer_reinit(I2C1, &packet, 1, I2C_TIMEOUT) != 0) goto init_error; 146 | 147 | packet.data = acc_resolution; 148 | if (i2c_master_xfer_reinit(I2C1, &packet, 1, I2C_TIMEOUT) != 0) goto init_error; 149 | 150 | packet.data = acc_50hz; 151 | if (i2c_master_xfer_reinit(I2C1, &packet, 1, I2C_TIMEOUT) != 0) goto init_error; 152 | 153 | // Initializing gyroscope 154 | packet.addr = GYRO_ADDR; 155 | packet.flags = 0; 156 | packet.data = gyro_reset; 157 | packet.length = 2; 158 | if (i2c_master_xfer_reinit(I2C1, &packet, 1, I2C_TIMEOUT) != 0) goto init_error; 159 | 160 | packet.data = gyro_scale; 161 | if (i2c_master_xfer_reinit(I2C1, &packet, 1, I2C_TIMEOUT) != 0) goto init_error; 162 | packet.data = gyro_50hz; 163 | if (i2c_master_xfer_reinit(I2C1, &packet, 1, I2C_TIMEOUT) != 0) goto init_error; 164 | packet.data = gyro_pll; 165 | if (i2c_master_xfer_reinit(I2C1, &packet, 1, I2C_TIMEOUT) != 0) goto init_error; 166 | 167 | initialized = true; 168 | return; 169 | 170 | init_error: 171 | initialized = false; 172 | } 173 | 174 | static bool calibrating = false; 175 | static bool first = false; 176 | static float calibrating_t = -1; 177 | 178 | void magn_update() 179 | { 180 | if (!initialized) return; 181 | 182 | packet.addr = MAGN_ADDR; 183 | packet.flags = 0; 184 | packet.data = magn_req; 185 | packet.length = 1; 186 | if (i2c_master_xfer_reinit(I2C1, &packet, 1, I2C_TIMEOUT) != 0) return; 187 | 188 | char buffer[6]; 189 | packet.flags = I2C_MSG_READ; 190 | packet.data = (uint8*)buffer; 191 | packet.length = 6; 192 | if (i2c_master_xfer_reinit(I2C1, &packet, 1, I2C_TIMEOUT) != 0) return; 193 | 194 | int magn_x_r = ((buffer[0]&0xff)<<8)|(buffer[1]&0xff); 195 | int magn_y_r = ((buffer[2]&0xff)<<8)|(buffer[3]&0xff); 196 | int magn_z_r = ((buffer[4]&0xff)<<8)|(buffer[5]&0xff); 197 | magn_x_r = VALUE_SIGN(magn_x_r, 16); 198 | magn_y_r = VALUE_SIGN(magn_y_r, 16); 199 | magn_z_r = VALUE_SIGN(magn_z_r, 16); 200 | 201 | if (calibrating) { 202 | if (first) { 203 | first = false; 204 | MAGN_X_MIN = MAGN_X_MAX = magn_x_r; 205 | MAGN_Y_MIN = MAGN_Y_MAX = magn_y_r; 206 | MAGN_Z_MIN = MAGN_Z_MAX = magn_z_r; 207 | } else { 208 | if (magn_x_r > MAGN_X_MAX) MAGN_X_MAX = magn_x_r; 209 | if (magn_x_r < MAGN_X_MIN) MAGN_X_MIN = magn_x_r; 210 | if (magn_y_r > MAGN_Y_MAX) MAGN_Y_MAX = magn_y_r; 211 | if (magn_y_r < MAGN_Y_MIN) MAGN_Y_MIN = magn_y_r; 212 | if (magn_z_r > MAGN_Z_MAX) MAGN_Z_MAX = magn_z_r; 213 | if (magn_z_r < MAGN_Z_MIN) MAGN_Z_MIN = magn_z_r; 214 | } 215 | } else { 216 | magn_x = (magn_x_r-MAGN_X_CENTER)/(float)MAGN_X_AMP; 217 | magn_y = (magn_y_r-MAGN_Y_CENTER)/(float)MAGN_Y_AMP; 218 | magn_z = (magn_z_r-MAGN_Z_CENTER)/(float)MAGN_Z_AMP; 219 | } 220 | 221 | if (calibrated) { 222 | float new_yaw = -atan2(magn_z, magn_x); 223 | float cur_yaw = DEG2RAD(yaw); 224 | yaw = RAD2DEG(weight_average(new_yaw, 0.01, cur_yaw, 0.99)); 225 | } else { 226 | yaw = gyro_yaw; 227 | } 228 | } 229 | 230 | void gyro_update() 231 | { 232 | if (!initialized) return; 233 | 234 | packet.addr = GYRO_ADDR; 235 | packet.flags = 0; 236 | packet.data = gyro_req; 237 | packet.length = 1; 238 | if (i2c_master_xfer_reinit(I2C1, &packet, 1, I2C_TIMEOUT) != 0) return; 239 | 240 | char buffer[6]; 241 | packet.flags = I2C_MSG_READ; 242 | packet.data = (uint8*)buffer; 243 | packet.length = 6; 244 | if (i2c_master_xfer_reinit(I2C1, &packet, 1, I2C_TIMEOUT) != 0) return; 245 | 246 | int gyro_x_r = ((buffer[0]&0xff)<<8)|(buffer[1]&0xff); 247 | gyro_x = GYRO_GAIN*VALUE_SIGN(gyro_x_r, 16); 248 | int gyro_y_r = ((buffer[2]&0xff)<<8)|(buffer[3]&0xff); 249 | gyro_y = GYRO_GAIN*VALUE_SIGN(gyro_y_r, 16); 250 | int gyro_z_r = ((buffer[4]&0xff)<<8)|(buffer[5]&0xff); 251 | gyro_z = GYRO_GAIN*VALUE_SIGN(gyro_z_r, 16); 252 | 253 | yaw += gyro_z * 0.02; 254 | yaw = normalize(yaw); 255 | 256 | gyro_yaw += gyro_z * 0.02; 257 | gyro_yaw = normalize(gyro_yaw); 258 | } 259 | 260 | void acc_update() 261 | { 262 | if (!initialized) return; 263 | 264 | packet.addr = ACC_ADDR; 265 | packet.flags = 0; 266 | packet.data = acc_req; 267 | packet.length = 1; 268 | if (i2c_master_xfer_reinit(I2C1, &packet, 1, I2C_TIMEOUT) != 0) return; 269 | 270 | char buffer[6]; 271 | packet.flags = I2C_MSG_READ; 272 | packet.data = (uint8*)buffer; 273 | packet.length = 6; 274 | if (i2c_master_xfer_reinit(I2C1, &packet, 1, I2C_TIMEOUT) != 0) return; 275 | 276 | int acc_x_r = ((buffer[1]&0xff)<<8)|(buffer[0]&0xff); 277 | acc_x = VALUE_SIGN(acc_x_r, 16); 278 | int acc_y_r = ((buffer[3]&0xff)<<8)|(buffer[2]&0xff); 279 | acc_y = VALUE_SIGN(acc_y_r, 16); 280 | int acc_z_r = ((buffer[5]&0xff)<<8)|(buffer[4]&0xff); 281 | acc_z = VALUE_SIGN(acc_z_r, 16); 282 | 283 | float new_roll = atan2(acc_x, acc_z); 284 | float new_pitch = -atan2(acc_y, sqrt(acc_x*acc_x + acc_z*acc_z)); 285 | 286 | pitch = RAD2DEG(weight_average(new_pitch, 0.05, DEG2RAD(pitch), 0.95)); 287 | roll = RAD2DEG(weight_average(new_roll, 0.05, DEG2RAD(roll), 0.95)); 288 | } 289 | 290 | void imu_tick() 291 | { 292 | int elapsed = millis()-last_update; 293 | 294 | // Every 20ms 295 | if (elapsed > 20) { 296 | last_update += 20; 297 | 298 | if (initialized) { 299 | gyro_update(); 300 | magn_update(); 301 | acc_update(); 302 | } else { 303 | imu_init(); 304 | } 305 | 306 | if (calibrating) { 307 | if (calibrating_t >= 0) { 308 | calibrating_t += 0.02; 309 | if (calibrating_t > 12) { 310 | motion_set_turn_speed(0); 311 | imu_calib_stop(); 312 | } 313 | } 314 | } 315 | 316 | if (imudbg) { 317 | terminal_io()->print(magn_x); 318 | terminal_io()->print(" "); 319 | terminal_io()->print(magn_y); 320 | terminal_io()->print(" "); 321 | terminal_io()->print(magn_z); 322 | terminal_io()->print(" "); 323 | 324 | terminal_io()->print(gyro_x); 325 | terminal_io()->print(" "); 326 | terminal_io()->print(gyro_y); 327 | terminal_io()->print(" "); 328 | terminal_io()->print(gyro_z); 329 | terminal_io()->print(" "); 330 | 331 | terminal_io()->print(acc_x); 332 | terminal_io()->print(" "); 333 | terminal_io()->print(acc_y); 334 | terminal_io()->print(" "); 335 | terminal_io()->print(acc_z); 336 | terminal_io()->print(" "); 337 | 338 | terminal_io()->print(gyro_yaw); 339 | terminal_io()->print(" "); 340 | 341 | terminal_io()->print(yaw); 342 | terminal_io()->print(" "); 343 | 344 | terminal_io()->println(); 345 | } 346 | } 347 | } 348 | 349 | TERMINAL_COMMAND(calib, "Calibrates the IMU") 350 | { 351 | calibrating_t = -1; 352 | if (!calibrating && argc) { 353 | imu_calib_start(); 354 | terminal_io()->println("Started calibration"); 355 | } else if (calibrating && !argc) { 356 | imu_calib_stop(); 357 | 358 | terminal_io()->println("Calibration over"); 359 | terminal_io()->println("X: "); 360 | terminal_io()->print(MAGN_X_MIN); 361 | terminal_io()->print(" -> "); 362 | terminal_io()->print(MAGN_X_MAX); 363 | terminal_io()->println(); 364 | 365 | terminal_io()->println("Y: "); 366 | terminal_io()->print(MAGN_Y_MIN); 367 | terminal_io()->print(" -> "); 368 | terminal_io()->print(MAGN_Y_MAX); 369 | terminal_io()->println(); 370 | 371 | terminal_io()->println("Z: "); 372 | terminal_io()->print(MAGN_Z_MIN); 373 | terminal_io()->print(" -> "); 374 | terminal_io()->print(MAGN_Z_MAX); 375 | terminal_io()->println(); 376 | } else { 377 | terminal_io()->println("Usage: calib start, then calib"); 378 | } 379 | } 380 | 381 | void imu_calib_start() 382 | { 383 | calibrating = true; 384 | first = true; 385 | } 386 | 387 | void imu_calib_stop() 388 | { 389 | calibrated = true; 390 | calibrating = false; 391 | } 392 | 393 | void imu_calib_rotate() 394 | { 395 | imu_calib_start(); 396 | motion_set_turn_speed(60); 397 | calibrating_t = 0.1; 398 | } 399 | 400 | float imu_yaw() 401 | { 402 | return yaw; 403 | } 404 | 405 | float imu_pitch() 406 | { 407 | return pitch; 408 | } 409 | 410 | float imu_roll() 411 | { 412 | return roll; 413 | } 414 | 415 | TERMINAL_COMMAND(calibrot, "Calibrating rotation") 416 | { 417 | imu_calib_rotate(); 418 | } 419 | -------------------------------------------------------------------------------- /firmware/src/imu.h: -------------------------------------------------------------------------------- 1 | #ifndef _IMU_H 2 | #define _IMU_H 3 | 4 | extern float magn_x, magn_y, magn_z; 5 | extern float gyro_x, gyro_y, gyro_z; 6 | extern float acc_x, acc_y, acc_z; 7 | 8 | /** 9 | * Initializes the IMU 10 | */ 11 | void imu_init(); 12 | 13 | /** 14 | * Ticks the IMU 15 | */ 16 | void imu_tick(); 17 | 18 | /** 19 | * Starts the calibration 20 | */ 21 | void imu_calib_start(); 22 | 23 | /** 24 | * Starts the rotating calibration 25 | */ 26 | void imu_calib_rotate(); 27 | 28 | /** 29 | * Stops the calibration 30 | */ 31 | void imu_calib_stop(); 32 | 33 | /** 34 | * Computed IMU yaw 35 | */ 36 | float imu_yaw(); 37 | 38 | /** 39 | * Computed IMU pitch 40 | */ 41 | float imu_pitch(); 42 | 43 | /** 44 | * Computed IMU roll 45 | */ 46 | float imu_roll(); 47 | 48 | #endif 49 | -------------------------------------------------------------------------------- /firmware/src/js/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8) 2 | project(metabot) 3 | 4 | option (JS "JavaScript mode" ON) 5 | 6 | # Directories 7 | set (BASE "${CMAKE_SOURCE_DIR}/..") 8 | set (RH_BASE "${BASE}/Maple/LibRhoban/") 9 | set (RHOCK_BASE "${BASE}/Maple/Rhock/src") 10 | 11 | # Includes 12 | include_directories (${RHOCK_BASE} ${RH_BASE}) 13 | 14 | # Sources 15 | set (SOURCES 16 | ${BASE}/kinematic.cpp 17 | ${BASE}/motion.cpp 18 | ${BASE}/leds.cpp 19 | ${BASE}/buzzer.cpp 20 | ${BASE}/motors.cpp 21 | ${BASE}/mapping.cpp 22 | ${BASE}/rhock-functions.cpp 23 | ${BASE}/rhock-stream.cpp 24 | ${BASE}/cubic.cpp 25 | ${RH_BASE}/function.cpp 26 | ) 27 | 28 | # Building rhock 29 | add_subdirectory ("${RHOCK_BASE}" rhock) 30 | 31 | # Compilation options 32 | set(CMAKE_C_COMPILER emcc) 33 | set(CMAKE_CXX_COMPILER em++) 34 | set(CMAKE_AR emar) 35 | set(CMAKE_RANLIB emranlib) 36 | set(CMAKE_C_FLAGS -g) 37 | set(CMAKE_CXX_FLAGS -g) 38 | set (CMAKE_C_FLAGS "--minify 0 -Os -std=c99 -Wall -U__STRICT_ANSI__ -Wno-warn-absolute-paths") 39 | set (CMAKE_CXX_FLAGS "--minify 0 -Os --bind -Wall -Wno-reorder -pedantic -U__STRICT_ANSI__ -Wno-warn-absolute-paths -s DISABLE_EXCEPTION_CATCHING=0") 40 | 41 | add_definitions (-DRHOCK_NATIVE_SYMBOLS -DRHOCK) 42 | 43 | # Produce the module 44 | add_executable (module.js ${SRC_METABOT} ${RHOCK_JS} ${SOURCES}) 45 | 46 | add_custom_target (module.min.js 47 | COMMAND uglifyjs module.js > module.min.js 48 | DEPENDS module.js 49 | ) 50 | 51 | add_custom_target (module.tgz 52 | COMMAND tar zcvf module.tgz module.min.js 53 | DEPENDS module.min.js 54 | ) 55 | -------------------------------------------------------------------------------- /firmware/src/kinematic.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "kinematic.h" 3 | 4 | /** 5 | * Solves the al Kashi problem 6 | * 7 | * This gives the angles of a triangle side, knowing 8 | * all the lengths of its sides. 9 | */ 10 | float alKashi(float a, float b, float c) 11 | { 12 | return acos(((a*a)+(b*b)-(c*c))/(2*a*b)); 13 | } 14 | 15 | /** 16 | * Compute the kinematics of the robot 17 | */ 18 | bool computeIK(float x, float y, float z, 19 | float *a, float *b, float *c, 20 | float l1, float l2, float l3) 21 | { 22 | // Alpha is simply the angle of the leg in the X/Y plane 23 | float alpha = atan2(y, x); 24 | 25 | // Distance between end of the leg and arm of the second motor, 26 | // in the X/Y plane 27 | float xp = sqrt(x*x+y*y)-l1; 28 | if (xp < 0) { 29 | xp = 0; 30 | } 31 | 32 | // Distance between second motor arm and the end of the leg, 33 | // in the plane of the leg 34 | float d = sqrt(pow(xp,2) + pow(z,2)); 35 | if (d > l2+l3) { 36 | d = l2+l3; 37 | } 38 | 39 | // Knowing l2, l3 and d beta and gamma can be computed using 40 | // the Al Kashi law 41 | float beta = alKashi(l2, d, l3) - atan2(-z, xp); 42 | float gamma = M_PI - alKashi(l2, l3, d); 43 | 44 | if (!isnan(alpha) && !isnan(beta) && !isnan(gamma)) { 45 | *a = RAD2DEG(alpha); 46 | *b = RAD2DEG(beta); 47 | *c = RAD2DEG(gamma); 48 | return true; 49 | } else { 50 | return false; 51 | } 52 | } 53 | 54 | void legFrame(float X, float Y, float *x, float *y, int leg, float l0) 55 | { 56 | switch (leg) { 57 | case 0: 58 | *x = cos(0*M_PI/2-M_PI/4)*X - sin(0*M_PI/2-M_PI/4)*Y - l0; 59 | *y = sin(0*M_PI/2-M_PI/4)*X + cos(0*M_PI/2-M_PI/4)*Y; 60 | break; 61 | case 1: 62 | *x = cos(1*M_PI/2-M_PI/4)*X - sin(1*M_PI/2-M_PI/4)*Y - l0; 63 | *y = sin(1*M_PI/2-M_PI/4)*X + cos(1*M_PI/2-M_PI/4)*Y; 64 | break; 65 | case 2: 66 | *x = cos(2*M_PI/2-M_PI/4)*X - sin(2*M_PI/2-M_PI/4)*Y - l0; 67 | *y = sin(2*M_PI/2-M_PI/4)*X + cos(2*M_PI/2-M_PI/4)*Y; 68 | break; 69 | case 3: 70 | *x = cos(3*M_PI/2-M_PI/4)*X - sin(3*M_PI/2-M_PI/4)*Y - l0; 71 | *y = sin(3*M_PI/2-M_PI/4)*X + cos(3*M_PI/2-M_PI/4)*Y; 72 | break; 73 | } 74 | } 75 | -------------------------------------------------------------------------------- /firmware/src/kinematic.h: -------------------------------------------------------------------------------- 1 | #ifndef _IK_H 2 | #define _IK_H 3 | 4 | // Math util 5 | #define MAX(a,b) (((a)>(b))?(a):(b)) 6 | #define MIN(a,b) (((a)<(b))?(a):(b)) 7 | #define RAD2DEG(a) ((a)*180.0/M_PI) 8 | #define DEG2RAD(a) ((a)*M_PI/180.0) 9 | 10 | /** 11 | * Computes the inverse kinematics for one leg 12 | * 13 | * Input x, y and z are the targe cartesian position, 14 | * in the referencial of the leg. The z axis is upward, 15 | * the x axis is alighed with the "0" angle of the first 16 | * servo. 17 | * 18 | * Output are a, b and c, angles for the three servos, 19 | * the first controls the yaw of the leg, through the 20 | * motor fixed on the body, and b and c controls the 21 | * length of the leg and the position of the end of 22 | * the leg. 23 | * 24 | * l1, l2 and l3 are the length between the first motor 25 | * arm and the second motor arm, between the second and the 26 | * third motor arm, and between the third motor arm and the 27 | * end of the leg. 28 | * 29 | * c 30 | * l2 / \ 31 | * / \ l3 32 | * body-a--l1--b \ 33 | * \ 34 | * - 35 | * (x, y, z) end of the leg 36 | * 37 | */ 38 | bool computeIK(float x, float y, float z, 39 | float *a, float *b, float *c, 40 | float l1, float l2, float l3); 41 | 42 | /** 43 | * Obtain the x/y position of a point in the frame of the leg from 44 | * x/y positions in the frame of the body 45 | */ 46 | void legFrame(float X, float Y, 47 | float *x, float *y, int leg, float l0); 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /firmware/src/leds.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "leds.h" 3 | #ifdef RHOCK 4 | #include 5 | #endif 6 | #ifndef __EMSCRIPTEN__ 7 | #include 8 | #endif 9 | 10 | static char leds[12]; 11 | static bool leds_custom_flag; 12 | 13 | static int led_value_to_dxl(int val) 14 | { 15 | char dxlv = 0; 16 | if (val & LED_R) dxlv |= 1; 17 | if (val & LED_G) dxlv |= 2; 18 | if (val & LED_B) dxlv |= 4; 19 | return dxlv; 20 | } 21 | 22 | bool leds_are_custom() 23 | { 24 | return leds_custom_flag; 25 | } 26 | 27 | void leds_decustom() 28 | { 29 | leds_custom_flag = false; 30 | } 31 | 32 | void led_set(int index, int value, bool custom) 33 | { 34 | if (custom) { 35 | leds_custom_flag = true; 36 | } 37 | leds[index-1] = value; 38 | #ifndef __EMSCRIPTEN__ 39 | dxl_write_byte(index, DXL_LED, led_value_to_dxl(value)); 40 | #endif 41 | } 42 | 43 | void led_set_all(int value, bool custom) 44 | { 45 | if (custom) { 46 | leds_custom_flag = true; 47 | } 48 | for (unsigned int i=0; i 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include "motors.h" 13 | #include "voltage.h" 14 | #include "buzzer.h" 15 | #include "distance.h" 16 | #include "config.h" 17 | #include "motion.h" 18 | #include "leds.h" 19 | #include "mapping.h" 20 | #include "behavior.h" 21 | #include "imu.h" 22 | #include "bt.h" 23 | 24 | bool isUSB = false; 25 | 26 | // Time 27 | TERMINAL_PARAMETER_FLOAT(t, "Time", 0.0); 28 | 29 | TERMINAL_COMMAND(version, "Getting firmware version") 30 | { 31 | terminal_io()->print("version="); 32 | terminal_io()->println(METABOT_VERSION); 33 | } 34 | 35 | TERMINAL_COMMAND(started, "Is the robot started?") 36 | { 37 | terminal_io()->print("started="); 38 | terminal_io()->println(started); 39 | } 40 | 41 | TERMINAL_COMMAND(rc, "Go to RC mode") 42 | { 43 | RC.begin(BT_BAUD); 44 | terminal_init(&RC); 45 | isUSB = false; 46 | } 47 | 48 | // Enabling/disabling move 49 | TERMINAL_PARAMETER_BOOL(move, "Enable/Disable move", true); 50 | 51 | // This destroys the fuse (used in dev) 52 | /* 53 | TERMINAL_COMMAND(suicide, "Lit the fuse") 54 | { 55 | digitalWrite(LIT, HIGH); 56 | } 57 | */ 58 | 59 | // Setting the flag, called @50hz 60 | bool flag = false; 61 | void setFlag() 62 | { 63 | flag = true; 64 | } 65 | 66 | /** 67 | * Checks wether there is enough voltage to start the motors 68 | */ 69 | bool can_start() 70 | { 71 | if (voltage_current() < 6 || voltage_error()) { 72 | buzzer_play(MELODY_WARNING); 73 | return false; 74 | } 75 | 76 | return true; 77 | } 78 | 79 | /** 80 | * Initializing 81 | */ 82 | void setup() 83 | { 84 | // Input button 85 | pinMode(BOARD_BUTTON_PIN, INPUT); 86 | 87 | // Initializing terminal on the RC port 88 | RC.begin(BT_BAUD); 89 | terminal_init(&RC); 90 | 91 | // Lit pin is output low 92 | digitalWrite(LIT, LOW); 93 | pinMode(LIT, OUTPUT); 94 | 95 | // Initializing bluetooth module 96 | bt_init(); 97 | 98 | // Initializing 99 | motion_init(); 100 | 101 | // Initializing voltage measurement 102 | voltage_init(); 103 | 104 | // Initializing the DXL bus 105 | delay(500); 106 | dxl_init(); 107 | 108 | // Initializing config (see config.h) 109 | config_init(); 110 | 111 | // initializing distance 112 | distance_init(); 113 | 114 | // Initializing the IMU 115 | imu_init(); 116 | 117 | // Initializing positions to 0 118 | for (int i=0; i<12; i++) { 119 | dxl_set_position(i+1, 0.0); 120 | } 121 | for (int i=0; i<4; i++) { 122 | l1[i] = l2[i] = l3[i] = 0; 123 | } 124 | 125 | // Configuring board LED as output 126 | pinMode(BOARD_LED_PIN, OUTPUT); 127 | digitalWrite(BOARD_LED_PIN, LOW); 128 | 129 | // Initializing the buzzer, and playing the start-up melody 130 | buzzer_init(); 131 | buzzer_play(MELODY_BOOT); 132 | 133 | // Enable 50hz ticking 134 | servos_init(); 135 | servos_attach_interrupt(setFlag); 136 | 137 | RC.begin(BT_BAUD); 138 | } 139 | 140 | /** 141 | * Computing the servo values 142 | */ 143 | void tick() 144 | { 145 | static bool wasMoving = false; 146 | 147 | if (voltage_error()) { 148 | // If there is a voltage error, blinks the LEDs orange and 149 | // stop any motor activity 150 | dxl_disable_all(); 151 | #ifdef RHOCK 152 | rhock_killall(); 153 | #endif 154 | if (t < 0.5) { 155 | led_set_all(LED_R|LED_G); 156 | } else { 157 | led_set_all(0); 158 | } 159 | t += 0.02; 160 | if (t > 1.0) t -= 1.0; 161 | return; 162 | } 163 | 164 | // The robot is disabled 165 | if (!move || !started) { 166 | motors_read(); 167 | t = 0.0; 168 | return; 169 | } 170 | 171 | // Incrementing and normalizing t 172 | t += motion_get_f()*0.02; 173 | if (t > 1.0) { 174 | t -= 1.0; 175 | colorize(); 176 | } 177 | if (t < 0.0) t += 1.0; 178 | 179 | if (!wasMoving && motion_is_moving()) { 180 | t = 0.0; 181 | } 182 | wasMoving = motion_is_moving(); 183 | 184 | // Robot behavior 185 | behavior_tick(0.02); 186 | 187 | motion_tick(t); 188 | 189 | // Sending order to servos 190 | dxl_set_position(mapping[0], l1[0]); 191 | dxl_set_position(mapping[3], l1[1]); 192 | dxl_set_position(mapping[6], l1[2]); 193 | dxl_set_position(mapping[9], l1[3]); 194 | 195 | dxl_set_position(mapping[1], l2[0]); 196 | dxl_set_position(mapping[4], l2[1]); 197 | dxl_set_position(mapping[7], l2[2]); 198 | dxl_set_position(mapping[10], l2[3]); 199 | 200 | dxl_set_position(mapping[2], l3[0]); 201 | dxl_set_position(mapping[5], l3[1]); 202 | dxl_set_position(mapping[8], l3[2]); 203 | dxl_set_position(mapping[11], l3[3]); 204 | } 205 | 206 | static int btnLast = 0; 207 | static bool btn = false; 208 | static int id = 13; 209 | 210 | void buttonPress() 211 | { 212 | btnLast = millis(); 213 | } 214 | 215 | void idAudio() 216 | { 217 | for (int k=0; k 6000) { 231 | // Pressed more than 6s, configuring bluetooth 232 | buzzer_play(MELODY_BEGIN); 233 | buzzer_wait_play(); 234 | bt_set_config("Metabot", "1234"); 235 | buzzer_play(MELODY_BEGIN); 236 | buzzer_wait_play(); 237 | } else if (millis()-btnLast > 2000) { 238 | // Pressed more than 2s, entering ID sequence 239 | id = 1; 240 | buzzer_play(MELODY_BEGIN); 241 | buzzer_wait_play(); 242 | } else { 243 | // Next id sequence 244 | if (id <= 12) { 245 | int success = 0; 246 | 247 | // Checking that there is exactly one servo on the bus 248 | int discover = 0; 249 | for (int k=1; k<=12; k++) { 250 | bool ok = false; 251 | for (int t=0; t<3; t++) { 252 | if (dxl_ping(k)) { 253 | ok = true; 254 | } 255 | delay(3); 256 | } 257 | if (ok) { 258 | discover++; 259 | } 260 | } 261 | 262 | // Configuring the servo on the bus to the target ID 263 | // (using broadcast as address) 264 | if (discover == 1) { 265 | for (int k=0; k<3; k++) { 266 | dxl_configure(DXL_BROADCAST, id); 267 | dxl_write_byte(id, DXL_LED, LED_G); 268 | delay(30); 269 | // Checking that the servo now is correct 270 | if (dxl_ping(id)) { 271 | success++; 272 | } 273 | } 274 | } 275 | 276 | // Next ID 277 | if (success > 0) { 278 | idAudio(); 279 | id++; 280 | 281 | if (id > 12) { 282 | // It's over 283 | delay(500); 284 | buzzer_play(MELODY_BEGIN); 285 | buzzer_wait_play(); 286 | } 287 | } else { 288 | buzzer_play(MELODY_WARNING); 289 | buzzer_wait_play(); 290 | } 291 | } 292 | } 293 | } 294 | 295 | void loop() 296 | { 297 | bool btnNew = digitalRead(BOARD_BUTTON_PIN); 298 | if (btnNew != btn) { 299 | if (btnNew) buttonPress(); 300 | else buttonRelease(); 301 | btn = btnNew; 302 | } 303 | 304 | // Buzzer update 305 | buzzer_tick(); 306 | // IMU ticking 307 | imu_tick(); 308 | // Sampling the voltage 309 | voltage_tick(); 310 | // Sampling the distance 311 | distance_tick(); 312 | 313 | // Updating the terminal 314 | terminal_tick(); 315 | #if defined(RHOCK) 316 | rhock_tick(); 317 | #endif 318 | if (SerialUSB.available() && !isUSB) { 319 | isUSB = true; 320 | terminal_init(&SerialUSB); 321 | } 322 | if (RC.available() && isUSB) { 323 | isUSB = false; 324 | terminal_init(&RC); 325 | } 326 | 327 | // Calling user motion tick 328 | if (flag) { 329 | flag = false; 330 | tick(); 331 | dxl_flush(); 332 | } 333 | } 334 | -------------------------------------------------------------------------------- /firmware/src/mapping.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "config.h" 3 | #include "mapping.h" 4 | #include "leds.h" 5 | #ifndef __EMSCRIPTEN__ 6 | #include 7 | #endif 8 | 9 | // Current mapping 10 | static uint8_t currentMapping = 0; 11 | 12 | // This is the servo mappings 13 | uint8_t mapping[12]; 14 | 15 | void colorize() 16 | { 17 | if (!leds_are_custom()) { 18 | for (int i=0; i<6; i++) { 19 | led_set(mapping[i], LED_R|LED_G|LED_B); 20 | } 21 | for (int i=6; i<12; i++) { 22 | led_set(mapping[i], 0); 23 | } 24 | } 25 | } 26 | 27 | void remap(int direction) 28 | { 29 | currentMapping = direction; 30 | for (int i=0; i<12; i++) { 31 | mapping[i] = servos_order[(i+3*direction)%12]; 32 | } 33 | } 34 | 35 | 36 | #ifndef __EMSCRIPTEN__ 37 | TERMINAL_COMMAND(remap, 38 | "Changes the mapping") 39 | { 40 | if (argc == 0) { 41 | terminal_io()->print("remap="); 42 | terminal_io()->println(currentMapping); 43 | } else { 44 | currentMapping = atoi(argv[0]); 45 | remap(currentMapping); 46 | } 47 | } 48 | #endif 49 | -------------------------------------------------------------------------------- /firmware/src/mapping.h: -------------------------------------------------------------------------------- 1 | #ifndef _METABOT_MAPPING 2 | #define _METABOT_MAPPING 3 | 4 | #include 5 | 6 | extern uint8_t mapping[12]; 7 | 8 | /** 9 | * Changes the servo mapping (the "front" of the robot) 10 | * @param map An int between 0 and 3 11 | */ 12 | void remap(int map); 13 | 14 | /** 15 | * Colorize the legs 16 | */ 17 | void colorize(); 18 | 19 | #endif 20 | -------------------------------------------------------------------------------- /firmware/src/motion.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #ifdef HAS_TERMINAL 4 | #include 5 | #else 6 | #define TERMINAL_PARAMETER_BOOL(name, desc, def) \ 7 | bool name = def; 8 | #define TERMINAL_PARAMETER_FLOAT(name, desc, def) \ 9 | float name = def; 10 | #define TERMINAL_PARAMETER_DOUBLE(name, desc, def) \ 11 | double name = def; 12 | #define TERMINAL_PARAMETER_INT(name, desc, def) \ 13 | int name = def; 14 | #endif 15 | #ifdef __EMSCRIPTEN__ 16 | #include 17 | #else 18 | #include "imu.h" 19 | #include "voltage.h" 20 | #include "distance.h" 21 | #endif 22 | #ifdef RHOCK 23 | #include 24 | #include 25 | #endif 26 | #include "config.h" 27 | #include "cubic.h" 28 | #include "motion.h" 29 | #include "kinematic.h" 30 | #include "mapping.h" 31 | #include "leds.h" 32 | #include "motors.h" 33 | 34 | // Angles for the legs motor 35 | float l1[4], l2[4], l3[4]; 36 | 37 | // Extra angles 38 | float a1[4], a2[4], a3[4]; 39 | 40 | // Extra x, y and z for each leg 41 | static float ex[4], ey[4], ez[4]; 42 | 43 | float motion_get_motor(int idx) 44 | { 45 | int c = (idx%3); 46 | switch (c) { 47 | case 0: 48 | return (l1[idx/3]); 49 | break; 50 | case 1: 51 | return (l2[idx/3]); 52 | break; 53 | case 2: 54 | return (l3[idx/3]); 55 | break; 56 | } 57 | 58 | return 0; 59 | } 60 | 61 | // Amplitude multiplier 62 | #define AMPLITUDE 30 63 | 64 | // Speed factor 65 | TERMINAL_PARAMETER_FLOAT(freq, "Time factor gain", 2.0); 66 | 67 | // Legs bacakward mode 68 | TERMINAL_PARAMETER_BOOL(backLegs, "Legs backwards", false); 69 | 70 | // Amplitude & altitude of the robot 71 | TERMINAL_PARAMETER_FLOAT(alt, "Height of the steps", 22.0); 72 | 73 | // Static position 74 | TERMINAL_PARAMETER_FLOAT(r, "Robot size", 153.0); 75 | TERMINAL_PARAMETER_FLOAT(h, "Robot height", -55.0); 76 | 77 | // Direction vector 78 | TERMINAL_PARAMETER_FLOAT(dx, "Dx", 0.0); 79 | TERMINAL_PARAMETER_FLOAT(dy, "Dy", 0.0); 80 | 81 | // Turning, in ° per step 82 | TERMINAL_PARAMETER_FLOAT(turn, "Turn", 0.0); 83 | 84 | // Crab 85 | TERMINAL_PARAMETER_FLOAT(crab, "Crab", 0.0); 86 | 87 | // Front delta h 88 | TERMINAL_PARAMETER_FLOAT(frontH, "Front delta H", 0.0); 89 | 90 | #ifdef HAS_TERMINAL 91 | TERMINAL_COMMAND(toggleBackLegs, "Toggle back legs") 92 | { 93 | if (backLegs == 0) backLegs = 1; 94 | else if (backLegs == 1) backLegs = 0; 95 | } 96 | 97 | TERMINAL_COMMAND(toggleCrab, "Toggle crab mode") 98 | { 99 | if (crab == 0) crab = 30; 100 | else if (crab == 30) crab = 0; 101 | } 102 | #endif 103 | 104 | // Gait selector 105 | TERMINAL_PARAMETER_FLOAT(gait, "Gait", 1); 106 | 107 | // Support 108 | float support = 0.5; 109 | 110 | // Functions 111 | Cubic rise; 112 | Cubic step; 113 | 114 | /** 115 | * Initializing functions 116 | */ 117 | void setup_functions() 118 | { 119 | rise.clear(); 120 | step.clear(); 121 | 122 | step.addPoint(0, 0.5, 0); 123 | step.addPoint(support, -0.5, 0); 124 | step.addPoint(support+(1-support)/2, 0, 1); 125 | step.addPoint(1, 0.5, 0); 126 | 127 | rise.addPoint(0, 0, 0); 128 | rise.addPoint(support, 0, 0); 129 | rise.addPoint(support+(1-support)/2, 1, 0); 130 | rise.addPoint(1, 0, 0); 131 | } 132 | 133 | #ifdef HAS_TERMINAL 134 | TERMINAL_COMMAND(support, "Setup functions") 135 | { 136 | if (argc == 1) { 137 | support = atof(argv[0]); 138 | setup_functions(); 139 | } else { 140 | terminal_io()->println("Usage: support [duty]"); 141 | } 142 | } 143 | #endif 144 | 145 | TERMINAL_PARAMETER_FLOAT(smoothBackLegs, "Smooth 180", 0.0); 146 | 147 | // Extra values 148 | float extra_h = 0; 149 | float extra_r = 0; 150 | 151 | // Is the robot moving? 152 | bool motion_is_moving() 153 | { 154 | return (fabs(dx)>0.5 || fabs(dy)>0.5 || fabs(turn)>5); 155 | } 156 | 157 | void motion_init() 158 | { 159 | // Setting the mapping to 0 160 | remap(0); 161 | 162 | for (int i=0; i<4; i++) { 163 | ex[i] = 0; 164 | ey[i] = 0; 165 | ez[i] = 0; 166 | a1[i] = 0; 167 | a2[i] = 0; 168 | a3[i] = 0; 169 | } 170 | 171 | extra_h = 0; 172 | extra_r = 0; 173 | freq = 2.0; 174 | } 175 | 176 | void motion_tick(float t) 177 | { 178 | if (!motors_enabled()) { 179 | return; 180 | } 181 | 182 | // Setting up functions 183 | setup_functions(); 184 | 185 | // Smoothing 180 186 | if (backLegs && smoothBackLegs < 1) { 187 | smoothBackLegs += 0.02; 188 | } 189 | if (!backLegs && smoothBackLegs > 0) { 190 | smoothBackLegs -= 0.02; 191 | } 192 | 193 | float crabRad; 194 | float phasesA[] = {0.0, 0.5, 1-1e-6, 0.5}; 195 | float phasesB[] = {0.0, 0.5, 0.75, 0.25}; 196 | 197 | for (int i=0; i<4; i++) { 198 | // Defining in which group of opposite legs this leg is 199 | bool group = ((i&1)==1); 200 | 201 | // This defines the phase of the gait 202 | float legPhase; 203 | 204 | // Defining gait 205 | legPhase = t + phasesA[i]*gait + phasesB[i]*(1-gait); 206 | 207 | // Leg target 208 | float x, y, z, a, b, c; 209 | 210 | // Computing the order in the referencial of the body 211 | float stepping = step.getMod(legPhase); 212 | 213 | // Add the radius to the leg, in the right direction 214 | float radius = (r+extra_r); 215 | 216 | // The leg position in the body frame 217 | float X = (cos(M_PI/4)*radius) * ((i==0||i==1) ? 1 : -1); 218 | float Y = (cos(M_PI/4)*radius) * ((i==0||i==3) ? 1 : -1); 219 | float X_ = X; 220 | float Y_ = Y; 221 | 222 | // Applying crab 223 | crabRad = DEG2RAD(crab) * (group ? 1 : -1); 224 | X = cos(crabRad)*X_ - sin(crabRad)*Y_; 225 | Y = sin(crabRad)*X_ + cos(crabRad)*Y_; 226 | 227 | // Extras 228 | X += ex[i]; 229 | Y += ey[i]; 230 | 231 | // Add dX and dY to the moving vector 232 | if (fabs(turn) > 0.5) { 233 | float turnRad = -DEG2RAD(turn); 234 | float theta = -stepping*turnRad; 235 | float l = sqrt(dx*dx+dy*dy)/turnRad; 236 | float r = atan2(dy, dx); 237 | float cr = cos(-r); 238 | float sr = sin(-r); 239 | 240 | X_ = X; Y_ = Y; 241 | X = X_*cr - Y_*sr; 242 | Y = X_*sr + Y_*cr; 243 | 244 | X_ = X; Y_ = Y; 245 | X = X_*cos(theta) - (Y_+l)*sin(theta); 246 | Y = X_*sin(theta) + (Y_+l)*cos(theta) - l; 247 | 248 | X_ = X; Y_ = Y; 249 | X = X_*cr - Y_*(-sr); 250 | Y = X_*(-sr) + Y_*cr; 251 | } else { 252 | X += stepping*dx; 253 | Y += stepping*dy; 254 | } 255 | 256 | // Move to the leg frame 257 | float vx, vy; 258 | legFrame(X, Y, &vx, &vy, i, L0); 259 | 260 | // The robot is moving if there is dynamics parameters 261 | bool moving = motion_is_moving(); 262 | 263 | // This is the x,y,z order in the referencial of the leg 264 | x = vx; 265 | y = vy; 266 | z = ez[i] + h - extra_h + (moving ? (rise.getMod(legPhase)*alt) : 0); 267 | if (i < 2) z += frontH; 268 | 269 | // Computing inverse kinematics 270 | if (computeIK(x, y, z, &a, &b, &c, L1, L2, backLegs ? L3_2 : L3_1)) { 271 | l1[i] = -SIGN_A*a + a1[i]; 272 | l2[i] = -SIGN_B*b + a2[i]; 273 | l3[i] = -SIGN_C*(c - 180*smoothBackLegs) + a3[i]; 274 | } 275 | } 276 | } 277 | 278 | #ifdef __EMSCRIPTEN__ 279 | float sim_t = 0.0; 280 | #endif 281 | 282 | void motion_reset() 283 | { 284 | motion_init(); 285 | 286 | #ifdef __EMSCRIPTEN__ 287 | sim_t = 0.0; 288 | #endif 289 | } 290 | 291 | void motion_set_f(float f_) 292 | { 293 | freq = f_; 294 | } 295 | 296 | float motion_get_f() 297 | { 298 | return freq; 299 | } 300 | 301 | void motion_set_h(float h_) 302 | { 303 | extra_h = h_; 304 | } 305 | 306 | void motion_set_r(float r_) 307 | { 308 | extra_r = r_; 309 | } 310 | 311 | void motion_set_x_speed(float x_speed) 312 | { 313 | dx = ODOMETRY_TRANSLATION*x_speed/(2.0*freq); 314 | } 315 | 316 | void motion_set_y_speed(float y_speed) 317 | { 318 | dy = ODOMETRY_TRANSLATION*y_speed/(2.0*freq); 319 | } 320 | 321 | void motion_set_turn_speed(float turn_speed) 322 | { 323 | turn = ODOMETRY_ROTATION*turn_speed/(2.0*freq); 324 | } 325 | 326 | void motion_extra_x(int index, float x) 327 | { 328 | if (index >= 4) { 329 | for (int k=0; k<4; k++) { 330 | ex[k] = x; 331 | } 332 | } else { 333 | ex[index] = x; 334 | } 335 | } 336 | 337 | void motion_extra_y(int index, float y) 338 | { 339 | if (index >= 4) { 340 | for (int k=0; k<4; k++) { 341 | ey[k] = y; 342 | } 343 | } else { 344 | ey[index] = y; 345 | } 346 | } 347 | 348 | void motion_extra_z(int index, float z) 349 | { 350 | if (index >= 4) { 351 | for (int k=0; k<4; k++) { 352 | ez[k] = z; 353 | } 354 | } else { 355 | ez[index] = z; 356 | } 357 | } 358 | 359 | void motion_extra_angle(int index, int motor, float angle) 360 | { 361 | for (int k=0; k<4; k++) { 362 | if (k == index || index >= 4) { 363 | if (motor == 0) a1[k] = angle; 364 | if (motor == 1) a2[k] = angle; 365 | if (motor == 2) a3[k] = angle; 366 | } 367 | } 368 | } 369 | 370 | float motion_get_dx() 371 | { 372 | return dx; 373 | } 374 | 375 | float motion_get_dy() 376 | { 377 | return dy; 378 | } 379 | 380 | float motion_get_turn() 381 | { 382 | return turn; 383 | } 384 | 385 | #ifdef RHOCK 386 | void rhock_on_monitor() 387 | { 388 | rhock_stream_begin(RHOCK_STREAM_USER); 389 | // Angles 390 | #ifdef __EMSCRIPTEN__ 391 | bool dontRead = true; 392 | #else 393 | bool dontRead = false; 394 | #endif 395 | if (dontRead || motors_enabled()) { 396 | for (int i=0; i<12; i++) { 397 | rhock_stream_append_short((uint16_t)((int16_t)(motion_get_motor(i)*10))); 398 | } 399 | } else { 400 | for (int i=0; i<12; i++) { 401 | rhock_stream_append_short((uint16_t)((int16_t)(motors_get_position(i)*10))); 402 | } 403 | } 404 | #ifdef __EMSCRIPTEN__ 405 | rhock_stream_append_short(0); 406 | rhock_stream_append_short(0); 407 | rhock_stream_append_short(0); 408 | #else 409 | rhock_stream_append_short((uint16_t)((int16_t)(imu_yaw()*10))); 410 | rhock_stream_append_short((uint16_t)((int16_t)(imu_pitch()*10))); 411 | rhock_stream_append_short((uint16_t)((int16_t)(imu_roll()*10))); 412 | #endif 413 | 414 | // Leds 415 | led_stream_state(); 416 | 417 | // Distance sensor 418 | #ifdef __EMSCRIPTEN__ 419 | rhock_stream_append_short((uint16_t)((int16_t)(100*10))); 420 | #else 421 | rhock_stream_append_short((uint16_t)((int16_t)(distance_get()*10))); 422 | #endif 423 | 424 | // Voltage 425 | #ifdef __EMSCRIPTEN__ 426 | rhock_stream_append_short((uint16_t)((int16_t)(8*10))); 427 | #else 428 | rhock_stream_append_short((uint16_t)((int16_t)(voltage_current()*10))); 429 | #endif 430 | 431 | rhock_stream_end(); 432 | } 433 | #endif 434 | 435 | #ifdef __EMSCRIPTEN__ 436 | 437 | void simulator_tick() 438 | { 439 | if (motors_enabled()) { 440 | sim_t += motion_get_f()*0.02; 441 | if (sim_t > 1) sim_t -= 1; 442 | if (!motion_is_moving()) { 443 | sim_t = 0; 444 | } 445 | motion_tick(sim_t); 446 | } 447 | } 448 | 449 | float simulator_get_dx() 450 | { 451 | return dx; 452 | } 453 | 454 | float simulator_get_dy() 455 | { 456 | return dy; 457 | } 458 | 459 | float simulator_get_turn() 460 | { 461 | return turn; 462 | } 463 | 464 | float simulator_get_f() 465 | { 466 | return motion_get_f(); 467 | } 468 | 469 | bool simulator_get_enabled() 470 | { 471 | return motors_enabled(); 472 | } 473 | 474 | using namespace emscripten; 475 | EMSCRIPTEN_BINDINGS(motion) { 476 | function("motion_get_dx", &motion_get_dx); 477 | function("motion_get_dy", &motion_get_dy); 478 | function("motion_get_turn", &motion_get_turn); 479 | function("motion_init", &motion_init); 480 | function("motion_get_motor", &motion_get_motor); 481 | 482 | function("simulator_tick", &simulator_tick); 483 | function("simulator_get_f", &simulator_get_f); 484 | function("simulator_get_dx", &simulator_get_dx); 485 | function("simulator_get_dy", &simulator_get_dy); 486 | function("simulator_get_turn", &simulator_get_turn); 487 | function("simulator_get_enabled", &simulator_get_enabled); 488 | } 489 | #endif 490 | -------------------------------------------------------------------------------- /firmware/src/motion.h: -------------------------------------------------------------------------------- 1 | #ifndef _METABOT_MOTION_H 2 | #define _METABOT_MOTION_H 3 | 4 | // Output angles 5 | extern float l1[4], l2[4], l3[4]; 6 | 7 | /** 8 | * Initializes the motion 9 | */ 10 | void motion_init(); 11 | 12 | /** 13 | * Updates the motion 14 | * @param t The time (between 0 and 1) of the motion 15 | */ 16 | void motion_tick(float t); 17 | 18 | /** 19 | * Resets the motion 20 | */ 21 | void motion_reset(); 22 | 23 | /** 24 | * Is the robot moving? 25 | */ 26 | bool motion_is_moving(); 27 | 28 | /** 29 | * Sets the target frequency (steps per second) 30 | * @param f The frequency (Hz) 31 | */ 32 | void motion_set_f(float f); 33 | 34 | /** 35 | * Sets the target legs height 36 | * @param h Height (mm) of the legs 37 | */ 38 | void motion_set_h(float h); 39 | 40 | /** 41 | * Sets the legs radius 42 | * @param r Legs radius (mm) 43 | */ 44 | void motion_set_r(float r); 45 | 46 | /** 47 | * Sets the X speed 48 | * @param x_speed x target speed (mm/s) 49 | */ 50 | void motion_set_x_speed(float x_speed); 51 | 52 | /** 53 | * Sets the Y speed 54 | * @param y_speed y target speed (mm/s) 55 | */ 56 | void motion_set_y_speed(float y_speed); 57 | 58 | /** 59 | * Sets the turn speed 60 | * @param turn_speed turn target speed (°/s) 61 | */ 62 | void motion_set_turn_speed(float turn_speed); 63 | 64 | /** 65 | * Add extra X value 66 | * @param index The index of the leg 67 | * @param x Extra x (mm) 68 | */ 69 | void motion_extra_x(int index, float x); 70 | 71 | /** 72 | * Add extra Y value 73 | * @param index The index of the leg 74 | * @param y Extra y (mm) 75 | */ 76 | void motion_extra_y(int index, float y); 77 | 78 | /** 79 | * Add extra Z value 80 | * @param index The index of the leg 81 | * @param z Extra z (mm) 82 | */ 83 | void motion_extra_z(int index, float z); 84 | 85 | /** 86 | * Adds extra angle 87 | * @param index Index of the leg (0-3, +: all) 88 | * @param motor Index of the motor (0-2) 89 | * @param angle Extra angle 90 | */ 91 | void motion_extra_angle(int index, int motor, float angle); 92 | 93 | /** 94 | * The current frequency 95 | */ 96 | float motion_get_f(); 97 | 98 | /** 99 | * Gets the target angle for a specific motor 100 | * @param idx The index of the motor 101 | */ 102 | float motion_get_motor(int idx); 103 | 104 | #endif 105 | -------------------------------------------------------------------------------- /firmware/src/motors.cpp: -------------------------------------------------------------------------------- 1 | #ifndef __EMSCRIPTEN__ 2 | #include 3 | #include 4 | #endif 5 | #include "leds.h" 6 | #include "motion.h" 7 | #include "mapping.h" 8 | #include "motors.h" 9 | 10 | static float motors_positions[12]; 11 | static bool is_enabled = false; 12 | 13 | void motors_colorize() 14 | { 15 | if (motors_enabled()) { 16 | // Colorizing the front of the robot 17 | colorize(); 18 | } else { 19 | // Turning all leds red 20 | led_set_all(LED_R); 21 | } 22 | } 23 | 24 | void motors_enable() 25 | { 26 | // Motors are enabled 27 | is_enabled = true; 28 | #ifndef __EMSCRIPTEN__ 29 | // Running the actual motors 30 | start(); 31 | #endif 32 | motors_colorize(); 33 | } 34 | 35 | void motors_disable() 36 | { 37 | // Motors are disabled 38 | is_enabled = false; 39 | // Marking leds as non custom 40 | #ifndef __EMSCRIPTEN__ 41 | // Stopping the actual motors 42 | stop(); 43 | #endif 44 | motors_colorize(); 45 | } 46 | 47 | bool motors_enabled() 48 | { 49 | #ifndef __EMSCRIPTEN__ 50 | return started; 51 | #else 52 | return is_enabled; 53 | #endif 54 | } 55 | 56 | void motors_read() 57 | { 58 | #ifndef __EMSCRIPTEN__ 59 | ui8 ids[12] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12}; 60 | uint16_t positions[24]; 61 | if (dxl_sync_read(ids, 12, DXL_POSITION, 2, (ui8*)positions)) { 62 | for (int k=0; k<12; k++) { 63 | motors_positions[k] = dxl_value_to_position(k+1, positions[k]); 64 | } 65 | } 66 | #endif 67 | } 68 | 69 | float motors_get_position(int i) 70 | { 71 | #ifndef __EMSCRIPTEN__ 72 | if (motors_enabled()) { 73 | return motion_get_motor(i); 74 | } else { 75 | return motors_positions[i]; 76 | } 77 | #else 78 | return motion_get_motor(i); 79 | #endif 80 | } 81 | -------------------------------------------------------------------------------- /firmware/src/motors.h: -------------------------------------------------------------------------------- 1 | #ifndef _METABOT_MOTORS_H 2 | #define _METABOT_MOTORS_H 3 | 4 | /** 5 | * Enables all the motors torque 6 | */ 7 | void motors_enable(); 8 | 9 | /** 10 | * Disables all the motors torque 11 | */ 12 | void motors_disable(); 13 | 14 | /** 15 | * Are the motors enabled? 16 | */ 17 | bool motors_enabled(); 18 | 19 | /** 20 | * Update the motors colors 21 | */ 22 | void motors_colorize(); 23 | 24 | /** 25 | * Read all the motors values 26 | */ 27 | void motors_read(); 28 | 29 | /** 30 | * Position of the ith motor 31 | * @param i The index of the motor (start at 0) 32 | */ 33 | float motors_get_position(int i); 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /firmware/src/prepare.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | if [ -d "Maple" ]; then 4 | cd Maple && 5 | git pull && 6 | cd Rhock && 7 | git pull 8 | else 9 | git clone https://github.com/Rhoban/Maple.git --depth=1 && 10 | cd Maple && 11 | git clone https://github.com/Rhoban/Rhock.git --depth=1 12 | fi 13 | -------------------------------------------------------------------------------- /firmware/src/rhock-functions.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "rhock-functions.h" 6 | #include "motion.h" 7 | #ifndef __EMSCRIPTEN__ 8 | #include 9 | #else 10 | #include 11 | #endif 12 | #include "distance.h" 13 | #include "motors.h" 14 | #include "leds.h" 15 | #include "mapping.h" 16 | #include "buzzer.h" 17 | #include "imu.h" 18 | #include "rhock-stream.h" 19 | 20 | struct rhock_context *controlling = NULL; 21 | struct rhock_context *controllingBuzzer = NULL; 22 | float save_x_speed, save_y_speed, save_turn_speed; 23 | 24 | void motion_stop() 25 | { 26 | // When reseting the VM, stopping the robot 27 | motion_set_x_speed(0); 28 | motion_set_y_speed(0); 29 | motion_set_turn_speed(0); 30 | save_x_speed = 0; 31 | save_y_speed = 0; 32 | save_turn_speed = 0; 33 | controlling = NULL; 34 | } 35 | 36 | void motion_resume() 37 | { 38 | // Resume the motion 39 | motion_set_x_speed(save_x_speed); 40 | motion_set_y_speed(save_y_speed); 41 | motion_set_turn_speed(save_turn_speed); 42 | } 43 | 44 | /** 45 | * Called when all rhock processes are stopped 46 | */ 47 | void rhock_on_all_stopped() 48 | { 49 | // Decustom the leds 50 | leds_decustom(); 51 | // Stopping the motion 52 | controlling = NULL; 53 | motion_stop(); 54 | motion_reset(); 55 | buzzer_stop(); 56 | } 57 | 58 | /** 59 | * called when rhock process starts 60 | */ 61 | void rhock_on_reset() 62 | { 63 | // Resetting the mapping and the color 64 | motors_colorize(); 65 | } 66 | 67 | /** 68 | * Handling thread pause, stop and start 69 | */ 70 | void rhock_on_pause(struct rhock_context *context) 71 | { 72 | if (context == controlling) { 73 | motion_stop(); 74 | } 75 | } 76 | 77 | void rhock_on_stop(struct rhock_context *context) 78 | { 79 | if (context == controlling) { 80 | motion_stop(); 81 | controlling = NULL; 82 | } 83 | if (context == controllingBuzzer) { 84 | buzzer_stop(); 85 | controllingBuzzer = NULL; 86 | } 87 | } 88 | 89 | void rhock_on_start(struct rhock_context *context) 90 | { 91 | if (context == controlling) { 92 | motion_resume(); 93 | } 94 | } 95 | 96 | RHOCK_NATIVE(robot_led) 97 | { 98 | int value = RHOCK_POPF(); 99 | int led = RHOCK_POPF(); 100 | led_set(led, value, true); 101 | return RHOCK_NATIVE_CONTINUE; 102 | } 103 | 104 | RHOCK_NATIVE(robot_leds) 105 | { 106 | int value = RHOCK_POPF(); 107 | led_set_all(value, true); 108 | return RHOCK_NATIVE_CONTINUE; 109 | } 110 | 111 | RHOCK_NATIVE(robot_leg_leds) 112 | { 113 | int value = RHOCK_POPF(); 114 | int leg = RHOCK_POPF(); 115 | leg = ((leg-1)&3); 116 | for (int k=0; k<3; k++) { 117 | led_set(mapping[3*leg+k], value, true); 118 | } 119 | return RHOCK_NATIVE_CONTINUE; 120 | } 121 | 122 | static void motion_control(float x_speed, float y_speed, float turn_speed, 123 | struct rhock_context *context) 124 | { 125 | controlling = context; 126 | save_x_speed = x_speed; 127 | save_y_speed = y_speed; 128 | save_turn_speed = turn_speed; 129 | motion_resume(); 130 | } 131 | 132 | RHOCK_NATIVE(robot_control) 133 | { 134 | float turn_speed = RHOCK_POPF(); 135 | float y_speed = RHOCK_POPF(); 136 | float x_speed = RHOCK_POPF(); 137 | motion_control(x_speed, y_speed, turn_speed, context); 138 | 139 | return RHOCK_NATIVE_CONTINUE; 140 | } 141 | 142 | RHOCK_NATIVE(robot_stop) 143 | { 144 | motion_stop(); 145 | 146 | return RHOCK_NATIVE_CONTINUE; 147 | } 148 | 149 | RHOCK_NATIVE(robot_x_speed) 150 | { 151 | float x_speed = RHOCK_POPF(); 152 | motion_control(x_speed, 0, 0, context); 153 | 154 | return RHOCK_NATIVE_CONTINUE; 155 | } 156 | 157 | RHOCK_NATIVE(robot_y_speed) 158 | { 159 | float y_speed = RHOCK_POPF(); 160 | motion_control(0, y_speed, 0, context); 161 | 162 | return RHOCK_NATIVE_CONTINUE; 163 | } 164 | 165 | RHOCK_NATIVE(robot_turn_speed) 166 | { 167 | float turn_speed = RHOCK_POPF(); 168 | motion_control(0, 0, turn_speed, context); 169 | 170 | return RHOCK_NATIVE_CONTINUE; 171 | } 172 | 173 | RHOCK_NATIVE(board_led) 174 | { 175 | #ifndef __EMSCRIPTEN__ 176 | digitalWrite(BOARD_LED_PIN, !RHOCK_POPI()); 177 | #endif 178 | 179 | return RHOCK_NATIVE_CONTINUE; 180 | } 181 | 182 | RHOCK_NATIVE(robot_f) 183 | { 184 | motion_set_f(RHOCK_POPF()); 185 | 186 | return RHOCK_NATIVE_CONTINUE; 187 | } 188 | 189 | RHOCK_NATIVE(robot_h) 190 | { 191 | motion_set_h(RHOCK_POPF()); 192 | 193 | return RHOCK_NATIVE_CONTINUE; 194 | } 195 | 196 | RHOCK_NATIVE(robot_r) 197 | { 198 | motion_set_r(RHOCK_POPF()); 199 | 200 | return RHOCK_NATIVE_CONTINUE; 201 | } 202 | 203 | RHOCK_NATIVE(robot_ex) 204 | { 205 | int leg = RHOCK_POPF(); 206 | float extra = RHOCK_POPF(); 207 | leg = leg-1; 208 | motion_extra_x(leg, extra); 209 | 210 | return RHOCK_NATIVE_CONTINUE; 211 | } 212 | RHOCK_NATIVE(robot_ey) 213 | { 214 | int leg = RHOCK_POPF(); 215 | float extra = RHOCK_POPF(); 216 | leg = leg-1; 217 | motion_extra_y(leg, extra); 218 | 219 | return RHOCK_NATIVE_CONTINUE; 220 | } 221 | RHOCK_NATIVE(robot_ez) 222 | { 223 | int leg = RHOCK_POPF(); 224 | float extra = RHOCK_POPF(); 225 | leg = leg-1; 226 | motion_extra_z(leg, extra); 227 | 228 | return RHOCK_NATIVE_CONTINUE; 229 | } 230 | 231 | RHOCK_NATIVE(robot_ea) 232 | { 233 | int leg = RHOCK_POPF(); 234 | int index = RHOCK_POPF(); 235 | float extra = RHOCK_POPF(); 236 | leg = leg-1; 237 | motion_extra_angle(leg, index, extra); 238 | 239 | return RHOCK_NATIVE_CONTINUE; 240 | } 241 | 242 | RHOCK_NATIVE(robot_reset) 243 | { 244 | motion_init(); 245 | 246 | return RHOCK_NATIVE_CONTINUE; 247 | } 248 | 249 | RHOCK_NATIVE(robot_turn) 250 | { 251 | ON_ENTER() { 252 | float turn_speed = RHOCK_POPF(); 253 | float deg = RHOCK_POPF(); 254 | if (deg < 0 && turn_speed > 0) { 255 | turn_speed = -turn_speed; 256 | } 257 | float time = fabs(deg/turn_speed); 258 | RHOCK_PUSHF(time*1000); 259 | 260 | motion_control(0, 0, turn_speed, context); 261 | return RHOCK_NATIVE_WAIT; 262 | } 263 | ON_ELAPSED() { 264 | RHOCK_SMASH(1); 265 | motion_stop(); 266 | return RHOCK_NATIVE_CONTINUE; 267 | } 268 | } 269 | 270 | RHOCK_NATIVE(robot_move_x) 271 | { 272 | ON_ENTER() { 273 | float speed = RHOCK_POPF(); 274 | float dist = RHOCK_POPF(); 275 | if (dist < 0 && speed > 0) { 276 | speed = -speed; 277 | } 278 | float time = fabs(dist/speed); 279 | RHOCK_PUSHF(time*1000); 280 | 281 | motion_control(speed, 0, 0, context); 282 | return RHOCK_NATIVE_WAIT; 283 | } 284 | ON_ELAPSED() { 285 | RHOCK_SMASH(1); 286 | motion_stop(); 287 | return RHOCK_NATIVE_CONTINUE; 288 | } 289 | } 290 | 291 | RHOCK_NATIVE(robot_move_y) 292 | { 293 | ON_ENTER() { 294 | float speed = RHOCK_POPF(); 295 | float dist = RHOCK_POPF(); 296 | if (dist < 0 && speed > 0) { 297 | speed = -speed; 298 | } 299 | float time = fabs(dist/speed); 300 | RHOCK_PUSHF(time*1000); 301 | 302 | motion_control(0, speed, 0, context); 303 | return RHOCK_NATIVE_WAIT; 304 | } 305 | ON_ELAPSED() { 306 | RHOCK_SMASH(1); 307 | motion_stop(); 308 | return RHOCK_NATIVE_CONTINUE; 309 | } 310 | } 311 | 312 | RHOCK_NATIVE(robot_dist) 313 | { 314 | #ifndef __EMSCRIPTEN__ 315 | RHOCK_PUSHF(distance_get()); 316 | #else 317 | // XXX: Simulate it, with EM ASM: 318 | // return EM_ASM_INT({ 319 | // return simulator_get_distance(); 320 | // }, 321 | RHOCK_PUSHF(100.0); 322 | #endif 323 | 324 | return RHOCK_NATIVE_CONTINUE; 325 | } 326 | 327 | RHOCK_NATIVE(robot_beep) 328 | { 329 | ON_ENTER() { 330 | float duration = RHOCK_POPF(); 331 | float freq = RHOCK_POPF(); 332 | RHOCK_PUSHF(duration); 333 | 334 | buzzer_beep(freq, duration); 335 | controllingBuzzer = context; 336 | 337 | return RHOCK_NATIVE_WAIT; 338 | } 339 | ON_ELAPSED() { 340 | RHOCK_SMASH(1); 341 | buzzer_stop(); 342 | return RHOCK_NATIVE_CONTINUE; 343 | } 344 | } 345 | 346 | RHOCK_NATIVE(robot_yaw) 347 | { 348 | #ifndef __EMSCRIPTEN__ 349 | RHOCK_PUSHF(imu_yaw()); 350 | #else 351 | RHOCK_PUSHF(EM_ASM_DOUBLE({ 352 | return simulator_get_yaw(); 353 | }, 0)); 354 | #endif 355 | 356 | return RHOCK_NATIVE_CONTINUE; 357 | } 358 | 359 | RHOCK_NATIVE(robot_pitch) 360 | { 361 | #ifndef __EMSCRIPTEN__ 362 | RHOCK_PUSHF(imu_pitch()); 363 | #else 364 | RHOCK_PUSHF(0); 365 | #endif 366 | 367 | return RHOCK_NATIVE_CONTINUE; 368 | } 369 | 370 | RHOCK_NATIVE(robot_roll) 371 | { 372 | #ifndef __EMSCRIPTEN__ 373 | RHOCK_PUSHF(imu_roll()); 374 | #else 375 | RHOCK_PUSHF(0); 376 | #endif 377 | 378 | return RHOCK_NATIVE_CONTINUE; 379 | } 380 | 381 | RHOCK_NATIVE(robot_motor_start) 382 | { 383 | motors_enable(); 384 | return RHOCK_NATIVE_CONTINUE; 385 | } 386 | 387 | RHOCK_NATIVE(robot_motor_stop) 388 | { 389 | motors_disable(); 390 | return RHOCK_NATIVE_CONTINUE; 391 | } 392 | 393 | RHOCK_NATIVE(robot_motor_position) 394 | { 395 | int i = RHOCK_POPF(); 396 | i = (i-1); 397 | if (i < 0) i = 0; 398 | if (i > 11) i = 11; 399 | 400 | RHOCK_PUSHF(motors_get_position(i)); 401 | 402 | return RHOCK_NATIVE_CONTINUE; 403 | } 404 | 405 | RHOCK_NATIVE(robot_get_control) 406 | { 407 | int32_t control = RHOCK_VALUE_TO_INT(RHOCK_POPI()); 408 | 409 | if (control < RHOCK_CONTROLS) { 410 | RHOCK_PUSHF(rhock_controls[control]/100.0); 411 | } else { 412 | RHOCK_PUSHF(0); 413 | } 414 | 415 | return RHOCK_NATIVE_CONTINUE; 416 | } 417 | -------------------------------------------------------------------------------- /firmware/src/rhock-functions.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | // Leds 4 | RHOCK_NATIVE_DECLARE(robot_led, 30); 5 | RHOCK_NATIVE_DECLARE(robot_leds, 31); 6 | RHOCK_NATIVE_DECLARE(robot_leg_leds, 32); 7 | 8 | // Moves 9 | RHOCK_NATIVE_DECLARE(robot_control, 40); 10 | RHOCK_NATIVE_DECLARE(robot_h, 41); 11 | RHOCK_NATIVE_DECLARE(robot_f, 42); 12 | RHOCK_NATIVE_DECLARE(robot_r, 43); 13 | RHOCK_NATIVE_DECLARE(robot_reset, 45); 14 | RHOCK_NATIVE_DECLARE(robot_ex, 46); 15 | RHOCK_NATIVE_DECLARE(robot_ey, 47); 16 | RHOCK_NATIVE_DECLARE(robot_ez, 44); 17 | RHOCK_NATIVE_DECLARE(robot_ea, 48); 18 | 19 | // Speed 20 | RHOCK_NATIVE_DECLARE(robot_stop, 50); 21 | RHOCK_NATIVE_DECLARE(robot_x_speed, 51); 22 | RHOCK_NATIVE_DECLARE(robot_y_speed, 52); 23 | RHOCK_NATIVE_DECLARE(robot_turn_speed, 53); 24 | 25 | // Distances sensor 26 | RHOCK_NATIVE_DECLARE(robot_dist, 54); 27 | 28 | // Distances 29 | RHOCK_NATIVE_DECLARE(robot_turn, 60); 30 | RHOCK_NATIVE_DECLARE(robot_move_x, 61); 31 | RHOCK_NATIVE_DECLARE(robot_move_y, 62); 32 | 33 | // Buzzer 34 | RHOCK_NATIVE_DECLARE(robot_beep, 55); 35 | 36 | // IMU 37 | RHOCK_NATIVE_DECLARE(robot_yaw, 33); 38 | RHOCK_NATIVE_DECLARE(robot_pitch, 34); 39 | RHOCK_NATIVE_DECLARE(robot_roll, 35); 40 | 41 | // Motors control 42 | RHOCK_NATIVE_DECLARE(robot_motor_start, 37); 43 | RHOCK_NATIVE_DECLARE(robot_motor_stop, 38); 44 | RHOCK_NATIVE_DECLARE(robot_motor_position, 39); 45 | 46 | // Controls 47 | RHOCK_NATIVE_DECLARE(robot_get_control, 58); 48 | 49 | -------------------------------------------------------------------------------- /firmware/src/rhock-stream.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include "rhock-stream.h" 5 | #include "leds.h" 6 | #include "mapping.h" 7 | #include "motion.h" 8 | #include "buzzer.h" 9 | #include "motors.h" 10 | #include "imu.h" 11 | #include "leds.h" 12 | #include "behavior.h" 13 | 14 | #define RHOCK_STREAM_METABOT 50 15 | 16 | short rhock_controls[16] = {0}; 17 | 18 | char rhock_on_packet(uint8_t type) 19 | { 20 | if (type == RHOCK_STREAM_METABOT) { 21 | if (rhock_stream_available() >= 1) { 22 | uint8_t command = rhock_stream_read(); 23 | switch (command) { 24 | case 0: // Starting the robot 25 | motors_enable(); 26 | return 1; 27 | break; 28 | case 1: // Stop 29 | motors_disable(); 30 | return 1; 31 | break; 32 | case 2: // Rotate calibration 33 | #ifndef __EMSCRIPTEN__ 34 | imu_calib_rotate(); 35 | #endif 36 | return 1; 37 | break; 38 | case 3: // Set control 39 | if (rhock_stream_available() == 3) { 40 | uint8_t control = rhock_stream_read(); 41 | if (control < RHOCK_CONTROLS) { 42 | uint16_t value = rhock_stream_read_short(); 43 | rhock_controls[control] = (short)value; 44 | } 45 | } 46 | return 1; 47 | break; 48 | case 4: // Speed 49 | if (rhock_stream_available() == 6) { 50 | motion_set_x_speed(((int16_t)rhock_stream_read_short())/10.0); 51 | motion_set_y_speed(((int16_t)rhock_stream_read_short())/10.0); 52 | motion_set_turn_speed(((int16_t)rhock_stream_read_short())/10.0); 53 | } 54 | return 1; 55 | break; 56 | case 5: // Beep 57 | if (rhock_stream_available() == 4) { 58 | uint16_t freq = rhock_stream_read_short(); 59 | uint16_t duration = rhock_stream_read_short(); 60 | buzzer_beep(freq, duration); 61 | } 62 | return 1; 63 | break; 64 | case 6: // Leds 65 | if (rhock_stream_available() == 1) { 66 | led_set_all(rhock_stream_read(), true); 67 | } 68 | return 1; 69 | break; 70 | case 7: // Reset 71 | buzzer_stop(); 72 | leds_decustom(); 73 | motion_set_x_speed(0.0); 74 | motion_set_y_speed(0.0); 75 | motion_set_turn_speed(0.0); 76 | motion_reset(); 77 | return 1; 78 | break; 79 | case 8: // Behavior 80 | if (rhock_stream_available() == 1) { 81 | behavior_set(rhock_stream_read()); 82 | } 83 | break; 84 | } 85 | } 86 | } 87 | return 0; 88 | } 89 | -------------------------------------------------------------------------------- /firmware/src/rhock-stream.h: -------------------------------------------------------------------------------- 1 | #ifndef ___RHOCK_STREAM_H 2 | #define ___RHOCK_STREAM_H 3 | 4 | #define RHOCK_CONTROLS 16 5 | extern short rhock_controls[RHOCK_CONTROLS]; 6 | 7 | #endif 8 | -------------------------------------------------------------------------------- /firmware/src/voltage.cpp: -------------------------------------------------------------------------------- 1 | #include "voltage.h" 2 | #include 3 | #include "buzzer.h" 4 | 5 | bool voltage_is_error; 6 | bool voltage_is_error_fast; 7 | int voltage_error_start = 0; 8 | int voltage_now; 9 | int voltage_limit_l; 10 | int voltage_limit_h; 11 | int voltage_limit_s; 12 | 13 | bool voltage_error() 14 | { 15 | return voltage_is_error; 16 | } 17 | 18 | float voltage_spv() 19 | { 20 | float stepPerVolt = 4095/3.3; 21 | stepPerVolt *= VOLTAGE_R2/(float)(VOLTAGE_R1+VOLTAGE_R2); 22 | 23 | return stepPerVolt; 24 | } 25 | 26 | void voltage_init() 27 | { 28 | pinMode(VOLTAGE_PIN, INPUT_FLOATING); 29 | 30 | float stepPerVolt = voltage_spv(); 31 | 32 | voltage_limit_l = VOLTAGE_LIMIT*stepPerVolt; 33 | voltage_limit_h = VOLTAGE_LIMIT*stepPerVolt + stepPerVolt*0.4; 34 | voltage_limit_s = VOLTAGE_SHUT*stepPerVolt; 35 | voltage_now = analogRead(VOLTAGE_PIN); 36 | voltage_is_error = false; 37 | } 38 | 39 | int voltage_last_ts = 0; 40 | 41 | void voltage_tick() 42 | { 43 | if (millis() - voltage_last_ts > 10) { 44 | voltage_last_ts = millis(); 45 | int newSample = analogRead(VOLTAGE_PIN); 46 | if (newSample < voltage_now) voltage_now--; 47 | if (newSample > voltage_now) voltage_now++; 48 | 49 | if (voltage_is_error) { 50 | if (voltage_now > voltage_limit_h || voltage_now < voltage_limit_s) { 51 | voltage_is_error = false; 52 | buzzer_stop(); 53 | } else { 54 | int duration = (millis() - voltage_error_start); 55 | 56 | if (duration > 45000) { 57 | digitalWrite(LIT, HIGH); 58 | } else if (duration > 30000) { 59 | if (!voltage_is_error_fast) { 60 | voltage_is_error_fast = true; 61 | buzzer_play(MELODY_ALERT_FAST, true); 62 | } 63 | } 64 | } 65 | } else { 66 | if (voltage_now < voltage_limit_l && voltage_now > voltage_limit_s) { 67 | voltage_is_error = true; 68 | voltage_is_error_fast = false; 69 | voltage_error_start = millis(); 70 | buzzer_play(MELODY_ALERT, true); 71 | } 72 | } 73 | } 74 | } 75 | 76 | float voltage_current() 77 | { 78 | return voltage_now / voltage_spv(); 79 | } 80 | 81 | TERMINAL_COMMAND(voltage, "Get the voltage") 82 | { 83 | terminal_io()->print("voltage="); 84 | terminal_io()->println((int)(10*voltage_current())); 85 | } 86 | -------------------------------------------------------------------------------- /firmware/src/voltage.h: -------------------------------------------------------------------------------- 1 | #ifndef _VOLTAGE_H 2 | #define _VOLTAGE_H 3 | 4 | #define VOLTAGE_PIN 7 5 | #define VOLTAGE_R1 20 6 | #define VOLTAGE_R2 10 7 | #define VOLTAGE_LIMIT 6.9 8 | #define VOLTAGE_SHUT 5.3 9 | 10 | #define LIT 22 11 | 12 | /** 13 | * Is there any voltage error? 14 | */ 15 | bool voltage_error(); 16 | 17 | /** 18 | * Initializes the voltage 19 | */ 20 | void voltage_init(); 21 | 22 | /** 23 | * Update the voltage 24 | */ 25 | void voltage_tick(); 26 | 27 | /** 28 | * Returns the current system voltage 29 | */ 30 | float voltage_current(); 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /mechanics/3d/README.md: -------------------------------------------------------------------------------- 1 | # Mechanics 2 | 3 | Here are the 3D mechanics parts, [documentation can be found here](/docs/bom.md) 4 | -------------------------------------------------------------------------------- /mechanics/3d/draws/body.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/3d/draws/body.pdf -------------------------------------------------------------------------------- /mechanics/3d/draws/leg.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/3d/draws/leg.pdf -------------------------------------------------------------------------------- /mechanics/3d/draws/side.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/3d/draws/side.pdf -------------------------------------------------------------------------------- /mechanics/3d/draws/u.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/3d/draws/u.pdf -------------------------------------------------------------------------------- /mechanics/3d/imgs/body.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/3d/imgs/body.png -------------------------------------------------------------------------------- /mechanics/3d/imgs/body_bottom.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/3d/imgs/body_bottom.png -------------------------------------------------------------------------------- /mechanics/3d/imgs/body_top.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/3d/imgs/body_top.png -------------------------------------------------------------------------------- /mechanics/3d/imgs/leg.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/3d/imgs/leg.png -------------------------------------------------------------------------------- /mechanics/3d/imgs/parts/body_bottom.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/3d/imgs/parts/body_bottom.png -------------------------------------------------------------------------------- /mechanics/3d/imgs/parts/body_top.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/3d/imgs/parts/body_top.png -------------------------------------------------------------------------------- /mechanics/3d/imgs/parts/leg.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/3d/imgs/parts/leg.png -------------------------------------------------------------------------------- /mechanics/3d/imgs/parts/side.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/3d/imgs/parts/side.png -------------------------------------------------------------------------------- /mechanics/3d/imgs/parts/u.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/3d/imgs/parts/u.png -------------------------------------------------------------------------------- /mechanics/3d/imgs/side.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/3d/imgs/side.png -------------------------------------------------------------------------------- /mechanics/3d/imgs/u.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/3d/imgs/u.png -------------------------------------------------------------------------------- /mechanics/3d/plater.conf: -------------------------------------------------------------------------------- 1 | body_bottom.stl 1 2 | body_top.stl 1 3 | leg.stl 4 4 | head.stl 1 5 | side.stl 8 6 | u.stl 8 7 | -------------------------------------------------------------------------------- /mechanics/3d/scad/.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | movie-out 3 | -------------------------------------------------------------------------------- /mechanics/3d/scad/Makefile: -------------------------------------------------------------------------------- 1 | # STLs to build 2 | PARTS := u side body_bottom body_top head leg 3 | 4 | # Output directory 5 | BUILD_DIR := build 6 | 7 | # Targets are $(BUILD_DIR)/file.stl 8 | TARGETS := $(addprefix $(BUILD_DIR)/, $(addsuffix .stl,$(PARTS))) 9 | 10 | all: $(TARGETS) 11 | 12 | build_dir: 13 | @mkdir -p $(BUILD_DIR) 14 | 15 | $(BUILD_DIR)/%.stl: metabot/print/%.scad build_dir 16 | @echo "Building $@..." 17 | @openscad $< -o $@ 18 | 19 | view: 20 | @openscad metabot/metabot.scad -o metabot.png --imgsize=1024,768 --camera=400,200,300,0,0,0 21 | @eog metabot.png 22 | 23 | csg: 24 | @openscad metabot/metabot.scad -o metabot.csg 25 | 26 | clean: 27 | rm -rf $(BUILD_DIR) *.csg movie-out 28 | -------------------------------------------------------------------------------- /mechanics/3d/scad/README.md: -------------------------------------------------------------------------------- 1 | # OpenSCAD parts 2 | 3 | This contains OpenSCAD version of the parts. 4 | 5 | Have a look to the [3D hacking](../../docs/hacking_3d.md) for more information. 6 | -------------------------------------------------------------------------------- /mechanics/3d/scad/joints/double_u.scad: -------------------------------------------------------------------------------- 1 | use <../parts/u.scad>; 2 | use <../models/motor_arm.scad>; 3 | 4 | module double_u(height=15, radius=8, thickness=2.2, screwsSpacing=10, screwsDiameter=2.6, screws=true, col="white", alpha=0) { 5 | translate([0,0,0]) { 6 | color(col) 7 | u(height, radius, thickness, screwsSpacing, screwsDiameter, screws); 8 | translate([0,(height+radius)*2,0]) { 9 | rotate([0,90,180]) { 10 | color(col) 11 | u(height, radius, thickness, screwsSpacing, screwsDiameter, screws); 12 | rotate([0,180,alpha]) { 13 | motorArm(); 14 | children(); 15 | } 16 | } 17 | } 18 | } 19 | } 20 | 21 | double_u(); 22 | -------------------------------------------------------------------------------- /mechanics/3d/scad/joints/side_to_side.scad: -------------------------------------------------------------------------------- 1 | include <../models/motor.scad>; 2 | use <../models/motor_arm.scad>; 3 | use <../parts/side.scad>; 4 | 5 | module side_to_side(size=40, holesToBorder=5, col="white", thickness=2.2) 6 | { 7 | for (side=[MotorWidth/2+thickness,-MotorWidth/2]) { 8 | translate([side,0,0]) { 9 | rotate([180,90,0]) { 10 | color(col) 11 | side(size, holesToBorder, thickness); 12 | } 13 | } 14 | } 15 | translate([0,-(size*2-2*holesToBorder),0]) { 16 | rotate([0,0,180]) { 17 | motorArm(); 18 | rotate([0,0,alpha]) 19 | children(); 20 | } 21 | } 22 | } 23 | 24 | side_to_side(); 25 | -------------------------------------------------------------------------------- /mechanics/3d/scad/metabot/body_bottom: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/3d/scad/metabot/body_bottom -------------------------------------------------------------------------------- /mechanics/3d/scad/metabot/config.scad: -------------------------------------------------------------------------------- 1 | // Color of the parts, for rendering 2 | PartsColor = [0.8, 0.8, 0.8]; 3 | 4 | // Head height 5 | HeadHeight=30; 6 | 7 | // Thickness of parts (2.2 is good for Ollo rivets) 8 | Thickness = 2.2; 9 | BodyThickness = 3; 10 | 11 | // Sizes of the parts of the leg 12 | // Ratio of leg parts 13 | LegSize = 100; 14 | LegSizeA = (0.60)*LegSize; 15 | LegSizeB = (0.20)*LegSize; 16 | LegSizeC = (0.20)*LegSize; 17 | 18 | // Width of the bottom part of the leg 19 | LegSizeBottom = 10; 20 | 21 | // Size of the "top" part of the legs, in mm 22 | LegSizeTop = 30; 23 | 24 | // Motor dimensions 25 | MotorWidth = 24; 26 | MotorHeight = 36; 27 | MotorDepth = 24; 28 | MotorArmOffset = 9; 29 | 30 | // U dimensions 31 | UHeight = 15.2; 32 | URadius = 8; 33 | UWidth = 30+Thickness/2; 34 | UTotalHeight = UHeight+URadius; 35 | UScrewsSpacing = 10; 36 | UScrewsDiameter= 3.3; 37 | 38 | // Side dimensions 39 | SideSize = 35; 40 | SideHolesToBorder = 5; 41 | 42 | // Defining the resolution 43 | $fn=35; 44 | 45 | module metabot_colorize() { 46 | color(PartsColor) 47 | children(); 48 | } 49 | -------------------------------------------------------------------------------- /mechanics/3d/scad/metabot/joints.scad: -------------------------------------------------------------------------------- 1 | include ; 2 | use <../joints/double_u.scad>; 3 | use <../joints/side_to_side.scad>; 4 | use <../parts/leg.scad>; 5 | 6 | module metabot_double_u(alpha=0) { 7 | double_u(UHeight, URadius, Thickness, UScrewsSpacing, UScrewsDiameter, col=PartsColor, alpha=alpha) { 8 | children(); 9 | } 10 | } 11 | 12 | module metabot_side_to_side(alpha=0) { 13 | side_to_side(SideSize, SideHolesToBorder, Thickness, col=PartsColor, alpha=alpha) { 14 | children(); 15 | } 16 | } 17 | 18 | module metabot_leg(print=false) { 19 | metabot_colorize() 20 | rotate([print ? 0 : 90,print ? 0 : 90,0]) 21 | leg(LegSizeA, LegSizeB, LegSizeC, LegSizeBottom, 22 | LegSizeTop, Thickness, print=print); 23 | } 24 | 25 | metabot_leg(); 26 | -------------------------------------------------------------------------------- /mechanics/3d/scad/metabot/metabot.scad: -------------------------------------------------------------------------------- 1 | include ; 2 | use <../util/screws.scad>; 3 | include <../models/ollo.scad>; 4 | use <../models/motor_arm.scad>; 5 | use ; 6 | use ; 7 | 8 | /** 9 | * Angles 10 | */ 11 | angles = [0, -30, -110]; 12 | 13 | module motor_on_body(alpha=0) { 14 | translate([0, 4*OlloSpacing, MotorDepth/2]) { 15 | motorArm(); 16 | rotate([0,0,alpha]) { 17 | children(); 18 | } 19 | } 20 | } 21 | 22 | module metabotLeg(a, b, c) { 23 | motor_on_body(a) { 24 | metabot_double_u(b) { 25 | metabot_side_to_side(c) { 26 | metabot_leg(); 27 | } 28 | } 29 | } 30 | } 31 | 32 | module metabot(angles = [0,0,0]) { 33 | metabot_body(type="bottom"); 34 | translate([0,0,MotorDepth+BodyThickness]) { 35 | metabot_body(type="top"); 36 | translate([0,0,-10]) 37 | metabot_head(); 38 | } 39 | 40 | rotate([0,0,45]) 41 | for (leg=[1:4]) { 42 | rotate([0,0,leg*360/4]) { 43 | translate([0,38,BodyThickness]) { 44 | metabotLeg(angles[0], angles[1], angles[2]); 45 | } 46 | } 47 | } 48 | } 49 | 50 | metabot(angles); -------------------------------------------------------------------------------- /mechanics/3d/scad/metabot/parts.scad: -------------------------------------------------------------------------------- 1 | include ; 2 | use <../util/screws.scad>; 3 | use <../parts/body.scad>; 4 | use <../parts/head.scad>; 5 | use <../parts/u.scad>; 6 | use <../parts/side.scad>; 7 | use <../parts/leg.scad>; 8 | 9 | module metabot_u(print=false) { 10 | metabot_colorize() 11 | u(UHeight, URadius, Thickness, UScrewsSpacing, UScrewsDiameter, print=print); 12 | } 13 | 14 | module metabot_body_screws() { 15 | squareScrews(BodyScrewsW, BodyScrewsH, BodyScrewsSize, Thickness); 16 | } 17 | 18 | module metabot_body(print=false, type="top") { 19 | metabot_colorize() 20 | body(BodyThickness, print=print, type=type); 21 | } 22 | 23 | module metabot_side(print=false) { 24 | metabot_colorize() 25 | side(SideSize, SideHolesToBorder, Thickness, print=print); 26 | } 27 | 28 | module metabot_head(print=false) { 29 | metabot_colorize() 30 | head(HeadHeight, Thickness, print=print); 31 | } 32 | -------------------------------------------------------------------------------- /mechanics/3d/scad/metabot/print/body_bottom.scad: -------------------------------------------------------------------------------- 1 | include <../config.scad>; 2 | use <../parts.scad>; 3 | 4 | metabot_body(print=true, type="bottom"); 5 | -------------------------------------------------------------------------------- /mechanics/3d/scad/metabot/print/body_projection.scad: -------------------------------------------------------------------------------- 1 | include <../config.scad>; 2 | use <../parts.scad>; 3 | 4 | projection() 5 | metabot_body(print=true); 6 | -------------------------------------------------------------------------------- /mechanics/3d/scad/metabot/print/body_top.scad: -------------------------------------------------------------------------------- 1 | include <../config.scad>; 2 | use <../parts.scad>; 3 | 4 | metabot_body(print=true, type="top"); 5 | -------------------------------------------------------------------------------- /mechanics/3d/scad/metabot/print/head.scad: -------------------------------------------------------------------------------- 1 | include <../config.scad>; 2 | use <../parts.scad>; 3 | 4 | metabot_head(print=true); 5 | -------------------------------------------------------------------------------- /mechanics/3d/scad/metabot/print/leg.scad: -------------------------------------------------------------------------------- 1 | include <../config.scad>; 2 | use <../joints.scad>; 3 | 4 | metabot_leg(print=true); 5 | -------------------------------------------------------------------------------- /mechanics/3d/scad/metabot/print/leg_projection.scad: -------------------------------------------------------------------------------- 1 | include <../config.scad>; 2 | use <../parts.scad>; 3 | 4 | projection() 5 | metabot_leg(print=true); 6 | -------------------------------------------------------------------------------- /mechanics/3d/scad/metabot/print/side.scad: -------------------------------------------------------------------------------- 1 | include <../config.scad>; 2 | use <../parts.scad>; 3 | 4 | metabot_side(print=true); 5 | -------------------------------------------------------------------------------- /mechanics/3d/scad/metabot/print/side_projection.scad: -------------------------------------------------------------------------------- 1 | include <../config.scad>; 2 | use <../parts.scad>; 3 | 4 | projection() 5 | metabot_side(print=true); 6 | -------------------------------------------------------------------------------- /mechanics/3d/scad/metabot/print/u.scad: -------------------------------------------------------------------------------- 1 | include <../config.scad>; 2 | use <../parts.scad>; 3 | 4 | metabot_u(print=true); 5 | -------------------------------------------------------------------------------- /mechanics/3d/scad/metabot/print/u_projection.scad: -------------------------------------------------------------------------------- 1 | include <../config.scad>; 2 | use <../parts.scad>; 3 | 4 | projection() 5 | metabot_u(print=true); 6 | -------------------------------------------------------------------------------- /mechanics/3d/scad/models/arm.scad: -------------------------------------------------------------------------------- 1 | include ; 2 | 3 | module arm(counter=false) { 4 | c = counter?[0.6,0.6,0.6]:[0.15,0.15,0.15]; 5 | color(c) { 6 | difference() { 7 | cylinder(d=20, h=OlloWidth); 8 | servoArm(OlloWidth); 9 | } 10 | } 11 | } 12 | 13 | arm(); 14 | -------------------------------------------------------------------------------- /mechanics/3d/scad/models/motor.scad: -------------------------------------------------------------------------------- 1 | include ; 2 | use <../util/rounded.scad>; 3 | 4 | // Motor dimensions 5 | MotorWidth = 24; 6 | MotorHeight = 36; 7 | MotorDepth = 24; 8 | MotorArmOffset = 9; 9 | 10 | module motor(width=2.2) { 11 | translate([0,-MotorArmOffset,-MotorDepth/2]) { 12 | color([0.25,0.25,0.25]) { 13 | difference() { 14 | rounded(MotorWidth,MotorHeight,MotorDepth,3,center=true); 15 | 16 | // Bottom 17 | translate([-MotorWidth/2-0.1,-MotorHeight/2-0.1,3]) 18 | cube([MotorWidth+0.2, 5.7, MotorDepth-6]); 19 | 20 | translate([0,-MotorHeight/2+3]) 21 | threeOllo(MotorDepth); 22 | 23 | // Motor shaft 24 | translate([0, MotorArmOffset]) 25 | olloHole(MotorDepth+5); 26 | 27 | // Side holes 28 | for (side=[-MotorWidth/2,MotorWidth/2-width]) { 29 | translate([side,9,MotorDepth/2]) { 30 | rotate([0,90,0]) { 31 | threeOllo(); 32 | translate([0,-3*OlloSpacing,0]) 33 | threeOllo(); 34 | } 35 | } 36 | } 37 | } 38 | } 39 | } 40 | } 41 | 42 | motor(); 43 | -------------------------------------------------------------------------------- /mechanics/3d/scad/models/motor_arm.scad: -------------------------------------------------------------------------------- 1 | include ; 2 | use ; 3 | 4 | module motorArm() { 5 | motor(); 6 | translate([0,0,MotorDepth/2]) 7 | arm(); 8 | translate([0,0,-MotorDepth/2-OlloWidth]) 9 | arm(true); 10 | } 11 | 12 | motorArm(); 13 | -------------------------------------------------------------------------------- /mechanics/3d/scad/models/ollo.scad: -------------------------------------------------------------------------------- 1 | // Diameter of plastic ollo holes 2 | OlloHoles = 4.3; 3 | 4 | // Width of Ollo "real" plastic 5 | OlloWidth = 3; 6 | 7 | // Distance between two ollo holes 8 | OlloSpacing = 6; 9 | 10 | // Do an Ollo hole on the part 11 | module olloHole(depth=2.2) 12 | { 13 | translate([0,0,-0.1]) 14 | cylinder(depth+0.2, OlloHoles/2, OlloHoles/2); 15 | } 16 | 17 | // Do 3 ollo holes 18 | module threeOllo(depth=2.2) 19 | { 20 | for (xy=[[-OlloSpacing,0],[0,0],[OlloSpacing,0]]) { 21 | translate([xy[0], xy[1], 0]) 22 | olloHole(depth); 23 | } 24 | } 25 | 26 | // Do a servo arm holes 27 | module servoArm(depth=2.2) 28 | { 29 | threeOllo(depth); 30 | rotate([0,0,90]) 31 | threeOllo(depth); 32 | } 33 | -------------------------------------------------------------------------------- /mechanics/3d/scad/parts/body.scad: -------------------------------------------------------------------------------- 1 | use <../models/ollo.scad>; 2 | use <../util/octopath.scad>; 3 | use <../util/repeat4.scad>; 4 | use <../util/hole3.scad>; 5 | 6 | $fn=25; 7 | 8 | module body(thickness=2.2, type="top", print=false) { 9 | // The layout path 10 | X=45; 11 | Y=15; 12 | 13 | difference() { 14 | // Making the exernal path 15 | octopath(X, Y, thickness=thickness); 16 | 17 | if (type == "top") { 18 | // Craving the board to make it accessible 19 | octopath(X-8, Y-8, thickness=99, center=true); 20 | 21 | // Adding holes for the magnets 22 | repeat4() 23 | translate([41,0,0]) 24 | hole3(); 25 | } else if (type == "bottom") { 26 | // Adding holes for the XBoard 27 | repeat4() 28 | translate([35, 11.5, 0]) 29 | hole3(); 30 | } 31 | 32 | // Motors fixation 33 | repeat4() 34 | rotate([0,0,45]) 35 | translate([38,0,0]) 36 | rotate([0,0,90]) 37 | threeOllo(thickness); 38 | } 39 | 40 | if (type == "bottom") { 41 | // GP-2 fixation 42 | translate([X,0,5.5]) 43 | difference() { 44 | cube([thickness, 44, 11], center=true); 45 | 46 | translate([0,-37/2,2]) 47 | rotate([0,90,0]) 48 | hole3(); 49 | 50 | translate([0,37/2,2]) 51 | rotate([0,90,0]) 52 | hole3(); 53 | 54 | } 55 | } 56 | } 57 | 58 | 59 | body(); -------------------------------------------------------------------------------- /mechanics/3d/scad/parts/head.scad: -------------------------------------------------------------------------------- 1 | use <../models/ollo.scad>; 2 | use <../util/octopath.scad>; 3 | use <../util/repeat4.scad>; 4 | use <../util/hole3.scad>; 5 | 6 | $fn=25; 7 | 8 | module head(height=25, thickness=2.2, print=false) { 9 | // The layout path 10 | X=48; 11 | Y=18; 12 | 13 | if (print) { 14 | translate([0,0,height]) 15 | rotate([-180,0,0]) 16 | head(height, thickness, false); 17 | } else { 18 | difference() { 19 | union() { 20 | difference() { 21 | octopath(X, Y, thickness=height); 22 | 23 | // Craving the inside 24 | translate([0,0,-0.1]) 25 | octopath(X-thickness, Y+3, thickness=height-thickness); 26 | } 27 | 28 | // If you don't have spacer, you can uncomment this and use long 29 | // screws 30 | /* 31 | repeat4() 32 | translate([41,0,0]) 33 | difference() { 34 | translate([0,0,10]) 35 | cylinder(d=6, h=height-10); 36 | translate([0,0,-1]) 37 | cylinder(d=4, h=height-10); 38 | } 39 | */ 40 | } 41 | 42 | 43 | // Holes for magnets 44 | repeat4() 45 | translate([41,0,0]) 46 | hole3(); 47 | } 48 | } 49 | } 50 | 51 | head(thickness=2.2); -------------------------------------------------------------------------------- /mechanics/3d/scad/parts/leg.scad: -------------------------------------------------------------------------------- 1 | use <../models/ollo.scad>; 2 | use <../util/rounded.scad>; 3 | 4 | module leg(sizeA=60, sizeB=20, sizeC=20, sizeBottom=10, 5 | sizeTop=15, thickness=2.2, print=false) 6 | { 7 | xOffset = 0; 8 | spacing = 30; 9 | 10 | module legSide() 11 | { 12 | difference() { 13 | union() { 14 | translate([0,0,sizeA/2]) 15 | rotate([0,90,0]) { 16 | cube([sizeA, 20, thickness], center=true); 17 | } 18 | } 19 | translate([-thickness, 0, sizeA-15]) { 20 | rotate([90,0,90]) { 21 | servoArm(2*thickness); 22 | } 23 | } 24 | } 25 | translate([-thickness/2,0,sizeA]) { 26 | rotate([90,0,90]) 27 | linear_extrude(thickness) 28 | polygon([[-10,0],[-1,sizeTop],[1,sizeTop],[10,0]]); 29 | } 30 | } 31 | 32 | module biais() { 33 | Dx = (spacing-sizeBottom)/2; 34 | Dy = sizeC; 35 | Dl = sqrt(pow(Dx,2)+pow(Dy,2)); 36 | 37 | translate([-(spacing/2)-thickness,-10,-(sizeB+thickness/2)]) 38 | rotate([0,atan2(Dy,Dx),0]) 39 | cube([Dl,20,thickness]); 40 | } 41 | 42 | if (print) { 43 | translate([0,0,10-xOffset]) 44 | rotate([90,0,0]) 45 | leg(sizeA, sizeB, sizeC, sizeBottom, 46 | sizeTop, thickness, false); 47 | } else { 48 | echo("[PART] leg"); 49 | translate([0,xOffset,-sizeA+15]) { 50 | cube([spacing+thickness*2, 20, thickness], center=true); 51 | 52 | translate([spacing/2+thickness/2,0,0]) 53 | legSide(); 54 | translate([-(spacing/2+thickness/2),0,0]) 55 | legSide(); 56 | 57 | translate([0,0,-sizeB]) 58 | cube([spacing+thickness*2, 20, thickness], center=true); 59 | translate([spacing/2+thickness/2,0,-sizeB/2]) 60 | cube([thickness,20,sizeB], center=true); 61 | translate([-(spacing/2+thickness/2),0,-sizeB/2]) 62 | cube([thickness,20,sizeB], center=true); 63 | 64 | translate([0,0,-(sizeB+sizeC)]) 65 | cube([sizeBottom+thickness*2, 20, thickness], center=true); 66 | 67 | biais(); 68 | mirror([1,0,0]) { 69 | biais(); 70 | } 71 | } 72 | } 73 | } 74 | 75 | leg(); 76 | -------------------------------------------------------------------------------- /mechanics/3d/scad/parts/side.scad: -------------------------------------------------------------------------------- 1 | include <../models/ollo.scad>; 2 | use <../util/rounded.scad>; 3 | 4 | module side(size=40, holesToBorder=5, thickness=2.2, print=false) { 5 | if (print) { 6 | translate([0,-size+holesToBorder,0]) 7 | side(size,holesToBorder,thickness,false); 8 | } else { 9 | echo("[PART] side"); 10 | translate([0,size-holesToBorder,0]) { 11 | difference() { 12 | rounded(20, size*2, thickness, 5, true); 13 | for (y=[size-holesToBorder, -size+holesToBorder, 14 | size-holesToBorder-3*OlloSpacing, -size+holesToBorder+3*OlloSpacing]) { 15 | translate([0, y, 0]) 16 | threeOllo(); 17 | } 18 | } 19 | } 20 | } 21 | } 22 | 23 | side(); 24 | -------------------------------------------------------------------------------- /mechanics/3d/scad/parts/u.scad: -------------------------------------------------------------------------------- 1 | use <../models/ollo.scad>; 2 | 3 | module u(height=15, radius=8, thickness=2.2, screwsSpacing=10, screwsDiameter=2.6, screws=true, olloScrew=false, widthSize=15, print=false) { 4 | module Ubranch() { 5 | union() { 6 | cylinder(thickness, 10, 10); 7 | translate([-10,0,0]) 8 | cube([20, height, thickness]); 9 | } 10 | } 11 | 12 | module USide() { 13 | difference() { 14 | union() { 15 | Ubranch(); 16 | translate([-10,height+radius-thickness,radius]) 17 | cube([20,thickness,widthSize-(radius-thickness)]); 18 | translate([-10,height,radius]) { 19 | rotate([0,90,0]) { 20 | difference() { 21 | cylinder(20,radius,radius); 22 | translate([0,0,-1]) 23 | cylinder(22,radius-thickness,radius-thickness); 24 | translate([-10+radius-thickness,-10,-1]) 25 | cube([10,10,22]); 26 | translate([-10,-20+radius-thickness,-1]) 27 | cube([10,20.01,22]); 28 | } 29 | } 30 | } 31 | } 32 | servoArm(depth=6); 33 | } 34 | } 35 | 36 | module UScrews() { 37 | for (x=[-screwsSpacing/2,screwsSpacing/2]) { 38 | for (y=[-screwsSpacing/2,screwsSpacing/2]) { 39 | rotate([270,0,0]) 40 | translate([x,y,0]) 41 | cylinder(d=screwsDiameter, h=100); 42 | } 43 | } 44 | } 45 | 46 | module UOllo() { 47 | rotate(-90, [1,0,0]) 48 | servoArm(2*height); 49 | } 50 | 51 | if (print) { 52 | translate([0,0,10]) 53 | rotate([0,90,0]) 54 | u(height,radius,thickness,screwsSpacing,screwsDiameter, 55 | screws,olloScrew,widthSize,false); 56 | } else { 57 | echo("[PART] u"); 58 | difference() { 59 | translate([0,0,-widthSize-thickness]) { 60 | USide(); 61 | mirror([0,0,1]) translate([0,0,-2*thickness-2*widthSize]) 62 | USide(); 63 | } 64 | if (screws) { 65 | UScrews(); 66 | } 67 | if (olloScrew) { 68 | UOllo(); 69 | } 70 | } 71 | } 72 | } 73 | 74 | u(); 75 | -------------------------------------------------------------------------------- /mechanics/3d/scad/util/bolt_trace.scad: -------------------------------------------------------------------------------- 1 | Debug=false; 2 | 3 | module bolt_trace(width=10, length=7, thickness=3) 4 | { 5 | d = length/(2*cos(30)); 6 | 7 | if (Debug) { 8 | color([1,0,0,0.3]) 9 | rotate([0,0,0*60]) 10 | cube([length,length,2],center=true); 11 | } 12 | 13 | linear_extrude(thickness) 14 | polygon([ 15 | [0,length/2], 16 | [cos(60)*d, sin(60)*d], 17 | [cos(0)*d, sin(0)*d], 18 | [cos(-60)*d, sin(-60)*d], 19 | [0,-length/2], 20 | [-width,-length/2], 21 | [-width,length/2] 22 | ]); 23 | } 24 | 25 | bolt_trace(); -------------------------------------------------------------------------------- /mechanics/3d/scad/util/hole3.scad: -------------------------------------------------------------------------------- 1 | 2 | module hole3() 3 | { 4 | cylinder(d=3.5, h=99, center=true); 5 | } 6 | -------------------------------------------------------------------------------- /mechanics/3d/scad/util/octopath.scad: -------------------------------------------------------------------------------- 1 | // Follow an octogonal path 2 | module octopath(X, Y, thickness, center=false) 3 | { 4 | union() { 5 | linear_extrude(thickness, center=center) 6 | polygon(points=[ 7 | [X, Y], [X, -Y], 8 | [Y, -X], [-Y, -X], 9 | [-X, -Y], [-X, Y], 10 | [-Y, X], [Y, X], 11 | ]); 12 | } 13 | } 14 | -------------------------------------------------------------------------------- /mechanics/3d/scad/util/repeat4.scad: -------------------------------------------------------------------------------- 1 | 2 | module repeat4() 3 | { 4 | for (r=[0:3]) 5 | rotate([0,0,90*r]) 6 | children(); 7 | } 8 | -------------------------------------------------------------------------------- /mechanics/3d/scad/util/rounded.scad: -------------------------------------------------------------------------------- 1 | // Do a rounded cube 2 | module rounded(x=10, y=10, z=10, r=3, center=false) { 3 | if (center) { 4 | translate([-(x/2),-(y/2),0]) 5 | rounded(x, y, z, r); 6 | } else { 7 | translate([r,r,0]) 8 | minkowski() { 9 | cube([x-2*r,y-2*r,z/2]); 10 | cylinder(h=z/2, r=r); 11 | } 12 | } 13 | } 14 | -------------------------------------------------------------------------------- /mechanics/3d/scad/util/screws.scad: -------------------------------------------------------------------------------- 1 | module screws(count=4, radius=5, size=3.3, thickness=2.2) 2 | { 3 | for (screw=[1:count]) { 4 | rotate([0,0,screw*360/count]) 5 | translate([radius,0,-1]) 6 | cylinder(d=size, h=thickness+2); 7 | } 8 | } 9 | 10 | module squareScrews(w=10, h=10, size=3.3, thickness=2.2) { 11 | for (X=[-w/2, w/2]) 12 | for (Y=[-h/2, h/2]) 13 | translate([X,Y,-1]) 14 | cylinder(d=size, h=thickness+2); 15 | } -------------------------------------------------------------------------------- /mechanics/3d/scad/util/thickLine.scad: -------------------------------------------------------------------------------- 1 | //Draw a line with given width in XY plane with given height 2 | //in Z direction from point 1 to point 2 3 | module thickLine(pt1=[0,0], pt2=[1,0], width=1, height=1) { 4 | dist = sqrt( 5 | (pt1[0]-pt2[0])*(pt1[0]-pt2[0])+ 6 | (pt1[1]-pt2[1])*(pt1[1]-pt2[1])); 7 | translate([pt1[0],pt1[1],0]) 8 | rotate(atan2(pt2[1]-pt1[1], pt2[0]-pt1[0])) 9 | translate([0,-width/2,0]) 10 | cube([dist, width, height]); 11 | } 12 | 13 | -------------------------------------------------------------------------------- /mechanics/README.md: -------------------------------------------------------------------------------- 1 | # Mechanics 2 | 3 | Here are the mechanics parts, [documentation can be found here](/doc/bom.md) 4 | -------------------------------------------------------------------------------- /mechanics/laser/README.md: -------------------------------------------------------------------------------- 1 | # Mechanics 2 | 3 | Here are the laser mechanics parts, [documentation can be found here](/docs/bom.md) 4 | -------------------------------------------------------------------------------- /mechanics/laser/autodesk/body_bottom.ipt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/autodesk/body_bottom.ipt -------------------------------------------------------------------------------- /mechanics/laser/autodesk/body_top.ipt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/autodesk/body_top.ipt -------------------------------------------------------------------------------- /mechanics/laser/autodesk/entretoise.ipt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/autodesk/entretoise.ipt -------------------------------------------------------------------------------- /mechanics/laser/autodesk/head.ipt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/autodesk/head.ipt -------------------------------------------------------------------------------- /mechanics/laser/autodesk/leg.ipt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/autodesk/leg.ipt -------------------------------------------------------------------------------- /mechanics/laser/autodesk/models/arm.ipt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/autodesk/models/arm.ipt -------------------------------------------------------------------------------- /mechanics/laser/autodesk/models/counter_arm.ipt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/autodesk/models/counter_arm.ipt -------------------------------------------------------------------------------- /mechanics/laser/autodesk/models/motor.ipt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/autodesk/models/motor.ipt -------------------------------------------------------------------------------- /mechanics/laser/autodesk/models/motor_arm.iam: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/autodesk/models/motor_arm.iam -------------------------------------------------------------------------------- /mechanics/laser/autodesk/models/opencm.ipt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/autodesk/models/opencm.ipt -------------------------------------------------------------------------------- /mechanics/laser/autodesk/models/x_board.ipt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/autodesk/models/x_board.ipt -------------------------------------------------------------------------------- /mechanics/laser/autodesk/parameters.ipt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/autodesk/parameters.ipt -------------------------------------------------------------------------------- /mechanics/laser/autodesk/pmma.styxml: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/autodesk/pmma.styxml -------------------------------------------------------------------------------- /mechanics/laser/autodesk/robot.dwg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/autodesk/robot.dwg -------------------------------------------------------------------------------- /mechanics/laser/autodesk/robot.iam: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/autodesk/robot.iam -------------------------------------------------------------------------------- /mechanics/laser/autodesk/robot.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/autodesk/robot.pdf -------------------------------------------------------------------------------- /mechanics/laser/autodesk/side.ipt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/autodesk/side.ipt -------------------------------------------------------------------------------- /mechanics/laser/autodesk/spacer.ipt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/autodesk/spacer.ipt -------------------------------------------------------------------------------- /mechanics/laser/autodesk/tip.ipt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/autodesk/tip.ipt -------------------------------------------------------------------------------- /mechanics/laser/autodesk/tip_up.ipt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/autodesk/tip_up.ipt -------------------------------------------------------------------------------- /mechanics/laser/autodesk/u.ipt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/autodesk/u.ipt -------------------------------------------------------------------------------- /mechanics/laser/autodesk/u_pattern/u_base.ipt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/autodesk/u_pattern/u_base.ipt -------------------------------------------------------------------------------- /mechanics/laser/autodesk/u_pattern/u_base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/autodesk/u_pattern/u_base.stl -------------------------------------------------------------------------------- /mechanics/laser/autodesk/u_pattern/u_base_hex.ipt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/autodesk/u_pattern/u_base_hex.ipt -------------------------------------------------------------------------------- /mechanics/laser/autodesk/u_pattern/u_base_hex.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/autodesk/u_pattern/u_base_hex.stl -------------------------------------------------------------------------------- /mechanics/laser/autodesk/u_pattern/u_side.ipt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/autodesk/u_pattern/u_side.ipt -------------------------------------------------------------------------------- /mechanics/laser/autodesk/u_pattern/u_side.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/autodesk/u_pattern/u_side.stl -------------------------------------------------------------------------------- /mechanics/laser/body_bottom.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/body_bottom.stl -------------------------------------------------------------------------------- /mechanics/laser/body_top.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/body_top.stl -------------------------------------------------------------------------------- /mechanics/laser/doc/body_bending.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/doc/body_bending.pdf -------------------------------------------------------------------------------- /mechanics/laser/doc/head_bending.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/doc/head_bending.pdf -------------------------------------------------------------------------------- /mechanics/laser/doc/u_bending.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/doc/u_bending.pdf -------------------------------------------------------------------------------- /mechanics/laser/head.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/head.stl -------------------------------------------------------------------------------- /mechanics/laser/leg.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/leg.stl -------------------------------------------------------------------------------- /mechanics/laser/metabot.rld: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/metabot.rld -------------------------------------------------------------------------------- /mechanics/laser/parts/body_top.dxf: -------------------------------------------------------------------------------- 1 | 999 2 | DXF created from Projekt 3 | 0 4 | SECTION 5 | 2 6 | HEADER 7 | 9 8 | $ACADVER 9 | 1 10 | AC1006 11 | 9 12 | $INSUNITS 13 | 70 14 | 4 15 | 9 16 | $INSBASE 17 | 10 18 | 0.0 19 | 20 20 | 0.0 21 | 30 22 | 0.0 23 | 9 24 | $EXTMIN 25 | 10 26 | -47.01 27 | 20 28 | -47.01 29 | 9 30 | $EXTMAX 31 | 10 32 | 47.01 33 | 20 34 | 47.01 35 | 0 36 | ENDSEC 37 | 0 38 | SECTION 39 | 2 40 | TABLES 41 | 0 42 | TABLE 43 | 2 44 | LTYPE 45 | 70 46 | 1 47 | 0 48 | LTYPE 49 | 2 50 | CONTINUOUS 51 | 70 52 | 64 53 | 3 54 | Solid line 55 | 72 56 | 65 57 | 73 58 | 0 59 | 40 60 | 0.000000 61 | 0 62 | ENDTAB 63 | 0 64 | TABLE 65 | 2 66 | LAYER 67 | 70 68 | 6 69 | 0 70 | LAYER 71 | 2 72 | 1 73 | 70 74 | 64 75 | 62 76 | 7 77 | 6 78 | CONTINUOUS 79 | 0 80 | ENDTAB 81 | 0 82 | TABLE 83 | 2 84 | STYLE 85 | 70 86 | 0 87 | 0 88 | ENDTAB 89 | 0 90 | ENDSEC 91 | 0 92 | SECTION 93 | 2 94 | BLOCKS 95 | 0 96 | ENDSEC 97 | 0 98 | SECTION 99 | 2 100 | ENTITIES 101 | 0 102 | LINE 103 | 8 104 | 1 105 | 10 106 | 47.01 107 | 20 108 | -15.832 109 | 11 110 | 47.01 111 | 21 112 | 15.832 113 | 62 114 | 0 115 | 0 116 | LINE 117 | 8 118 | 1 119 | 10 120 | 47.01 121 | 20 122 | 15.832 123 | 11 124 | 15.832 125 | 21 126 | 47.01 127 | 62 128 | 0 129 | 0 130 | LINE 131 | 8 132 | 1 133 | 10 134 | 15.832 135 | 20 136 | 47.01 137 | 11 138 | -15.832 139 | 21 140 | 47.01 141 | 62 142 | 0 143 | 0 144 | LINE 145 | 8 146 | 1 147 | 10 148 | -15.832 149 | 20 150 | 47.01 151 | 11 152 | -47.01 153 | 21 154 | 15.832 155 | 62 156 | 0 157 | 0 158 | LINE 159 | 8 160 | 1 161 | 10 162 | -47.01 163 | 20 164 | 15.832 165 | 11 166 | -47.01 167 | 21 168 | -15.832 169 | 62 170 | 0 171 | 0 172 | LINE 173 | 8 174 | 1 175 | 10 176 | -47.01 177 | 20 178 | -15.832 179 | 11 180 | -15.832 181 | 21 182 | -47.01 183 | 62 184 | 0 185 | 0 186 | LINE 187 | 8 188 | 1 189 | 10 190 | -15.832 191 | 20 192 | -47.01 193 | 11 194 | 15.832 195 | 21 196 | -47.01 197 | 62 198 | 0 199 | 0 200 | LINE 201 | 8 202 | 1 203 | 10 204 | 15.832 205 | 20 206 | -47.01 207 | 11 208 | 47.01 209 | 21 210 | -15.832 211 | 62 212 | 0 213 | 0 214 | CIRCLE 215 | 8 216 | 1 217 | 10 218 | 0 219 | 20 220 | 40.999 221 | 40 222 | 1.58957 223 | 62 224 | 0 225 | 0 226 | LINE 227 | 8 228 | 1 229 | 10 230 | -11.268 231 | 20 232 | -35.99 233 | 11 234 | -35.99 235 | 21 236 | -11.268 237 | 62 238 | 0 239 | 0 240 | LINE 241 | 8 242 | 1 243 | 10 244 | -35.99 245 | 20 246 | -11.268 247 | 11 248 | -35.99 249 | 21 250 | 11.268 251 | 62 252 | 0 253 | 0 254 | LINE 255 | 8 256 | 1 257 | 10 258 | -35.99 259 | 20 260 | 11.268 261 | 11 262 | -11.268 263 | 21 264 | 35.99 265 | 62 266 | 0 267 | 0 268 | LINE 269 | 8 270 | 1 271 | 10 272 | -11.268 273 | 20 274 | 35.99 275 | 11 276 | 11.268 277 | 21 278 | 35.99 279 | 62 280 | 0 281 | 0 282 | LINE 283 | 8 284 | 1 285 | 10 286 | 11.268 287 | 20 288 | 35.99 289 | 11 290 | 35.99 291 | 21 292 | 11.268 293 | 62 294 | 0 295 | 0 296 | LINE 297 | 8 298 | 1 299 | 10 300 | 35.99 301 | 20 302 | 11.268 303 | 11 304 | 35.99 305 | 21 306 | -11.268 307 | 62 308 | 0 309 | 0 310 | LINE 311 | 8 312 | 1 313 | 10 314 | 35.99 315 | 20 316 | -11.268 317 | 11 318 | 11.268 319 | 21 320 | -35.99 321 | 62 322 | 0 323 | 0 324 | LINE 325 | 8 326 | 1 327 | 10 328 | 11.268 329 | 20 330 | -35.99 331 | 11 332 | -11.268 333 | 21 334 | -35.99 335 | 62 336 | 0 337 | 0 338 | CIRCLE 339 | 8 340 | 1 341 | 10 342 | -22.221 343 | 20 344 | 30.706 345 | 40 346 | 1.98989 347 | 62 348 | 0 349 | 0 350 | CIRCLE 351 | 8 352 | 1 353 | 10 354 | 22.22 355 | 20 356 | 30.706 357 | 40 358 | 1.98989 359 | 62 360 | 0 361 | 0 362 | CIRCLE 363 | 8 364 | 1 365 | 10 366 | -26.464 367 | 20 368 | 26.465 369 | 40 370 | 1.98966 371 | 62 372 | 0 373 | 0 374 | CIRCLE 375 | 8 376 | 1 377 | 10 378 | 26.463 379 | 20 380 | 26.465 381 | 40 382 | 1.98971 383 | 62 384 | 0 385 | 0 386 | CIRCLE 387 | 8 388 | 1 389 | 10 390 | -30.707 391 | 20 392 | 22.222 393 | 40 394 | 1.98976 395 | 62 396 | 0 397 | 0 398 | CIRCLE 399 | 8 400 | 1 401 | 10 402 | 30.706 403 | 20 404 | 22.222 405 | 40 406 | 1.98979 407 | 62 408 | 0 409 | 0 410 | CIRCLE 411 | 8 412 | 1 413 | 10 414 | 40.999 415 | 20 416 | 0 417 | 40 418 | 1.58957 419 | 62 420 | 0 421 | 0 422 | CIRCLE 423 | 8 424 | 1 425 | 10 426 | -40.999 427 | 20 428 | 0 429 | 40 430 | 1.58957 431 | 62 432 | 0 433 | 0 434 | CIRCLE 435 | 8 436 | 1 437 | 10 438 | -30.706 439 | 20 440 | -22.221 441 | 40 442 | 1.9898 443 | 62 444 | 0 445 | 0 446 | CIRCLE 447 | 8 448 | 1 449 | 10 450 | 30.706 451 | 20 452 | -22.221 453 | 40 454 | 1.98986 455 | 62 456 | 0 457 | 0 458 | CIRCLE 459 | 8 460 | 1 461 | 10 462 | -26.464 463 | 20 464 | -26.462 465 | 40 466 | 1.98964 467 | 62 468 | 0 469 | 0 470 | CIRCLE 471 | 8 472 | 1 473 | 10 474 | 26.463 475 | 20 476 | -26.462 477 | 40 478 | 1.98968 479 | 62 480 | 0 481 | 0 482 | CIRCLE 483 | 8 484 | 1 485 | 10 486 | -22.221 487 | 20 488 | -30.705 489 | 40 490 | 1.98983 491 | 62 492 | 0 493 | 0 494 | CIRCLE 495 | 8 496 | 1 497 | 10 498 | 22.22 499 | 20 500 | -30.705 501 | 40 502 | 1.98982 503 | 62 504 | 0 505 | 0 506 | CIRCLE 507 | 8 508 | 1 509 | 10 510 | 0 511 | 20 512 | -40.999 513 | 40 514 | 1.58957 515 | 62 516 | 0 517 | 0 518 | ENDSEC 519 | 0 520 | EOF 521 | -------------------------------------------------------------------------------- /mechanics/laser/parts/leg.dxf: -------------------------------------------------------------------------------- 1 | 999 2 | DXF created from Projekt 3 | 0 4 | SECTION 5 | 2 6 | HEADER 7 | 9 8 | $ACADVER 9 | 1 10 | AC1006 11 | 9 12 | $INSUNITS 13 | 70 14 | 4 15 | 9 16 | $INSBASE 17 | 10 18 | 0.0 19 | 20 20 | 0.0 21 | 30 22 | 0.0 23 | 9 24 | $EXTMIN 25 | 10 26 | -41.493 27 | 20 28 | -11.486 29 | 9 30 | $EXTMAX 31 | 10 32 | 85.01 33 | 20 34 | 11.489 35 | 0 36 | ENDSEC 37 | 0 38 | SECTION 39 | 2 40 | TABLES 41 | 0 42 | TABLE 43 | 2 44 | LTYPE 45 | 70 46 | 1 47 | 0 48 | LTYPE 49 | 2 50 | CONTINUOUS 51 | 70 52 | 64 53 | 3 54 | Solid line 55 | 72 56 | 65 57 | 73 58 | 0 59 | 40 60 | 0.000000 61 | 0 62 | ENDTAB 63 | 0 64 | TABLE 65 | 2 66 | LAYER 67 | 70 68 | 6 69 | 0 70 | LAYER 71 | 2 72 | 1 73 | 70 74 | 64 75 | 62 76 | 7 77 | 6 78 | CONTINUOUS 79 | 0 80 | ENDTAB 81 | 0 82 | TABLE 83 | 2 84 | STYLE 85 | 70 86 | 0 87 | 0 88 | ENDTAB 89 | 0 90 | ENDSEC 91 | 0 92 | SECTION 93 | 2 94 | BLOCKS 95 | 0 96 | ENDSEC 97 | 0 98 | SECTION 99 | 2 100 | ENTITIES 101 | 0 102 | LINE 103 | 8 104 | 1 105 | 10 106 | 21.427 107 | 20 108 | -11.324 109 | 11 110 | 85.01 111 | 21 112 | -4.009 113 | 62 114 | 0 115 | 0 116 | LINE 117 | 8 118 | 1 119 | 10 120 | 85.01 121 | 20 122 | -4.009 123 | 11 124 | 85.01 125 | 21 126 | 4.009 127 | 62 128 | 0 129 | 0 130 | LINE 131 | 8 132 | 1 133 | 10 134 | 85.01 135 | 20 136 | 4.009 137 | 11 138 | 18.561 139 | 21 140 | 11.489 141 | 62 142 | 0 143 | 0 144 | LINE 145 | 8 146 | 1 147 | 10 148 | 18.561 149 | 20 150 | 11.489 151 | 11 152 | -6.431 153 | 21 154 | 11.299 155 | 62 156 | 0 157 | 0 158 | LINE 159 | 8 160 | 1 161 | 10 162 | -6.431 163 | 20 164 | 11.299 165 | 11 166 | -10.98 167 | 21 168 | 10.668 169 | 62 170 | 0 171 | 0 172 | LINE 173 | 8 174 | 1 175 | 10 176 | -10.98 177 | 20 178 | 10.668 179 | 11 180 | -38.915 181 | 21 182 | 3.753 183 | 62 184 | 0 185 | 0 186 | LINE 187 | 8 188 | 1 189 | 10 190 | -38.915 191 | 20 192 | 3.753 193 | 11 194 | -39.309 195 | 21 196 | 3.582 197 | 62 198 | 0 199 | 0 200 | LINE 201 | 8 202 | 1 203 | 10 204 | -39.309 205 | 20 206 | 3.582 207 | 11 208 | -39.704 209 | 21 210 | 3.353 211 | 62 212 | 0 213 | 0 214 | LINE 215 | 8 216 | 1 217 | 10 218 | -39.704 219 | 20 220 | 3.353 221 | 11 222 | -40.051 223 | 21 224 | 3.1 225 | 62 226 | 0 227 | 0 228 | LINE 229 | 8 230 | 1 231 | 10 232 | -40.051 233 | 20 234 | 3.1 235 | 11 236 | -40.668 237 | 21 238 | 2.467 239 | 62 240 | 0 241 | 0 242 | LINE 243 | 8 244 | 1 245 | 10 246 | -40.668 247 | 20 248 | 2.467 249 | 11 250 | -41.132 251 | 21 252 | 1.715 253 | 62 254 | 0 255 | 0 256 | LINE 257 | 8 258 | 1 259 | 10 260 | -41.132 261 | 20 262 | 1.715 263 | 11 264 | -41.42 265 | 21 266 | 0.879 267 | 62 268 | 0 269 | 0 270 | LINE 271 | 8 272 | 1 273 | 10 274 | -41.42 275 | 20 276 | 0.879 277 | 11 278 | -41.493 279 | 21 280 | 0.442 281 | 62 282 | 0 283 | 0 284 | LINE 285 | 8 286 | 1 287 | 10 288 | -41.493 289 | 20 290 | 0.442 291 | 11 292 | -41.493 293 | 21 294 | -0.442 295 | 62 296 | 0 297 | 0 298 | LINE 299 | 8 300 | 1 301 | 10 302 | -41.493 303 | 20 304 | -0.442 305 | 11 306 | -41.42 307 | 21 308 | -0.878 309 | 62 310 | 0 311 | 0 312 | LINE 313 | 8 314 | 1 315 | 10 316 | -41.42 317 | 20 318 | -0.878 319 | 11 320 | -41.293 321 | 21 322 | -1.317 323 | 62 324 | 0 325 | 0 326 | LINE 327 | 8 328 | 1 329 | 10 330 | -41.293 331 | 20 332 | -1.317 333 | 11 334 | -41.132 335 | 21 336 | -1.714 337 | 62 338 | 0 339 | 0 340 | LINE 341 | 8 342 | 1 343 | 10 344 | -41.132 345 | 20 346 | -1.714 347 | 11 348 | -40.913 349 | 21 350 | -2.116 351 | 62 352 | 0 353 | 0 354 | LINE 355 | 8 356 | 1 357 | 10 358 | -40.913 359 | 20 360 | -2.116 361 | 11 362 | -40.659 363 | 21 364 | -2.478 365 | 62 366 | 0 367 | 0 368 | LINE 369 | 8 370 | 1 371 | 10 372 | -40.659 373 | 20 374 | -2.478 375 | 11 376 | -40.366 377 | 21 378 | -2.811 379 | 62 380 | 0 381 | 0 382 | LINE 383 | 8 384 | 1 385 | 10 386 | -40.366 387 | 20 388 | -2.811 389 | 11 390 | -40.05 391 | 21 392 | -3.1 393 | 62 394 | 0 395 | 0 396 | LINE 397 | 8 398 | 1 399 | 10 400 | -40.05 401 | 20 402 | -3.1 403 | 11 404 | -39.309 405 | 21 406 | -3.582 407 | 62 408 | 0 409 | 0 410 | LINE 411 | 8 412 | 1 413 | 10 414 | -39.309 415 | 20 416 | -3.582 417 | 11 418 | -38.48 419 | 21 420 | -3.89 421 | 62 422 | 0 423 | 0 424 | LINE 425 | 8 426 | 1 427 | 10 428 | -38.48 429 | 20 430 | -3.89 431 | 11 432 | -9.472 433 | 21 434 | -10.925 435 | 62 436 | 0 437 | 0 438 | LINE 439 | 8 440 | 1 441 | 10 442 | -9.472 443 | 20 444 | -10.925 445 | 11 446 | -3.376 447 | 21 448 | -11.486 449 | 62 450 | 0 451 | 0 452 | LINE 453 | 8 454 | 1 455 | 10 456 | -3.376 457 | 20 458 | -11.486 459 | 11 460 | 21.427 461 | 21 462 | -11.324 463 | 62 464 | 0 465 | 0 466 | CIRCLE 467 | 8 468 | 1 469 | 10 470 | 0 471 | 20 472 | 5.999 473 | 40 474 | 2.01457 475 | 62 476 | 0 477 | 0 478 | CIRCLE 479 | 8 480 | 1 481 | 10 482 | -5.999 483 | 20 484 | 0 485 | 40 486 | 2.01457 487 | 62 488 | 0 489 | 0 490 | CIRCLE 491 | 8 492 | 1 493 | 10 494 | 0 495 | 20 496 | 0 497 | 40 498 | 2.01429 499 | 62 500 | 0 501 | 0 502 | CIRCLE 503 | 8 504 | 1 505 | 10 506 | 5.999 507 | 20 508 | 0 509 | 40 510 | 2.01457 511 | 62 512 | 0 513 | 0 514 | CIRCLE 515 | 8 516 | 1 517 | 10 518 | 55.45 519 | 20 520 | 0 521 | 40 522 | 2.03909 523 | 62 524 | 0 525 | 0 526 | LINE 527 | 8 528 | 1 529 | 10 530 | 74.91 531 | 20 532 | -1.565 533 | 11 534 | 74.91 535 | 21 536 | 1.565 537 | 62 538 | 0 539 | 0 540 | LINE 541 | 8 542 | 1 543 | 10 544 | 74.91 545 | 20 546 | 1.565 547 | 11 548 | 81.99 549 | 21 550 | 1.565 551 | 62 552 | 0 553 | 0 554 | LINE 555 | 8 556 | 1 557 | 10 558 | 81.99 559 | 20 560 | 1.565 561 | 11 562 | 81.99 563 | 21 564 | -1.565 565 | 62 566 | 0 567 | 0 568 | LINE 569 | 8 570 | 1 571 | 10 572 | 81.99 573 | 20 574 | -1.565 575 | 11 576 | 74.91 577 | 21 578 | -1.565 579 | 62 580 | 0 581 | 0 582 | LINE 583 | 8 584 | 1 585 | 10 586 | 60.91 587 | 20 588 | -1.565 589 | 11 590 | 60.91 591 | 21 592 | 1.565 593 | 62 594 | 0 595 | 0 596 | LINE 597 | 8 598 | 1 599 | 10 600 | 60.91 601 | 20 602 | 1.565 603 | 11 604 | 67.99 605 | 21 606 | 1.565 607 | 62 608 | 0 609 | 0 610 | LINE 611 | 8 612 | 1 613 | 10 614 | 67.99 615 | 20 616 | 1.565 617 | 11 618 | 67.99 619 | 21 620 | -1.565 621 | 62 622 | 0 623 | 0 624 | LINE 625 | 8 626 | 1 627 | 10 628 | 67.99 629 | 20 630 | -1.565 631 | 11 632 | 60.91 633 | 21 634 | -1.565 635 | 62 636 | 0 637 | 0 638 | LINE 639 | 8 640 | 1 641 | 10 642 | 42.91 643 | 20 644 | -1.565 645 | 11 646 | 42.91 647 | 21 648 | 1.565 649 | 62 650 | 0 651 | 0 652 | LINE 653 | 8 654 | 1 655 | 10 656 | 42.91 657 | 20 658 | 1.565 659 | 11 660 | 49.99 661 | 21 662 | 1.565 663 | 62 664 | 0 665 | 0 666 | LINE 667 | 8 668 | 1 669 | 10 670 | 49.99 671 | 20 672 | 1.565 673 | 11 674 | 49.99 675 | 21 676 | -1.565 677 | 62 678 | 0 679 | 0 680 | LINE 681 | 8 682 | 1 683 | 10 684 | 49.99 685 | 20 686 | -1.565 687 | 11 688 | 42.91 689 | 21 690 | -1.565 691 | 62 692 | 0 693 | 0 694 | CIRCLE 695 | 8 696 | 1 697 | 10 698 | 0 699 | 20 700 | -5.999 701 | 40 702 | 2.01457 703 | 62 704 | 0 705 | 0 706 | CIRCLE 707 | 8 708 | 1 709 | 10 710 | 0 711 | 20 712 | 5.999 713 | 40 714 | 2.71459 715 | 62 716 | 1 717 | 0 718 | CIRCLE 719 | 8 720 | 1 721 | 10 722 | 0 723 | 20 724 | 5.999 725 | 40 726 | 2.01457 727 | 62 728 | 1 729 | 0 730 | CIRCLE 731 | 8 732 | 1 733 | 10 734 | -5.999 735 | 20 736 | 0 737 | 40 738 | 2.71457 739 | 62 740 | 1 741 | 0 742 | CIRCLE 743 | 8 744 | 1 745 | 10 746 | 0 747 | 20 748 | 0 749 | 40 750 | 2.71427 751 | 62 752 | 1 753 | 0 754 | CIRCLE 755 | 8 756 | 1 757 | 10 758 | 5.999 759 | 20 760 | 0 761 | 40 762 | 2.71457 763 | 62 764 | 1 765 | 0 766 | CIRCLE 767 | 8 768 | 1 769 | 10 770 | 5.999 771 | 20 772 | 0 773 | 40 774 | 2.01457 775 | 62 776 | 1 777 | 0 778 | CIRCLE 779 | 8 780 | 1 781 | 10 782 | 0 783 | 20 784 | 0 785 | 40 786 | 2.01429 787 | 62 788 | 1 789 | 0 790 | CIRCLE 791 | 8 792 | 1 793 | 10 794 | -5.999 795 | 20 796 | 0 797 | 40 798 | 2.01457 799 | 62 800 | 1 801 | 0 802 | CIRCLE 803 | 8 804 | 1 805 | 10 806 | 0 807 | 20 808 | -5.999 809 | 40 810 | 2.71459 811 | 62 812 | 1 813 | 0 814 | CIRCLE 815 | 8 816 | 1 817 | 10 818 | 0 819 | 20 820 | -5.999 821 | 40 822 | 2.01457 823 | 62 824 | 1 825 | 0 826 | ENDSEC 827 | 0 828 | EOF 829 | -------------------------------------------------------------------------------- /mechanics/laser/parts/side.dxf: -------------------------------------------------------------------------------- 1 | 999 2 | DXF created from Projekt 3 | 0 4 | SECTION 5 | 2 6 | HEADER 7 | 9 8 | $ACADVER 9 | 1 10 | AC1006 11 | 9 12 | $INSUNITS 13 | 70 14 | 4 15 | 9 16 | $INSBASE 17 | 10 18 | 0.0 19 | 20 20 | 0.0 21 | 30 22 | 0.0 23 | 9 24 | $EXTMIN 25 | 10 26 | -22.01 27 | 20 28 | -0.01 29 | 9 30 | $EXTMAX 31 | 10 32 | 0.01 33 | 20 34 | 66.01 35 | 0 36 | ENDSEC 37 | 0 38 | SECTION 39 | 2 40 | TABLES 41 | 0 42 | TABLE 43 | 2 44 | LTYPE 45 | 70 46 | 1 47 | 0 48 | LTYPE 49 | 2 50 | CONTINUOUS 51 | 70 52 | 64 53 | 3 54 | Solid line 55 | 72 56 | 65 57 | 73 58 | 0 59 | 40 60 | 0.000000 61 | 0 62 | ENDTAB 63 | 0 64 | TABLE 65 | 2 66 | LAYER 67 | 70 68 | 6 69 | 0 70 | LAYER 71 | 2 72 | 1 73 | 70 74 | 64 75 | 62 76 | 7 77 | 6 78 | CONTINUOUS 79 | 0 80 | ENDTAB 81 | 0 82 | TABLE 83 | 2 84 | STYLE 85 | 70 86 | 0 87 | 0 88 | ENDTAB 89 | 0 90 | ENDSEC 91 | 0 92 | SECTION 93 | 2 94 | BLOCKS 95 | 0 96 | ENDSEC 97 | 0 98 | SECTION 99 | 2 100 | ENTITIES 101 | 0 102 | LINE 103 | 8 104 | 1 105 | 10 106 | -2.722 107 | 20 108 | 0.002 109 | 11 110 | -2.187 111 | 21 112 | 0.101 113 | 62 114 | 0 115 | 0 116 | LINE 117 | 8 118 | 1 119 | 10 120 | -2.187 121 | 20 122 | 0.101 123 | 11 124 | -1.913 125 | 21 126 | 0.193 127 | 62 128 | 0 129 | 0 130 | LINE 131 | 8 132 | 1 133 | 10 134 | -1.913 135 | 20 136 | 0.193 137 | 11 138 | -1.415 139 | 21 140 | 0.44 141 | 62 142 | 0 143 | 0 144 | LINE 145 | 8 146 | 1 147 | 10 148 | -1.415 149 | 20 150 | 0.44 151 | 11 152 | -0.972 153 | 21 154 | 0.774 155 | 62 156 | 0 157 | 0 158 | LINE 159 | 8 160 | 1 161 | 10 162 | -0.972 163 | 20 164 | 0.774 165 | 11 166 | -0.603 167 | 21 168 | 1.177 169 | 62 170 | 0 171 | 0 172 | LINE 173 | 8 174 | 1 175 | 10 176 | -0.603 177 | 20 178 | 1.177 179 | 11 180 | -0.441 181 | 21 182 | 1.415 183 | 62 184 | 0 185 | 0 186 | LINE 187 | 8 188 | 1 189 | 10 190 | -0.441 191 | 20 192 | 1.415 193 | 11 194 | -0.193 195 | 21 196 | 1.912 197 | 62 198 | 0 199 | 0 200 | LINE 201 | 8 202 | 1 203 | 10 204 | -0.193 205 | 20 206 | 1.912 207 | 11 208 | -0.041 209 | 21 210 | 2.446 211 | 62 212 | 0 213 | 0 214 | LINE 215 | 8 216 | 1 217 | 10 218 | -0.041 219 | 20 220 | 2.446 221 | 11 222 | 0.01 223 | 21 224 | 2.999 225 | 62 226 | 0 227 | 0 228 | LINE 229 | 8 230 | 1 231 | 10 232 | 0.01 233 | 20 234 | 2.999 235 | 11 236 | -0.002 237 | 21 238 | 63.277 239 | 62 240 | 0 241 | 0 242 | LINE 243 | 8 244 | 1 245 | 10 246 | -0.002 247 | 20 248 | 63.277 249 | 11 250 | -0.101 251 | 21 252 | 63.812 253 | 62 254 | 0 255 | 0 256 | LINE 257 | 8 258 | 1 259 | 10 260 | -0.101 261 | 20 262 | 63.812 263 | 11 264 | -0.193 265 | 21 266 | 64.086 267 | 62 268 | 0 269 | 0 270 | LINE 271 | 8 272 | 1 273 | 10 274 | -0.193 275 | 20 276 | 64.086 277 | 11 278 | -0.44 279 | 21 280 | 64.584 281 | 62 282 | 0 283 | 0 284 | LINE 285 | 8 286 | 1 287 | 10 288 | -0.44 289 | 20 290 | 64.584 291 | 11 292 | -0.774 293 | 21 294 | 65.027 295 | 62 296 | 0 297 | 0 298 | LINE 299 | 8 300 | 1 301 | 10 302 | -0.774 303 | 20 304 | 65.027 305 | 11 306 | -1.177 307 | 21 308 | 65.396 309 | 62 310 | 0 311 | 0 312 | LINE 313 | 8 314 | 1 315 | 10 316 | -1.177 317 | 20 318 | 65.396 319 | 11 320 | -1.415 321 | 21 322 | 65.558 323 | 62 324 | 0 325 | 0 326 | LINE 327 | 8 328 | 1 329 | 10 330 | -1.415 331 | 20 332 | 65.558 333 | 11 334 | -1.912 335 | 21 336 | 65.806 337 | 62 338 | 0 339 | 0 340 | LINE 341 | 8 342 | 1 343 | 10 344 | -1.912 345 | 20 346 | 65.806 347 | 11 348 | -2.446 349 | 21 350 | 65.958 351 | 62 352 | 0 353 | 0 354 | LINE 355 | 8 356 | 1 357 | 10 358 | -2.446 359 | 20 360 | 65.958 361 | 11 362 | -2.999 363 | 21 364 | 66.01 365 | 62 366 | 0 367 | 0 368 | LINE 369 | 8 370 | 1 371 | 10 372 | -2.999 373 | 20 374 | 66.01 375 | 11 376 | -19.277 377 | 21 378 | 65.997 379 | 62 380 | 0 381 | 0 382 | LINE 383 | 8 384 | 1 385 | 10 386 | -19.277 387 | 20 388 | 65.997 389 | 11 390 | -19.812 391 | 21 392 | 65.898 393 | 62 394 | 0 395 | 0 396 | LINE 397 | 8 398 | 1 399 | 10 400 | -19.812 401 | 20 402 | 65.898 403 | 11 404 | -20.086 405 | 21 406 | 65.806 407 | 62 408 | 0 409 | 0 410 | LINE 411 | 8 412 | 1 413 | 10 414 | -20.086 415 | 20 416 | 65.806 417 | 11 418 | -20.584 419 | 21 420 | 65.559 421 | 62 422 | 0 423 | 0 424 | LINE 425 | 8 426 | 1 427 | 10 428 | -20.584 429 | 20 430 | 65.559 431 | 11 432 | -21.027 433 | 21 434 | 65.225 435 | 62 436 | 0 437 | 0 438 | LINE 439 | 8 440 | 1 441 | 10 442 | -21.027 443 | 20 444 | 65.225 445 | 11 446 | -21.396 447 | 21 448 | 64.822 449 | 62 450 | 0 451 | 0 452 | LINE 453 | 8 454 | 1 455 | 10 456 | -21.396 457 | 20 458 | 64.822 459 | 11 460 | -21.558 461 | 21 462 | 64.584 463 | 62 464 | 0 465 | 0 466 | LINE 467 | 8 468 | 1 469 | 10 470 | -21.558 471 | 20 472 | 64.584 473 | 11 474 | -21.806 475 | 21 476 | 64.087 477 | 62 478 | 0 479 | 0 480 | LINE 481 | 8 482 | 1 483 | 10 484 | -21.806 485 | 20 486 | 64.087 487 | 11 488 | -21.958 489 | 21 490 | 63.553 491 | 62 492 | 0 493 | 0 494 | LINE 495 | 8 496 | 1 497 | 10 498 | -21.958 499 | 20 500 | 63.553 501 | 11 502 | -22.01 503 | 21 504 | 63.001 505 | 62 506 | 0 507 | 0 508 | LINE 509 | 8 510 | 1 511 | 10 512 | -22.01 513 | 20 514 | 63.001 515 | 11 516 | -21.997 517 | 21 518 | 2.722 519 | 62 520 | 0 521 | 0 522 | LINE 523 | 8 524 | 1 525 | 10 526 | -21.997 527 | 20 528 | 2.722 529 | 11 530 | -21.898 531 | 21 532 | 2.187 533 | 62 534 | 0 535 | 0 536 | LINE 537 | 8 538 | 1 539 | 10 540 | -21.898 541 | 20 542 | 2.187 543 | 11 544 | -21.806 545 | 21 546 | 1.913 547 | 62 548 | 0 549 | 0 550 | LINE 551 | 8 552 | 1 553 | 10 554 | -21.806 555 | 20 556 | 1.913 557 | 11 558 | -21.559 559 | 21 560 | 1.415 561 | 62 562 | 0 563 | 0 564 | LINE 565 | 8 566 | 1 567 | 10 568 | -21.559 569 | 20 570 | 1.415 571 | 11 572 | -21.225 573 | 21 574 | 0.972 575 | 62 576 | 0 577 | 0 578 | LINE 579 | 8 580 | 1 581 | 10 582 | -21.225 583 | 20 584 | 0.972 585 | 11 586 | -20.822 587 | 21 588 | 0.603 589 | 62 590 | 0 591 | 0 592 | LINE 593 | 8 594 | 1 595 | 10 596 | -20.822 597 | 20 598 | 0.603 599 | 11 600 | -20.584 601 | 21 602 | 0.441 603 | 62 604 | 0 605 | 0 606 | LINE 607 | 8 608 | 1 609 | 10 610 | -20.584 611 | 20 612 | 0.441 613 | 11 614 | -20.087 615 | 21 616 | 0.193 617 | 62 618 | 0 619 | 0 620 | LINE 621 | 8 622 | 1 623 | 10 624 | -20.087 625 | 20 626 | 0.193 627 | 11 628 | -19.553 629 | 21 630 | 0.041 631 | 62 632 | 0 633 | 0 634 | LINE 635 | 8 636 | 1 637 | 10 638 | -19.553 639 | 20 640 | 0.041 641 | 11 642 | -19.014 643 | 21 644 | -0.009 645 | 62 646 | 0 647 | 0 648 | LINE 649 | 8 650 | 1 651 | 10 652 | -19.014 653 | 20 654 | -0.009 655 | 11 656 | -19.013 657 | 21 658 | -0.01 659 | 62 660 | 0 661 | 0 662 | LINE 663 | 8 664 | 1 665 | 10 666 | -19.013 667 | 20 668 | -0.01 669 | 11 670 | -2.722 671 | 21 672 | 0.002 673 | 62 674 | 0 675 | 0 676 | CIRCLE 677 | 8 678 | 1 679 | 10 680 | -16.999 681 | 20 682 | 60.999 683 | 40 684 | 2.01477 685 | 62 686 | 0 687 | 0 688 | CIRCLE 689 | 8 690 | 1 691 | 10 692 | -10.999 693 | 20 694 | 60.999 695 | 40 696 | 2.01477 697 | 62 698 | 0 699 | 0 700 | CIRCLE 701 | 8 702 | 1 703 | 10 704 | -4.999 705 | 20 706 | 60.999 707 | 40 708 | 2.01481 709 | 62 710 | 0 711 | 0 712 | CIRCLE 713 | 8 714 | 1 715 | 10 716 | -16.999 717 | 20 718 | 42.999 719 | 40 720 | 2.01477 721 | 62 722 | 0 723 | 0 724 | CIRCLE 725 | 8 726 | 1 727 | 10 728 | -10.999 729 | 20 730 | 42.999 731 | 40 732 | 2.01477 733 | 62 734 | 0 735 | 0 736 | CIRCLE 737 | 8 738 | 1 739 | 10 740 | -4.999 741 | 20 742 | 42.999 743 | 40 744 | 2.01481 745 | 62 746 | 0 747 | 0 748 | CIRCLE 749 | 8 750 | 1 751 | 10 752 | -16.999 753 | 20 754 | 22.999 755 | 40 756 | 2.01477 757 | 62 758 | 0 759 | 0 760 | CIRCLE 761 | 8 762 | 1 763 | 10 764 | -10.999 765 | 20 766 | 22.999 767 | 40 768 | 2.01477 769 | 62 770 | 0 771 | 0 772 | CIRCLE 773 | 8 774 | 1 775 | 10 776 | -4.999 777 | 20 778 | 22.999 779 | 40 780 | 2.01481 781 | 62 782 | 0 783 | 0 784 | CIRCLE 785 | 8 786 | 1 787 | 10 788 | -16.999 789 | 20 790 | 4.999 791 | 40 792 | 2.01477 793 | 62 794 | 0 795 | 0 796 | CIRCLE 797 | 8 798 | 1 799 | 10 800 | -10.999 801 | 20 802 | 4.999 803 | 40 804 | 2.01477 805 | 62 806 | 0 807 | 0 808 | CIRCLE 809 | 8 810 | 1 811 | 10 812 | -4.999 813 | 20 814 | 4.999 815 | 40 816 | 2.01481 817 | 62 818 | 0 819 | 0 820 | CIRCLE 821 | 8 822 | 1 823 | 10 824 | -4.999 825 | 20 826 | 60.999 827 | 40 828 | 2.71493 829 | 62 830 | 1 831 | 0 832 | CIRCLE 833 | 8 834 | 1 835 | 10 836 | -10.999 837 | 20 838 | 60.999 839 | 40 840 | 2.71493 841 | 62 842 | 1 843 | 0 844 | CIRCLE 845 | 8 846 | 1 847 | 10 848 | -16.999 849 | 20 850 | 60.999 851 | 40 852 | 2.71493 853 | 62 854 | 1 855 | 0 856 | CIRCLE 857 | 8 858 | 1 859 | 10 860 | -4.999 861 | 20 862 | 60.999 863 | 40 864 | 2.01481 865 | 62 866 | 1 867 | 0 868 | CIRCLE 869 | 8 870 | 1 871 | 10 872 | -10.999 873 | 20 874 | 60.999 875 | 40 876 | 2.01477 877 | 62 878 | 1 879 | 0 880 | CIRCLE 881 | 8 882 | 1 883 | 10 884 | -16.999 885 | 20 886 | 60.999 887 | 40 888 | 2.01477 889 | 62 890 | 1 891 | 0 892 | CIRCLE 893 | 8 894 | 1 895 | 10 896 | -4.999 897 | 20 898 | 42.999 899 | 40 900 | 2.71493 901 | 62 902 | 1 903 | 0 904 | CIRCLE 905 | 8 906 | 1 907 | 10 908 | -10.999 909 | 20 910 | 42.999 911 | 40 912 | 2.71493 913 | 62 914 | 1 915 | 0 916 | CIRCLE 917 | 8 918 | 1 919 | 10 920 | -16.999 921 | 20 922 | 42.999 923 | 40 924 | 2.71493 925 | 62 926 | 1 927 | 0 928 | CIRCLE 929 | 8 930 | 1 931 | 10 932 | -4.999 933 | 20 934 | 42.999 935 | 40 936 | 2.01481 937 | 62 938 | 1 939 | 0 940 | CIRCLE 941 | 8 942 | 1 943 | 10 944 | -10.999 945 | 20 946 | 42.999 947 | 40 948 | 2.01477 949 | 62 950 | 1 951 | 0 952 | CIRCLE 953 | 8 954 | 1 955 | 10 956 | -16.999 957 | 20 958 | 42.999 959 | 40 960 | 2.01477 961 | 62 962 | 1 963 | 0 964 | CIRCLE 965 | 8 966 | 1 967 | 10 968 | -4.999 969 | 20 970 | 22.999 971 | 40 972 | 2.71493 973 | 62 974 | 1 975 | 0 976 | CIRCLE 977 | 8 978 | 1 979 | 10 980 | -10.999 981 | 20 982 | 22.999 983 | 40 984 | 2.71493 985 | 62 986 | 1 987 | 0 988 | CIRCLE 989 | 8 990 | 1 991 | 10 992 | -16.999 993 | 20 994 | 22.999 995 | 40 996 | 2.71493 997 | 62 998 | 1 999 | 0 1000 | CIRCLE 1001 | 8 1002 | 1 1003 | 10 1004 | -4.999 1005 | 20 1006 | 22.999 1007 | 40 1008 | 2.01481 1009 | 62 1010 | 1 1011 | 0 1012 | CIRCLE 1013 | 8 1014 | 1 1015 | 10 1016 | -10.999 1017 | 20 1018 | 22.999 1019 | 40 1020 | 2.01477 1021 | 62 1022 | 1 1023 | 0 1024 | CIRCLE 1025 | 8 1026 | 1 1027 | 10 1028 | -16.999 1029 | 20 1030 | 22.999 1031 | 40 1032 | 2.01477 1033 | 62 1034 | 1 1035 | 0 1036 | CIRCLE 1037 | 8 1038 | 1 1039 | 10 1040 | -4.999 1041 | 20 1042 | 4.999 1043 | 40 1044 | 2.71493 1045 | 62 1046 | 1 1047 | 0 1048 | CIRCLE 1049 | 8 1050 | 1 1051 | 10 1052 | -10.999 1053 | 20 1054 | 4.999 1055 | 40 1056 | 2.71493 1057 | 62 1058 | 1 1059 | 0 1060 | CIRCLE 1061 | 8 1062 | 1 1063 | 10 1064 | -16.999 1065 | 20 1066 | 4.999 1067 | 40 1068 | 2.71493 1069 | 62 1070 | 1 1071 | 0 1072 | CIRCLE 1073 | 8 1074 | 1 1075 | 10 1076 | -4.999 1077 | 20 1078 | 4.999 1079 | 40 1080 | 2.01481 1081 | 62 1082 | 1 1083 | 0 1084 | CIRCLE 1085 | 8 1086 | 1 1087 | 10 1088 | -10.999 1089 | 20 1090 | 4.999 1091 | 40 1092 | 2.01477 1093 | 62 1094 | 1 1095 | 0 1096 | CIRCLE 1097 | 8 1098 | 1 1099 | 10 1100 | -16.999 1101 | 20 1102 | 4.999 1103 | 40 1104 | 2.01477 1105 | 62 1106 | 1 1107 | 0 1108 | ENDSEC 1109 | 0 1110 | EOF 1111 | -------------------------------------------------------------------------------- /mechanics/laser/parts/spacer.dxf: -------------------------------------------------------------------------------- 1 | 999 2 | DXF created from Projekt 3 | 0 4 | SECTION 5 | 2 6 | HEADER 7 | 9 8 | $ACADVER 9 | 1 10 | AC1006 11 | 9 12 | $INSUNITS 13 | 70 14 | 4 15 | 9 16 | $INSBASE 17 | 10 18 | 0.0 19 | 20 20 | 0.0 21 | 30 22 | 0.0 23 | 9 24 | $EXTMIN 25 | 10 26 | -10.01 27 | 20 28 | -0.01 29 | 9 30 | $EXTMAX 31 | 10 32 | 0.01 33 | 20 34 | 22.01 35 | 0 36 | ENDSEC 37 | 0 38 | SECTION 39 | 2 40 | TABLES 41 | 0 42 | TABLE 43 | 2 44 | LTYPE 45 | 70 46 | 1 47 | 0 48 | LTYPE 49 | 2 50 | CONTINUOUS 51 | 70 52 | 64 53 | 3 54 | Solid line 55 | 72 56 | 65 57 | 73 58 | 0 59 | 40 60 | 0.000000 61 | 0 62 | ENDTAB 63 | 0 64 | TABLE 65 | 2 66 | LAYER 67 | 70 68 | 6 69 | 0 70 | LAYER 71 | 2 72 | 1 73 | 70 74 | 64 75 | 62 76 | 7 77 | 6 78 | CONTINUOUS 79 | 0 80 | ENDTAB 81 | 0 82 | TABLE 83 | 2 84 | STYLE 85 | 70 86 | 0 87 | 0 88 | ENDTAB 89 | 0 90 | ENDSEC 91 | 0 92 | SECTION 93 | 2 94 | BLOCKS 95 | 0 96 | ENDSEC 97 | 0 98 | SECTION 99 | 2 100 | ENTITIES 101 | 0 102 | LINE 103 | 8 104 | 1 105 | 10 106 | -1.867 107 | 20 108 | -0.007 109 | 11 110 | -1.462 111 | 21 112 | 0.062 113 | 62 114 | 0 115 | 0 116 | LINE 117 | 8 118 | 1 119 | 10 120 | -1.462 121 | 20 122 | 0.062 123 | 11 124 | -1.075 125 | 21 126 | 0.215 127 | 62 128 | 0 129 | 0 130 | LINE 131 | 8 132 | 1 133 | 10 134 | -1.075 135 | 20 136 | 0.215 137 | 11 138 | -0.845 139 | 21 140 | 0.355 141 | 62 142 | 0 143 | 0 144 | LINE 145 | 8 146 | 1 147 | 10 148 | -0.845 149 | 20 150 | 0.355 151 | 11 152 | -0.631 153 | 21 154 | 0.526 155 | 62 156 | 0 157 | 0 158 | LINE 159 | 8 160 | 1 161 | 10 162 | -0.631 163 | 20 164 | 0.526 165 | 11 166 | -0.44 167 | 21 168 | 0.73 169 | 62 170 | 0 171 | 0 172 | LINE 173 | 8 174 | 1 175 | 10 176 | -0.44 177 | 20 178 | 0.73 179 | 11 180 | -0.283 181 | 21 182 | 0.955 183 | 62 184 | 0 185 | 0 186 | LINE 187 | 8 188 | 1 189 | 10 190 | -0.283 191 | 20 192 | 0.955 193 | 11 194 | -0.158 195 | 21 196 | 1.194 197 | 62 198 | 0 199 | 0 200 | LINE 201 | 8 202 | 1 203 | 10 204 | -0.158 205 | 20 206 | 1.194 207 | 11 208 | -0.064 209 | 21 210 | 1.457 211 | 62 212 | 0 213 | 0 214 | LINE 215 | 8 216 | 1 217 | 10 218 | -0.064 219 | 20 220 | 1.457 221 | 11 222 | -0.033 223 | 21 224 | 1.586 225 | 62 226 | 0 227 | 0 228 | LINE 229 | 8 230 | 1 231 | 10 232 | -0.033 233 | 20 234 | 1.586 235 | 11 236 | -0.031 237 | 21 238 | 1.59 239 | 62 240 | 0 241 | 0 242 | LINE 243 | 8 244 | 1 245 | 10 246 | -0.031 247 | 20 248 | 1.59 249 | 11 250 | 0.006 251 | 21 252 | 1.862 253 | 62 254 | 0 255 | 0 256 | LINE 257 | 8 258 | 1 259 | 10 260 | 0.006 261 | 20 262 | 1.862 263 | 11 264 | 0.009 265 | 21 266 | 1.995 267 | 62 268 | 0 269 | 0 270 | LINE 271 | 8 272 | 1 273 | 10 274 | 0.009 275 | 20 276 | 1.995 277 | 11 278 | 0.01 279 | 21 280 | 1.999 281 | 62 282 | 0 283 | 0 284 | LINE 285 | 8 286 | 1 287 | 10 288 | 0.01 289 | 20 290 | 1.999 291 | 11 292 | 0.007 293 | 21 294 | 20.132 295 | 62 296 | 0 297 | 0 298 | LINE 299 | 8 300 | 1 301 | 10 302 | 0.007 303 | 20 304 | 20.132 305 | 11 306 | -0.062 307 | 21 308 | 20.537 309 | 62 310 | 0 311 | 0 312 | LINE 313 | 8 314 | 1 315 | 10 316 | -0.062 317 | 20 318 | 20.537 319 | 11 320 | -0.215 321 | 21 322 | 20.924 323 | 62 324 | 0 325 | 0 326 | LINE 327 | 8 328 | 1 329 | 10 330 | -0.215 331 | 20 332 | 20.924 333 | 11 334 | -0.355 335 | 21 336 | 21.154 337 | 62 338 | 0 339 | 0 340 | LINE 341 | 8 342 | 1 343 | 10 344 | -0.355 345 | 20 346 | 21.154 347 | 11 348 | -0.526 349 | 21 350 | 21.368 351 | 62 352 | 0 353 | 0 354 | LINE 355 | 8 356 | 1 357 | 10 358 | -0.526 359 | 20 360 | 21.368 361 | 11 362 | -0.73 363 | 21 364 | 21.559 365 | 62 366 | 0 367 | 0 368 | LINE 369 | 8 370 | 1 371 | 10 372 | -0.73 373 | 20 374 | 21.559 375 | 11 376 | -0.955 377 | 21 378 | 21.716 379 | 62 380 | 0 381 | 0 382 | LINE 383 | 8 384 | 1 385 | 10 386 | -0.955 387 | 20 388 | 21.716 389 | 11 390 | -1.194 391 | 21 392 | 21.841 393 | 62 394 | 0 395 | 0 396 | LINE 397 | 8 398 | 1 399 | 10 400 | -1.194 401 | 20 402 | 21.841 403 | 11 404 | -1.457 405 | 21 406 | 21.935 407 | 62 408 | 0 409 | 0 410 | LINE 411 | 8 412 | 1 413 | 10 414 | -1.457 415 | 20 416 | 21.935 417 | 11 418 | -1.586 419 | 21 420 | 21.966 421 | 62 422 | 0 423 | 0 424 | LINE 425 | 8 426 | 1 427 | 10 428 | -1.586 429 | 20 430 | 21.966 431 | 11 432 | -1.59 433 | 21 434 | 21.968 435 | 62 436 | 0 437 | 0 438 | LINE 439 | 8 440 | 1 441 | 10 442 | -1.59 443 | 20 444 | 21.968 445 | 11 446 | -1.862 447 | 21 448 | 22.005 449 | 62 450 | 0 451 | 0 452 | LINE 453 | 8 454 | 1 455 | 10 456 | -1.862 457 | 20 458 | 22.005 459 | 11 460 | -1.993 461 | 21 462 | 22.009 463 | 62 464 | 0 465 | 0 466 | LINE 467 | 8 468 | 1 469 | 10 470 | -1.993 471 | 20 472 | 22.009 473 | 11 474 | -1.999 475 | 21 476 | 22.01 477 | 62 478 | 0 479 | 0 480 | LINE 481 | 8 482 | 1 483 | 10 484 | -1.999 485 | 20 486 | 22.01 487 | 11 488 | -8.132 489 | 21 490 | 22.006 491 | 62 492 | 0 493 | 0 494 | LINE 495 | 8 496 | 1 497 | 10 498 | -8.132 499 | 20 500 | 22.006 501 | 11 502 | -8.537 503 | 21 504 | 21.937 505 | 62 506 | 0 507 | 0 508 | LINE 509 | 8 510 | 1 511 | 10 512 | -8.537 513 | 20 514 | 21.937 515 | 11 516 | -8.924 517 | 21 518 | 21.784 519 | 62 520 | 0 521 | 0 522 | LINE 523 | 8 524 | 1 525 | 10 526 | -8.924 527 | 20 528 | 21.784 529 | 11 530 | -9.154 531 | 21 532 | 21.644 533 | 62 534 | 0 535 | 0 536 | LINE 537 | 8 538 | 1 539 | 10 540 | -9.154 541 | 20 542 | 21.644 543 | 11 544 | -9.368 545 | 21 546 | 21.473 547 | 62 548 | 0 549 | 0 550 | LINE 551 | 8 552 | 1 553 | 10 554 | -9.368 555 | 20 556 | 21.473 557 | 11 558 | -9.559 559 | 21 560 | 21.269 561 | 62 562 | 0 563 | 0 564 | LINE 565 | 8 566 | 1 567 | 10 568 | -9.559 569 | 20 570 | 21.269 571 | 11 572 | -9.716 573 | 21 574 | 21.044 575 | 62 576 | 0 577 | 0 578 | LINE 579 | 8 580 | 1 581 | 10 582 | -9.716 583 | 20 584 | 21.044 585 | 11 586 | -9.841 587 | 21 588 | 20.805 589 | 62 590 | 0 591 | 0 592 | LINE 593 | 8 594 | 1 595 | 10 596 | -9.841 597 | 20 598 | 20.805 599 | 11 600 | -9.935 601 | 21 602 | 20.542 603 | 62 604 | 0 605 | 0 606 | LINE 607 | 8 608 | 1 609 | 10 610 | -9.935 611 | 20 612 | 20.542 613 | 11 614 | -9.966 615 | 21 616 | 20.413 617 | 62 618 | 0 619 | 0 620 | LINE 621 | 8 622 | 1 623 | 10 624 | -9.966 625 | 20 626 | 20.413 627 | 11 628 | -9.968 629 | 21 630 | 20.409 631 | 62 632 | 0 633 | 0 634 | LINE 635 | 8 636 | 1 637 | 10 638 | -9.968 639 | 20 640 | 20.409 641 | 11 642 | -10.005 643 | 21 644 | 20.137 645 | 62 646 | 0 647 | 0 648 | LINE 649 | 8 650 | 1 651 | 10 652 | -10.005 653 | 20 654 | 20.137 655 | 11 656 | -10.009 657 | 21 658 | 20.009 659 | 62 660 | 0 661 | 0 662 | LINE 663 | 8 664 | 1 665 | 10 666 | -10.009 667 | 20 668 | 20.009 669 | 11 670 | -10.01 671 | 21 672 | 20.008 673 | 62 674 | 0 675 | 0 676 | LINE 677 | 8 678 | 1 679 | 10 680 | -10.01 681 | 20 682 | 20.008 683 | 11 684 | -10.006 685 | 21 686 | 1.867 687 | 62 688 | 0 689 | 0 690 | LINE 691 | 8 692 | 1 693 | 10 694 | -10.006 695 | 20 696 | 1.867 697 | 11 698 | -9.937 699 | 21 700 | 1.462 701 | 62 702 | 0 703 | 0 704 | LINE 705 | 8 706 | 1 707 | 10 708 | -9.937 709 | 20 710 | 1.462 711 | 11 712 | -9.784 713 | 21 714 | 1.075 715 | 62 716 | 0 717 | 0 718 | LINE 719 | 8 720 | 1 721 | 10 722 | -9.784 723 | 20 724 | 1.075 725 | 11 726 | -9.644 727 | 21 728 | 0.845 729 | 62 730 | 0 731 | 0 732 | LINE 733 | 8 734 | 1 735 | 10 736 | -9.644 737 | 20 738 | 0.845 739 | 11 740 | -9.473 741 | 21 742 | 0.631 743 | 62 744 | 0 745 | 0 746 | LINE 747 | 8 748 | 1 749 | 10 750 | -9.473 751 | 20 752 | 0.631 753 | 11 754 | -9.269 755 | 21 756 | 0.44 757 | 62 758 | 0 759 | 0 760 | LINE 761 | 8 762 | 1 763 | 10 764 | -9.269 765 | 20 766 | 0.44 767 | 11 768 | -9.044 769 | 21 770 | 0.283 771 | 62 772 | 0 773 | 0 774 | LINE 775 | 8 776 | 1 777 | 10 778 | -9.044 779 | 20 780 | 0.283 781 | 11 782 | -8.805 783 | 21 784 | 0.158 785 | 62 786 | 0 787 | 0 788 | LINE 789 | 8 790 | 1 791 | 10 792 | -8.805 793 | 20 794 | 0.158 795 | 11 796 | -8.542 797 | 21 798 | 0.064 799 | 62 800 | 0 801 | 0 802 | LINE 803 | 8 804 | 1 805 | 10 806 | -8.542 807 | 20 808 | 0.064 809 | 11 810 | -8.413 811 | 21 812 | 0.033 813 | 62 814 | 0 815 | 0 816 | LINE 817 | 8 818 | 1 819 | 10 820 | -8.413 821 | 20 822 | 0.033 823 | 11 824 | -8.409 825 | 21 826 | 0.031 827 | 62 828 | 0 829 | 0 830 | LINE 831 | 8 832 | 1 833 | 10 834 | -8.409 835 | 20 836 | 0.031 837 | 11 838 | -8.137 839 | 21 840 | -0.006 841 | 62 842 | 0 843 | 0 844 | LINE 845 | 8 846 | 1 847 | 10 848 | -8.137 849 | 20 850 | -0.006 851 | 11 852 | -8.007 853 | 21 854 | -0.009 855 | 62 856 | 0 857 | 0 858 | LINE 859 | 8 860 | 1 861 | 10 862 | -8.007 863 | 20 864 | -0.009 865 | 11 866 | -8.001 867 | 21 868 | -0.01 869 | 62 870 | 0 871 | 0 872 | LINE 873 | 8 874 | 1 875 | 10 876 | -8.001 877 | 20 878 | -0.01 879 | 11 880 | -1.867 881 | 21 882 | -0.007 883 | 62 884 | 0 885 | 0 886 | CIRCLE 887 | 8 888 | 1 889 | 10 890 | -4.999 891 | 20 892 | 16.999 893 | 40 894 | 2.03999 895 | 62 896 | 0 897 | 0 898 | CIRCLE 899 | 8 900 | 1 901 | 10 902 | -4.999 903 | 20 904 | 10.999 905 | 40 906 | 2.04003 907 | 62 908 | 0 909 | 0 910 | CIRCLE 911 | 8 912 | 1 913 | 10 914 | -4.999 915 | 20 916 | 4.999 917 | 40 918 | 2.03999 919 | 62 920 | 0 921 | 0 922 | ENDSEC 923 | 0 924 | EOF 925 | -------------------------------------------------------------------------------- /mechanics/laser/parts/tip.dxf: -------------------------------------------------------------------------------- 1 | 999 2 | DXF created from Projekt 3 | 0 4 | SECTION 5 | 2 6 | HEADER 7 | 9 8 | $ACADVER 9 | 1 10 | AC1006 11 | 9 12 | $INSUNITS 13 | 70 14 | 4 15 | 9 16 | $INSBASE 17 | 10 18 | 0.0 19 | 20 20 | 0.0 21 | 30 22 | 0.0 23 | 9 24 | $EXTMIN 25 | 10 26 | -25.51 27 | 20 28 | -3.01 29 | 9 30 | $EXTMAX 31 | 10 32 | 3.506 33 | 20 34 | 33.01 35 | 0 36 | ENDSEC 37 | 0 38 | SECTION 39 | 2 40 | TABLES 41 | 0 42 | TABLE 43 | 2 44 | LTYPE 45 | 70 46 | 1 47 | 0 48 | LTYPE 49 | 2 50 | CONTINUOUS 51 | 70 52 | 64 53 | 3 54 | Solid line 55 | 72 56 | 65 57 | 73 58 | 0 59 | 40 60 | 0.000000 61 | 0 62 | ENDTAB 63 | 0 64 | TABLE 65 | 2 66 | LAYER 67 | 70 68 | 6 69 | 0 70 | LAYER 71 | 2 72 | 1 73 | 70 74 | 64 75 | 62 76 | 7 77 | 6 78 | CONTINUOUS 79 | 0 80 | ENDTAB 81 | 0 82 | TABLE 83 | 2 84 | STYLE 85 | 70 86 | 0 87 | 0 88 | ENDTAB 89 | 0 90 | ENDSEC 91 | 0 92 | SECTION 93 | 2 94 | BLOCKS 95 | 0 96 | ENDSEC 97 | 0 98 | SECTION 99 | 2 100 | ENTITIES 101 | 0 102 | LINE 103 | 8 104 | 1 105 | 10 106 | -16.99 107 | 20 108 | -0.01 109 | 11 110 | -10.01 111 | 21 112 | -0.01 113 | 62 114 | 0 115 | 0 116 | LINE 117 | 8 118 | 1 119 | 10 120 | -10.01 121 | 20 122 | -0.01 123 | 11 124 | -10.01 125 | 21 126 | -3.01 127 | 62 128 | 0 129 | 0 130 | LINE 131 | 8 132 | 1 133 | 10 134 | -10.01 135 | 20 136 | -3.01 137 | 11 138 | -2.99 139 | 21 140 | -3.01 141 | 62 142 | 0 143 | 0 144 | LINE 145 | 8 146 | 1 147 | 10 148 | -2.99 149 | 20 150 | -3.01 151 | 11 152 | -2.99 153 | 21 154 | -0.01 155 | 62 156 | 0 157 | 0 158 | LINE 159 | 8 160 | 1 161 | 10 162 | -2.99 163 | 20 164 | -0.01 165 | 11 166 | 0.006 167 | 21 168 | -0.01 169 | 62 170 | 0 171 | 0 172 | LINE 173 | 8 174 | 1 175 | 10 176 | 0.006 177 | 20 178 | -0.01 179 | 11 180 | 1.7 181 | 21 182 | 4.214 183 | 62 184 | 0 185 | 0 186 | LINE 187 | 8 188 | 1 189 | 10 190 | 1.7 191 | 20 192 | 4.214 193 | 11 194 | 2.74 195 | 21 196 | 7.965 197 | 62 198 | 0 199 | 0 200 | LINE 201 | 8 202 | 1 203 | 10 204 | 2.74 205 | 20 206 | 7.965 207 | 11 208 | 3.291 209 | 21 210 | 11.248 211 | 62 212 | 0 213 | 0 214 | LINE 215 | 8 216 | 1 217 | 10 218 | 3.291 219 | 20 220 | 11.248 221 | 11 222 | 3.506 223 | 21 224 | 14.53 225 | 62 226 | 0 227 | 0 228 | LINE 229 | 8 230 | 1 231 | 10 232 | 3.506 233 | 20 234 | 14.53 235 | 11 236 | 3.427 237 | 21 238 | 17.311 239 | 62 240 | 0 241 | 0 242 | LINE 243 | 8 244 | 1 245 | 10 246 | 3.427 247 | 20 248 | 17.311 249 | 11 250 | 2.847 251 | 21 252 | 21.532 253 | 62 254 | 0 255 | 0 256 | LINE 257 | 8 258 | 1 259 | 10 260 | 2.847 261 | 20 262 | 21.532 263 | 11 264 | 1.855 265 | 21 266 | 25.315 267 | 62 268 | 0 269 | 0 270 | LINE 271 | 8 272 | 1 273 | 10 274 | 1.855 275 | 20 276 | 25.315 277 | 11 278 | 0.239 279 | 21 280 | 29.503 281 | 62 282 | 0 283 | 0 284 | LINE 285 | 8 286 | 1 287 | 10 288 | 0.239 289 | 20 290 | 29.503 291 | 11 292 | 0.006 293 | 21 294 | 30.01 295 | 62 296 | 0 297 | 0 298 | LINE 299 | 8 300 | 1 301 | 10 302 | 0.006 303 | 20 304 | 30.01 305 | 11 306 | -2.99 307 | 21 308 | 30.01 309 | 62 310 | 0 311 | 0 312 | LINE 313 | 8 314 | 1 315 | 10 316 | -2.99 317 | 20 318 | 30.01 319 | 11 320 | -2.99 321 | 21 322 | 33.01 323 | 62 324 | 0 325 | 0 326 | LINE 327 | 8 328 | 1 329 | 10 330 | -2.99 331 | 20 332 | 33.01 333 | 11 334 | -10.01 335 | 21 336 | 33.01 337 | 62 338 | 0 339 | 0 340 | LINE 341 | 8 342 | 1 343 | 10 344 | -10.01 345 | 20 346 | 33.01 347 | 11 348 | -10.01 349 | 21 350 | 30.01 351 | 62 352 | 0 353 | 0 354 | LINE 355 | 8 356 | 1 357 | 10 358 | -10.01 359 | 20 360 | 30.01 361 | 11 362 | -16.99 363 | 21 364 | 30.01 365 | 62 366 | 0 367 | 0 368 | LINE 369 | 8 370 | 1 371 | 10 372 | -16.99 373 | 20 374 | 30.01 375 | 11 376 | -16.99 377 | 21 378 | 33.01 379 | 62 380 | 0 381 | 0 382 | LINE 383 | 8 384 | 1 385 | 10 386 | -16.99 387 | 20 388 | 33.01 389 | 11 390 | -24.01 391 | 21 392 | 33.01 393 | 62 394 | 0 395 | 0 396 | LINE 397 | 8 398 | 1 399 | 10 400 | -24.01 401 | 20 402 | 33.01 403 | 11 404 | -24.01 405 | 21 406 | 30.01 407 | 62 408 | 0 409 | 0 410 | LINE 411 | 8 412 | 1 413 | 10 414 | -24.01 415 | 20 416 | 30.01 417 | 11 418 | -25.51 419 | 21 420 | 30.01 421 | 62 422 | 0 423 | 0 424 | LINE 425 | 8 426 | 1 427 | 10 428 | -25.51 429 | 20 430 | 30.01 431 | 11 432 | -25.51 433 | 21 434 | -0.01 435 | 62 436 | 0 437 | 0 438 | LINE 439 | 8 440 | 1 441 | 10 442 | -25.51 443 | 20 444 | -0.01 445 | 11 446 | -24.01 447 | 21 448 | -0.01 449 | 62 450 | 0 451 | 0 452 | LINE 453 | 8 454 | 1 455 | 10 456 | -24.01 457 | 20 458 | -0.01 459 | 11 460 | -24.01 461 | 21 462 | -3.01 463 | 62 464 | 0 465 | 0 466 | LINE 467 | 8 468 | 1 469 | 10 470 | -24.01 471 | 20 472 | -3.01 473 | 11 474 | -16.99 475 | 21 476 | -3.01 477 | 62 478 | 0 479 | 0 480 | LINE 481 | 8 482 | 1 483 | 10 484 | -16.99 485 | 20 486 | -3.01 487 | 11 488 | -16.99 489 | 21 490 | -0.01 491 | 62 492 | 0 493 | 0 494 | ENDSEC 495 | 0 496 | EOF 497 | -------------------------------------------------------------------------------- /mechanics/laser/parts/tip_up.dxf: -------------------------------------------------------------------------------- 1 | 999 2 | DXF created from Projekt 3 | 0 4 | SECTION 5 | 2 6 | HEADER 7 | 9 8 | $ACADVER 9 | 1 10 | AC1006 11 | 9 12 | $INSUNITS 13 | 70 14 | 4 15 | 9 16 | $INSBASE 17 | 10 18 | 0.0 19 | 20 20 | 0.0 21 | 30 22 | 0.0 23 | 9 24 | $EXTMIN 25 | 10 26 | -14.014 27 | 20 28 | -3.01 29 | 9 30 | $EXTMAX 31 | 10 32 | 0.01 33 | 20 34 | 33.01 35 | 0 36 | ENDSEC 37 | 0 38 | SECTION 39 | 2 40 | TABLES 41 | 0 42 | TABLE 43 | 2 44 | LTYPE 45 | 70 46 | 1 47 | 0 48 | LTYPE 49 | 2 50 | CONTINUOUS 51 | 70 52 | 64 53 | 3 54 | Solid line 55 | 72 56 | 65 57 | 73 58 | 0 59 | 40 60 | 0.000000 61 | 0 62 | ENDTAB 63 | 0 64 | TABLE 65 | 2 66 | LAYER 67 | 70 68 | 6 69 | 0 70 | LAYER 71 | 2 72 | 1 73 | 70 74 | 64 75 | 62 76 | 7 77 | 6 78 | CONTINUOUS 79 | 0 80 | ENDTAB 81 | 0 82 | TABLE 83 | 2 84 | STYLE 85 | 70 86 | 0 87 | 0 88 | ENDTAB 89 | 0 90 | ENDSEC 91 | 0 92 | SECTION 93 | 2 94 | BLOCKS 95 | 0 96 | ENDSEC 97 | 0 98 | SECTION 99 | 2 100 | ENTITIES 101 | 0 102 | LINE 103 | 8 104 | 1 105 | 10 106 | -1.99 107 | 20 108 | -0.01 109 | 11 110 | 0.01 111 | 21 112 | -0.01 113 | 62 114 | 0 115 | 0 116 | LINE 117 | 8 118 | 1 119 | 10 120 | 0.01 121 | 20 122 | -0.01 123 | 11 124 | 0.01 125 | 21 126 | 30.01 127 | 62 128 | 0 129 | 0 130 | LINE 131 | 8 132 | 1 133 | 10 134 | 0.01 135 | 20 136 | 30.01 137 | 11 138 | -1.99 139 | 21 140 | 30.01 141 | 62 142 | 0 143 | 0 144 | LINE 145 | 8 146 | 1 147 | 10 148 | -1.99 149 | 20 150 | 30.01 151 | 11 152 | -1.99 153 | 21 154 | 33.01 155 | 62 156 | 0 157 | 0 158 | LINE 159 | 8 160 | 1 161 | 10 162 | -1.99 163 | 20 164 | 33.01 165 | 11 166 | -9.01 167 | 21 168 | 33.01 169 | 62 170 | 0 171 | 0 172 | LINE 173 | 8 174 | 1 175 | 10 176 | -9.01 177 | 20 178 | 33.01 179 | 11 180 | -9.01 181 | 21 182 | 30.01 183 | 62 184 | 0 185 | 0 186 | LINE 187 | 8 188 | 1 189 | 10 190 | -9.01 191 | 20 192 | 30.01 193 | 11 194 | -14.014 195 | 21 196 | 30.01 197 | 62 198 | 0 199 | 0 200 | LINE 201 | 8 202 | 1 203 | 10 204 | -14.014 205 | 20 206 | 30.01 207 | 11 208 | -12.586 209 | 21 210 | 24.84 211 | 62 212 | 0 213 | 0 214 | LINE 215 | 8 216 | 1 217 | 10 218 | -12.586 219 | 20 220 | 24.84 221 | 11 222 | -11.805 223 | 21 224 | 20.154 225 | 62 226 | 0 227 | 0 228 | LINE 229 | 8 230 | 1 231 | 10 232 | -11.805 233 | 20 234 | 20.154 235 | 11 236 | -11.512 237 | 21 238 | 15.467 239 | 62 240 | 0 241 | 0 242 | LINE 243 | 8 244 | 1 245 | 10 246 | -11.512 247 | 20 248 | 15.467 249 | 11 250 | -11.707 251 | 21 252 | 10.781 253 | 62 254 | 0 255 | 0 256 | LINE 257 | 8 258 | 1 259 | 10 260 | -11.707 261 | 20 262 | 10.781 263 | 11 264 | -12.384 265 | 21 266 | 6.126 267 | 62 268 | 0 269 | 0 270 | LINE 271 | 8 272 | 1 273 | 10 274 | -12.384 275 | 20 276 | 6.126 277 | 11 278 | -13.845 279 | 21 280 | 0.502 281 | 62 282 | 0 283 | 0 284 | LINE 285 | 8 286 | 1 287 | 10 288 | -13.845 289 | 20 290 | 0.502 291 | 11 292 | -13.844 293 | 21 294 | 0.502 295 | 62 296 | 0 297 | 0 298 | LINE 299 | 8 300 | 1 301 | 10 302 | -13.844 303 | 20 304 | 0.502 305 | 11 306 | -13.997 307 | 21 308 | 0.038 309 | 62 310 | 0 311 | 0 312 | LINE 313 | 8 314 | 1 315 | 10 316 | -13.997 317 | 20 318 | 0.038 319 | 11 320 | -13.999 321 | 21 322 | 0.036 323 | 62 324 | 0 325 | 0 326 | LINE 327 | 8 328 | 1 329 | 10 330 | -13.999 331 | 20 332 | 0.036 333 | 11 334 | -14.014 335 | 21 336 | -0.01 337 | 62 338 | 0 339 | 0 340 | LINE 341 | 8 342 | 1 343 | 10 344 | -14.014 345 | 20 346 | -0.01 347 | 11 348 | -9.01 349 | 21 350 | -0.01 351 | 62 352 | 0 353 | 0 354 | LINE 355 | 8 356 | 1 357 | 10 358 | -9.01 359 | 20 360 | -0.01 361 | 11 362 | -9.01 363 | 21 364 | -3.01 365 | 62 366 | 0 367 | 0 368 | LINE 369 | 8 370 | 1 371 | 10 372 | -9.01 373 | 20 374 | -3.01 375 | 11 376 | -1.99 377 | 21 378 | -3.01 379 | 62 380 | 0 381 | 0 382 | LINE 383 | 8 384 | 1 385 | 10 386 | -1.99 387 | 20 388 | -3.01 389 | 11 390 | -1.99 391 | 21 392 | -0.01 393 | 62 394 | 0 395 | 0 396 | ENDSEC 397 | 0 398 | EOF 399 | -------------------------------------------------------------------------------- /mechanics/laser/plater.conf: -------------------------------------------------------------------------------- 1 | u.stl 8 left 2 | tip_up.stl 4 front 3 | tip.stl 4 front 4 | side.stl 8 front 5 | leg.stl 8 front 6 | head.stl 1 front 7 | body_top.stl 1 front 8 | body_bottom.stl 1 front 9 | -------------------------------------------------------------------------------- /mechanics/laser/side.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/side.stl -------------------------------------------------------------------------------- /mechanics/laser/spacer.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/spacer.stl -------------------------------------------------------------------------------- /mechanics/laser/tip.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/tip.stl -------------------------------------------------------------------------------- /mechanics/laser/tip_up.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/tip_up.stl -------------------------------------------------------------------------------- /mechanics/laser/u.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Rhoban/Metabot/bb70c21a561bfb6d6c717a3fb47ac1f1e50dba6d/mechanics/laser/u.stl -------------------------------------------------------------------------------- /mechanics/laser/update.sh: -------------------------------------------------------------------------------- 1 | projekt -z -Z 0.1 -e 2.5:1 leg.stl > parts/leg.dxf 2 | projekt -y -Z 0.1 -e 2.5:1 u.stl > parts/u.dxf 3 | projekt -z -Z 0.1 -e 2.5:1 side.stl > parts/side.dxf 4 | projekt -z -Z 0.1 -e 2.5:1 tip.stl > parts/tip.dxf 5 | projekt -z -Z 0.1 -e 2.5:1 tip_up.stl > parts/tip_up.dxf 6 | projekt -z -Z 0.1 -e 2.5:1 body_bottom.stl > parts/body_bottom.dxf 7 | projekt -z -Z 0.1 -e 2.5:1 body_top.stl > parts/body_top.dxf 8 | projekt -z -Z 0.1 -e 2.5:1 head.stl > parts/head.dxf 9 | projekt -z -Z 0.1 spacer.stl > parts/spacer.dxf 10 | -------------------------------------------------------------------------------- /scripts/nameBT.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Simple script to give name to bluetooth devices in the udev 4 | 5 | # Ensuring the default rules are copied 6 | cp rules/* /etc/udev/rules 7 | 8 | devices=`ls /dev/rfcom* 2>/dev/null` 9 | 10 | if [ "$devices" == "" ]; then 11 | echo "No devices plugged in (searching for /dev/ttyUSB* and /dev/ttyACM* and /dev/rfcom*)" 12 | else 13 | for i in $devices; do 14 | address=`udevadm info -a -n $i|grep "{address}"|head -n 1|cut -d"\"" -f2` 15 | file="/etc/udev/rules.d/45-$address.rules" 16 | name="" 17 | 18 | echo "" 19 | echo "How would you name $i (address: $address) ? (Leave blank to keep the name)" 20 | 21 | if [ -e $file ]; then 22 | name=`head -n 1 $file|cut -d" " -f2` 23 | echo "Note: This device is already recognized by a rule $file (name: $name)" 24 | fi 25 | 26 | read deviceName 27 | 28 | if [ "$deviceName" != "" ]; then 29 | echo "# $deviceName" > $file 30 | echo "# File generated using nameBTs.sh" >> $file 31 | echo "ATTRS{address}==\"$address\", SYMLINK+=\"$deviceName\"" >> $file 32 | echo "OK, creating udev rule to name it /dev/$deviceName" 33 | echo "Rule file name: $file" 34 | else 35 | if [ "$name" != "" ]; then 36 | echo "OK, let's it named $name for now" 37 | else 38 | echo "OK, let's let it unnamed for now" 39 | fi 40 | fi 41 | done 42 | fi 43 | --------------------------------------------------------------------------------