├── processing ├── .gitignore ├── lib │ ├── core.jar │ ├── jssc.jar │ ├── serial.jar │ ├── linux32 │ │ └── libjSSC-2.8.so │ ├── linux64 │ │ └── libjSSC-2.8.so │ ├── windows32 │ │ └── jSSC-2.8.dll │ ├── windows64 │ │ └── jSSC-2.8.dll │ └── macosx │ │ └── libjSSC-2.8.jnilib ├── src │ ├── library.properties │ └── ca │ │ └── digitalcave │ │ └── stubby │ │ ├── Message.java │ │ ├── StubbySerial.java │ │ └── Stubby.java ├── .project ├── .classpath ├── test │ └── Test.java └── build.xml ├── .kateproject ├── avr ├── lib │ ├── pwm │ │ ├── pwm.h │ │ └── pwm.S │ ├── Ring │ │ ├── Ring.cpp │ │ └── Ring.h │ ├── protocol │ │ ├── protocol.txt │ │ ├── protocol_serial.c │ │ ├── protocol_client.py │ │ ├── protocol.h │ │ └── protocol.c │ ├── serial │ │ ├── serial.c │ │ ├── serial.h │ │ └── serial_async_tx.c │ ├── universal_controller │ │ ├── protocol.txt │ │ ├── client.h │ │ └── client.c │ └── twi │ │ └── twi.h ├── hardware │ ├── timer2.h │ ├── servo.h │ ├── status.h │ ├── distance.h │ ├── servo.c │ ├── battery.h │ ├── magnetometer.h │ ├── timer2.c │ ├── battery.c │ ├── status.c │ ├── distance_HCSR04.c │ └── magnetometer_HMC5883L.c ├── util │ ├── delays.h │ ├── convert.h │ ├── math.h │ ├── convert.c │ └── delays.c ├── gait │ ├── gait.h │ └── gait_tripod.c ├── controllers │ ├── universal_controller.h │ ├── calibration.h │ └── processing.h ├── types │ ├── Point.cpp │ └── Point.h ├── hardware.mk ├── Stubby.h ├── Makefile ├── Stubby.cpp ├── Leg.h └── Leg.cpp ├── doc ├── diagrams.pdf ├── todo.txt └── protocol.txt ├── kicad ├── rev1 │ ├── hex.pdf │ ├── hex.png │ ├── gerber │ │ ├── stubby.zip │ │ ├── hex-Edge_Cuts.gbr │ │ ├── hex.txt │ │ ├── hex-B_Mask.gbs │ │ └── hex-F_Mask.gts │ ├── hex.dcm │ ├── hex.lib │ ├── hex.lst │ ├── hex.pro │ ├── custom.dcm │ ├── custom.bck │ ├── hex.mod │ └── hex.cmp ├── rev2 │ ├── hex.pdf │ ├── hex.png │ ├── gerber │ │ ├── stubby.zip │ │ ├── hex-NPTH.txt │ │ ├── hex-B_SilkS.gbo │ │ ├── hex.drl │ │ ├── hex-Edge_Cuts.gbr │ │ └── hex-B_Mask.gbs │ ├── hex.dcm │ ├── hex.lib │ ├── hex.pro │ ├── custom.dcm │ ├── custom.bck │ ├── hex.lst │ └── hex.mod └── rev2.1 │ ├── stubby.ods │ ├── stubby.dcm │ ├── stubby.lib │ ├── stubby.pro │ ├── custom.dcm │ ├── custom.bck │ └── stubby.mod ├── simulation ├── ik_top_rotation │ ├── sketch.properties │ └── ik_top_rotation.pde ├── inverse_kinematics_simulator │ ├── sketch.properties │ └── inverse_kinematics_simulator.pde ├── debug.c ├── Makefile ├── debug.h └── Main.cpp ├── .gitignore ├── README.md └── python └── controller.py /processing/.gitignore: -------------------------------------------------------------------------------- 1 | /bin/ 2 | /build/ 3 | -------------------------------------------------------------------------------- /.kateproject: -------------------------------------------------------------------------------- 1 | { 2 | "name": "Stubby", 3 | "files": [ { "git": 1 } ] 4 | } 5 | -------------------------------------------------------------------------------- /avr/lib/pwm/pwm.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thebiguno/stubby/HEAD/avr/lib/pwm/pwm.h -------------------------------------------------------------------------------- /doc/diagrams.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thebiguno/stubby/HEAD/doc/diagrams.pdf -------------------------------------------------------------------------------- /kicad/rev1/hex.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thebiguno/stubby/HEAD/kicad/rev1/hex.pdf -------------------------------------------------------------------------------- /kicad/rev1/hex.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thebiguno/stubby/HEAD/kicad/rev1/hex.png -------------------------------------------------------------------------------- /kicad/rev2/hex.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thebiguno/stubby/HEAD/kicad/rev2/hex.pdf -------------------------------------------------------------------------------- /kicad/rev2/hex.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thebiguno/stubby/HEAD/kicad/rev2/hex.png -------------------------------------------------------------------------------- /kicad/rev2.1/stubby.ods: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thebiguno/stubby/HEAD/kicad/rev2.1/stubby.ods -------------------------------------------------------------------------------- /processing/lib/core.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thebiguno/stubby/HEAD/processing/lib/core.jar -------------------------------------------------------------------------------- /processing/lib/jssc.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thebiguno/stubby/HEAD/processing/lib/jssc.jar -------------------------------------------------------------------------------- /processing/lib/serial.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thebiguno/stubby/HEAD/processing/lib/serial.jar -------------------------------------------------------------------------------- /simulation/ik_top_rotation/sketch.properties: -------------------------------------------------------------------------------- 1 | mode.id=processing.mode.java.JavaMode 2 | mode=Java 3 | -------------------------------------------------------------------------------- /kicad/rev1/gerber/stubby.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thebiguno/stubby/HEAD/kicad/rev1/gerber/stubby.zip -------------------------------------------------------------------------------- /kicad/rev2/gerber/stubby.zip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thebiguno/stubby/HEAD/kicad/rev2/gerber/stubby.zip -------------------------------------------------------------------------------- /simulation/inverse_kinematics_simulator/sketch.properties: -------------------------------------------------------------------------------- 1 | mode.id=processing.mode.java.JavaMode 2 | mode=Java 3 | -------------------------------------------------------------------------------- /processing/lib/linux32/libjSSC-2.8.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thebiguno/stubby/HEAD/processing/lib/linux32/libjSSC-2.8.so -------------------------------------------------------------------------------- /processing/lib/linux64/libjSSC-2.8.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thebiguno/stubby/HEAD/processing/lib/linux64/libjSSC-2.8.so -------------------------------------------------------------------------------- /processing/lib/windows32/jSSC-2.8.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thebiguno/stubby/HEAD/processing/lib/windows32/jSSC-2.8.dll -------------------------------------------------------------------------------- /processing/lib/windows64/jSSC-2.8.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thebiguno/stubby/HEAD/processing/lib/windows64/jSSC-2.8.dll -------------------------------------------------------------------------------- /kicad/rev1/hex.dcm: -------------------------------------------------------------------------------- 1 | EESchema-DOCLIB Version 2.0 Date: Wednesday, April 02, 2014 'amt' 11:14:58 am 2 | # 3 | #End Doc Library 4 | -------------------------------------------------------------------------------- /kicad/rev2/hex.dcm: -------------------------------------------------------------------------------- 1 | EESchema-DOCLIB Version 2.0 Date: Wednesday, April 02, 2014 'amt' 11:14:58 am 2 | # 3 | #End Doc Library 4 | -------------------------------------------------------------------------------- /processing/lib/macosx/libjSSC-2.8.jnilib: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thebiguno/stubby/HEAD/processing/lib/macosx/libjSSC-2.8.jnilib -------------------------------------------------------------------------------- /kicad/rev2.1/stubby.dcm: -------------------------------------------------------------------------------- 1 | EESchema-DOCLIB Version 2.0 Date: Wednesday, April 02, 2014 'amt' 11:14:58 am 2 | # 3 | #End Doc Library 4 | -------------------------------------------------------------------------------- /avr/hardware/timer2.h: -------------------------------------------------------------------------------- 1 | #ifndef TIMER_2_H 2 | #define TIMER_2_H 3 | 4 | #include 5 | 6 | void timer2_init(); 7 | 8 | #endif -------------------------------------------------------------------------------- /simulation/debug.c: -------------------------------------------------------------------------------- 1 | #include "debug.h" 2 | 3 | void pwm_set_phase_batch(uint8_t index, uint16_t phase){ 4 | printf(" index: %d, Phase: %d\n", index, phase); 5 | } 6 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | $* 2 | *.hex 3 | *.bak 4 | *.kicad_pcb-bak 5 | *.brd-bak 6 | *-cache.lib 7 | .DS_Store 8 | *~.skp 9 | ~*.dxf 10 | *~ 11 | .#* 12 | \#*\# 13 | .~lock.*# 14 | .kateproject.d/ 15 | -------------------------------------------------------------------------------- /simulation/Makefile: -------------------------------------------------------------------------------- 1 | all: 2 | g++ -DDEBUG_SIMULATION -pedantic -Os -Wall -I./ -o simulation.out Main.cpp ../source/Leg.cpp ../source/Point.cpp ../source/util/math.c debug.c 3 | ./simulation.out 4 | -------------------------------------------------------------------------------- /doc/todo.txt: -------------------------------------------------------------------------------- 1 | Outstanding TODO Items: 2 | -Rotate via Processing API 3 | -Move individual feet to specified position via Processing API 4 | -Rev 2 board assembly and testing 5 | -Distance sensor via Processing API -------------------------------------------------------------------------------- /simulation/debug.h: -------------------------------------------------------------------------------- 1 | #ifndef DEBUG_H 2 | #define DEBUG_H 3 | #include 4 | #include 5 | #include 6 | 7 | void pwm_set_phase_batch(uint8_t index, uint16_t phase); 8 | #endif 9 | 10 | -------------------------------------------------------------------------------- /avr/util/delays.h: -------------------------------------------------------------------------------- 1 | #ifndef DELAY_H 2 | #define DELAY_H 3 | 4 | #define __DELAY_BACKWARD_COMPATIBLE__ 5 | 6 | #include 7 | 8 | #include "../hardware/battery.h" 9 | 10 | void delay_ms(uint16_t ms); 11 | 12 | #endif 13 | -------------------------------------------------------------------------------- /avr/hardware/servo.h: -------------------------------------------------------------------------------- 1 | #ifndef SERVO_H 2 | #define SERVO_H 3 | 4 | #include 5 | 6 | #include "../Leg.h" 7 | #include "../hardware/status.h" 8 | 9 | #include "../lib/pwm/pwm.h" 10 | 11 | void servo_init(Leg *legs); 12 | 13 | #endif -------------------------------------------------------------------------------- /processing/src/library.properties: -------------------------------------------------------------------------------- 1 | name=Stubby Control Interface 2 | authorList=[Wyatt Olson](http://stubby.digitalcave.ca) 3 | url=http://stubby.digitalcave.ca 4 | category=Hardware 5 | sentence=Interface for controlling Stubby the Hexapod over a serial link (Bluetooth, FTDI, etc). 6 | version=1 7 | prettyVersion=@git@ -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Stubby 2 | ====== 3 | 4 | Source Code for Stubby the Hexapod (http://stubby.digitalcave.ca/stubby/ or http://hackaday.io/project/770) 5 | 6 | Repository includes AVR source code, KiCAD schematics + routed PCBs, frame design (.dxf for cutting on a scroll saw, laser cutter, CNC, etc), python programs for calibration and control, etc. Please visit the web pages for more information and build instructions. 7 | -------------------------------------------------------------------------------- /avr/hardware/status.h: -------------------------------------------------------------------------------- 1 | #ifndef STATUS_H 2 | #define STATUS_H 3 | 4 | #include 5 | 6 | #include "../Stubby.h" 7 | 8 | #include "../lib/pwm/pwm.h" 9 | 10 | void status_init(volatile uint8_t **ports, uint8_t *pins); 11 | 12 | void status_flash(uint8_t red, uint8_t green, uint8_t blue, uint8_t count); 13 | 14 | void status_set_color(uint8_t red, uint8_t green, uint8_t blue); 15 | 16 | #endif 17 | -------------------------------------------------------------------------------- /processing/src/ca/digitalcave/stubby/Message.java: -------------------------------------------------------------------------------- 1 | package ca.digitalcave.stubby; 2 | 3 | public class Message { 4 | private final int command; 5 | private final int[] data; 6 | 7 | public Message(int command, int[] data) { 8 | this.command = command; 9 | this.data = data; 10 | } 11 | 12 | public int getCommand() { 13 | return command; 14 | } 15 | 16 | public int[] getData() { 17 | return data; 18 | } 19 | } 20 | -------------------------------------------------------------------------------- /processing/.project: -------------------------------------------------------------------------------- 1 | 2 | 3 | processing 4 | 5 | 6 | 7 | 8 | 9 | org.eclipse.jdt.core.javabuilder 10 | 11 | 12 | 13 | 14 | 15 | org.eclipse.jdt.core.javanature 16 | 17 | 18 | -------------------------------------------------------------------------------- /kicad/rev2/gerber/hex-NPTH.txt: -------------------------------------------------------------------------------- 1 | M48 2 | ;DRILL file {Pcbnew (2013-dec-23)-stable} date Thu 04 Sep 2014 08:14:07 PM MDT 3 | ;FORMAT={-:-/ absolute / inch / decimal} 4 | FMAT,2 5 | INCH,TZ 6 | T1C0.030 7 | % 8 | G90 9 | G05 10 | M72 11 | T1 12 | X1.55Y-2.175 13 | X1.6Y-2.175 14 | X1.65Y-2.175 15 | X2.125Y-2.175 16 | X2.175Y-2.175 17 | X2.225Y-2.175 18 | X2.55Y-2.175 19 | X2.6Y-2.175 20 | X2.65Y-2.175 21 | X3.1Y-2.175 22 | X3.15Y-2.175 23 | X3.2Y-2.175 24 | T0 25 | M30 26 | -------------------------------------------------------------------------------- /avr/hardware/distance.h: -------------------------------------------------------------------------------- 1 | #ifndef DISTANCE_SENSOR_H 2 | #define DISTANCE_SENSOR_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | /* 10 | * Initializes the distance sensor hardware 11 | */ 12 | void distance_init(); 13 | 14 | /* 15 | * Takes a distance reading, and adds to the running average. 16 | */ 17 | void distance_measure(); 18 | 19 | /* 20 | * Read the filtered distance in mm 21 | */ 22 | uint16_t distance_read(); 23 | 24 | #endif -------------------------------------------------------------------------------- /processing/.classpath: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /avr/gait/gait.h: -------------------------------------------------------------------------------- 1 | #ifndef GAIT_H 2 | #define GAIT_H 3 | 4 | #ifndef DEBUG_SIMULATION 5 | #include 6 | #else 7 | #include 8 | #include 9 | #include 10 | #endif 11 | 12 | #include "../types/Point.h" 13 | #include "../Leg.h" 14 | 15 | /* 16 | * Returns the offset for the given leg at the given step index, scaled by the given velocity 17 | */ 18 | Point gait_step(Leg leg, uint8_t step_index, double linear_velocity, double linear_angle, double rotational_velocity); 19 | 20 | /* 21 | * Reteurns the number of steps in this gait 22 | */ 23 | uint8_t gait_step_count(); 24 | 25 | #endif 26 | -------------------------------------------------------------------------------- /avr/lib/Ring/Ring.cpp: -------------------------------------------------------------------------------- 1 | #include "Ring.h" 2 | 3 | Ring::Ring(uint8_t size) { 4 | this->buffer = (uint8_t*) malloc(size); 5 | this->size = size; 6 | this->head = 0x00; 7 | this->tail = 0x00; 8 | } 9 | 10 | uint8_t Ring::isEmpty(){ 11 | return (this->head == this->tail); 12 | } 13 | 14 | uint8_t Ring::isFull(){ 15 | return ((this->head + 1) % this->size == this->tail); 16 | } 17 | 18 | uint8_t Ring::get(){ 19 | char c = this->buffer[this->tail]; 20 | if (++this->tail >= this->size) this->tail = 0; 21 | return c; 22 | } 23 | 24 | void Ring::put(uint8_t data){ 25 | this->buffer[this->head] = data; 26 | if (++this->head >= this->size) this->head = 0; 27 | } -------------------------------------------------------------------------------- /avr/util/convert.h: -------------------------------------------------------------------------------- 1 | #ifndef CONVERT_H 2 | #define CONVERT_H 3 | 4 | #include 5 | #include 6 | 7 | #include "math.h" 8 | 9 | double convert_byte_to_radian(uint8_t x); 10 | uint8_t convert_radian_to_byte(double x); 11 | 12 | void convert_double_to_bytes(double value, uint8_t *buf, uint8_t offset); 13 | double convert_bytes_to_double(uint8_t *buf, uint8_t offset); 14 | 15 | /* 16 | * Normalize an angle (in radians) such that it is always in the range from 17 | * -M_PI to M_PI. 18 | */ 19 | double normalize_angle(double radians); 20 | 21 | /* 22 | * Find the difference between two angles, taking into account wraparound. 23 | * Each angle must be between -PI and PI. If a2 is clockwise from a1, 24 | * the result is positive; otherwise the result is negative. 25 | */ 26 | double difference_between_angles(double a1, double a2); 27 | 28 | #endif 29 | -------------------------------------------------------------------------------- /avr/util/math.h: -------------------------------------------------------------------------------- 1 | #ifndef TRIG_H 2 | #define TRIG_H 3 | 4 | #ifndef DEBUG_SIMULATION 5 | #include 6 | #else 7 | #include 8 | #include 9 | #include 10 | #endif 11 | #include 12 | 13 | /* 14 | * Returns an angle (in degrees) given the input value. If the value is 15 | * outside of the domain [-1, 1] then it returns 255 to indicate NaN. 16 | */ 17 | double acos_f(double value); 18 | 19 | /* 20 | * Returns the cosine value of the given angle (in degrees). 21 | */ 22 | double cos_f(double angle); 23 | 24 | /* 25 | * Returns the sine value of the given angle (in degrees). 26 | */ 27 | double sin_f(double angle); 28 | 29 | /* 30 | * Fast square root function; from http://www.mikrocontroller.net/articles/AVR_Arithmetik#avr-gcc_Implementierung_.2816_Bit.29 31 | */ 32 | uint16_t sqrt_f(uint16_t q); 33 | 34 | #endif 35 | -------------------------------------------------------------------------------- /kicad/rev1/gerber/hex-Edge_Cuts.gbr: -------------------------------------------------------------------------------- 1 | G04 (created by PCBNEW (2013-dec-23)-stable) date Thu 03 Jul 2014 07:48:38 AM MDT* 2 | %MOIN*% 3 | G04 Gerber Fmt 3.4, Leading zero omitted, Abs format* 4 | %FSLAX34Y34*% 5 | G01* 6 | G70* 7 | G90* 8 | G04 APERTURE LIST* 9 | %ADD10C,0.00590551*% 10 | %ADD11C,0.00393701*% 11 | G04 APERTURE END LIST* 12 | G54D10* 13 | G54D11* 14 | X13779Y-6692D02* 15 | X13779Y-24803D01* 16 | X32677Y-5905D02* 17 | X14566Y-5905D01* 18 | X33464Y-24803D02* 19 | X33464Y-6692D01* 20 | X14566Y-25590D02* 21 | X32677Y-25590D01* 22 | X33464Y-6692D02* 23 | G75* 24 | G03X32677Y-5905I-787J0D01* 25 | G74* 26 | G01* 27 | X32677Y-25590D02* 28 | G75* 29 | G03X33464Y-24803I0J787D01* 30 | G74* 31 | G01* 32 | X13779Y-24803D02* 33 | G75* 34 | G03X14566Y-25590I787J0D01* 35 | G74* 36 | G01* 37 | X14566Y-5905D02* 38 | G75* 39 | G03X13779Y-6692I0J-787D01* 40 | G74* 41 | G01* 42 | M02* 43 | -------------------------------------------------------------------------------- /avr/controllers/universal_controller.h: -------------------------------------------------------------------------------- 1 | #ifndef UNIVERSAL_CONTROLLER_CLIENT_STUBBY_H 2 | #define UNIVERSAL_CONTROLLER_CLIENT_STUBBY_H 3 | 4 | #include 5 | #include 6 | 7 | #include "../Stubby.h" 8 | #include "../Leg.h" 9 | #include "../gait/gait.h" 10 | #include "../hardware/battery.h" 11 | #include "../hardware/status.h" 12 | #include "../types/Point.h" 13 | #include "../util/delays.h" 14 | 15 | #include "../lib/universal_controller/client.h" 16 | #include "../lib/pwm/pwm.h" 17 | 18 | /* 19 | * Main entry point to the remote control code. When controller is set to UC, this method is called from main(). 20 | */ 21 | void uc_command_executor(); 22 | 23 | /* 24 | * Called from the Stubby protocol dispatch function; handles the UC-specific dispatching 25 | */ 26 | void uc_dispatch_message(uint8_t cmd, uint8_t *message, uint8_t length); 27 | 28 | #endif -------------------------------------------------------------------------------- /processing/src/ca/digitalcave/stubby/StubbySerial.java: -------------------------------------------------------------------------------- 1 | package ca.digitalcave.stubby; 2 | 3 | import jssc.SerialPortEvent; 4 | import jssc.SerialPortException; 5 | import processing.core.PApplet; 6 | import processing.serial.Serial; 7 | 8 | public class StubbySerial extends Serial { 9 | private Protocol protocol; 10 | 11 | public StubbySerial(PApplet parent, String iname, int irate) { 12 | super(parent, iname, irate); 13 | } 14 | 15 | synchronized public void serialEvent(SerialPortEvent serialEvent) { 16 | try { 17 | if (serialEvent.getEventType() == SerialPortEvent.RXCHAR) { 18 | if (protocol != null) 19 | protocol.receiveByte(port.readBytes(1)[0] & 0xFF); 20 | } 21 | } catch (SerialPortException e) { 22 | System.err.println(e.getMessage()); 23 | } 24 | } 25 | 26 | public void setProtocol(Protocol protocol){ 27 | this.protocol = protocol; 28 | } 29 | } 30 | -------------------------------------------------------------------------------- /avr/hardware/servo.c: -------------------------------------------------------------------------------- 1 | #include "servo.h" 2 | 3 | void servo_init(Leg *legs){ 4 | //Set up the servos using the leg details + status LEDs 5 | volatile uint8_t *ports[PWM_COUNT]; 6 | uint8_t pins[PWM_COUNT]; 7 | 8 | for (uint8_t l = 0; l < LEG_COUNT; l++){ 9 | for (uint8_t j = 0; j < JOINT_COUNT; j++){ 10 | ports[(l * JOINT_COUNT) + j] = legs[l].getPort(j); 11 | pins[(l * JOINT_COUNT) + j] = legs[l].getPin(j); 12 | } 13 | 14 | for (uint8_t j = 0; j < CALIBRATION_COUNT; j++){ 15 | legs[l].setCalibration(j, eeprom_read_byte((uint8_t*) (l * CALIBRATION_COUNT) + j)); 16 | } 17 | } 18 | 19 | status_init(ports, pins); 20 | 21 | pwm_init(ports, pins, PWM_COUNT, 20000); 22 | status_set_color(0x00, 0x00, 0x00); 23 | 24 | for (uint8_t l = 0; l < LEG_COUNT; l++){ 25 | legs[l].resetPosition(); 26 | } 27 | pwm_apply_batch(); 28 | 29 | delay_ms(500); 30 | 31 | pwm_stop(); 32 | } 33 | -------------------------------------------------------------------------------- /avr/hardware/battery.h: -------------------------------------------------------------------------------- 1 | #ifndef BATTERY_H 2 | #define BATTERY_H 3 | 4 | #include 5 | 6 | #include "status.h" 7 | 8 | #include "../lib/pwm/pwm.h" 9 | 10 | #if PCB_REVISION == 1 11 | //The minimum ADC value which indicates a full battery 12 | #define BATTERY_LEVEL_FULL 180 13 | //The maximum ADC value which indicates an empty battery 14 | #define BATTERY_LEVEL_EMPTY 150 15 | #elif PCB_REVISION == 2 16 | //The minimum ADC value which indicates a full battery 17 | #define BATTERY_LEVEL_FULL 128 18 | //The maximum ADC value which indicates an empty battery 19 | #define BATTERY_LEVEL_EMPTY 100 20 | #else 21 | #warning Non-standard PCB revision specified. Please calibrate battery. 22 | #endif 23 | 24 | //Defines a linear progression for valid battery values. 25 | #define BATTERY_STATUS_MULTIPLIER 255 / (BATTERY_LEVEL_FULL - BATTERY_LEVEL_EMPTY) 26 | 27 | void battery_init(); 28 | void battery_enable_status(); 29 | void battery_disable_status(); 30 | void battery_set_level(); 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /avr/types/Point.cpp: -------------------------------------------------------------------------------- 1 | #include "Point.h" 2 | 3 | Point::Point(){ 4 | } 5 | Point::Point(int16_t x, int16_t y, int16_t z){ 6 | this->x = x; 7 | this->y = y; 8 | this->z = z; 9 | } 10 | 11 | void Point::add(Point offset){ 12 | this->x += offset.x; 13 | this->y += offset.y; 14 | this->z += offset.z; 15 | } 16 | 17 | void Point::set(int16_t x, int16_t y, int16_t z){ 18 | this->x = x; 19 | this->y = y; 20 | this->z = z; 21 | } 22 | 23 | void rotate2D(int16_t* c1, int16_t* c2, double angle){ 24 | //Use rotational matrix to rotate point c1,c2 around the origin, resulting in new point n1,n2 25 | int16_t n1 = *c1 * cos_f(angle) - *c2 * sin_f(angle); 26 | int16_t n2 = *c1 * sin_f(angle) + *c2 * cos_f(angle); 27 | 28 | *c1 = n1; 29 | *c2 = n2; 30 | } 31 | 32 | void Point::rotateXY(double angle){ 33 | rotate2D(&this->x, &this->y, angle); 34 | } 35 | 36 | void Point::rotateXZ(double angle){ 37 | rotate2D(&this->z, &this->x, angle); 38 | } 39 | 40 | void Point::rotateYZ(double angle){ 41 | rotate2D(&this->y, &this->z, angle); 42 | } 43 | 44 | -------------------------------------------------------------------------------- /kicad/rev2/gerber/hex-B_SilkS.gbo: -------------------------------------------------------------------------------- 1 | G04 (created by PCBNEW (2013-dec-23)-stable) date Thu 04 Sep 2014 08:14:04 PM MDT* 2 | %MOIN*% 3 | G04 Gerber Fmt 3.4, Leading zero omitted, Abs format* 4 | %FSLAX34Y34*% 5 | G01* 6 | G70* 7 | G90* 8 | G04 APERTURE LIST* 9 | %ADD10C,0.00590551*% 10 | %ADD11C,0.005*% 11 | %ADD12C,0.011811*% 12 | G04 APERTURE END LIST* 13 | G54D10* 14 | G54D11* 15 | X14714Y-23080D02* 16 | X14714Y-22880D01* 17 | X14600Y-23080D02* 18 | X14685Y-22966D01* 19 | X14600Y-22880D02* 20 | X14714Y-22995D01* 21 | X14438Y-23071D02* 22 | X14457Y-23080D01* 23 | X14495Y-23080D01* 24 | X14514Y-23071D01* 25 | X14523Y-23052D01* 26 | X14523Y-22976D01* 27 | X14514Y-22957D01* 28 | X14495Y-22947D01* 29 | X14457Y-22947D01* 30 | X14438Y-22957D01* 31 | X14428Y-22976D01* 32 | X14428Y-22995D01* 33 | X14523Y-23014D01* 34 | X14361Y-22947D02* 35 | X14314Y-23080D01* 36 | X14266Y-22947D02* 37 | X14314Y-23080D01* 38 | X14333Y-23128D01* 39 | X14342Y-23138D01* 40 | X14361Y-23147D01* 41 | G54D12* 42 | X18514Y-7275D02* 43 | X18514Y-7724D01* 44 | X18724Y-6264D02* 45 | X18275Y-6264D01* 46 | X18500Y-6489D02* 47 | X18500Y-6039D01* 48 | M02* 49 | -------------------------------------------------------------------------------- /avr/hardware/magnetometer.h: -------------------------------------------------------------------------------- 1 | #ifndef MAGNETOMETER_H 2 | #define MAGNETOMETER_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "../Stubby.h" 9 | #include "status.h" 10 | #include "../util/convert.h" 11 | 12 | #include "../lib/twi/twi.h" 13 | 14 | 15 | /* 16 | * Initializes the magnetometer and performs a self calibration 17 | */ 18 | void magnetometer_init(); 19 | 20 | /* 21 | * Get the currently set offsets (x & y). 22 | */ 23 | void magnetometer_get_offsets(int16_t *x, int16_t *y); 24 | 25 | /* 26 | * Set the offsets (x & y). This is used to center the readings around zero. These 27 | * values will depend on the location and surroundings of the magnetometer, and should 28 | * be obtained from an in-situ calibration. 29 | */ 30 | void magnetometer_set_offsets(int16_t x, int16_t y); 31 | 32 | /* 33 | * Read the raw values into x and y 34 | */ 35 | void magnetometer_get_raw(int16_t *x, int16_t *y); 36 | 37 | /* 38 | * Read the compass heading, with 0 pointing straight north, positive values 39 | * pointing east, and negative values pointing west. Value is in radians. 40 | */ 41 | double magnetometer_read_heading(); 42 | 43 | #endif -------------------------------------------------------------------------------- /avr/lib/Ring/Ring.h: -------------------------------------------------------------------------------- 1 | #ifndef RING_H 2 | #define RING_H 3 | 4 | #include 5 | #include 6 | 7 | class Ring { 8 | private: 9 | //Just like a snake eating something and pooping it out... 10 | uint8_t size; 11 | volatile uint8_t head; //You put data into head... 12 | volatile uint8_t tail; //...and take it off of tail. 13 | // If they are equal, then there is nothing in the buffer. 14 | // If (head + 1) % length == tail then the buffer is full. 15 | // The current position of head points to the location where the next byte will 16 | // be written (and head will then be incremented after the byte is written); 17 | // the position of tail points to the location of the last byte which was read, 18 | // and must be incremented before the next read. 19 | // NOTE: You *cannot* use anything larger than uint8_t for the head / tail 20 | // indices; doing so will result in corrupt output, presumably because it 21 | // takes more than one instruction to do comparisons. 22 | volatile uint8_t *buffer; 23 | 24 | public: 25 | Ring(uint8_t size); 26 | 27 | uint8_t isEmpty(); 28 | uint8_t isFull(); 29 | 30 | uint8_t get(); 31 | void put(uint8_t data); 32 | }; 33 | 34 | #endif 35 | 36 | -------------------------------------------------------------------------------- /avr/util/convert.c: -------------------------------------------------------------------------------- 1 | #include "convert.h" 2 | 3 | union udouble { 4 | double d; 5 | uint8_t u[sizeof(double)]; 6 | }; 7 | 8 | void convert_double_to_bytes(double value, uint8_t *buffer, uint8_t offset){ 9 | union udouble converter; 10 | converter.d = value; 11 | for (uint8_t i = 0; i < sizeof(double); i++) { 12 | buffer[i + offset] = converter.u[i]; 13 | } 14 | } 15 | double convert_bytes_to_double(uint8_t *buffer, uint8_t offset){ 16 | union udouble converter; 17 | for (uint8_t i = 0; i < sizeof(double); i++) { 18 | converter.u[i] = buffer[i + offset]; 19 | } 20 | return converter.d; 21 | } 22 | 23 | double convert_byte_to_radian(uint8_t x){ 24 | double r = 0.024639942381096 * x; 25 | return normalize_angle(r); 26 | } 27 | uint8_t convert_radian_to_byte(double r){ 28 | r = normalize_angle(r); 29 | return (uint8_t) (40.584510488433314 * r); 30 | } 31 | 32 | double normalize_angle(double radians){ 33 | while (radians > M_PI){ 34 | radians += (M_PI * -2); 35 | } 36 | while (radians < (M_PI * -1)){ 37 | radians += (M_PI * 2); 38 | } 39 | return radians; 40 | } 41 | 42 | double difference_between_angles(double a1, double a2){ 43 | //return normalize_angle(atan2(sin_f(a2), cos_f(a2)) - atan2(sin_f(a1), cos_f(a1))); 44 | return normalize_angle(a1 - a2); 45 | } -------------------------------------------------------------------------------- /processing/test/Test.java: -------------------------------------------------------------------------------- 1 | import processing.core.PApplet; 2 | import processing.serial.Serial; 3 | import ca.digitalcave.stubby.Stubby; 4 | 5 | public class Test extends PApplet { 6 | public static void main(String args[]) { 7 | PApplet.main(new String[] { "Test" }); 8 | } 9 | 10 | public void setup(){ 11 | Stubby stubby = new Stubby(this, "/dev/ttyUSB0"); 12 | stubby.enableDebug(); 13 | stubby.turnOn(); 14 | stubby.moveForward(1000); 15 | stubby.moveBackward(1000); 16 | //stubby.turnAroundClockwise(); 17 | //stubby.moveForward(1000); 18 | // stubby.moveRight(1000); 19 | // stubby.moveLeft(1000); 20 | // stubby.turnLeft(); 21 | // stubby.turnRight(); 22 | //stubby.moveForward(1000); 23 | // stubby.moveForward(300); 24 | // stubby.turnAroundClockwise(); 25 | // stubby.moveForward(300); 26 | // stubby.turnAround(); 27 | // for (int i = 0; i < 100; i++){ 28 | // stubby.requestHeading(); 29 | // try {Thread.sleep(100);} catch (InterruptedException e) {} 30 | // } 31 | // try {Thread.sleep(10000);} catch (InterruptedException e) {} 32 | stubby.turnOff(); 33 | } 34 | 35 | public void serialEvent(Serial serial){ 36 | System.out.println(Integer.toHexString(serial.read())); 37 | } 38 | 39 | public void draw() { 40 | System.exit(0); 41 | } 42 | } 43 | -------------------------------------------------------------------------------- /avr/types/Point.h: -------------------------------------------------------------------------------- 1 | #ifndef POINT_H 2 | #define POINT_H 3 | 4 | #ifndef DEBUG_SIMULATION 5 | #include 6 | #else 7 | #include 8 | #include 9 | #include 10 | #endif 11 | 12 | #include "../util/math.h" 13 | 14 | 15 | class Point { 16 | public: 17 | Point(); 18 | Point(int16_t x, int16_t y, int16_t z); 19 | 20 | /* 21 | * Adds the specified offset point to this point. 22 | */ 23 | void add(Point offset); 24 | 25 | /* 26 | * Replace this point with the specified point 27 | */ 28 | void set(int16_t x, int16_t y, int16_t z); 29 | 30 | /* 31 | * Rotate this point by specified angle on the x,y plane 32 | * around 0,0 33 | */ 34 | void rotateXY(double angle); 35 | 36 | /* 37 | * Rotate this point by specified angle on the x,z plane 38 | * around 0,0 39 | */ 40 | void rotateXZ(double angle); 41 | 42 | /* 43 | * Rotate this point by specified angle on the y,z plane 44 | * around 0,0 45 | */ 46 | void rotateYZ(double angle); 47 | 48 | /* 49 | * Rotate this point by the specified angle around 50 | * the axis intercepting points 0,0,0 and v. 51 | */ 52 | //void rotateXYZ(Point v, double angle); 53 | 54 | int16_t x; 55 | int16_t y; 56 | int16_t z; 57 | }; 58 | 59 | #endif 60 | -------------------------------------------------------------------------------- /avr/lib/protocol/protocol.txt: -------------------------------------------------------------------------------- 1 | Protocol Info 2 | ------------- 3 | 4 | Message Structure (bi-directional): 5 | Segment Length 6 | --------------------------------- 7 | Frame start 1 byte (0x7e) 8 | Length 1 byte 9 | Command 1 byte 10 | Payload 0 - 254 bytes 11 | Checksum 1 byte 12 | 13 | Commands: 14 | --------- 15 | Commands have a variable payload. A description of payloads is below. 16 | Portions in [square brackets] are optional. 17 | 18 | Command Direction Payload Response 19 | ------------------------------------------------------------------------------------------------------------------------------------------------------------- 20 | -- General commands 21 | AnnounceControlId Control -> Device Controller ID; UC = 0x55 ('U'), Processing = 0x50 ('P'), etc None 22 | 23 | SendAcknowledge Device -> Control Command being ack'd (1 byte) None 24 | SendComplete Device -> Control Command completed (1 byte) None 25 | 26 | RequestEnableDebug Control -> Device Null Ack 27 | RequestDisableDebug Control -> Device Null Ack 28 | SendDebug Device -> Control Variable depending on message, max 254 bytes None 29 | 30 | RequestBattery Control -> Device Null Battery message 31 | SendBattery Device -> Control 1 byte None 32 | 33 | Specific implementations can extend the commands. All the commands listed above are in the 0x0X range (see protocol.h, MESSAGE_* defines, for specific values) -------------------------------------------------------------------------------- /avr/lib/serial/serial.c: -------------------------------------------------------------------------------- 1 | /* 2 | * A modular implementation of serial library. You only need to include those 3 | * parts which you need. 4 | * 5 | * This module defines the init methods; add rx / tx modules as you need. 6 | */ 7 | 8 | #include "serial.h" 9 | 10 | //Defined in rx / tx modules 11 | void _serial_init_rx(); 12 | void _serial_init_tx(); 13 | 14 | void serial_init(uint32_t baud, uint8_t data_bits, uint8_t parity, uint8_t stop_bits){ 15 | //Set baud rate 16 | uint16_t calculated_baud = (F_CPU / 16 / baud) - 1; 17 | UBRR0H = calculated_baud >> 8; 18 | UBRR0L = calculated_baud & 0xFF; 19 | 20 | //Make sure 2x and multi processor comm modes are off 21 | UCSR0A &= ~(_BV(U2X0) | _BV(MPCM0)); 22 | 23 | //Calculate frame format 24 | //Init to 0; we populate this later 25 | UCSR0C = 0x0; 26 | 27 | //Data bits 28 | if (data_bits == 9){ 29 | //9 bits use an extra bit in register UCSR0B 30 | UCSR0B |= _BV(UCSZ02); 31 | UCSR0C |= _BV(UCSZ01) | _BV(UCSZ00); 32 | } 33 | else { 34 | UCSR0C |= ((data_bits - 5) << UCSZ00); 35 | } 36 | 37 | //Parity, Stop bits 38 | UCSR0C |= (parity << UPM00) | ((stop_bits - 1) << USBS0); 39 | 40 | //Enable Rx / Tx if they are not disabled, and call init methods of rx/tx modules 41 | #ifndef SERIAL_DISABLE_RX 42 | UCSR0B |= _BV(RXEN0); 43 | _serial_init_rx(); 44 | #endif 45 | #ifndef SERIAL_DISABLE_TX 46 | UCSR0B |= _BV(TXEN0); 47 | _serial_init_tx(); 48 | #endif 49 | } 50 | 51 | void serial_init_b(uint32_t baud){ 52 | serial_init(baud, 8, 0, 1); 53 | } 54 | 55 | -------------------------------------------------------------------------------- /avr/controllers/calibration.h: -------------------------------------------------------------------------------- 1 | #ifndef CALIBRATION_H 2 | #define CALIBRATION_H 3 | 4 | #include 5 | #include 6 | 7 | #include "../Stubby.h" 8 | #include "../Leg.h" 9 | #include "../gait/gait.h" 10 | #include "../hardware/battery.h" 11 | #include "../hardware/magnetometer.h" 12 | #include "../types/Point.h" 13 | #include "../util/delays.h" 14 | 15 | #include "../lib/pwm/pwm.h" 16 | 17 | //Calibration-specific messages are in 0x3X space... 18 | #define MESSAGE_SAVE_CALIBRATION 0x30 19 | #define MESSAGE_LOAD_CALIBRATION 0x31 20 | #define MESSAGE_RESET_CALIBRATION 0x32 21 | #define MESSAGE_REQUEST_JOINT_CALIBRATION 0x33 22 | #define MESSAGE_SEND_JOINT_CALIBRATION 0x34 23 | #define MESSAGE_REQUEST_FOOT_CALIBRATION 0x35 24 | #define MESSAGE_SEND_FOOT_CALIBRATION 0x36 25 | #define MESSAGE_REQUEST_MAGNETOMETER_CALIBRATION 0x37 26 | #define MESSAGE_SEND_MAGNETOMETER_CALIBRATION 0x38 27 | #define MESSAGE_START_MAGNETOMETER_CALIBRATION 0x39 28 | 29 | //Reset Calibration modes 30 | #define MODE_CALIBRATION_JOINTS 0x01 31 | #define MODE_CALIBRATION_FEET 0x02 32 | #define MODE_CALIBRATION_MAGNETOMETER 0x03 33 | 34 | /* 35 | * Main entry point to the calibration code. When controller is set to Calibration, this method is called from main(). 36 | */ 37 | void calibration_command_executor(); 38 | 39 | /* 40 | * Called from the Stubby protocol dispatch function 41 | */ 42 | void calibration_dispatch_message(uint8_t cmd, uint8_t *message, uint8_t length); 43 | 44 | #endif -------------------------------------------------------------------------------- /simulation/Main.cpp: -------------------------------------------------------------------------------- 1 | #include "../source/Stubby.h" 2 | #include "../source/Leg.h" 3 | 4 | int main (void){ 5 | //Set up the leg objects, including servo details and mounting angle 6 | volatile uint8_t p = 0; 7 | Leg legs[LEG_COUNT] = { 8 | Leg(FRONT_LEFT, &p, p, &p, p, &p, p, 2 * LEG_MOUNTING_ANGLE, Point(-60, 104, 0)), 9 | Leg(MIDDLE_LEFT, &p, p, &p, p, &p, p, 3 * LEG_MOUNTING_ANGLE, Point(-120, 0, 0)), 10 | Leg(REAR_LEFT, &p, p, &p, p, &p, p, 4 * LEG_MOUNTING_ANGLE, Point(-60, -104, 0)), 11 | Leg(REAR_RIGHT, &p, p, &p, p, &p, p, 5 * LEG_MOUNTING_ANGLE, Point(60, -104, 0)), 12 | Leg(MIDDLE_RIGHT, &p, p, &p, p, &p, p, 0 * LEG_MOUNTING_ANGLE, Point(120, 0, 0)), 13 | Leg(FRONT_RIGHT, &p, p, &p, p, &p, p, 1 * LEG_MOUNTING_ANGLE, Point(60, 104, 0)) 14 | }; 15 | 16 | //for (uint8_t l = 0; l < LEG_COUNT; l++){ 17 | // legs[l].resetPosition(); 18 | //} 19 | 20 | //legs[MIDDLE_RIGHT].resetPosition(); 21 | legs[MIDDLE_RIGHT].setOffset(Point(0,-30,0)); 22 | legs[MIDDLE_RIGHT].setOffset(Point(0,-20,0)); 23 | legs[MIDDLE_RIGHT].setOffset(Point(0,-10,0)); 24 | legs[MIDDLE_RIGHT].setOffset(Point(0,0,0)); 25 | legs[MIDDLE_RIGHT].setOffset(Point(0,10,0)); 26 | legs[MIDDLE_RIGHT].setOffset(Point(0,20,0)); 27 | legs[MIDDLE_RIGHT].setOffset(Point(0,30,0)); 28 | 29 | // for (uint8_t l = 0; l < LEG_COUNT; l++){ 30 | // legs[l].resetPosition(); 31 | // } 32 | 33 | // for (uint8_t j = 0; j < JOINT_COUNT; j++){ 34 | // pwm_set_phase_batch(MIDDLE_RIGHT * JOINT_COUNT + j, PHASE_NEUTRAL + (legs[MIDDLE_RIGHT].getCalibration(j) * 10)); 35 | // } 36 | 37 | 38 | return 0; 39 | } 40 | -------------------------------------------------------------------------------- /avr/hardware.mk: -------------------------------------------------------------------------------- 1 | # This file is the single location which defines what hardware is available on the system. 2 | # Override lines to change PCB revision, magentometer, distance sensor, etc. Comment 3 | # out a given line to revert to default value. 4 | ######################## 5 | 6 | #PCB Revision. Set to 1 for 1.x, 2 for 2.x, etc. 7 | #Defaults to 2 8 | #PCB_REVISION=2 9 | 10 | #Is a magnetometer on board? Set to 1 for yes, 0 for no. 11 | #Defaults to 1 12 | #MAGNETOMETER=1 13 | 14 | #The orientation of the magnetometer. This value will be added to the output heading 15 | # to make 0 point North. For rev 2.1 (and 2.0 if you mount the Adafruit magnetometer 16 | # in the recommended way) boards, this is PI radians (180 degrees). 17 | #MAGNETOMETER_ORIENTATION_OFFSET=3.1415926 18 | 19 | #Is a distance sensor on board? Set to 1 for yes, 0 for no. 20 | #Defaults to 1 21 | #DISTANCE_SENSOR=1 22 | 23 | #Manually set the F_CPU speed. Only do this if you are using a non-standard value 24 | # (i.e. something other than 20MHz for Rev2 boards, or 12MHz for Rev1 boards). 25 | #Defaults to the nominal value for the specified board revision. 26 | #F_CPU=20000000 27 | 28 | #Manually set the TWI_FREQ speed. Only do this if you are having problems with the default 29 | # (most likely happening if you don't have high enough pull-ups on SDA / SCL). 30 | #Defaults to 400000L 31 | #TWI_FREQ=400000L 32 | 33 | #Change the AVR programmer if needed. Defaults to usbtiny 34 | #AVRDUDE_PROGRAMMER=stk500v2 35 | 36 | #Change the AVR Programmer port if needed. Defaults to nothing (as usbtiny uses USB VID/PID) 37 | #AVRDUDE_PORT= -------------------------------------------------------------------------------- /avr/controllers/processing.h: -------------------------------------------------------------------------------- 1 | #ifndef PROCESSING_H 2 | #define PROCESSING_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include "../Stubby.h" 9 | #include "../Leg.h" 10 | #include "../gait/gait.h" 11 | #include "../hardware/battery.h" 12 | #include "../hardware/distance.h" 13 | #include "../hardware/magnetometer.h" 14 | #include "../hardware/status.h" 15 | #include "../util/convert.h" 16 | 17 | //PC / Stubby-specific messages are in 0x2X space... 18 | #define MESSAGE_REQUEST_POWER_ON 0x20 19 | #define MESSAGE_REQUEST_POWER_OFF 0x21 20 | #define MESSAGE_REQUEST_MOVE 0x22 21 | #define MESSAGE_REQUEST_TURN 0x23 22 | #define MESSAGE_REQUEST_TRANSLATE 0x24 23 | #define MESSAGE_REQUEST_ROTATE 0x25 24 | #define MESSAGE_REQUEST_HEADING 0x26 25 | #define MESSAGE_SEND_HEADING 0x27 26 | #define MESSAGE_REQUEST_DISTANCE 0x28 27 | #define MESSAGE_SEND_DISTANCE 0x29 28 | #define MESSAGE_REQUEST_OPTICAL 0x2A 29 | #define MESSAGE_SEND_OPTICAL 0x2B 30 | #define MESSAGE_REQUEST_SET_LED 0x2C 31 | 32 | #if MAGNETOMETER == 0 33 | #warning Without a magnetometer, Processing API command MESSAGE_REQUEST_TURN will not work, and MESSAGE_REQUEST_MOVE will not be accurate. 34 | #endif 35 | 36 | /* 37 | * Main processing method, where we look for incoming commands and perform actions as requested. 38 | */ 39 | void processing_command_executor(); 40 | 41 | /* 42 | * Processing-specific message dispatching. This is called from an ISR, so keep things as short as possible here. 43 | */ 44 | void processing_dispatch_message(uint8_t cmd, uint8_t *message, uint8_t length); 45 | 46 | #endif -------------------------------------------------------------------------------- /avr/hardware/timer2.c: -------------------------------------------------------------------------------- 1 | #include "timer2.h" 2 | 3 | extern volatile uint8_t interval_do_magnetometer_reading; //Defined in magnetometer_HMC5883L.c 4 | extern volatile uint8_t interval_do_distance_reading; //Defined in distance_HCSR04.c 5 | 6 | void timer2_init(){ 7 | //Set up the timer to run at F_CPU / 1024, in normal mode 8 | TCCR2A = 0x0; 9 | TCCR2B |= _BV(CS22) | _BV(CS21) | _BV(CS20); 10 | TIMSK2 = _BV(TOIE2); 11 | } 12 | 13 | EMPTY_INTERRUPT(TIMER2_COMPA_vect) 14 | EMPTY_INTERRUPT(TIMER2_COMPB_vect) 15 | ISR(TIMER2_OVF_vect){ 16 | sei(); 17 | 18 | #if F_CPU == 12000000 19 | #define MAGNETOMETER_RESET 3 20 | #define BATTERY_RESET 31 21 | #define DISTANCE_RESET 63 22 | #elif F_CPU == 18432000 23 | #define MAGNETOMETER_RESET 6 24 | #define BATTERY_RESET 63 25 | #define DISTANCE_RESET 127 26 | #elif F_CPU == 20000000 27 | #define MAGNETOMETER_RESET 7 28 | #define BATTERY_RESET 63 29 | #define DISTANCE_RESET 127 30 | #else 31 | #warning Non-standard CPU speed. Please specify a delay. 32 | #endif 33 | 34 | static uint8_t magnetometer_counter = 0; 35 | static uint8_t battery_counter = 0; 36 | static uint8_t distance_counter = 0; 37 | 38 | magnetometer_counter++; 39 | battery_counter++; 40 | distance_counter++; 41 | 42 | if (magnetometer_counter >= MAGNETOMETER_RESET){ 43 | magnetometer_counter = 0; 44 | interval_do_magnetometer_reading = 0x01; 45 | } 46 | if (battery_counter >= BATTERY_RESET){ 47 | battery_counter = 0; 48 | ADCSRA |= _BV(ADSC); //Start ADC 49 | } 50 | if (distance_counter >= DISTANCE_RESET){ 51 | distance_counter = 0; 52 | interval_do_distance_reading = 0x01; 53 | } 54 | } 55 | -------------------------------------------------------------------------------- /avr/hardware/battery.c: -------------------------------------------------------------------------------- 1 | #include "battery.h" 2 | 3 | #define ALPHA 0.2 4 | 5 | uint8_t battery_status_enabled = 0x00; //Should the battery levels show on status light? 6 | volatile uint8_t battery_level = 0x00; //Raw battery level; set in ADC ISR, cleared in status_set_battery_level() 7 | 8 | //The value is the running average; initially we set this to 0x00 as a flag to take the first 9 | // ADC value directly without averaging. 10 | static uint8_t value = 0x00; 11 | 12 | 13 | void battery_init(){ 14 | //ADC Enable, ADC interrupt, prescaler F_CPU / 128 15 | ADCSRA |= _BV(ADEN) | _BV(ADIE) | _BV(ADPS2) | _BV(ADPS1) | _BV(ADPS0); 16 | 17 | //Free running mode (the default, but in case it was set previously) 18 | ADCSRB = 0x00; 19 | 20 | //Set AREF mode: AREF = VCC, ADC Left Adjust, Pin A7 21 | ADMUX = _BV(REFS0) | _BV(ADLAR) | 0x07; 22 | } 23 | 24 | void battery_enable_status(){ 25 | battery_status_enabled = 0x01; 26 | } 27 | 28 | void battery_disable_status(){ 29 | battery_status_enabled = 0x00; 30 | } 31 | 32 | void battery_set_level(){ 33 | if (value == 0x00){ 34 | value = battery_level; 35 | } 36 | else { 37 | value = (uint8_t) (ALPHA * battery_level + (1 - ALPHA) * value); 38 | } 39 | 40 | if (battery_status_enabled){ 41 | if (value >= BATTERY_LEVEL_FULL){ 42 | status_set_color(0x00, 0xFF, 0x00); 43 | } 44 | else if (value <= BATTERY_LEVEL_EMPTY){ 45 | status_set_color(0xFF, 0x00, 0x00); 46 | } 47 | else { 48 | status_set_color((BATTERY_LEVEL_FULL - value) * BATTERY_STATUS_MULTIPLIER, (value - BATTERY_LEVEL_EMPTY) * BATTERY_STATUS_MULTIPLIER, 0x00); 49 | } 50 | } 51 | 52 | battery_level = 0x00; 53 | } 54 | 55 | ISR(ADC_vect){ 56 | battery_level = ADCH; 57 | } 58 | -------------------------------------------------------------------------------- /avr/lib/pwm/pwm.S: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | .global TIMER1_COMPB_vect 4 | TIMER1_COMPB_vect: 5 | 6 | ;Save calling state 7 | PUSH r0 8 | IN r0, _SFR_IO_ADDR(SREG) 9 | PUSH r24 10 | PUSH r25 11 | PUSH ZL 12 | PUSH ZH 13 | 14 | ;Load the pointer address into register Z. As with all 15 | ; 16 bit register accesses, you read the low byte first. 16 | LDS ZL, _pwm_events_low_ptr 17 | LDS ZH, _pwm_events_low_ptr+1 18 | 19 | ; Move the pointer past the current OCR1B value 20 | ADIW ZL, 0x02 21 | 22 | #ifndef PWM_PORTA_UNUSED 23 | IN r24, _SFR_IO_ADDR(PORTA) 24 | LD r25, Z+ 25 | AND r24, r25 26 | OUT _SFR_IO_ADDR(PORTA), r24 27 | #endif 28 | 29 | 30 | #ifndef PWM_PORTB_UNUSED 31 | IN r24, _SFR_IO_ADDR(PORTB) 32 | LD r25, Z+ 33 | AND r24, r25 34 | OUT _SFR_IO_ADDR(PORTB), r24 35 | #endif 36 | 37 | 38 | #ifndef PWM_PORTC_UNUSED 39 | IN r24, _SFR_IO_ADDR(PORTC) 40 | LD r25, Z+ 41 | AND r24, r25 42 | OUT _SFR_IO_ADDR(PORTC), r24 43 | #endif 44 | 45 | 46 | #ifndef PWM_PORTD_UNUSED 47 | IN r24, _SFR_IO_ADDR(PORTD) 48 | LD r25, Z+ 49 | AND r24, r25 50 | OUT _SFR_IO_ADDR(PORTD), r24 51 | #endif 52 | 53 | ; Update pointer with next value in array 54 | STS _pwm_events_low_ptr+1, ZH 55 | STS _pwm_events_low_ptr, ZL 56 | 57 | ;Load the new compare value into OCRnB. As with all 58 | ; 16 bit register access, you write the high byte first. 59 | LD r24, Z+ 60 | LD r25, Z+ 61 | STS OCR1BH, r25 62 | STS OCR1BL, r24 63 | 64 | ;Restore calling state 65 | POP ZH 66 | POP ZL 67 | POP r25 68 | POP r24 69 | OUT _SFR_IO_ADDR(SREG), r0 70 | POP r0 71 | 72 | RETI -------------------------------------------------------------------------------- /avr/lib/protocol/protocol_serial.c: -------------------------------------------------------------------------------- 1 | #include "../serial/serial.h" 2 | #include "protocol.h" 3 | #include 4 | 5 | void _serial_init_rx(){ 6 | //Enable RX interrupts 7 | UCSR0B |= _BV(RXCIE0); 8 | 9 | //Enable interrupts if the NO_INTERRUPT_ENABLE define is not set. If it is, you need to call sei() elsewhere. 10 | #ifndef NO_INTERRUPT_ENABLE 11 | sei(); 12 | #endif 13 | } 14 | 15 | void protocol_write(uint8_t byte){ 16 | serial_write_b(byte); 17 | } 18 | 19 | //Note: These defines are only a small subset of those which are available (and are 20 | // pretty much only the chips which I personally use). You can add more chips to 21 | // these defines, and as long as you match the correct chips with the correct vector 22 | // names, it should just work. 23 | #if defined(__AVR_ATtiny2313__) || \ 24 | defined(__AVR_ATmega48P__) || \ 25 | defined(__AVR_ATmega88P__) || \ 26 | defined(__AVR_ATmega168P__) || \ 27 | defined(__AVR_ATmega328P__) || \ 28 | defined(__AVR_ATmega48__) || \ 29 | defined(__AVR_ATmega88__) || \ 30 | defined(__AVR_ATmega168__) 31 | ISR(USART_RX_vect){ 32 | #elif defined(__AVR_ATmega324P__) || \ 33 | defined(__AVR_ATmega644__) || \ 34 | defined(__AVR_ATmega644P__) || \ 35 | defined(__AVR_ATmega644PA__) || \ 36 | defined(__AVR_ATmega1284P__) 37 | ISR(USART0_RX_vect){ 38 | #else 39 | #error You must define USART vectors for your chip! Please verify that MMCU is set correctly, and that there is a matching vector definition in serial_asynchronous.c 40 | void error1() { 41 | #endif 42 | UCSR0B &= ~_BV(RXCIE0); //Disable RX interrupts so we don't recurse 43 | sei(); 44 | 45 | uint8_t b = UDR0; 46 | protocol_receive_byte(b); 47 | UCSR0B |= _BV(RXCIE0); //Re-enable RX interrupts 48 | } 49 | -------------------------------------------------------------------------------- /kicad/rev1/hex.lib: -------------------------------------------------------------------------------- 1 | EESchema-LIBRARY Version 2.3 Date: Wednesday, April 02, 2014 'amt' 11:14:58 am 2 | #encoding utf-8 3 | # 4 | # DPDT 5 | # 6 | DEF DPDT S 0 40 Y Y 1 F N 7 | F0 "S" 0 100 60 H V C CNN 8 | F1 "DPDT" 0 -100 60 H V C CNN 9 | F2 "~" 0 0 60 H V C CNN 10 | F3 "~" 0 0 60 H V C CNN 11 | DRAW 12 | S 150 -250 -150 250 0 1 0 N 13 | X 1 1 -450 200 300 R 50 50 1 1 I 14 | X 2 2 -450 0 300 R 50 50 1 1 I 15 | X 3 3 -450 -200 300 R 50 50 1 1 I 16 | X 4 4 450 200 300 L 50 50 1 1 I 17 | X 5 5 450 0 300 L 50 50 1 1 I 18 | X 6 6 450 -200 300 L 50 50 1 1 I 19 | ENDDRAW 20 | ENDDEF 21 | # 22 | # LEG 23 | # 24 | DEF LEG S 0 40 Y Y 1 F N 25 | F0 "S" 400 -50 60 H V C CNN 26 | F1 "LEG" 400 100 60 H V C CNN 27 | F2 "~" 0 0 60 H V C CNN 28 | F3 "~" 0 0 60 H V C CNN 29 | DRAW 30 | S -350 200 550 -200 0 1 0 N 31 | X COXA 1 -650 100 300 R 50 50 1 1 I 32 | X FEMUR 2 -650 0 300 R 50 50 1 1 I 33 | X TIBIA 3 -650 -100 300 R 50 50 1 1 I 34 | X BAT 4 0 250 50 D 50 50 1 1 I 35 | X BAT 5 100 250 50 D 50 50 1 1 I 36 | X BAT 6 200 250 50 D 50 50 1 1 I 37 | X GND 7 0 -250 50 U 50 50 1 1 I 38 | X GND 8 100 -250 50 U 50 50 1 1 I 39 | X GND 9 200 -250 50 U 50 50 1 1 I 40 | ENDDRAW 41 | ENDDEF 42 | # 43 | # LEG_REVERSE 44 | # 45 | DEF LEG_REVERSE S 0 40 Y Y 1 F N 46 | F0 "S" 400 -50 60 H V C CNN 47 | F1 "LEG_REVERSE" 400 100 60 H V C CNN 48 | F2 "~" 0 0 60 H V C CNN 49 | F3 "~" 0 0 60 H V C CNN 50 | DRAW 51 | S -350 200 550 -200 0 1 0 N 52 | X TIBIA 1 -650 100 300 R 50 50 1 1 I 53 | X FEMUR 2 -650 0 300 R 50 50 1 1 I 54 | X COXA 3 -650 -100 300 R 50 50 1 1 I 55 | X BAT 4 0 250 50 D 50 50 1 1 I 56 | X BAT 5 100 250 50 D 50 50 1 1 I 57 | X BAT 6 200 250 50 D 50 50 1 1 I 58 | X GND 7 0 -250 50 U 50 50 1 1 I 59 | X GND 8 100 -250 50 U 50 50 1 1 I 60 | X GND 9 200 -250 50 U 50 50 1 1 I 61 | ENDDRAW 62 | ENDDEF 63 | # 64 | #End Library 65 | -------------------------------------------------------------------------------- /kicad/rev2/hex.lib: -------------------------------------------------------------------------------- 1 | EESchema-LIBRARY Version 2.3 Date: Wednesday, April 02, 2014 'amt' 11:14:58 am 2 | #encoding utf-8 3 | # 4 | # DPDT 5 | # 6 | DEF DPDT S 0 40 Y Y 1 F N 7 | F0 "S" 0 100 60 H V C CNN 8 | F1 "DPDT" 0 -100 60 H V C CNN 9 | F2 "~" 0 0 60 H V C CNN 10 | F3 "~" 0 0 60 H V C CNN 11 | DRAW 12 | S 150 -250 -150 250 0 1 0 N 13 | X 1 1 -450 200 300 R 50 50 1 1 I 14 | X 2 2 -450 0 300 R 50 50 1 1 I 15 | X 3 3 -450 -200 300 R 50 50 1 1 I 16 | X 4 4 450 200 300 L 50 50 1 1 I 17 | X 5 5 450 0 300 L 50 50 1 1 I 18 | X 6 6 450 -200 300 L 50 50 1 1 I 19 | ENDDRAW 20 | ENDDEF 21 | # 22 | # LEG 23 | # 24 | DEF LEG S 0 40 Y Y 1 F N 25 | F0 "S" 400 -50 60 H V C CNN 26 | F1 "LEG" 400 100 60 H V C CNN 27 | F2 "~" 0 0 60 H V C CNN 28 | F3 "~" 0 0 60 H V C CNN 29 | DRAW 30 | S -350 200 550 -200 0 1 0 N 31 | X COXA 1 -650 100 300 R 50 50 1 1 I 32 | X FEMUR 2 -650 0 300 R 50 50 1 1 I 33 | X TIBIA 3 -650 -100 300 R 50 50 1 1 I 34 | X BAT 4 0 250 50 D 50 50 1 1 I 35 | X BAT 5 100 250 50 D 50 50 1 1 I 36 | X BAT 6 200 250 50 D 50 50 1 1 I 37 | X GND 7 0 -250 50 U 50 50 1 1 I 38 | X GND 8 100 -250 50 U 50 50 1 1 I 39 | X GND 9 200 -250 50 U 50 50 1 1 I 40 | ENDDRAW 41 | ENDDEF 42 | # 43 | # LEG_REVERSE 44 | # 45 | DEF LEG_REVERSE S 0 40 Y Y 1 F N 46 | F0 "S" 400 -50 60 H V C CNN 47 | F1 "LEG_REVERSE" 400 100 60 H V C CNN 48 | F2 "~" 0 0 60 H V C CNN 49 | F3 "~" 0 0 60 H V C CNN 50 | DRAW 51 | S -350 200 550 -200 0 1 0 N 52 | X TIBIA 1 -650 100 300 R 50 50 1 1 I 53 | X FEMUR 2 -650 0 300 R 50 50 1 1 I 54 | X COXA 3 -650 -100 300 R 50 50 1 1 I 55 | X BAT 4 0 250 50 D 50 50 1 1 I 56 | X BAT 5 100 250 50 D 50 50 1 1 I 57 | X BAT 6 200 250 50 D 50 50 1 1 I 58 | X GND 7 0 -250 50 U 50 50 1 1 I 59 | X GND 8 100 -250 50 U 50 50 1 1 I 60 | X GND 9 200 -250 50 U 50 50 1 1 I 61 | ENDDRAW 62 | ENDDEF 63 | # 64 | #End Library 65 | -------------------------------------------------------------------------------- /kicad/rev2.1/stubby.lib: -------------------------------------------------------------------------------- 1 | EESchema-LIBRARY Version 2.3 Date: Wednesday, April 02, 2014 'amt' 11:14:58 am 2 | #encoding utf-8 3 | # 4 | # DPDT 5 | # 6 | DEF DPDT S 0 40 Y Y 1 F N 7 | F0 "S" 0 100 60 H V C CNN 8 | F1 "DPDT" 0 -100 60 H V C CNN 9 | F2 "~" 0 0 60 H V C CNN 10 | F3 "~" 0 0 60 H V C CNN 11 | DRAW 12 | S 150 -250 -150 250 0 1 0 N 13 | X 1 1 -450 200 300 R 50 50 1 1 I 14 | X 2 2 -450 0 300 R 50 50 1 1 I 15 | X 3 3 -450 -200 300 R 50 50 1 1 I 16 | X 4 4 450 200 300 L 50 50 1 1 I 17 | X 5 5 450 0 300 L 50 50 1 1 I 18 | X 6 6 450 -200 300 L 50 50 1 1 I 19 | ENDDRAW 20 | ENDDEF 21 | # 22 | # LEG 23 | # 24 | DEF LEG S 0 40 Y Y 1 F N 25 | F0 "S" 400 -50 60 H V C CNN 26 | F1 "LEG" 400 100 60 H V C CNN 27 | F2 "~" 0 0 60 H V C CNN 28 | F3 "~" 0 0 60 H V C CNN 29 | DRAW 30 | S -350 200 550 -200 0 1 0 N 31 | X COXA 1 -650 100 300 R 50 50 1 1 I 32 | X FEMUR 2 -650 0 300 R 50 50 1 1 I 33 | X TIBIA 3 -650 -100 300 R 50 50 1 1 I 34 | X BAT 4 0 250 50 D 50 50 1 1 I 35 | X BAT 5 100 250 50 D 50 50 1 1 I 36 | X BAT 6 200 250 50 D 50 50 1 1 I 37 | X GND 7 0 -250 50 U 50 50 1 1 I 38 | X GND 8 100 -250 50 U 50 50 1 1 I 39 | X GND 9 200 -250 50 U 50 50 1 1 I 40 | ENDDRAW 41 | ENDDEF 42 | # 43 | # LEG_REVERSE 44 | # 45 | DEF LEG_REVERSE S 0 40 Y Y 1 F N 46 | F0 "S" 400 -50 60 H V C CNN 47 | F1 "LEG_REVERSE" 400 100 60 H V C CNN 48 | F2 "~" 0 0 60 H V C CNN 49 | F3 "~" 0 0 60 H V C CNN 50 | DRAW 51 | S -350 200 550 -200 0 1 0 N 52 | X TIBIA 1 -650 100 300 R 50 50 1 1 I 53 | X FEMUR 2 -650 0 300 R 50 50 1 1 I 54 | X COXA 3 -650 -100 300 R 50 50 1 1 I 55 | X BAT 4 0 250 50 D 50 50 1 1 I 56 | X BAT 5 100 250 50 D 50 50 1 1 I 57 | X BAT 6 200 250 50 D 50 50 1 1 I 58 | X GND 7 0 -250 50 U 50 50 1 1 I 59 | X GND 8 100 -250 50 U 50 50 1 1 I 60 | X GND 9 200 -250 50 U 50 50 1 1 I 61 | ENDDRAW 62 | ENDDEF 63 | # 64 | #End Library 65 | -------------------------------------------------------------------------------- /processing/build.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /avr/hardware/status.c: -------------------------------------------------------------------------------- 1 | #include "status.h" 2 | 3 | //The magic number 78 comes from X/20000 = color/255; X = color * 20000 / 255; X = color * 78. 4 | #define PHASE_MULTIPLIER (uint8_t) (20000/255) 5 | 6 | void status_init(volatile uint8_t *ports[3], uint8_t pins[3]){ 7 | #if PCB_REVISION == 1 8 | ports[LED_RED] = &PORTD; //Red LED 9 | pins[LED_RED] = PORTD4; 10 | ports[LED_GREEN] = &PORTD; //Green LED 11 | pins[LED_GREEN] = PORTD6; 12 | ports[LED_BLUE] = &PORTD; //Blue LED 13 | pins[LED_BLUE] = PORTD5; 14 | #elif PCB_REVISION == 2 15 | ports[LED_RED] = &PORTC; //Red LED 16 | pins[LED_RED] = PORTC5; 17 | ports[LED_GREEN] = &PORTC; //Green LED 18 | pins[LED_GREEN] = PORTC7; 19 | ports[LED_BLUE] = &PORTC; //Blue LED 20 | pins[LED_BLUE] = PORTC6; 21 | #else 22 | #error Unsupported PCB_REVISION value. 23 | #endif 24 | } 25 | 26 | void status_flash(uint8_t red, uint8_t green, uint8_t blue, uint8_t count){ 27 | for (uint8_t i = 0; i < count; i++){ 28 | wdt_reset(); 29 | status_set_color(red, green, blue); 30 | delay_ms(100); 31 | status_set_color(0x00, 0x00, 0x00); 32 | delay_ms(100); 33 | } 34 | } 35 | 36 | void status_set_color(uint8_t red, uint8_t green, uint8_t blue){ 37 | #if PCB_REVISION == 1 38 | pwm_set_phase_batch(LED_RED, ((uint16_t) red) * PHASE_MULTIPLIER); 39 | pwm_set_phase_batch(LED_GREEN, ((uint16_t) green) * PHASE_MULTIPLIER); 40 | pwm_set_phase_batch(LED_BLUE, ((uint16_t) blue) * PHASE_MULTIPLIER); 41 | #elif PCB_REVISION == 2 42 | pwm_set_phase_batch(LED_RED, ((uint16_t) 0xFF - red) * PHASE_MULTIPLIER); 43 | pwm_set_phase_batch(LED_GREEN, ((uint16_t) 0xFF - green) * PHASE_MULTIPLIER); 44 | pwm_set_phase_batch(LED_BLUE, ((uint16_t) 0xFF - blue) * PHASE_MULTIPLIER); 45 | #else 46 | #error Unsupported PCB_REVISION value. 47 | #endif 48 | 49 | pwm_apply_batch(); 50 | } 51 | 52 | -------------------------------------------------------------------------------- /avr/Stubby.h: -------------------------------------------------------------------------------- 1 | #ifndef STUBBY_H 2 | #define STUBBY_H 3 | 4 | #ifndef DEBUG_SIMULATION 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include "util/delays.h" 11 | #include "lib/protocol/protocol.h" 12 | #include "lib/universal_controller/client.h" 13 | #include "lib/serial/serial.h" 14 | #include "lib/pwm/pwm.h" 15 | #else 16 | #include 17 | #include 18 | #include 19 | #endif 20 | 21 | #define LEG_COUNT 6 22 | 23 | #define FRONT_LEFT 0 24 | #define MIDDLE_LEFT 1 25 | #define REAR_LEFT 2 26 | #define REAR_RIGHT 3 27 | #define MIDDLE_RIGHT 4 28 | #define FRONT_RIGHT 5 29 | 30 | #define JOINT_COUNT 3 31 | 32 | #define TIBIA 0 33 | #define FEMUR 1 34 | #define COXA 2 35 | 36 | #define LED_RED ((LEG_COUNT * JOINT_COUNT) + 0) 37 | #define LED_GREEN ((LEG_COUNT * JOINT_COUNT) + 1) 38 | #define LED_BLUE ((LEG_COUNT * JOINT_COUNT) + 2) 39 | 40 | #define PWM_COUNT ((LEG_COUNT * JOINT_COUNT) + 3) 41 | 42 | #define CALIBRATION_COUNT (JOINT_COUNT + 3) 43 | #define CALIBRATION_X (JOINT_COUNT + 0) 44 | #define CALIBRATION_Y (JOINT_COUNT + 1) 45 | #define CALIBRATION_Z (JOINT_COUNT + 2) 46 | 47 | #define MAGNETOMETER_EEPROM_BASE (LEG_COUNT * CALIBRATION_COUNT) 48 | #define MAGNETOMETER_EEPROM_OFFSET 2 49 | 50 | //State variables 51 | #define POWER_OFF 0x00 52 | #define POWER_ON 0x01 53 | 54 | #define CONTROLLER_NONE 0x00 55 | #define CONTROLLER_UC 0x01 56 | #define CONTROLLER_PROCESSING 0x02 57 | #define CONTROLLER_CALIBRATION 0x03 58 | 59 | 60 | void servo_init(); 61 | 62 | uint8_t get_power(); 63 | void set_power(uint8_t power); 64 | 65 | uint8_t get_controller(); 66 | 67 | void doAcknowledgeCommand(uint8_t command); 68 | void doCompleteCommand(uint8_t command); 69 | void doSendDebug(char* message); 70 | void doResetLegs(); 71 | 72 | void mode_select(); 73 | void mode_remote_control(); 74 | void mode_calibration(); 75 | 76 | #endif 77 | -------------------------------------------------------------------------------- /kicad/rev2.1/stubby.pro: -------------------------------------------------------------------------------- 1 | update=Wed 31 Dec 2014 09:15:50 PM MST 2 | version=1 3 | last_client=pcbnew 4 | [cvpcb] 5 | version=1 6 | NetIExt=net 7 | [cvpcb/libraries] 8 | EquName1=devcms 9 | [general] 10 | version=1 11 | [eeschema] 12 | version=1 13 | LibDir=../../../../lib/kicad/library 14 | NetFmtName=PcbnewAdvanced 15 | RptD_X=0 16 | RptD_Y=100 17 | RptLab=1 18 | LabSize=60 19 | [eeschema/libraries] 20 | LibName1=power 21 | LibName2=device 22 | LibName3=transistors 23 | LibName4=conn 24 | LibName5=linear 25 | LibName6=regul 26 | LibName7=74xx 27 | LibName8=cmos4000 28 | LibName9=adc-dac 29 | LibName10=memory 30 | LibName11=xilinx 31 | LibName12=special 32 | LibName13=microcontrollers 33 | LibName14=dsp 34 | LibName15=microchip 35 | LibName16=analog_switches 36 | LibName17=motorola 37 | LibName18=texas 38 | LibName19=intel 39 | LibName20=audio 40 | LibName21=interface 41 | LibName22=digital-audio 42 | LibName23=philips 43 | LibName24=display 44 | LibName25=cypress 45 | LibName26=siliconi 46 | LibName27=opto 47 | LibName28=atmel 48 | LibName29=contrib 49 | LibName30=valves 50 | LibName31=custom 51 | LibName32=stubby 52 | [pcbnew] 53 | version=1 54 | LastNetListRead= 55 | UseCmpFile=1 56 | PadDrill=0.000000000000 57 | PadDrillOvalY=0.000000000000 58 | PadSizeH=1.700000000000 59 | PadSizeV=1.800000000000 60 | PcbTextSizeV=1.500000000000 61 | PcbTextSizeH=1.500000000000 62 | PcbTextThickness=0.300000000000 63 | ModuleTextSizeV=1.000000000000 64 | ModuleTextSizeH=1.000000000000 65 | ModuleTextSizeThickness=0.150000000000 66 | SolderMaskClearance=0.000000000000 67 | SolderMaskMinWidth=0.000000000000 68 | DrawSegmentWidth=0.200000000000 69 | BoardOutlineThickness=0.100000000000 70 | ModuleOutlineThickness=0.150000000000 71 | [pcbnew/libraries] 72 | LibDir=../../../../lib/kicad/modules 73 | LibName1=sockets 74 | LibName2=connect 75 | LibName3=discret 76 | LibName4=pin_array 77 | LibName5=divers 78 | LibName6=smd_capacitors 79 | LibName7=smd_resistors 80 | LibName8=smd_transistors 81 | LibName9=libcms 82 | LibName10=display 83 | LibName11=led 84 | LibName12=dip_sockets 85 | LibName13=pga_sockets 86 | LibName14=valves 87 | LibName15=custom 88 | -------------------------------------------------------------------------------- /avr/lib/protocol/protocol_client.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | # 3 | # Python implementation of the Universal Controller framing protocol. Reads the raw values, 4 | # and interprets them using the specified logic. 5 | # 6 | ################### 7 | 8 | import serial, sys 9 | from time import sleep 10 | 11 | # Argument is serial port 12 | ser = serial.Serial(sys.argv[1], 38400) 13 | 14 | sleep(1); 15 | 16 | START = 0x7e 17 | ESCAPE = 0x7d 18 | MAX_SIZE = 40 19 | 20 | err = False 21 | esc = False 22 | pos = 0 23 | length = 0 24 | cmd = 0 25 | chk = 0x00 26 | 27 | buf = {} 28 | 29 | while True: 30 | b = ord(ser.read()) 31 | 32 | if (err and b == START): 33 | # recover from error condition 34 | sys.stdout.write("Recover from error condition\n") 35 | err = False 36 | pos = 0 37 | elif (err): 38 | continue 39 | 40 | if (pos > 0 and b == START): 41 | # unexpected start of frame 42 | sys.stdout.write("Unexpected start of frame\n") 43 | err = True 44 | continue 45 | 46 | if (pos > 0 and b == ESCAPE): 47 | # unescape next byte 48 | esc = True 49 | continue 50 | 51 | if (esc): 52 | # unescape current byte 53 | b = 0x20 ^ b 54 | esc = False 55 | 56 | if (pos > 1): 57 | chk = (chk + b) & 0xFF 58 | 59 | if (pos == 0): 60 | pos = pos + 1 61 | sys.stdout.write("Start of Frame: 0x7e\n"); 62 | continue 63 | elif (pos == 1): 64 | length = b 65 | sys.stdout.write("Length: " + hex(b) + "\n"); 66 | pos = pos + 1 67 | continue 68 | elif (pos == 2): 69 | cmd = b 70 | sys.stdout.write("Command: " + hex(b) + "\n"); 71 | pos = pos + 1 72 | continue 73 | else: 74 | if (pos > MAX_SIZE): 75 | # this probably can't happen 76 | sys.stdout.write("Position > MAX_SIZE\n") 77 | continue 78 | 79 | if (pos == (length + 2)): 80 | if (chk == 0xff): 81 | #Finished the message; decode it 82 | sys.stdout.write("Finished Command " + hex(cmd) + "\n") 83 | 84 | else: 85 | err = True; 86 | sys.stdout.write("Invalid checksum\n") 87 | pos = 0; 88 | chk = 0; 89 | else: 90 | buf[pos - 3] = b 91 | sys.stdout.write("Data: " + hex(b) + "\n"); 92 | pos = pos + 1 93 | 94 | -------------------------------------------------------------------------------- /avr/lib/universal_controller/protocol.txt: -------------------------------------------------------------------------------- 1 | Protocol Info 2 | ------------- 3 | 4 | Message Structure (bi-directional): 5 | Segment Length 6 | --------------------------------- 7 | Frame start 1 byte (0x7e) 8 | Length 1 byte 9 | Command 1 byte 10 | Payload 0 - 254 bytes 11 | Checksum 1 byte 12 | 13 | Commands: 14 | --------- 15 | 16 | Command Direction Payload Response 17 | ------------------------------------------------------------------------------------------------------------------------------------------------------------- 18 | -- General commands 19 | SendAcknowledge Device -> Control Command being ack'd (1 byte) None 20 | SendComplete Device -> Control Command completed (1 byte) None 21 | 22 | RequestControlConfig Control -> Device Controller ID; UC = 0x55 ('U') ControlConfig message 23 | SendControlConfig Device -> Control Configuration for controller None 24 | 25 | RequestEnableDebug Control -> Device Null Ack 26 | RequestDisableDebug Control -> Device Null Ack 27 | SendDebug Device -> Control Variable depending on message, max 254 bytes None 28 | 29 | RequestBattery Control -> Device Null Battery message 30 | SendBattery Device -> Control 1 byte None 31 | 32 | -- Universal Controller commands 33 | UCButtonPush Control -> Device Index of button which was just pressed (1 byte) None 34 | UCButtonRelease Control -> Device Index of button which was just released (1 byte) None 35 | UCJoystickMove Control -> Device Value of all joystick axis (4 bytes, order LX:LY:RX:RY) None 36 | 37 | UCButtonEnable Device -> Control Index of button to enable (1 - 16) (1 byte) None 38 | UCButtonDisable Device -> Control Index of button to disable (1 - 16) (1 byte) None 39 | UCJoystickEnable Device -> Control Null None 40 | UCJoystickDisable Device -> Control Null None 41 | UCSetPollFrequency Device -> Control 8 bit unsigned value (will be shifted 6 bits left) (1 byte) None 42 | UCSetAnalogFrequency Device -> Control 8 bit compare value (1 byte) None 43 | -------------------------------------------------------------------------------- /kicad/rev1/hex.lst: -------------------------------------------------------------------------------- 1 | eeschema (2014-01-10 BZR 4027)-stable >> Creation date: Wednesday, April 16, 2014 'amt' 10:40:17 am 2 | #Cmp ( order = Reference ) 3 | | C1 1uF 4 | | C2 1uF 5 | | C3 1uF 6 | | C4 20pF 7 | | C5 20pF 8 | | D1 LED 9 | | IC1 ATMEGA1284P-P 10 | | K1 XTAL 11 | | K2 REGUL 12 | | L1 RGB LED 13 | | P1 PROG 14 | | P2 BATT (4.8V-6V) 15 | | P3 CONN_8 16 | | R1 1k 17 | | R2 1k 18 | | R3 1k 19 | | R4 1k 20 | | R5 1k 21 | | R6 1k 22 | | R7 1k 23 | | R8 1k 24 | | R9 1k 25 | | R10 1k 26 | | S1 FRNT LFT 27 | | S2 FRNT RGT 28 | | S3 MID LFT 29 | | S4 MID RGT 30 | | S5 REAR LFT 31 | | S6 REAR RGT 32 | | SW1 DPDT 33 | | X1 XBee (Sparkfun Breakout) 34 | | ZD1 ZD 3v3 35 | #End Cmp 36 | 37 | #Cmp ( order = Value ) 38 | | 20pF C4 39 | | 20pF C5 40 | | 1uF C1 41 | | 1uF C2 42 | | 1uF C3 43 | | LED D1 44 | | ATMEGA1284P-P IC1 45 | | XTAL K1 46 | | REGUL K2 47 | | RGB LED L1 48 | | PROG P1 49 | | BATT (4.8V-6V) P2 50 | | CONN_8 P3 51 | | 1k R1 52 | | 1k R2 53 | | 1k R3 54 | | 1k R4 55 | | 1k R5 56 | | 1k R6 57 | | 1k R7 58 | | 1k R8 59 | | 1k R9 60 | | 1k R10 61 | | FRNT LFT S1 62 | | FRNT RGT S2 63 | | MID LFT S3 64 | | MID RGT S4 65 | | REAR LFT S5 66 | | REAR RGT S6 67 | | DPDT SW1 68 | | XBee (Sparkfun Breakout) X1 69 | | ZD 3v3 ZD1 70 | #End Cmp 71 | 72 | #End List 73 | -------------------------------------------------------------------------------- /kicad/rev1/hex.pro: -------------------------------------------------------------------------------- 1 | update=Wednesday, May 21, 2014 'amt' 10:48:47 am 2 | version=1 3 | last_client=eeschema 4 | [cvpcb] 5 | version=1 6 | NetIExt=net 7 | [cvpcb/libraries] 8 | EquName1=devcms 9 | [general] 10 | version=1 11 | [pcbnew] 12 | version=1 13 | LastNetListRead= 14 | UseCmpFile=1 15 | PadDrill=0.000000000000 16 | PadDrillOvalY=0.000000000000 17 | PadSizeH=0.889000000000 18 | PadSizeV=1.200000000000 19 | PcbTextSizeV=1.500000000000 20 | PcbTextSizeH=1.500000000000 21 | PcbTextThickness=0.300000000000 22 | ModuleTextSizeV=1.000000000000 23 | ModuleTextSizeH=1.000000000000 24 | ModuleTextSizeThickness=0.150000000000 25 | SolderMaskClearance=0.000000000000 26 | SolderMaskMinWidth=0.000000000000 27 | DrawSegmentWidth=0.200000000000 28 | BoardOutlineThickness=0.100000000000 29 | ModuleOutlineThickness=0.150000000000 30 | [pcbnew/libraries] 31 | LibDir=../../../lib/kicad/modules 32 | LibName1=sockets 33 | LibName2=connect 34 | LibName3=discret 35 | LibName4=pin_array 36 | LibName5=divers 37 | LibName6=smd_capacitors 38 | LibName7=smd_resistors 39 | LibName8=smd_crystal&oscillator 40 | LibName9=smd_dil 41 | LibName10=smd_transistors 42 | LibName11=libcms 43 | LibName12=display 44 | LibName13=led 45 | LibName14=dip_sockets 46 | LibName15=pga_sockets 47 | LibName16=valves 48 | LibName17=hex 49 | LibName18=custom 50 | [eeschema] 51 | version=1 52 | LibDir=../../../lib/kicad/library 53 | NetFmtName=PcbnewAdvanced 54 | RptD_X=0 55 | RptD_Y=100 56 | RptLab=1 57 | LabSize=60 58 | [eeschema/libraries] 59 | LibName1=power 60 | LibName2=device 61 | LibName3=transistors 62 | LibName4=conn 63 | LibName5=linear 64 | LibName6=regul 65 | LibName7=74xx 66 | LibName8=cmos4000 67 | LibName9=adc-dac 68 | LibName10=memory 69 | LibName11=xilinx 70 | LibName12=special 71 | LibName13=microcontrollers 72 | LibName14=dsp 73 | LibName15=microchip 74 | LibName16=analog_switches 75 | LibName17=motorola 76 | LibName18=texas 77 | LibName19=intel 78 | LibName20=audio 79 | LibName21=interface 80 | LibName22=digital-audio 81 | LibName23=philips 82 | LibName24=display 83 | LibName25=cypress 84 | LibName26=siliconi 85 | LibName27=opto 86 | LibName28=atmel 87 | LibName29=contrib 88 | LibName30=valves 89 | LibName31=hex 90 | LibName32=custom 91 | -------------------------------------------------------------------------------- /kicad/rev2/hex.pro: -------------------------------------------------------------------------------- 1 | update=Friday, July 25, 2014 'amt' 12:11:11 am 2 | version=1 3 | last_client=kicad 4 | [cvpcb] 5 | version=1 6 | NetIExt=net 7 | [cvpcb/libraries] 8 | EquName1=devcms 9 | [pcbnew] 10 | version=1 11 | LastNetListRead= 12 | UseCmpFile=1 13 | PadDrill=1.016000000000 14 | PadDrillOvalY=1.016000000000 15 | PadSizeH=1.524000000000 16 | PadSizeV=1.524000000000 17 | PcbTextSizeV=1.500000000000 18 | PcbTextSizeH=1.500000000000 19 | PcbTextThickness=0.300000000000 20 | ModuleTextSizeV=1.000000000000 21 | ModuleTextSizeH=1.000000000000 22 | ModuleTextSizeThickness=0.150000000000 23 | SolderMaskClearance=0.000000000000 24 | SolderMaskMinWidth=0.000000000000 25 | DrawSegmentWidth=0.200000000000 26 | BoardOutlineThickness=0.100000000000 27 | ModuleOutlineThickness=0.150000000000 28 | [pcbnew/libraries] 29 | LibDir=../../../../lib/kicad/modules 30 | LibName1=sockets 31 | LibName2=connect 32 | LibName3=discret 33 | LibName4=pin_array 34 | LibName5=divers 35 | LibName6=smd_capacitors 36 | LibName7=smd_resistors 37 | LibName8=smd_crystal&oscillator 38 | LibName9=smd_dil 39 | LibName10=smd_transistors 40 | LibName11=libcms 41 | LibName12=display 42 | LibName13=led 43 | LibName14=dip_sockets 44 | LibName15=pga_sockets 45 | LibName16=valves 46 | LibName17=hex 47 | LibName18=custom 48 | [eeschema] 49 | version=1 50 | LibDir=../../../../lib/kicad/library 51 | NetFmtName=PcbnewAdvanced 52 | RptD_X=0 53 | RptD_Y=100 54 | RptLab=1 55 | LabSize=60 56 | [eeschema/libraries] 57 | LibName1=power 58 | LibName2=device 59 | LibName3=transistors 60 | LibName4=conn 61 | LibName5=linear 62 | LibName6=regul 63 | LibName7=74xx 64 | LibName8=cmos4000 65 | LibName9=adc-dac 66 | LibName10=memory 67 | LibName11=xilinx 68 | LibName12=special 69 | LibName13=microcontrollers 70 | LibName14=dsp 71 | LibName15=microchip 72 | LibName16=analog_switches 73 | LibName17=motorola 74 | LibName18=texas 75 | LibName19=intel 76 | LibName20=audio 77 | LibName21=interface 78 | LibName22=digital-audio 79 | LibName23=philips 80 | LibName24=display 81 | LibName25=cypress 82 | LibName26=siliconi 83 | LibName27=opto 84 | LibName28=atmel 85 | LibName29=contrib 86 | LibName30=valves 87 | LibName31=hex 88 | LibName32=custom 89 | [general] 90 | version=1 91 | -------------------------------------------------------------------------------- /avr/hardware/distance_HCSR04.c: -------------------------------------------------------------------------------- 1 | #include "distance.h" 2 | 3 | volatile uint8_t interval_do_distance_reading = 0x00; //Mailbox value, set in timer2 ISR and read in delay 4 | 5 | #if DISTANCE_SENSOR == 1 6 | 7 | #define TEN_MICROSECONDS (F_CPU / 100000) 8 | 9 | static volatile int16_t distance; 10 | 11 | static volatile uint16_t micros = 0; //Microsecond counter 12 | static volatile uint8_t mutex = 0x00; //Only start a new reading if we are not already in one. 13 | 14 | void distance_init(){ 15 | DDRA |= _BV(PORTA0); 16 | } 17 | 18 | void distance_measure(){ 19 | if (mutex) return; 20 | mutex = 0x01; 21 | 22 | //Enable pin change interrupt 1 (start and stop timer as the pin changes state) 23 | PCICR |= _BV(PCIE0); 24 | PCMSK0 |= _BV(PCINT1); 25 | 26 | //Set up output compare register to fire every 10 microseconds 27 | OCR0A = TEN_MICROSECONDS; 28 | 29 | //Bring trigger high for more than 10uS. 20uS seems to work fine. 30 | PORTA |= _BV(PORTA0); 31 | _delay_us(20); 32 | PORTA &= ~_BV(PORTA0); 33 | 34 | //That's it! The pin change ISR will then handle starting the timer, stopping the timer, etc. 35 | } 36 | 37 | uint16_t distance_read(){ 38 | return distance; 39 | } 40 | 41 | ISR(TIMER0_COMPA_vect){ 42 | sei(); 43 | TCNT0 = 0x00; 44 | micros += 10; 45 | } 46 | 47 | void doSendDebug(char* message, uint8_t length); 48 | 49 | ISR(PCINT0_vect){ 50 | sei(); 51 | 52 | //Rising value; we are starting a distance sample. 53 | if (PINA & _BV(PORTA1)){ 54 | //Set up timer0 to run at F_CPU (no prescaler), in normal mode 55 | TCNT0 = 0; 56 | TCCR0B = _BV(CS00); 57 | TCCR0A = 0x0; 58 | TIMSK0 |= _BV(OCIE0A); 59 | 60 | micros = 0; 61 | } 62 | //Falling value; we are finishing a distance sample. 63 | else { 64 | //Get any partial time remaining 65 | micros += ((TCNT0 * (uint16_t) 10) / TEN_MICROSECONDS); 66 | 67 | //Turn off the timer 68 | TCCR0B = 0x00; 69 | 70 | //Disable pin change interrupts 71 | PCICR &= ~_BV(PCIE0); 72 | PCMSK0 &= ~_BV(PCINT1); 73 | 74 | //Calculate distance; 5.8 is a magic number calculated from the speed of sound and found in the datasheet. 75 | distance = (uint16_t) (micros / 5.8); 76 | 77 | mutex = 0x00; 78 | } 79 | } 80 | 81 | #else 82 | 83 | void distance_init(){}; 84 | 85 | void distance_measure(){}; 86 | 87 | uint16_t distance_read(){ return 0xFFFF; } 88 | 89 | #endif -------------------------------------------------------------------------------- /avr/util/delays.c: -------------------------------------------------------------------------------- 1 | #include "delays.h" 2 | 3 | extern volatile uint8_t interval_do_magnetometer_reading; //Defined in magnetometer_HMC5883L.c 4 | extern volatile uint8_t interval_do_distance_reading; //Defined in distance_HCSR04.c 5 | 6 | void magnetometer_take_reading(); 7 | void distance_measure(); 8 | 9 | //Defined in hardware/battery.c 10 | void battery_set_level(); 11 | extern volatile uint8_t battery_level; 12 | extern uint8_t battery_status_enabled; 13 | 14 | //Defined in Stubby.cpp 15 | extern uint8_t pending_acknowledge; 16 | extern uint8_t pending_complete; 17 | 18 | void delay_ms(uint16_t ms){ 19 | double _ms = (double) ms; 20 | //Check various mailbox flags for stuff to do, and decrease ms accordingly 21 | #if MAGNETOMETER == 1 22 | if (interval_do_magnetometer_reading){ 23 | magnetometer_take_reading(); 24 | #if F_CPU == 12000000 25 | _ms -= 0.33; //Determined from logic probes at 12MHz 26 | #elif F_CPU == 18432000 27 | _ms -= 0.22; //Approximation based on 12MHz one 28 | #elif F_CPU == 20000000 29 | _ms -= 0.2; //Approximation based on 12MHz one 30 | #else 31 | #error Unknown CPU speed; please update delay_ms 32 | #endif 33 | interval_do_magnetometer_reading = 0x00; 34 | } 35 | #endif 36 | 37 | if (battery_level){ 38 | battery_set_level(); 39 | //We only adjust ms if we are updating the status lights... otherwise, the extra delay is negligable 40 | if (battery_status_enabled){ 41 | #if F_CPU == 12000000 42 | _ms -= 0.22; //Determined from logic probes at 12MHz 43 | #elif F_CPU == 18432000 44 | _ms -= 0.15; //Approximation based on 12MHz one 45 | #elif F_CPU == 20000000 46 | _ms -= 0.13; //Approximation based on 12MHz one 47 | #else 48 | #error Unknown CPU speed; please update delay_ms 49 | #endif 50 | } 51 | } 52 | 53 | #if DISTANCE_SENSOR == 1 54 | if (interval_do_distance_reading){ 55 | distance_measure(); 56 | _ms -= 0.33; //TODO Figure out average time to read distance sensor 57 | interval_do_distance_reading = 0x00; 58 | } 59 | #endif 60 | 61 | 62 | if (pending_acknowledge){ 63 | uint8_t data[1]; 64 | data[0] = pending_acknowledge; 65 | protocol_send_message(MESSAGE_SEND_ACKNOWLEDGE, data, 1); 66 | pending_acknowledge = 0x00; 67 | } 68 | 69 | if (pending_complete){ 70 | uint8_t data[1]; 71 | data[0] = pending_complete; 72 | protocol_send_message(MESSAGE_SEND_COMPLETE, data, 1); 73 | pending_complete = 0x00; 74 | } 75 | 76 | _delay_ms(_ms); 77 | } -------------------------------------------------------------------------------- /kicad/rev1/custom.dcm: -------------------------------------------------------------------------------- 1 | <<<<<<< HEAD 2 | EESchema-DOCLIB Version 2.0 Date: Tuesday, January 06, 2015 'pmt' 03:53:55 pm 3 | ======= 4 | EESchema-DOCLIB Version 2.0 Date: Monday, January 05, 2015 'pmt' 03:52:05 pm 5 | >>>>>>> 3f0499d570f7b64b3b8d98e1d412284ea0223885 6 | # 7 | $CMP 74HC4067 8 | D 16 Channel Analog Multiplexer / Demultiplexer 9 | K multiplexer, demultiplexer, analog 10 | $ENDCMP 11 | # 12 | $CMP 74HC4852 13 | D 2x4 Channel Analog Multiplexer / Demultiplexer 14 | K multiplexer, demultiplexer 15 | $ENDCMP 16 | # 17 | $CMP CRYSTAL_RESONATOR 18 | D Either a Crystal with external caps, or a resonator with built in caps 19 | K CRYSTAL RESONATOR 20 | $ENDCMP 21 | # 22 | $CMP DPDT 23 | D DPDT Switch (e.g. GW22LCP) 24 | K GW22LCP, DPDT 25 | $ENDCMP 26 | # 27 | $CMP ISP 28 | D ISP Header 29 | K ISP HEADER 30 | $ENDCMP 31 | # 32 | $CMP L293D 33 | D L293D Quad Half Bridge 34 | K MOTOR DRIVER HALF BRIDGE 35 | $ENDCMP 36 | # 37 | $CMP LE-MM103 38 | D 8x8 LED Matrix Bi-Color Red Green 39 | $ENDCMP 40 | # 41 | $CMP LSMALL 42 | D Inductor 43 | K inductor 44 | $ENDCMP 45 | # 46 | $CMP MC34063A 47 | D Peak Boost Buck Inverting Switching Regulator 48 | K Buck, Power 49 | $ENDCMP 50 | # 51 | $CMP MCP4451 52 | D Digital Potentiometer 53 | K potentiometer 54 | $ENDCMP 55 | # 56 | $CMP MOSFET-SOT-23-3 57 | D N-Channel MOSFET, SOT-23 58 | K N-Channel MOSFET Logic Level 59 | $ENDCMP 60 | # 61 | $CMP NCP1400A 62 | D NCP1400A_Boost_Regulator 63 | $ENDCMP 64 | # 65 | $CMP OPAMP 66 | D Generic OP-AMP 67 | K opamp, amp, operational, amplifier 68 | $ENDCMP 69 | # 70 | $CMP R_PACK5 71 | D 5 resistors Pack 72 | K R DEV 73 | $ENDCMP 74 | # 75 | $CMP RESISTOR 76 | D Resistance 77 | K R DEV 78 | $ENDCMP 79 | # 80 | $CMP RESONATOR 81 | D Either a Crystal with external caps, or a resonator with built in caps 82 | K CRYSTAL RESONATOR 83 | $ENDCMP 84 | # 85 | $CMP RGB_LED_COMMON_ANODE 86 | D RGB LED (Common Anode) 87 | K RGB_LED_COMMON_ANODE 88 | $ENDCMP 89 | # 90 | $CMP RGB_LED_COMMON_ANODE_ALT1 91 | D RGB LED (Common Anode) 92 | K RGB_LED_COMMON_ANODE 93 | $ENDCMP 94 | # 95 | $CMP RGB_LED_COMMON_CATHODE 96 | D RGB LED (Common Cathode) 97 | K RGB_LED_COMMON_CATHODE 98 | $ENDCMP 99 | # 100 | $CMP RSMALL 101 | D RSMALL 102 | K R DEV 103 | $ENDCMP 104 | # 105 | $CMP STPIC6D595 106 | D Shift Register / Open Drain 107 | K shift 108 | $ENDCMP 109 | # 110 | $CMP TB6552FNG 111 | D Motor Controller, Full H-Bridge x2 112 | K Motor, Bridge, H-Bridge 113 | $ENDCMP 114 | # 115 | #End Doc Library 116 | -------------------------------------------------------------------------------- /kicad/rev2.1/custom.dcm: -------------------------------------------------------------------------------- 1 | <<<<<<< HEAD 2 | EESchema-DOCLIB Version 2.0 Date: Tuesday, January 06, 2015 'pmt' 03:53:55 pm 3 | ======= 4 | EESchema-DOCLIB Version 2.0 Date: Monday, January 05, 2015 'pmt' 03:52:05 pm 5 | >>>>>>> 3f0499d570f7b64b3b8d98e1d412284ea0223885 6 | # 7 | $CMP 74HC4067 8 | D 16 Channel Analog Multiplexer / Demultiplexer 9 | K multiplexer, demultiplexer, analog 10 | $ENDCMP 11 | # 12 | $CMP 74HC4852 13 | D 2x4 Channel Analog Multiplexer / Demultiplexer 14 | K multiplexer, demultiplexer 15 | $ENDCMP 16 | # 17 | $CMP CRYSTAL_RESONATOR 18 | D Either a Crystal with external caps, or a resonator with built in caps 19 | K CRYSTAL RESONATOR 20 | $ENDCMP 21 | # 22 | $CMP DPDT 23 | D DPDT Switch (e.g. GW22LCP) 24 | K GW22LCP, DPDT 25 | $ENDCMP 26 | # 27 | $CMP ISP 28 | D ISP Header 29 | K ISP HEADER 30 | $ENDCMP 31 | # 32 | $CMP L293D 33 | D L293D Quad Half Bridge 34 | K MOTOR DRIVER HALF BRIDGE 35 | $ENDCMP 36 | # 37 | $CMP LE-MM103 38 | D 8x8 LED Matrix Bi-Color Red Green 39 | $ENDCMP 40 | # 41 | $CMP LSMALL 42 | D Inductor 43 | K inductor 44 | $ENDCMP 45 | # 46 | $CMP MC34063A 47 | D Peak Boost Buck Inverting Switching Regulator 48 | K Buck, Power 49 | $ENDCMP 50 | # 51 | $CMP MCP4451 52 | D Digital Potentiometer 53 | K potentiometer 54 | $ENDCMP 55 | # 56 | $CMP MOSFET-SOT-23-3 57 | D N-Channel MOSFET, SOT-23 58 | K N-Channel MOSFET Logic Level 59 | $ENDCMP 60 | # 61 | $CMP NCP1400A 62 | D NCP1400A_Boost_Regulator 63 | $ENDCMP 64 | # 65 | $CMP OPAMP 66 | D Generic OP-AMP 67 | K opamp, amp, operational, amplifier 68 | $ENDCMP 69 | # 70 | $CMP R_PACK5 71 | D 5 resistors Pack 72 | K R DEV 73 | $ENDCMP 74 | # 75 | $CMP RESISTOR 76 | D Resistance 77 | K R DEV 78 | $ENDCMP 79 | # 80 | $CMP RESONATOR 81 | D Either a Crystal with external caps, or a resonator with built in caps 82 | K CRYSTAL RESONATOR 83 | $ENDCMP 84 | # 85 | $CMP RGB_LED_COMMON_ANODE 86 | D RGB LED (Common Anode) 87 | K RGB_LED_COMMON_ANODE 88 | $ENDCMP 89 | # 90 | $CMP RGB_LED_COMMON_ANODE_ALT1 91 | D RGB LED (Common Anode) 92 | K RGB_LED_COMMON_ANODE 93 | $ENDCMP 94 | # 95 | $CMP RGB_LED_COMMON_CATHODE 96 | D RGB LED (Common Cathode) 97 | K RGB_LED_COMMON_CATHODE 98 | $ENDCMP 99 | # 100 | $CMP RSMALL 101 | D RSMALL 102 | K R DEV 103 | $ENDCMP 104 | # 105 | $CMP STPIC6D595 106 | D Shift Register / Open Drain 107 | K shift 108 | $ENDCMP 109 | # 110 | $CMP TB6552FNG 111 | D Motor Controller, Full H-Bridge x2 112 | K Motor, Bridge, H-Bridge 113 | $ENDCMP 114 | # 115 | #End Doc Library 116 | -------------------------------------------------------------------------------- /kicad/rev2/custom.dcm: -------------------------------------------------------------------------------- 1 | <<<<<<< HEAD 2 | EESchema-DOCLIB Version 2.0 Date: Tuesday, January 06, 2015 'pmt' 03:53:55 pm 3 | ======= 4 | EESchema-DOCLIB Version 2.0 Date: Monday, January 05, 2015 'pmt' 03:52:05 pm 5 | >>>>>>> 3f0499d570f7b64b3b8d98e1d412284ea0223885 6 | # 7 | $CMP 74HC4067 8 | D 16 Channel Analog Multiplexer / Demultiplexer 9 | K multiplexer, demultiplexer, analog 10 | $ENDCMP 11 | # 12 | $CMP 74HC4852 13 | D 2x4 Channel Analog Multiplexer / Demultiplexer 14 | K multiplexer, demultiplexer 15 | $ENDCMP 16 | # 17 | $CMP CRYSTAL_RESONATOR 18 | D Either a Crystal with external caps, or a resonator with built in caps 19 | K CRYSTAL RESONATOR 20 | $ENDCMP 21 | # 22 | $CMP DPDT 23 | D DPDT Switch (e.g. GW22LCP) 24 | K GW22LCP, DPDT 25 | $ENDCMP 26 | # 27 | $CMP ISP 28 | D ISP Header 29 | K ISP HEADER 30 | $ENDCMP 31 | # 32 | $CMP L293D 33 | D L293D Quad Half Bridge 34 | K MOTOR DRIVER HALF BRIDGE 35 | $ENDCMP 36 | # 37 | $CMP LE-MM103 38 | D 8x8 LED Matrix Bi-Color Red Green 39 | $ENDCMP 40 | # 41 | $CMP LSMALL 42 | D Inductor 43 | K inductor 44 | $ENDCMP 45 | # 46 | $CMP MC34063A 47 | D Peak Boost Buck Inverting Switching Regulator 48 | K Buck, Power 49 | $ENDCMP 50 | # 51 | $CMP MCP4451 52 | D Digital Potentiometer 53 | K potentiometer 54 | $ENDCMP 55 | # 56 | $CMP MOSFET-SOT-23-3 57 | D N-Channel MOSFET, SOT-23 58 | K N-Channel MOSFET Logic Level 59 | $ENDCMP 60 | # 61 | $CMP NCP1400A 62 | D NCP1400A_Boost_Regulator 63 | $ENDCMP 64 | # 65 | $CMP OPAMP 66 | D Generic OP-AMP 67 | K opamp, amp, operational, amplifier 68 | $ENDCMP 69 | # 70 | $CMP R_PACK5 71 | D 5 resistors Pack 72 | K R DEV 73 | $ENDCMP 74 | # 75 | $CMP RESISTOR 76 | D Resistance 77 | K R DEV 78 | $ENDCMP 79 | # 80 | $CMP RESONATOR 81 | D Either a Crystal with external caps, or a resonator with built in caps 82 | K CRYSTAL RESONATOR 83 | $ENDCMP 84 | # 85 | $CMP RGB_LED_COMMON_ANODE 86 | D RGB LED (Common Anode) 87 | K RGB_LED_COMMON_ANODE 88 | $ENDCMP 89 | # 90 | $CMP RGB_LED_COMMON_ANODE_ALT1 91 | D RGB LED (Common Anode) 92 | K RGB_LED_COMMON_ANODE 93 | $ENDCMP 94 | # 95 | $CMP RGB_LED_COMMON_CATHODE 96 | D RGB LED (Common Cathode) 97 | K RGB_LED_COMMON_CATHODE 98 | $ENDCMP 99 | # 100 | $CMP RSMALL 101 | D RSMALL 102 | K R DEV 103 | $ENDCMP 104 | # 105 | $CMP STPIC6D595 106 | D Shift Register / Open Drain 107 | K shift 108 | $ENDCMP 109 | # 110 | $CMP TB6552FNG 111 | D Motor Controller, Full H-Bridge x2 112 | K Motor, Bridge, H-Bridge 113 | $ENDCMP 114 | # 115 | #End Doc Library 116 | -------------------------------------------------------------------------------- /avr/lib/protocol/protocol.h: -------------------------------------------------------------------------------- 1 | #ifndef PROTOCOL_H 2 | #define PROTOCOL_H 3 | 4 | #include 5 | 6 | #define PROTOCOL_ERROR_UNEXPECTED_START_OF_FRAME 1 7 | #define PROTOCOL_ERROR_INVALID_CHECKSUM 2 8 | #define PROTOCOL_ERROR_INVALID_LENGTH 3 9 | 10 | 11 | 12 | 13 | /* 14 | * This library allows for bi-directional, multi byte commands to be sent between two devices. It is designed to be 15 | * link agnostic; thus, there are a few functions which you need to implement to link this library into your communications. 16 | * Be sure to read all the function comments in this file to see what functions must be implemented, and which ones 17 | * must be called from the main code. 18 | */ 19 | 20 | /* 21 | * This function MUST be implemented by the calling code. This function will be called from protocol_receive_byte when 22 | * the final byte of a given message has arrived and the checksum is validated. 23 | * 24 | * A possible implementation would be to read the incoming message and put it in a mailbox for later use; another possible 25 | * implementation would be to immediately consume the message and act on it. This function should be as quick as possible 26 | * to ensure that a new incoming message does not clobber the existing message before it has been consumed. If you have 27 | * flow control, it MAY be good to disable other incoming messages until this message has been handled. 28 | */ 29 | void protocol_dispatch_message(uint8_t cmd, uint8_t *bytes, uint8_t length); 30 | 31 | /* 32 | * This function MUST be implemented by the calling code. This function's implementation would pass the byte to whatever 33 | * link layer is in use. 34 | * 35 | * For a default implementation using the serial library, you can use file protocol_serial.c (instead of using serial_*_rx.c) 36 | */ 37 | void protocol_write(uint8_t byte); 38 | 39 | /* 40 | * Receive a single byte from the link layer. This function MUST be called from the main code whenever a byte is received 41 | * from the link layer. 42 | * 43 | * For a default implemenation using the serial library, you can use file protocol_serial.c (instead of using serial_*_rx.c) 44 | */ 45 | void protocol_receive_byte(uint8_t b); 46 | 47 | /* 48 | * Send a message, properly escaping it, using the function protocol_write() to send the individual bytes. 49 | */ 50 | void protocol_send_message(uint8_t cmd, uint8_t *bytes, uint8_t length); 51 | 52 | /* 53 | * Gets the latest error status code. 0 means no error, non-zero is error. 54 | */ 55 | uint8_t protocol_get_error(); 56 | 57 | #endif 58 | -------------------------------------------------------------------------------- /simulation/inverse_kinematics_simulator/inverse_kinematics_simulator.pde: -------------------------------------------------------------------------------- 1 | void setup(){ 2 | size(200, 200); 3 | } 4 | 5 | void draw(){ 6 | float scale = 2; 7 | 8 | fill(255); 9 | rect(0, 0, width, height); 10 | /* 11 | 12 | | 13 | B o 14 | | \ 15 | | \ a 16 | | \ 17 | h| o 18 | | / b 19 | |--o 20 | z| | 21 | |__|______ 22 | x 23 | 24 | */ 25 | 26 | //Height of coxa / femur joint 27 | float h = 68 * scale; 28 | 29 | //Desired co-ordinates of foot 30 | float x = mouseX; 31 | float z = height - mouseY; 32 | 33 | //a is the femur; b is the tibia, c is the distance from coxa / femur joint to end of tibia. 34 | float a, b, c; 35 | //The angles opposite their lower case counterparts 36 | float A, B, C; 37 | //The co-ordinates in x,z space for points A, B, and C 38 | float Ax,Az,Bx,Bz,Cx,Cz; 39 | 40 | //Bx,Bz is the location on X,Z plane where the leg originates 41 | Bx = 0; 42 | Bz = h; 43 | 44 | //We know a and b by measurement (length of leg segments) 45 | a = 20 * scale; 46 | b = 57 * scale; 47 | //We can find c using pythagorean theorem with (h-z), x 48 | c = sqrt((h-z)*(h-z) + x*x); 49 | 50 | //Use low of cosines to find A, B, and C 51 | A = acos((b*b + c*c - a*a) / (2 * b * c)); 52 | B = acos((a*a + c*c - b*b) / (2 * a * c)); 53 | C = acos((a*a + b*b - c*c) / (2 * a * b)); 54 | 55 | //Find X using arc cosine: cos(X) = z/a; 56 | float X = acos((h-z)/c); 57 | 58 | //To find Cx,Cz we first find the angle (Temp1) between vertical and side 'a' 59 | float Temp1 = PI - B - X; 60 | Cx = sin(Temp1) * a; 61 | //Use pythagorean theorem to find rise of Cz over Bz. Subtract this value from 62 | // Bz to find absolute co-ordinates 63 | Cz = Temp1 > PI / 2 ? (Bz - sqrt(a*a - Cx*Cx)) : (Bz + sqrt(a*a - Cx*Cx)); 64 | 65 | //Now we need to find Ax,Az. 66 | Ax = x; 67 | Az = z; 68 | //TODO 69 | 70 | println(); 71 | println("a=" + a + ", b=" + b + ", c=" + c); 72 | println("A=" + degrees(A) + ", B=" + degrees(B + X) + ", C=" + degrees(C)); 73 | println("x=" + x + ", z=" + z); 74 | println("X=" + degrees(X)); 75 | println("Ax=" + Ax + ", Az=" + Az + ", Cx=" + Cx + ", Cz=" + Cz); 76 | 77 | //Print everything to the screen 78 | fill(0); 79 | stroke(255, 0, 0, 100); 80 | ellipse(Bx, height - Bz, 10, 10); 81 | stroke(0, 255, 0, 100); 82 | ellipse(Cx, height - Cz, 10, 10); 83 | stroke(0, 0, 255, 100); 84 | ellipse(Ax, height - Az, 10, 10); 85 | 86 | stroke(0); 87 | line(Bx, height - Bz, Cx, height - Cz); 88 | line(Cx, height - Cz, Ax, height - Az); 89 | } 90 | -------------------------------------------------------------------------------- /avr/gait/gait_tripod.c: -------------------------------------------------------------------------------- 1 | #include "gait.h" 2 | 3 | #define STEP_COUNT 22 4 | 5 | //The lookup table for moving a foot such that the robot 6 | // moves in a straight direction. The foot starts from 7 | // neutral, and moves along the X axis in a negative 8 | // direction, then lifts up and moves forward. 9 | static Point linear_step_points[STEP_COUNT] = { 10 | Point(0,0,0), 11 | Point(-5,0,0), 12 | Point(-10,0,0), 13 | Point(-15,0,0), 14 | Point(-20,0,0), 15 | Point(-25,0,0), 16 | Point(-30,0,0), 17 | 18 | Point(-25,0,20), 19 | Point(-15,0,40), 20 | Point(-5,0,50), 21 | Point(5,0,50), 22 | Point(10,0,50), 23 | Point(15,0,50), 24 | Point(20,0,50), 25 | Point(24,0,40), 26 | Point(25,0,20), 27 | Point(25,0,10), 28 | 29 | Point(25,0,0), 30 | Point(20,0,0), 31 | Point(15,0,0), 32 | Point(10,0,0), 33 | Point(5,0,0), 34 | }; 35 | 36 | //The lookup table for moving a foot such that the robot 37 | // rotates clockwise on the spot. Starting at neutral, 38 | // the foot rotates counter clockwise, then lifts, and 39 | // moves clockwise. We do not include any Z component, 40 | // since the linear_step_points have it, and they are 41 | // always included. 42 | static Point rotational_step_points[STEP_COUNT] = { 43 | Point(0,0,0), 44 | Point(0,4,0), 45 | Point(-1,8,0), 46 | Point(-1,12,0), 47 | Point(-2,16,0), 48 | Point(-2,20,0), 49 | Point(-3,24,0), 50 | 51 | Point(-2,24,0), 52 | Point(-1,16,0), 53 | Point(-1,8,0), 54 | Point(0,0,0), 55 | Point(0,-8,0), 56 | Point(0,-14,0), 57 | Point(-1,-18,0), 58 | Point(-1,-22,0), 59 | Point(-2,-24,0), 60 | 61 | Point(-2,-24,0), 62 | Point(-2,-20,0), 63 | Point(-2,-16,0), 64 | Point(-1,-12,0), 65 | Point(-1,-8,0), 66 | Point(0,-4,0), 67 | }; 68 | 69 | Point gait_step(Leg leg, uint8_t step_index, double linear_velocity, double linear_angle, double rotational_velocity){ 70 | if (leg.getIndex() == FRONT_RIGHT || leg.getIndex() == MIDDLE_LEFT || leg.getIndex() == REAR_RIGHT){ 71 | step_index = (step_index + (STEP_COUNT / 2)) % STEP_COUNT; 72 | } 73 | 74 | Point result(0,0,0); 75 | 76 | Point linear_result(0,0,0); 77 | linear_result.add(linear_step_points[step_index]); 78 | linear_result.x *= linear_velocity; 79 | linear_result.y *= linear_velocity; 80 | 81 | linear_result.rotateXY(linear_angle); 82 | 83 | result.add(linear_result); 84 | 85 | Point rotational_result(0,0,0); 86 | rotational_result.add(rotational_step_points[step_index]); 87 | rotational_result.x *= fabs(rotational_velocity); 88 | rotational_result.y *= rotational_velocity; 89 | rotational_result.rotateXY(leg.getMountingAngle()); 90 | 91 | result.add(rotational_result); 92 | 93 | return result; 94 | } 95 | 96 | uint8_t gait_step_count(){ 97 | return STEP_COUNT; 98 | } 99 | -------------------------------------------------------------------------------- /kicad/rev1/custom.bck: -------------------------------------------------------------------------------- 1 | <<<<<<< HEAD 2 | EESchema-DOCLIB Version 2.0 Date: Tuesday, January 06, 2015 'pmt' 03:53:39 pm 3 | ======= 4 | EESchema-DOCLIB Version 2.0 Date: Monday, January 05, 2015 'pmt' 03:50:30 pm 5 | >>>>>>> 3f0499d570f7b64b3b8d98e1d412284ea0223885 6 | # 7 | $CMP 74HC4067 8 | D 16 Channel Analog Multiplexer / Demultiplexer 9 | K multiplexer, demultiplexer, analog 10 | $ENDCMP 11 | # 12 | $CMP 74HC4852 13 | D 2x4 Channel Analog Multiplexer / Demultiplexer 14 | K multiplexer, demultiplexer 15 | $ENDCMP 16 | # 17 | $CMP CRYSTAL_RESONATOR 18 | D Either a Crystal with external caps, or a resonator with built in caps 19 | K CRYSTAL RESONATOR 20 | $ENDCMP 21 | # 22 | $CMP DPDT 23 | D DPDT Switch (e.g. GW22LCP) 24 | K GW22LCP, DPDT 25 | $ENDCMP 26 | # 27 | $CMP ISP 28 | D ISP Header 29 | K ISP HEADER 30 | $ENDCMP 31 | # 32 | $CMP L293D 33 | D L293D Quad Half Bridge 34 | K MOTOR DRIVER HALF BRIDGE 35 | $ENDCMP 36 | # 37 | $CMP LE-MM103 38 | D 8x8 LED Matrix Bi-Color Red Green 39 | $ENDCMP 40 | # 41 | $CMP LSMALL 42 | D Inductor 43 | K inductor 44 | $ENDCMP 45 | # 46 | $CMP MC34063A 47 | D Peak Boost Buck Inverting Switching Regulator 48 | K Buck, Power 49 | $ENDCMP 50 | # 51 | $CMP MCP4451 52 | D Digital Potentiometer 53 | K potentiometer 54 | $ENDCMP 55 | # 56 | $CMP MOSFET-SOT-23-3 57 | <<<<<<< HEAD 58 | D N-Channel MOSFET, SOT-23 59 | K N-Channel MOSFET Logic Level 60 | ======= 61 | D BSS138, 50V Vds, 0.22A Id, N-Channel MOSFET, SOT-23 62 | K N-Channel MOSFET Logic Level 63 | F http://www.fairchildsemi.com/ds/BS/BSS138.pdf 64 | >>>>>>> 3f0499d570f7b64b3b8d98e1d412284ea0223885 65 | $ENDCMP 66 | # 67 | $CMP NCP1400A 68 | D NCP1400A_Boost_Regulator 69 | $ENDCMP 70 | # 71 | $CMP OPAMP 72 | D Generic OP-AMP 73 | K opamp, amp, operational, amplifier 74 | $ENDCMP 75 | # 76 | $CMP R_PACK5 77 | D 5 resistors Pack 78 | K R DEV 79 | $ENDCMP 80 | # 81 | $CMP RESISTOR 82 | D Resistance 83 | K R DEV 84 | $ENDCMP 85 | # 86 | $CMP RESONATOR 87 | D Either a Crystal with external caps, or a resonator with built in caps 88 | K CRYSTAL RESONATOR 89 | $ENDCMP 90 | # 91 | $CMP RGB_LED_COMMON_ANODE 92 | D RGB LED (Common Anode) 93 | K RGB_LED_COMMON_ANODE 94 | $ENDCMP 95 | # 96 | $CMP RGB_LED_COMMON_ANODE_ALT1 97 | D RGB LED (Common Anode) 98 | K RGB_LED_COMMON_ANODE 99 | $ENDCMP 100 | # 101 | $CMP RGB_LED_COMMON_CATHODE 102 | D RGB LED (Common Cathode) 103 | K RGB_LED_COMMON_CATHODE 104 | $ENDCMP 105 | # 106 | $CMP RSMALL 107 | D RSMALL 108 | K R DEV 109 | $ENDCMP 110 | # 111 | $CMP STPIC6D595 112 | D Shift Register / Open Drain 113 | K shift 114 | $ENDCMP 115 | # 116 | $CMP TB6552FNG 117 | D Motor Controller, Full H-Bridge x2 118 | K Motor, Bridge, H-Bridge 119 | $ENDCMP 120 | # 121 | #End Doc Library 122 | -------------------------------------------------------------------------------- /kicad/rev2.1/custom.bck: -------------------------------------------------------------------------------- 1 | <<<<<<< HEAD 2 | EESchema-DOCLIB Version 2.0 Date: Tuesday, January 06, 2015 'pmt' 03:53:39 pm 3 | ======= 4 | EESchema-DOCLIB Version 2.0 Date: Monday, January 05, 2015 'pmt' 03:50:30 pm 5 | >>>>>>> 3f0499d570f7b64b3b8d98e1d412284ea0223885 6 | # 7 | $CMP 74HC4067 8 | D 16 Channel Analog Multiplexer / Demultiplexer 9 | K multiplexer, demultiplexer, analog 10 | $ENDCMP 11 | # 12 | $CMP 74HC4852 13 | D 2x4 Channel Analog Multiplexer / Demultiplexer 14 | K multiplexer, demultiplexer 15 | $ENDCMP 16 | # 17 | $CMP CRYSTAL_RESONATOR 18 | D Either a Crystal with external caps, or a resonator with built in caps 19 | K CRYSTAL RESONATOR 20 | $ENDCMP 21 | # 22 | $CMP DPDT 23 | D DPDT Switch (e.g. GW22LCP) 24 | K GW22LCP, DPDT 25 | $ENDCMP 26 | # 27 | $CMP ISP 28 | D ISP Header 29 | K ISP HEADER 30 | $ENDCMP 31 | # 32 | $CMP L293D 33 | D L293D Quad Half Bridge 34 | K MOTOR DRIVER HALF BRIDGE 35 | $ENDCMP 36 | # 37 | $CMP LE-MM103 38 | D 8x8 LED Matrix Bi-Color Red Green 39 | $ENDCMP 40 | # 41 | $CMP LSMALL 42 | D Inductor 43 | K inductor 44 | $ENDCMP 45 | # 46 | $CMP MC34063A 47 | D Peak Boost Buck Inverting Switching Regulator 48 | K Buck, Power 49 | $ENDCMP 50 | # 51 | $CMP MCP4451 52 | D Digital Potentiometer 53 | K potentiometer 54 | $ENDCMP 55 | # 56 | $CMP MOSFET-SOT-23-3 57 | <<<<<<< HEAD 58 | D N-Channel MOSFET, SOT-23 59 | K N-Channel MOSFET Logic Level 60 | ======= 61 | D BSS138, 50V Vds, 0.22A Id, N-Channel MOSFET, SOT-23 62 | K N-Channel MOSFET Logic Level 63 | F http://www.fairchildsemi.com/ds/BS/BSS138.pdf 64 | >>>>>>> 3f0499d570f7b64b3b8d98e1d412284ea0223885 65 | $ENDCMP 66 | # 67 | $CMP NCP1400A 68 | D NCP1400A_Boost_Regulator 69 | $ENDCMP 70 | # 71 | $CMP OPAMP 72 | D Generic OP-AMP 73 | K opamp, amp, operational, amplifier 74 | $ENDCMP 75 | # 76 | $CMP R_PACK5 77 | D 5 resistors Pack 78 | K R DEV 79 | $ENDCMP 80 | # 81 | $CMP RESISTOR 82 | D Resistance 83 | K R DEV 84 | $ENDCMP 85 | # 86 | $CMP RESONATOR 87 | D Either a Crystal with external caps, or a resonator with built in caps 88 | K CRYSTAL RESONATOR 89 | $ENDCMP 90 | # 91 | $CMP RGB_LED_COMMON_ANODE 92 | D RGB LED (Common Anode) 93 | K RGB_LED_COMMON_ANODE 94 | $ENDCMP 95 | # 96 | $CMP RGB_LED_COMMON_ANODE_ALT1 97 | D RGB LED (Common Anode) 98 | K RGB_LED_COMMON_ANODE 99 | $ENDCMP 100 | # 101 | $CMP RGB_LED_COMMON_CATHODE 102 | D RGB LED (Common Cathode) 103 | K RGB_LED_COMMON_CATHODE 104 | $ENDCMP 105 | # 106 | $CMP RSMALL 107 | D RSMALL 108 | K R DEV 109 | $ENDCMP 110 | # 111 | $CMP STPIC6D595 112 | D Shift Register / Open Drain 113 | K shift 114 | $ENDCMP 115 | # 116 | $CMP TB6552FNG 117 | D Motor Controller, Full H-Bridge x2 118 | K Motor, Bridge, H-Bridge 119 | $ENDCMP 120 | # 121 | #End Doc Library 122 | -------------------------------------------------------------------------------- /kicad/rev2/custom.bck: -------------------------------------------------------------------------------- 1 | <<<<<<< HEAD 2 | EESchema-DOCLIB Version 2.0 Date: Tuesday, January 06, 2015 'pmt' 03:53:39 pm 3 | ======= 4 | EESchema-DOCLIB Version 2.0 Date: Monday, January 05, 2015 'pmt' 03:50:30 pm 5 | >>>>>>> 3f0499d570f7b64b3b8d98e1d412284ea0223885 6 | # 7 | $CMP 74HC4067 8 | D 16 Channel Analog Multiplexer / Demultiplexer 9 | K multiplexer, demultiplexer, analog 10 | $ENDCMP 11 | # 12 | $CMP 74HC4852 13 | D 2x4 Channel Analog Multiplexer / Demultiplexer 14 | K multiplexer, demultiplexer 15 | $ENDCMP 16 | # 17 | $CMP CRYSTAL_RESONATOR 18 | D Either a Crystal with external caps, or a resonator with built in caps 19 | K CRYSTAL RESONATOR 20 | $ENDCMP 21 | # 22 | $CMP DPDT 23 | D DPDT Switch (e.g. GW22LCP) 24 | K GW22LCP, DPDT 25 | $ENDCMP 26 | # 27 | $CMP ISP 28 | D ISP Header 29 | K ISP HEADER 30 | $ENDCMP 31 | # 32 | $CMP L293D 33 | D L293D Quad Half Bridge 34 | K MOTOR DRIVER HALF BRIDGE 35 | $ENDCMP 36 | # 37 | $CMP LE-MM103 38 | D 8x8 LED Matrix Bi-Color Red Green 39 | $ENDCMP 40 | # 41 | $CMP LSMALL 42 | D Inductor 43 | K inductor 44 | $ENDCMP 45 | # 46 | $CMP MC34063A 47 | D Peak Boost Buck Inverting Switching Regulator 48 | K Buck, Power 49 | $ENDCMP 50 | # 51 | $CMP MCP4451 52 | D Digital Potentiometer 53 | K potentiometer 54 | $ENDCMP 55 | # 56 | $CMP MOSFET-SOT-23-3 57 | <<<<<<< HEAD 58 | D N-Channel MOSFET, SOT-23 59 | K N-Channel MOSFET Logic Level 60 | ======= 61 | D BSS138, 50V Vds, 0.22A Id, N-Channel MOSFET, SOT-23 62 | K N-Channel MOSFET Logic Level 63 | F http://www.fairchildsemi.com/ds/BS/BSS138.pdf 64 | >>>>>>> 3f0499d570f7b64b3b8d98e1d412284ea0223885 65 | $ENDCMP 66 | # 67 | $CMP NCP1400A 68 | D NCP1400A_Boost_Regulator 69 | $ENDCMP 70 | # 71 | $CMP OPAMP 72 | D Generic OP-AMP 73 | K opamp, amp, operational, amplifier 74 | $ENDCMP 75 | # 76 | $CMP R_PACK5 77 | D 5 resistors Pack 78 | K R DEV 79 | $ENDCMP 80 | # 81 | $CMP RESISTOR 82 | D Resistance 83 | K R DEV 84 | $ENDCMP 85 | # 86 | $CMP RESONATOR 87 | D Either a Crystal with external caps, or a resonator with built in caps 88 | K CRYSTAL RESONATOR 89 | $ENDCMP 90 | # 91 | $CMP RGB_LED_COMMON_ANODE 92 | D RGB LED (Common Anode) 93 | K RGB_LED_COMMON_ANODE 94 | $ENDCMP 95 | # 96 | $CMP RGB_LED_COMMON_ANODE_ALT1 97 | D RGB LED (Common Anode) 98 | K RGB_LED_COMMON_ANODE 99 | $ENDCMP 100 | # 101 | $CMP RGB_LED_COMMON_CATHODE 102 | D RGB LED (Common Cathode) 103 | K RGB_LED_COMMON_CATHODE 104 | $ENDCMP 105 | # 106 | $CMP RSMALL 107 | D RSMALL 108 | K R DEV 109 | $ENDCMP 110 | # 111 | $CMP STPIC6D595 112 | D Shift Register / Open Drain 113 | K shift 114 | $ENDCMP 115 | # 116 | $CMP TB6552FNG 117 | D Motor Controller, Full H-Bridge x2 118 | K Motor, Bridge, H-Bridge 119 | $ENDCMP 120 | # 121 | #End Doc Library 122 | -------------------------------------------------------------------------------- /avr/lib/protocol/protocol.c: -------------------------------------------------------------------------------- 1 | #include "protocol.h" 2 | 3 | #define START 0x7e 4 | #define ESCAPE 0x7d 5 | 6 | #define MAX_SIZE 40 7 | 8 | static uint8_t position; // current position in the frame 9 | static uint8_t length; // frame length 10 | static uint8_t command; // frame api code (command) 11 | static uint8_t checksum; // checksum 12 | static uint8_t escape; // escape byte seen, unescape next byte 13 | static uint8_t error; // error condition, ignore bytes until next frame start byte 14 | static uint8_t message[MAX_SIZE]; 15 | 16 | void _protocol_send_byte(uint8_t b, uint8_t escape) 17 | { 18 | if (escape && (b == START || b == ESCAPE)) { 19 | protocol_write(ESCAPE); 20 | protocol_write(b ^ 0x20); 21 | } else { 22 | protocol_write(b); 23 | } 24 | } 25 | 26 | void protocol_send_message(uint8_t cmd, uint8_t *bytes, uint8_t length) 27 | { 28 | _protocol_send_byte(START, 0); 29 | _protocol_send_byte(length + 1, 1); 30 | _protocol_send_byte(cmd, 0); 31 | 32 | uint8_t checksum = cmd; 33 | 34 | for (uint8_t i = 0; i < length; i++) { 35 | _protocol_send_byte(bytes[i], 1); 36 | checksum += bytes[i]; 37 | } 38 | 39 | checksum = 0xff - checksum; 40 | 41 | _protocol_send_byte(checksum, 1); 42 | } 43 | 44 | uint8_t protocol_geterroror(){ 45 | return error; 46 | } 47 | 48 | void protocol_receive_byte(uint8_t b){ 49 | if (error > 0 && b == START) { 50 | // recover from any previous error condition 51 | error = 0; 52 | position = 0; 53 | } else if (error > 0) { 54 | return; 55 | } 56 | 57 | if (position > 0 && b == START) { 58 | // unexpected start of frame 59 | error = PROTOCOL_ERROR_UNEXPECTED_START_OF_FRAME; 60 | return; 61 | } 62 | if (position > 0 && b == ESCAPE) { 63 | // unescape next byte 64 | escape = 1; 65 | return; 66 | } 67 | if (escape) { 68 | // unescape current byte 69 | b = 0x20 ^ b; 70 | escape = 0; 71 | } 72 | if (position > 1) { // start byte and length byte not included in checksum 73 | checksum += b; 74 | } 75 | 76 | switch(position) { 77 | case 0: // start frame 78 | position++; 79 | return; 80 | case 1: // length 81 | if (b == 0){ 82 | error = PROTOCOL_ERROR_INVALID_LENGTH; 83 | } 84 | else { 85 | length = b; 86 | position++; 87 | } 88 | return; 89 | case 2: 90 | command = b; 91 | position++; 92 | return; 93 | default: 94 | if (position > MAX_SIZE) return; 95 | if (position == (length + 2)) { 96 | if (checksum == 0xff) { 97 | protocol_dispatch_message(command, message, length - 1); 98 | } else { 99 | error = PROTOCOL_ERROR_INVALID_CHECKSUM; 100 | } 101 | position = 0; 102 | checksum = 0; 103 | } else { 104 | message[position++ - 3] = b; 105 | } 106 | } 107 | } 108 | -------------------------------------------------------------------------------- /simulation/ik_top_rotation/ik_top_rotation.pde: -------------------------------------------------------------------------------- 1 | float[] x = new float[6]; 2 | float[] y = new float[6]; 3 | 4 | 5 | void setup(){ 6 | size(500, 500); 7 | } 8 | 9 | void draw(){ 10 | fill(255); 11 | rect(0, 0, width, height); 12 | 13 | float scale = 2; 14 | 15 | //Center 16 | float cx = 0, cy = 0; 17 | 18 | //Body radius 19 | float br = 45; 20 | 21 | //Draw the body 22 | float cos60 = cos(radians(60)); 23 | float sin60 = sin(radians(60)); 24 | 25 | //Body co-ordinates (+45) 26 | float xb = 45, yb = 0, xb1, yb1; 27 | //Leg Coxa co-ordinates (+48) 28 | float xlc = 93, ylc = 0, xlc1, ylc1; 29 | //Leg Foot co-ordinates (+35) 30 | float xlf = 128, ylf = 0, xlf1, ylf1; 31 | 32 | for (int i = 0; i < 6; i++){ 33 | //Rotational transformation matrix 34 | xb1 = xb * cos60 - yb * sin60; 35 | yb1 = xb * sin60 + yb * cos60; 36 | 37 | //Draw the body 38 | stroke(0); 39 | line(width/2 + xb, height/2 + yb * -1, width/2 + xb1, height/2 + yb1 * -1); 40 | 41 | //Calculate the required leg rotation 42 | stroke(200); 43 | line(width/2 + xb, height/2 + yb * -1, width/2 + xlc, height/2 + ylc * -1); 44 | line(width/2 + xlf, height/2 + ylf * -1, width/2 + xlc, height/2 + ylc * -1); 45 | ellipse(width/2 + xb, height/2 + yb * -1, 5, 5); 46 | ellipse(width/2 + xlc, height/2 + ylc * -1, 5, 5); 47 | ellipse(width/2 + xlf, height/2 + ylf * -1, 5, 5); 48 | 49 | xb = xb1; 50 | yb = yb1; 51 | 52 | xlc1 = xlc * cos60 - ylc * sin60; 53 | ylc1 = xlc * sin60 + ylc * cos60; 54 | xlc = xlc1; 55 | ylc = ylc1; 56 | 57 | xlf1 = xlf * cos60 - ylf * sin60; 58 | ylf1 = xlf * sin60 + ylf * cos60; 59 | xlf = xlf1; 60 | ylf = ylf1; 61 | } 62 | 63 | //Simulations for FrontLeft leg 64 | moveLeg(110, 10, 0); 65 | moveLeg(80, 110, 60); 66 | moveLeg(-67, 79, 120); 67 | moveLeg(-67, -79, -120); 68 | 69 | 70 | //Simulations for MiddleRight leg 71 | } 72 | 73 | void moveLeg(float x, float y, float mountingAngle){ 74 | stroke(255, 0, 0); 75 | ellipse(width/2 + x, height/2 + y * -1, 5, 5); 76 | 77 | //Rotate and translate the leg to get to 'leg' co-ordinate system, with 0,0 at coxa/femur joint 78 | float x1 = x * cos(radians(mountingAngle * -1)) - y * sin(radians(mountingAngle * -1)) - 45; 79 | float y1 = x * sin(radians(mountingAngle * -1)) + y * cos(radians(mountingAngle * -1)); 80 | stroke(0, 255, 0); 81 | ellipse(width/2 + x1 + 45, height/2 + y1 * -1, 3, 3); 82 | 83 | // float angle = atan2(y1, x1); 84 | float l = sqrt(x1*x1 + y1*y1); 85 | println("l: " + l); 86 | 87 | //Translate and Rotate back for drawing 88 | x1 += 45; 89 | float x2 = x1 * cos(radians(mountingAngle)) - y1 * sin(radians(mountingAngle)); 90 | float y2 = x1 * sin(radians(mountingAngle)) + y1 * cos(radians(mountingAngle)); 91 | 92 | stroke(0, 0, 255); 93 | ellipse(width/2 + x2, height/2 + y2 * -1, 1, 1); 94 | } 95 | -------------------------------------------------------------------------------- /kicad/rev2/gerber/hex.drl: -------------------------------------------------------------------------------- 1 | M48 2 | ;DRILL file {Pcbnew (2013-dec-23)-stable} date Thu 04 Sep 2014 08:14:07 PM MDT 3 | ;FORMAT={-:-/ absolute / inch / decimal} 4 | FMAT,2 5 | INCH,TZ 6 | T1C0.020 7 | T2C0.032 8 | T3C0.040 9 | T4C0.045 10 | T5C0.050 11 | T6C0.120 12 | % 13 | G90 14 | G05 15 | M72 16 | T1 17 | X1.7Y-1.9 18 | X1.85Y-1.9 19 | X2.025Y-1.65 20 | X2.025Y-1.725 21 | X2.025Y-1.8 22 | X2.075Y-1.375 23 | X2.1Y-1.825 24 | X2.15Y-1.85 25 | X2.2Y-1.875 26 | X2.225Y-1.475 27 | X2.275Y-2.35 28 | X2.375Y-1.6 29 | X2.375Y-1.775 30 | X2.38Y-1.675 31 | X2.43Y-1.725 32 | X2.45Y-1.5 33 | X2.45Y-1.55 34 | X2.45Y-1.65 35 | X2.515Y-1.81 36 | X2.575Y-1.35 37 | X2.575Y-1.775 38 | X2.61Y-1.955 39 | X2.625Y-1.725 40 | X2.642Y-1.925 41 | X2.675Y-1.2 42 | X2.675Y-1.425 43 | X2.675Y-1.7 44 | X2.75Y-1.175 45 | X2.875Y-1.025 46 | X2.925Y-1.875 47 | X3.075Y-1.825 48 | T2 49 | X1.5213Y-0.625 50 | X1.6787Y-0.625 51 | X1.95Y-0.975 52 | X1.95Y-1.075 53 | X2.05Y-0.975 54 | X2.45Y-1.2 55 | X2.475Y-0.7 56 | X2.55Y-1.2 57 | X2.575Y-0.7 58 | X3.0713Y-0.625 59 | X3.2287Y-0.625 60 | T3 61 | X1.475Y-2.45 62 | X1.5Y-2.075 63 | X1.575Y-2.45 64 | X1.6Y-2.075 65 | X1.7Y-2.075 66 | X1.725Y-2.45 67 | X1.8Y-2.075 68 | X1.825Y-2.45 69 | X1.85Y-1.3 70 | X1.85Y-1.4 71 | X1.85Y-1.5 72 | X1.85Y-1.6 73 | X1.85Y-1.7 74 | X1.85Y-1.8 75 | X1.9Y-2.075 76 | X1.925Y-2.45 77 | X2.Y-2.075 78 | X2.025Y-2.45 79 | X2.05Y-1.3 80 | X2.1Y-0.675 81 | X2.1Y-0.775 82 | X2.1Y-2.075 83 | X2.125Y-2.45 84 | X2.15Y-1.3 85 | X2.2Y-0.675 86 | X2.2Y-0.775 87 | X2.2Y-2.075 88 | X2.225Y-2.45 89 | X2.25Y-1.3 90 | X2.3Y-0.675 91 | X2.3Y-0.775 92 | X2.3Y-2.075 93 | X2.4Y-2.075 94 | X2.4957Y-2.275 95 | X2.5Y-2.075 96 | X2.5744Y-2.275 97 | X2.6531Y-2.275 98 | X2.6543Y-1.9957 99 | X2.725Y-1.925 100 | X2.7319Y-2.275 101 | X2.775Y-2.4 102 | X2.7957Y-1.8543 103 | X2.8106Y-2.275 104 | X2.875Y-1.975 105 | X2.875Y-2.075 106 | X2.875Y-2.4 107 | X2.8894Y-2.275 108 | X2.9681Y-2.275 109 | X2.975Y-1.975 110 | X2.975Y-2.075 111 | X2.975Y-2.4 112 | X3.0469Y-2.275 113 | X3.075Y-1.975 114 | X3.075Y-2.075 115 | X3.075Y-2.4 116 | X3.1256Y-2.275 117 | X3.175Y-2.4 118 | X3.2043Y-2.275 119 | X3.275Y-2.4 120 | T4 121 | X1.95Y-0.65 122 | X1.95Y-0.75 123 | T5 124 | X1.525Y-0.825 125 | X1.525Y-0.925 126 | X1.525Y-1.025 127 | X1.525Y-1.175 128 | X1.525Y-1.275 129 | X1.525Y-1.375 130 | X1.525Y-1.525 131 | X1.525Y-1.625 132 | X1.525Y-1.725 133 | X1.625Y-0.825 134 | X1.625Y-0.925 135 | X1.625Y-1.025 136 | X1.625Y-1.175 137 | X1.625Y-1.275 138 | X1.625Y-1.375 139 | X1.625Y-1.525 140 | X1.625Y-1.625 141 | X1.625Y-1.725 142 | X1.725Y-0.825 143 | X1.725Y-0.925 144 | X1.725Y-1.025 145 | X1.725Y-1.175 146 | X1.725Y-1.275 147 | X1.725Y-1.375 148 | X1.725Y-1.525 149 | X1.725Y-1.625 150 | X1.725Y-1.725 151 | X3.025Y-0.825 152 | X3.025Y-0.925 153 | X3.025Y-1.025 154 | X3.025Y-1.175 155 | X3.025Y-1.275 156 | X3.025Y-1.375 157 | X3.025Y-1.525 158 | X3.025Y-1.625 159 | X3.025Y-1.725 160 | X3.125Y-0.825 161 | X3.125Y-0.925 162 | X3.125Y-1.025 163 | X3.125Y-1.175 164 | X3.125Y-1.275 165 | X3.125Y-1.375 166 | X3.125Y-1.525 167 | X3.125Y-1.625 168 | X3.125Y-1.725 169 | X3.225Y-0.825 170 | X3.225Y-0.925 171 | X3.225Y-1.025 172 | X3.225Y-1.175 173 | X3.225Y-1.275 174 | X3.225Y-1.375 175 | X3.225Y-1.525 176 | X3.225Y-1.625 177 | X3.225Y-1.725 178 | T6 179 | X1.525Y-1.9 180 | X3.225Y-1.9 181 | T0 182 | M30 183 | -------------------------------------------------------------------------------- /kicad/rev2/hex.lst: -------------------------------------------------------------------------------- 1 | eeschema (2014-01-10 BZR 4027)-stable >> Creation date: Wednesday, July 30, 2014 'pmt' 02:42:10 pm 2 | #Cmp ( order = Reference ) 3 | | C1 10uF 4 | | C2 2200uF+ 5 | | C3 1uF 6 | | C4 1uF 7 | | C5 1uF 8 | | C6 20pF 9 | | C7 20pF 10 | | C8 47uF 11 | | C9 2200uF+ 12 | | C10 47uF+ 13 | | C11 2200uF+ 14 | | C12 2200uF+ 15 | | D1 DFLS130L 16 | | D2 LED 17 | | D3 CLVBA-FKA 18 | | IC1 ATMEGA1284-A 19 | | JP1 CONN_3 20 | | L1 22uH 21 | | P1 CONN_6 22 | | P2 CONN_6 23 | | P3 CONN_10 24 | | P4 CONN_13 25 | | P5 CONN_2 26 | | P6 CONN_9 27 | | P7 CONN_1 28 | | P8 BATT (4.8V-6V) 29 | | P9 PROG 30 | | P10 CONN_6 31 | | R1 1k 32 | | R2 1k 33 | | R3 4k7 34 | | R4 4k7 35 | | R5 1k 36 | | R6 100k 37 | | R7 100k 38 | | R8 1k 39 | | R9 1k 40 | | R10 1k 41 | | R11 1k 42 | | S1 FRNT LFT 43 | | S2 FRNT RGT 44 | | S3 MID LFT 45 | | S4 MID RGT 46 | | S5 REAR LFT 47 | | S6 REAR RGT 48 | | SW1 DPDT 49 | | VR1 MCP1702 50 | | VR2 NCP1400A 51 | | X1 CRYSTAL_RESONATOR 52 | | ZD1 ZD 3v3 53 | | ZD2 ZD 3v3 54 | #End Cmp 55 | 56 | #Cmp ( order = Value ) 57 | | 20pF C6 58 | | 20pF C7 59 | | 1uF C3 60 | | 1uF C4 61 | | 1uF C5 62 | | 10uF C1 63 | | 47uF C8 64 | | 47uF+ C10 65 | | 2200uF+ C2 66 | | 2200uF+ C9 67 | | 2200uF+ C11 68 | | 2200uF+ C12 69 | | DFLS130L D1 70 | | LED D2 71 | | CLVBA-FKA D3 72 | | ATMEGA1284-A IC1 73 | | CONN_3 JP1 74 | | 22uH L1 75 | | CONN_6 P1 76 | | CONN_6 P2 77 | | CONN_10 P3 78 | | CONN_13 P4 79 | | CONN_2 P5 80 | | CONN_9 P6 81 | | CONN_1 P7 82 | | BATT (4.8V-6V) P8 83 | | PROG P9 84 | | CONN_6 P10 85 | | 1k R1 86 | | 1k R2 87 | | 1k R5 88 | | 1k R8 89 | | 1k R9 90 | | 1k R10 91 | | 1k R11 92 | | 4k7 R3 93 | | 4k7 R4 94 | | 100k R6 95 | | 100k R7 96 | | FRNT LFT S1 97 | | FRNT RGT S2 98 | | MID LFT S3 99 | | MID RGT S4 100 | | REAR LFT S5 101 | | REAR RGT S6 102 | | DPDT SW1 103 | | MCP1702 VR1 104 | | NCP1400A VR2 105 | | CRYSTAL_RESONATOR X1 106 | | ZD 3v3 ZD1 107 | | ZD 3v3 ZD2 108 | #End Cmp 109 | 110 | #End List 111 | -------------------------------------------------------------------------------- /avr/lib/serial/serial.h: -------------------------------------------------------------------------------- 1 | #ifndef SERIAL_H 2 | #define SERIAL_H 3 | 4 | #include 5 | 6 | //The buffer size; if separate buffer sizes are not defined for rx and tx, this one is 7 | // used for both RX and TX buffers. Defaults to 64 bytes. You can change it by 8 | // redefining SERIAL_BUFFER_SIZE in your makefile (in the CDEFS variable, 9 | // beside where F_CPU is defined). 10 | #ifdef SERIAL_BUFFER_SIZE 11 | #if SERIAL_BUFFER_SIZE > 255 12 | #undef SERIAL_BUFFER_SIZE 13 | #define SERIAL_BUFFER_SIZE 255 14 | #endif 15 | #else 16 | #define SERIAL_BUFFER_SIZE 64 17 | #endif 18 | 19 | /* 20 | * Initializes the USART with the given parameters. Valid arguments include: 21 | * baud: Any valid baud rate based on hardware support 22 | * dataBits: Between 5 and 9 inclusive. 8 recommended. 23 | * parity: 0 (no parity bit), 2 (even parity), 3 (odd parity). 0 or 2 recommended. 24 | * stopBits: 1 or 2. 25 | * interruptsEnabled: if true, enable interrupts for Rx Complete, Tx Complete, and 26 | * USART Data Register Empty 27 | */ 28 | void serial_init(uint32_t baud, uint8_t data, uint8_t parity, uint8_t stopBits); 29 | 30 | 31 | /* 32 | * Simplified init method which only asks for baud, and gives sane defaults for the rest. 33 | * Implementations should call serial_init with values baud, 8, 0, 1. 34 | */ 35 | void serial_init_b(uint32_t baud); 36 | 37 | /* 38 | * Reads a single character from the serial port. Pass in a pointer to a byte, and 39 | * the function will write a single byte to that pointer. If the read was successful, 40 | * return 1; otherwise return 0. Implementations MAY block until a byte is received. 41 | */ 42 | uint8_t serial_read_c(char *c); 43 | 44 | /* 45 | * Reads a single character from the serial port. Pass in a pointer to a byte, and 46 | * the function will write a single byte to that pointer. If the read was successful, 47 | * return 1; otherwise return 0. Implementations MAY block until a byte is received. 48 | */ 49 | uint8_t serial_read_b(uint8_t *b); 50 | 51 | /* 52 | * Reads data into buffer of (at most) the given length - 1. Returns the number of bytes 53 | * which were read. Implementations MAY block until the entire buffer is filled. 54 | * The character after the last read character will be null terminated (which is why 55 | * the most you can read is length - 1). 56 | */ 57 | uint8_t serial_read_s(char *s, uint8_t len); 58 | 59 | /* 60 | * Reads data into buffer of (at most) the given length - 1. Returns the number of bytes 61 | * which were read. Implementations MAY block until the entire buffer is filled. 62 | */ 63 | uint8_t serial_read_a(uint8_t *a, uint8_t len); 64 | 65 | /* 66 | * Writes a string to the serial port. Implementations MAY block until 67 | * all bytes are written. 68 | */ 69 | void serial_write_s(char *data); 70 | 71 | /* 72 | * Writes a byte array to the serial port. Implementations MAY block until 73 | * all bytes are written. 74 | */ 75 | void serial_write_a(uint8_t *data, uint8_t len); 76 | 77 | /* 78 | * Writes a single character to the serial port. Implementations MAY block until the 79 | * write is completed. 80 | */ 81 | void serial_write_c(char data); 82 | 83 | /* 84 | * Writes a single byte to the serial port. Implementations MAY block until the 85 | * write is completed. 86 | */ 87 | void serial_write_b(uint8_t data); 88 | 89 | /* 90 | * Checks if any bytes are available for read. Returns 0 when no bytes are available; 91 | * returns non-zero when there are any bytes avilable. Implementations MUST NOT block. 92 | * Implementations MAY return the total number of bytes available. 93 | */ 94 | uint8_t serial_available(); 95 | 96 | #endif 97 | -------------------------------------------------------------------------------- /kicad/rev2/gerber/hex-Edge_Cuts.gbr: -------------------------------------------------------------------------------- 1 | G04 (created by PCBNEW (2013-dec-23)-stable) date Thu 04 Sep 2014 08:14:04 PM MDT* 2 | %MOIN*% 3 | G04 Gerber Fmt 3.4, Leading zero omitted, Abs format* 4 | %FSLAX34Y34*% 5 | G01* 6 | G70* 7 | G90* 8 | G04 APERTURE LIST* 9 | %ADD10C,0.00590551*% 10 | %ADD11C,0.00393701*% 11 | G04 APERTURE END LIST* 12 | G54D10* 13 | G54D11* 14 | X24250Y-24250D02* 15 | X24250Y-22500D01* 16 | X33000Y-24750D02* 17 | X24750Y-24750D01* 18 | X33500Y-22500D02* 19 | X33500Y-24250D01* 20 | X33500Y-6750D02* 21 | X33500Y-20500D01* 22 | X14000Y-20500D02* 23 | X14000Y-6750D01* 24 | X32500Y-22000D02* 25 | X33000Y-22000D01* 26 | X24750Y-22000D02* 27 | X25000Y-22000D01* 28 | X23000Y-22000D02* 29 | X22750Y-22000D01* 30 | X14500Y-25250D02* 31 | X23000Y-25250D01* 32 | X15000Y-22000D02* 33 | X14500Y-22000D01* 34 | X30500Y-22000D02* 35 | X27000Y-22000D01* 36 | X27000Y-21500D02* 37 | X30500Y-21500D01* 38 | X26750Y-21750D02* 39 | G75* 40 | G03X27000Y-22000I250J0D01* 41 | G74* 42 | G01* 43 | X27000Y-21500D02* 44 | G75* 45 | G03X26750Y-21750I0J-250D01* 46 | G74* 47 | G01* 48 | X30500Y-22000D02* 49 | G75* 50 | G03X30750Y-21750I0J250D01* 51 | G74* 52 | G01* 53 | X30750Y-21750D02* 54 | G75* 55 | G03X30500Y-21500I-250J0D01* 56 | G74* 57 | G01* 58 | X25000Y-21500D02* 59 | X22750Y-21500D01* 60 | X33000Y-24750D02* 61 | G75* 62 | G03X33500Y-24250I0J500D01* 63 | G74* 64 | G01* 65 | X33500Y-22500D02* 66 | G75* 67 | G03X33000Y-22000I-500J0D01* 68 | G74* 69 | G01* 70 | X32250Y-21750D02* 71 | G75* 72 | G03X32500Y-22000I250J0D01* 73 | G74* 74 | G01* 75 | X32500Y-21500D02* 76 | G75* 77 | G03X32250Y-21750I0J-250D01* 78 | G74* 79 | G01* 80 | X25250Y-21750D02* 81 | G75* 82 | G03X25000Y-21500I-250J0D01* 83 | G74* 84 | G01* 85 | X25000Y-22000D02* 86 | G75* 87 | G03X25250Y-21750I0J250D01* 88 | G74* 89 | G01* 90 | X24250Y-24250D02* 91 | G75* 92 | G03X24750Y-24750I500J0D01* 93 | G74* 94 | G01* 95 | X24750Y-22000D02* 96 | G75* 97 | G03X24250Y-22500I0J-500D01* 98 | G74* 99 | G01* 100 | X20750Y-21500D02* 101 | X17000Y-21500D01* 102 | X17000Y-22000D02* 103 | X20750Y-22000D01* 104 | X14000Y-24750D02* 105 | X14000Y-22500D01* 106 | X23500Y-22500D02* 107 | X23500Y-24750D01* 108 | X23500Y-22500D02* 109 | G75* 110 | G03X23000Y-22000I-500J0D01* 111 | G74* 112 | G01* 113 | X23000Y-25250D02* 114 | G75* 115 | G03X23500Y-24750I0J500D01* 116 | G74* 117 | G01* 118 | X14000Y-24750D02* 119 | G75* 120 | G03X14500Y-25250I500J0D01* 121 | G74* 122 | G01* 123 | X14500Y-22000D02* 124 | G75* 125 | G03X14000Y-22500I0J-500D01* 126 | G74* 127 | G01* 128 | X22500Y-21750D02* 129 | G75* 130 | G03X22750Y-22000I250J0D01* 131 | G74* 132 | G01* 133 | X22750Y-21500D02* 134 | G75* 135 | G03X22500Y-21750I0J-250D01* 136 | G74* 137 | G01* 138 | X20750Y-22000D02* 139 | G75* 140 | G03X21000Y-21750I0J250D01* 141 | G74* 142 | G01* 143 | X21000Y-21750D02* 144 | G75* 145 | G03X20750Y-21500I-250J0D01* 146 | G74* 147 | G01* 148 | X17000Y-21500D02* 149 | G75* 150 | G03X16750Y-21750I0J-250D01* 151 | G74* 152 | G01* 153 | X16750Y-21750D02* 154 | G75* 155 | G03X17000Y-22000I250J0D01* 156 | G74* 157 | G01* 158 | X15000Y-22000D02* 159 | G75* 160 | G03X15250Y-21750I0J250D01* 161 | G74* 162 | G01* 163 | X15250Y-21750D02* 164 | G75* 165 | G03X15000Y-21500I-250J0D01* 166 | G74* 167 | G01* 168 | X32500Y-21500D02* 169 | G75* 170 | G03X33500Y-20500I0J1000D01* 171 | G74* 172 | G01* 173 | X14000Y-20500D02* 174 | G75* 175 | G03X15000Y-21500I1000J0D01* 176 | G74* 177 | G01* 178 | X15000Y-5750D02* 179 | G75* 180 | G03X14000Y-6750I0J-1000D01* 181 | G74* 182 | G01* 183 | X33500Y-6750D02* 184 | G75* 185 | G03X32500Y-5750I-1000J0D01* 186 | G74* 187 | G01* 188 | X15000Y-5750D02* 189 | X32500Y-5750D01* 190 | M02* 191 | -------------------------------------------------------------------------------- /avr/lib/serial/serial_async_tx.c: -------------------------------------------------------------------------------- 1 | /* 2 | * An asynchronous implementation of the tx portion of the serial library. This will let you submit a bunch 3 | * of data to a buffer, and let the AVR send it using interrupts. This results in shorter 4 | * send times (as the write methods do not block), but there are downsides. It a) causes code 5 | * to be run in interrupts (which may interere with timing-critical code), b) adds substantially 6 | * to the total SRAM used by a program (defaults to a 64 byte buffer for transmitting and receiving, 7 | * although this buffer size can be configured at compile time), c) introduces the possibility 8 | * that data will be partially unsent, if the program tries to write too much at a time and overflows 9 | * the buffer, and d) for single byte messages which are not sent continuously, there is actually a 10 | * performance penalty as there is an extra layer of abstraction (the buffer) in between the hardware 11 | * and the sending program. 12 | * 13 | * With the downsides, though, come a definite advantage -- for longer strings (experiments 14 | * show this seems to be effective for more than 2 bytes), the speed is much faster. When 15 | * sending 3 bytes, it takes 1215µs (1.2ms) to return from the synchronous implementation; 16 | * while taking only 280µs (0.3ms) to return from the asynchronous one. For longer 17 | * strings the difference is even more pronounced: if you send a 26 character string, it 18 | * will take the synchronous implementation about 25131µs (25ms) to return from the 19 | * write_s function; the asynchronous implementation takes about 669µs (less than 1ms) 20 | * to return. 21 | */ 22 | 23 | #include "serial.h" 24 | #include "../Ring/Ring.h" 25 | #include 26 | 27 | static Ring tx_buffer(SERIAL_BUFFER_SIZE); 28 | 29 | void _serial_init_tx(){ 30 | //Enable interrupts if the NO_INTERRUPT_ENABLE define is not set. If it is, you need to call sei() elsewhere. 31 | #ifndef NO_INTERRUPT_ENABLE 32 | sei(); 33 | #endif 34 | } 35 | 36 | void serial_write_c(char data){ 37 | //Disable UCSR interrupts temporarily to avoid clobbering the buffer 38 | UCSR0B &= ~_BV(UDRIE0); 39 | tx_buffer.put(data); 40 | 41 | //Signal that there is data available; the UDRE interrupt will fire. 42 | UCSR0B |= _BV(UDRIE0); 43 | } 44 | 45 | void serial_write_s(char *data){ 46 | while (*data){ 47 | serial_write_c(*data++); 48 | } 49 | } 50 | 51 | void serial_write_a(uint8_t *data, uint8_t len){ 52 | for (uint8_t i = 0; i < len; i++){ 53 | serial_write_b(data[i]); 54 | } 55 | } 56 | 57 | void serial_write_b(uint8_t data){ 58 | serial_write_c((char) data); 59 | } 60 | 61 | 62 | #if defined(__AVR_ATtiny2313__) || \ 63 | defined(__AVR_ATmega48P__) || \ 64 | defined(__AVR_ATmega88P__) || \ 65 | defined(__AVR_ATmega168P__) || \ 66 | defined(__AVR_ATmega328P__) || \ 67 | defined(__AVR_ATmega48__) || \ 68 | defined(__AVR_ATmega88__) || \ 69 | defined(__AVR_ATmega168__) 70 | ISR(USART_UDRE_vect){ 71 | #elif defined(__AVR_ATmega324P__) || \ 72 | defined(__AVR_ATmega644__) || \ 73 | defined(__AVR_ATmega644P__) || \ 74 | defined(__AVR_ATmega644PA__) || \ 75 | defined(__AVR_ATmega1284P__) 76 | ISR(USART0_UDRE_vect){ 77 | #else 78 | #error You must define USART vectors for your chip! Please verify that MMCU is set correctly, and that there is a matching vector definition in serial_asynch_tx.c 79 | void error2() { 80 | #endif 81 | if (!tx_buffer.isEmpty()){ 82 | UDR0 = tx_buffer.get(); 83 | } 84 | else { 85 | //Once the ring buffer is empty (i.e. head == tail), we disable 86 | // USART Data Register Empty interrupts until someone writes to 87 | // the tx buffer again. 88 | UCSR0B &= ~_BV(UDRIE0); 89 | } 90 | } 91 | -------------------------------------------------------------------------------- /avr/hardware/magnetometer_HMC5883L.c: -------------------------------------------------------------------------------- 1 | #include "magnetometer.h" 2 | 3 | volatile uint8_t interval_do_magnetometer_reading = 0x00; //Mailbox value, set in timer2 ISR and read in delay 4 | 5 | #define MAGNETOMETER_ADDRESS 0x1E 6 | #define ALPHA 0.2 7 | 8 | #if MAGNETOMETER == 1 9 | static int16_t filtered[2]; 10 | static int16_t offsets[2]; 11 | 12 | void magnetometer_init(){ 13 | int16_t x = eeprom_read_word((uint16_t*) MAGNETOMETER_EEPROM_BASE); 14 | int16_t y = eeprom_read_word((uint16_t*) (MAGNETOMETER_EEPROM_BASE + MAGNETOMETER_EEPROM_OFFSET)); 15 | magnetometer_set_offsets(x, y); 16 | 17 | twi_init(); 18 | 19 | uint8_t message[6]; 20 | message[0] = 0x00; //Register 0 21 | message[1] = 0x78; //8 sample average, 75 Hz, normal mode 22 | twi_write_to(MAGNETOMETER_ADDRESS, message, 2, TWI_BLOCK, TWI_STOP); 23 | message[0] = 0x01; //Register 1 24 | message[1] = 0x20; //Gain 1 (± 1.3 Ga) 25 | twi_write_to(MAGNETOMETER_ADDRESS, message, 2, TWI_BLOCK, TWI_STOP); 26 | message[0] = 0x02; //Register 2 27 | message[1] = 0x00; //Continuous measurement mode 28 | twi_write_to(MAGNETOMETER_ADDRESS, message, 2, TWI_BLOCK, TWI_STOP); 29 | 30 | //Discard the first reading after mode change 31 | message[0] = 0x03; 32 | twi_write_to(MAGNETOMETER_ADDRESS, message, 1, TWI_BLOCK, TWI_STOP); 33 | delay_ms(100); 34 | message[0] = 0x06; 35 | twi_read_from(MAGNETOMETER_ADDRESS, message, 6, TWI_STOP); 36 | 37 | //Read an actual value 38 | message[0] = 0x03; 39 | twi_write_to(MAGNETOMETER_ADDRESS, message, 1, TWI_BLOCK, TWI_STOP); 40 | delay_ms(100); 41 | message[0] = 0x06; 42 | twi_read_from(MAGNETOMETER_ADDRESS, message, 6, TWI_STOP); 43 | 44 | //Initialize the filtered values 45 | filtered[0] = (((uint16_t) message[0]) << 8 | message[1]) + offsets[0]; 46 | filtered[1] = (((uint16_t) message[4]) << 8 | message[5]) + offsets[1]; 47 | } 48 | 49 | void magnetometer_get_offsets(int16_t *x, int16_t *y){ 50 | *x = offsets[0]; 51 | *y = offsets[1]; 52 | } 53 | 54 | void magnetometer_set_offsets(int16_t x, int16_t y){ 55 | offsets[0] = x; 56 | offsets[1] = y; 57 | } 58 | 59 | void magnetometer_get_raw(int16_t *x, int16_t *y){ 60 | //Read 6 bytes starting at X MSB (0x03, reset to here prior to this invocation) 61 | uint8_t message[6]; 62 | message[0] = 0x06; 63 | twi_read_from(MAGNETOMETER_ADDRESS, message, 6, TWI_STOP); 64 | 65 | *x = (((uint16_t) message[0]) << 8 | message[1]); 66 | *y = (((uint16_t) message[4]) << 8 | message[5]); 67 | 68 | //Reset pointer to read next time 69 | message[0] = 0x03; 70 | twi_write_to(MAGNETOMETER_ADDRESS, message, 1, TWI_BLOCK, TWI_STOP); 71 | } 72 | 73 | void magnetometer_take_reading(){ 74 | int16_t x = 0; 75 | int16_t y = 0; 76 | 77 | magnetometer_get_raw(&x, &y); 78 | 79 | x += offsets[0]; 80 | y += offsets[1]; 81 | 82 | //From what I have seen, spurious readings (saturation?) seem to show up as very large values 83 | // in the thousands... most normal readings seem to be in the -250..250 range. By filtering 84 | // out any readings outside of |500| we should eliminate most (all?) of the spurious readings. 85 | if (x >= -500 && x <= 500 && y >= -500 && y <= 500){ 86 | filtered[0] = ALPHA * x + (1 - ALPHA) * filtered[0]; 87 | filtered[1] = ALPHA * y + (1 - ALPHA) * filtered[1]; 88 | } 89 | } 90 | 91 | double magnetometer_read_heading(){ 92 | return normalize_angle(atan2(filtered[1], filtered[0]) + MAGNETOMETER_ORIENTATION_OFFSET); 93 | } 94 | #else 95 | 96 | void magnetometer_init(){} 97 | 98 | void magnetometer_get_offsets(int16_t *x, int16_t *y){} 99 | 100 | void magnetometer_set_offsets(int16_t x, int16_t y){} 101 | 102 | void magnetometer_get_raw(int16_t *x, int16_t *y){} 103 | 104 | double magnetometer_read_heading(){ return 0; } 105 | 106 | #endif -------------------------------------------------------------------------------- /avr/lib/universal_controller/client.h: -------------------------------------------------------------------------------- 1 | #ifndef UNIVERSAL_CONTROLLER_CLIENT_H 2 | #define UNIVERSAL_CONTROLLER_CLIENT_H 3 | 4 | //Generic messages are in 0x0X space... 5 | #define MESSAGE_ANNOUNCE_CONTROL_ID 0x00 6 | #define MESSAGE_SEND_ACKNOWLEDGE 0x01 7 | #define MESSAGE_SEND_COMPLETE 0x02 8 | #define MESSAGE_REQUEST_ENABLE_DEBUG 0x03 9 | #define MESSAGE_REQUEST_DISABLE_DEBUG 0x04 10 | #define MESSAGE_SEND_DEBUG 0x05 11 | #define MESSAGE_REQUEST_BATTERY 0x06 12 | #define MESSAGE_SEND_BATTERY 0x07 13 | 14 | //Universal Controller messages are in 0x1X space... 15 | #define MESSAGE_UC_BUTTON_PUSH 0x10 16 | #define MESSAGE_UC_BUTTON_RELEASE 0x11 17 | #define MESSAGE_UC_BUTTON_NONE 0x12 18 | #define MESSAGE_UC_JOYSTICK_MOVE 0x13 19 | #define MESSAGE_UC_BUTTON_ENABLE 0x14 20 | #define MESSAGE_UC_BUTTON_DISABLE 0x15 21 | #define MESSAGE_UC_JOYSTICK_ENABLE 0x16 22 | #define MESSAGE_UC_JOYSTICK_DISABLE 0x17 23 | #define MESSAGE_UC_SET_POLL_FREQUENCY 0x18 24 | #define MESSAGE_UC_SET_ANALOG_FREQUENCY 0x19 25 | 26 | 27 | 28 | //Bit 8: 0 == analog stick message, 1 == digital button message 29 | #define CONTROLLER_MESSAGE_TYPE_MASK 0x80 30 | #define CONTROLLER_MESSAGE_TYPE_ANALOG 0x00 31 | #define CONTROLLER_MESSAGE_TYPE_BUTTON 0x80 32 | 33 | 34 | //Bit 7 (Analog): Left / Right stick 35 | #define CONTROLLER_ANALOG_STICK 0x40 36 | #define CONTROLLER_ANALOG_STICK_LEFT 0x00 37 | #define CONTROLLER_ANALOG_STICK_RIGHT 0x40 38 | 39 | //Bit 6 (Analog): X / Y axis 40 | #define CONTROLLER_ANALOG_AXIS 0x20 41 | #define CONTROLLER_ANALOG_AXIS_X 0x00 42 | #define CONTROLLER_ANALOG_AXIS_Y 0x20 43 | 44 | //Bit 5::1 (Analog): Value 45 | #define CONTROLLER_ANALOG_VALUE_MASK 0x1F 46 | 47 | //No Buttons Pressed message 48 | #define CONTROLLER_BUTTON_NONE 0xC0 49 | 50 | //Bit 5 (Button): Press / Release 51 | #define CONTROLLER_BUTTON_PRESS_MASK 0x10 52 | #define CONTROLLER_BUTTON_RELEASE 0x00 53 | #define CONTROLLER_BUTTON_PRESS 0x10 54 | 55 | //Bit 4::1 (Button): Button address 56 | #define CONTROLLER_BUTTON_VALUE_MASK 0x0F 57 | 58 | #define CONTROLLER_BUTTON_VALUE_SELECT 0x00 59 | #define CONTROLLER_BUTTON_VALUE_LEFT3 0x01 60 | #define CONTROLLER_BUTTON_VALUE_RIGHT3 0x02 61 | #define CONTROLLER_BUTTON_VALUE_START 0x03 62 | #define CONTROLLER_BUTTON_VALUE_PADUP 0x04 63 | #define CONTROLLER_BUTTON_VALUE_PADRIGHT 0x05 64 | #define CONTROLLER_BUTTON_VALUE_PADDOWN 0x06 65 | #define CONTROLLER_BUTTON_VALUE_PADLEFT 0x07 66 | #define CONTROLLER_BUTTON_VALUE_LEFT2 0x08 67 | #define CONTROLLER_BUTTON_VALUE_RIGHT2 0x09 68 | #define CONTROLLER_BUTTON_VALUE_LEFT1 0x0A 69 | #define CONTROLLER_BUTTON_VALUE_RIGHT1 0x0B 70 | #define CONTROLLER_BUTTON_VALUE_TRIANGLE 0x0C 71 | #define CONTROLLER_BUTTON_VALUE_CIRCLE 0x0D 72 | #define CONTROLLER_BUTTON_VALUE_CROSS 0x0E 73 | #define CONTROLLER_BUTTON_VALUE_SQUARE 0x0F 74 | 75 | #include 76 | 77 | typedef struct uc_stick_t { 78 | int16_t x; 79 | int16_t y; 80 | } uc_stick_t; 81 | 82 | void uc_init(); 83 | 84 | /* 85 | * Read all buttons which have been pressed since the last 86 | * call to uc_read_pressed_buttons(). 87 | */ 88 | uint16_t uc_read_pressed_buttons(); 89 | 90 | /* 91 | * Read all buttons which have been released since the last 92 | * call to uc_read_released_buttons(); 93 | */ 94 | uint16_t uc_read_released_buttons(); 95 | 96 | /* 97 | * Read all buttons which are currently being held. 98 | */ 99 | uint16_t uc_read_held_buttons(); 100 | 101 | /* 102 | * Read the left stick (x and y). 0 is neutral, negative 103 | * numbers are left / down, positive numbers are right / up. 104 | */ 105 | uc_stick_t uc_read_left(); 106 | 107 | /* 108 | * Read the right stick (x and y). 0 is neutral, negative 109 | * numbers are left / down, positive numbers are right / up. 110 | */ 111 | uc_stick_t uc_read_right(); 112 | 113 | #endif -------------------------------------------------------------------------------- /kicad/rev1/gerber/hex.txt: -------------------------------------------------------------------------------- 1 | M48 2 | ;DRILL file {Pcbnew (2013-dec-23)-stable} date Thu 03 Jul 2014 07:49:17 AM MDT 3 | ;FORMAT={-:-/ absolute / inch / decimal} 4 | FMAT,2 5 | INCH,TZ 6 | T1C0.020 7 | T2C0.028 8 | T3C0.032 9 | T4C0.040 10 | T5C0.050 11 | T6C0.120 12 | % 13 | G90 14 | G05 15 | M72 16 | T1 17 | X1.4917Y-2.1589 18 | X1.6854Y-1.8876 19 | X2.0317Y-2.3739 20 | X2.1842Y-1.5389 21 | X2.3077Y-1.9459 22 | X2.4217Y-1.6314 23 | X2.465Y-0.918 24 | X2.4917Y-1.5614 25 | X2.4987Y-2.3309 26 | X2.5117Y-1.4414 27 | X2.6792Y-1.3264 28 | X2.6967Y-1.2039 29 | X2.7067Y-0.9439 30 | X2.7692Y-1.0064 31 | X2.8892Y-0.8314 32 | X2.9807Y-1.0619 33 | X3.1092Y-1.0514 34 | X3.1567Y-1.3389 35 | T2 36 | X1.4217Y-1.5589 37 | X1.4717Y-1.5589 38 | X1.5217Y-1.5589 39 | X1.5717Y-1.5589 40 | T3 41 | X1.4446Y-1.3646 42 | X1.4728Y-2.0285 43 | X1.5154Y-1.4354 44 | X1.5435Y-1.9578 45 | X1.6142Y-1.8871 46 | X1.6849Y-1.8164 47 | X1.7557Y-1.7457 48 | X1.7746Y-2.0354 49 | X1.8264Y-1.675 50 | X1.8454Y-1.9646 51 | X1.8971Y-1.6043 52 | X1.8971Y-2.4528 53 | X1.9678Y-1.5335 54 | X1.9678Y-2.3821 55 | X2.0385Y-1.4628 56 | X2.0385Y-2.3114 57 | X2.1092Y-1.3921 58 | X2.1092Y-2.2406 59 | X2.1799Y-1.3214 60 | X2.1799Y-2.1699 61 | X2.2506Y-1.2507 62 | X2.2506Y-2.0992 63 | X2.3117Y-2.3919 64 | X2.3213Y-1.18 65 | X2.3213Y-2.0285 66 | X2.392Y-1.1093 67 | X2.392Y-1.9578 68 | X2.4117Y-2.3919 69 | X2.4628Y-1.0386 70 | X2.4628Y-1.8871 71 | X2.5335Y-0.9679 72 | X2.5335Y-1.8164 73 | X2.6042Y-0.8971 74 | X2.6042Y-1.7457 75 | X2.6749Y-0.8264 76 | X2.6749Y-1.675 77 | X2.7456Y-0.7557 78 | X2.7456Y-1.6043 79 | X2.8163Y-0.685 80 | X2.8163Y-1.5335 81 | X2.887Y-1.4628 82 | X2.9577Y-1.3921 83 | X3.0284Y-1.3214 84 | X3.0992Y-1.2507 85 | X3.1699Y-1.18 86 | X3.1867Y-1.4739 87 | X3.1867Y-1.5739 88 | X3.19Y-1.66 89 | X3.2406Y-1.1093 90 | X3.2867Y-1.5739 91 | X3.29Y-1.66 92 | T4 93 | X1.4206Y-2.3735 94 | X1.4913Y-2.3028 95 | X1.4913Y-2.4443 96 | X1.562Y-2.3735 97 | X1.562Y-2.515 98 | X1.6328Y-2.4443 99 | X1.866Y-1.4296 100 | X1.9367Y-1.3589 101 | X1.9867Y-1.8353 102 | X2.0074Y-1.2882 103 | X2.0574Y-1.7646 104 | X2.1281Y-1.6939 105 | X2.1989Y-1.6232 106 | X2.2696Y-1.5525 107 | X2.3217Y-0.6539 108 | X2.3403Y-1.4818 109 | X2.3403Y-2.1889 110 | X2.411Y-1.4111 111 | X2.411Y-2.1182 112 | X2.4217Y-0.6539 113 | X2.4817Y-1.3403 114 | X2.4817Y-2.0475 115 | X2.5524Y-1.2696 116 | X2.5524Y-1.9767 117 | X2.6231Y-1.1989 118 | X2.6231Y-1.906 119 | X2.6938Y-1.8353 120 | X2.7645Y-1.7646 121 | X2.7999Y-2.5067 122 | X2.8352Y-1.6939 123 | X2.8706Y-2.436 124 | X2.906Y-1.6232 125 | X2.9413Y-2.3653 126 | X2.9767Y-1.5525 127 | X3.012Y-2.2946 128 | X3.0706Y-0.7085 129 | X3.0827Y-2.2239 130 | X3.1413Y-0.6378 131 | X3.1413Y-0.7793 132 | X3.1534Y-2.1532 133 | X3.212Y-0.7085 134 | X3.212Y-0.85 135 | X3.2241Y-2.0825 136 | X3.2828Y-0.7793 137 | X3.2948Y-2.0118 138 | T5 139 | X1.5046Y-1.2643 140 | X1.5753Y-1.1935 141 | X1.5753Y-1.335 142 | X1.646Y-1.1228 143 | X1.646Y-1.2643 144 | X1.646Y-1.4057 145 | X1.7166Y-1.0523 146 | X1.7167Y-1.1935 147 | X1.7167Y-1.335 148 | X1.7873Y-0.9815 149 | X1.7873Y-1.123 150 | X1.7874Y-1.2643 151 | X1.858Y-0.9108 152 | X1.858Y-1.0523 153 | X1.858Y-1.1937 154 | X1.9286Y-0.8403 155 | X1.9287Y-0.9815 156 | X1.9287Y-1.123 157 | X1.9993Y-0.7695 158 | X1.9993Y-0.911 159 | X1.9994Y-1.0523 160 | X2.07Y-0.6988 161 | X2.07Y-0.8403 162 | X2.07Y-0.9817 163 | X2.1407Y-0.7695 164 | X2.1407Y-0.911 165 | X2.2114Y-0.8403 166 | X2.515Y-2.2485 167 | X2.5857Y-2.1778 168 | X2.5857Y-2.3193 169 | X2.6564Y-2.1071 170 | X2.6564Y-2.2485 171 | X2.6564Y-2.39 172 | X2.727Y-2.0365 173 | X2.7271Y-2.1778 174 | X2.7271Y-2.3193 175 | X2.7977Y-1.9658 176 | X2.7977Y-2.1073 177 | X2.7978Y-2.2485 178 | X2.8684Y-1.8951 179 | X2.8684Y-2.0365 180 | X2.8684Y-2.178 181 | X2.939Y-1.8245 182 | X2.9391Y-1.9658 183 | X2.9391Y-2.1073 184 | X3.0097Y-1.7538 185 | X3.0097Y-1.8953 186 | X3.0098Y-2.0365 187 | X3.0804Y-1.6831 188 | X3.0804Y-1.8245 189 | X3.0804Y-1.966 190 | X3.1511Y-1.7538 191 | X3.1511Y-1.8953 192 | X3.2218Y-1.8245 193 | T6 194 | X1.5354Y-0.748 195 | X3.189Y-2.4016 196 | T0 197 | M30 198 | -------------------------------------------------------------------------------- /kicad/rev2/gerber/hex-B_Mask.gbs: -------------------------------------------------------------------------------- 1 | G04 (created by PCBNEW (2013-dec-23)-stable) date Thu 04 Sep 2014 08:14:04 PM MDT* 2 | %MOIN*% 3 | G04 Gerber Fmt 3.4, Leading zero omitted, Abs format* 4 | %FSLAX34Y34*% 5 | G01* 6 | G70* 7 | G90* 8 | G04 APERTURE LIST* 9 | %ADD10C,0.00590551*% 10 | %ADD11C,0.08*% 11 | %ADD12C,0.06*% 12 | %ADD13R,0.065X0.065*% 13 | %ADD14C,0.065*% 14 | %ADD15R,0.06X0.06*% 15 | %ADD16C,0.16*% 16 | %ADD17C,0.075*% 17 | %ADD18R,0.055X0.055*% 18 | %ADD19C,0.055*% 19 | %ADD20C,0.03*% 20 | G04 APERTURE END LIST* 21 | G54D10* 22 | G54D11* 23 | X19500Y-6500D03* 24 | X19500Y-7500D03* 25 | G54D12* 26 | X22000Y-20750D03* 27 | X23000Y-20750D03* 28 | X24000Y-20750D03* 29 | X25000Y-20750D03* 30 | X21000Y-20750D03* 31 | X20000Y-20750D03* 32 | X19000Y-20750D03* 33 | X18000Y-20750D03* 34 | X15000Y-20750D03* 35 | X16000Y-20750D03* 36 | X17000Y-20750D03* 37 | G54D13* 38 | X16787Y-6250D03* 39 | G54D14* 40 | X15212Y-6250D03* 41 | G54D12* 42 | X18500Y-13000D03* 43 | X18500Y-14000D03* 44 | X18500Y-15000D03* 45 | X18500Y-16000D03* 46 | X18500Y-17000D03* 47 | X18500Y-18000D03* 48 | X22500Y-13000D03* 49 | X21500Y-13000D03* 50 | X20500Y-13000D03* 51 | G54D13* 52 | X30712Y-6250D03* 53 | G54D14* 54 | X32287Y-6250D03* 55 | G54D15* 56 | X27750Y-24000D03* 57 | G54D12* 58 | X28750Y-24000D03* 59 | X29750Y-24000D03* 60 | X30750Y-24000D03* 61 | X31750Y-24000D03* 62 | X32750Y-24000D03* 63 | G54D15* 64 | X24956Y-22750D03* 65 | G54D12* 66 | X25744Y-22750D03* 67 | X26531Y-22750D03* 68 | X27318Y-22750D03* 69 | X28106Y-22750D03* 70 | X28893Y-22750D03* 71 | X29681Y-22750D03* 72 | X30468Y-22750D03* 73 | X31255Y-22750D03* 74 | X32043Y-22750D03* 75 | X14750Y-24500D03* 76 | X15750Y-24500D03* 77 | G54D15* 78 | X17250Y-24500D03* 79 | G54D12* 80 | X18250Y-24500D03* 81 | X19250Y-24500D03* 82 | X20250Y-24500D03* 83 | X21250Y-24500D03* 84 | X22250Y-24500D03* 85 | G54D15* 86 | X14500Y-23500D03* 87 | G54D16* 88 | X32250Y-19000D03* 89 | X15250Y-19000D03* 90 | G54D12* 91 | X21000Y-7750D03* 92 | X21000Y-6750D03* 93 | X22000Y-7750D03* 94 | X22000Y-6750D03* 95 | X23000Y-7750D03* 96 | X23000Y-6750D03* 97 | G54D17* 98 | X17250Y-13750D03* 99 | X17250Y-12750D03* 100 | X17250Y-11750D03* 101 | X16250Y-13750D03* 102 | X16250Y-12750D03* 103 | X16250Y-11750D03* 104 | X15250Y-13750D03* 105 | X15250Y-12750D03* 106 | X15250Y-11750D03* 107 | X17250Y-10250D03* 108 | X17250Y-9250D03* 109 | X17250Y-8250D03* 110 | X16250Y-10250D03* 111 | X16250Y-9250D03* 112 | X16250Y-8250D03* 113 | X15250Y-10250D03* 114 | X15250Y-9250D03* 115 | X15250Y-8250D03* 116 | X17250Y-17250D03* 117 | X17250Y-16250D03* 118 | X17250Y-15250D03* 119 | X16250Y-17250D03* 120 | X16250Y-16250D03* 121 | X16250Y-15250D03* 122 | X15250Y-17250D03* 123 | X15250Y-16250D03* 124 | X15250Y-15250D03* 125 | X30250Y-8250D03* 126 | X30250Y-9250D03* 127 | X30250Y-10250D03* 128 | X31250Y-8250D03* 129 | X31250Y-9250D03* 130 | X31250Y-10250D03* 131 | X32250Y-8250D03* 132 | X32250Y-9250D03* 133 | X32250Y-10250D03* 134 | X30250Y-11750D03* 135 | X30250Y-12750D03* 136 | X30250Y-13750D03* 137 | X31250Y-11750D03* 138 | X31250Y-12750D03* 139 | X31250Y-13750D03* 140 | X32250Y-11750D03* 141 | X32250Y-12750D03* 142 | X32250Y-13750D03* 143 | X30250Y-15250D03* 144 | X30250Y-16250D03* 145 | X30250Y-17250D03* 146 | X31250Y-15250D03* 147 | X31250Y-16250D03* 148 | X31250Y-17250D03* 149 | X32250Y-15250D03* 150 | X32250Y-16250D03* 151 | X32250Y-17250D03* 152 | G54D12* 153 | X30750Y-19750D03* 154 | X30750Y-20750D03* 155 | X29750Y-19750D03* 156 | X29750Y-20750D03* 157 | X28750Y-19750D03* 158 | X28750Y-20750D03* 159 | G54D18* 160 | X20500Y-9750D03* 161 | G54D19* 162 | X19500Y-9750D03* 163 | X19500Y-10750D03* 164 | G54D12* 165 | X27957Y-18542D03* 166 | X26542Y-19957D03* 167 | X27250Y-19250D03* 168 | X24500Y-12000D03* 169 | X25500Y-12000D03* 170 | G54D15* 171 | X24750Y-7000D03* 172 | G54D12* 173 | X25750Y-7000D03* 174 | G54D20* 175 | X31000Y-21750D03* 176 | X31500Y-21750D03* 177 | X32000Y-21750D03* 178 | X25500Y-21750D03* 179 | X26000Y-21750D03* 180 | X26500Y-21750D03* 181 | X21250Y-21750D03* 182 | X21750Y-21750D03* 183 | X22250Y-21750D03* 184 | X15500Y-21750D03* 185 | X16000Y-21750D03* 186 | X16500Y-21750D03* 187 | M02* 188 | -------------------------------------------------------------------------------- /doc/protocol.txt: -------------------------------------------------------------------------------- 1 | Protocol Info 2 | ------------- 3 | 4 | Message Structure (bi-directional): 5 | Segment Length 6 | --------------------------------- 7 | Frame start 1 byte (0x7e) 8 | Length 1 byte 9 | Command 1 byte 10 | Payload 0 - 254 bytes 11 | Checksum 1 byte 12 | 13 | Commands: 14 | --------- 15 | There are four logical message types: 16 | 1) PC initiates a message to Stubby 17 | 2) Stubby responds to a message from PC 18 | 3) Stubby initiates a message to PC 19 | 4) PC responds to a message from Stubby 20 | 21 | Message types 1 and 2 comprise the majority of communication. Message 22 | types 3 and 4 imply an event-driven programming model. Initially, we 23 | do not want to do this as it adds complexity to the new programmer, 24 | however we do not want to preclude this from being added at a later 25 | date. 26 | 27 | Commands have a variable payload. A description of payloads is below. 28 | Portions in [square brackets] are optional. 29 | 30 | Command Direction Payload Response 31 | ------------------------------------------------------------------------------------------------------------------------------------------------------------- 32 | -- General commands 33 | AnnounceControlId Control -> Stubby Controller ID; UC = 0x55 ('U'), Processing = 0x50 ('P'), Calibration = 'C', etc None 34 | SendAcknowledge Stubby -> Control Command being ack'd (1 byte) None 35 | SendComplete Stubby -> Control Command completed (1 byte) None 36 | 37 | RequestEnableDebug Control -> Stubby Null Ack 38 | RequestDisableDebug Control -> Stubby Null Ack 39 | SendDebug Stubby -> Control Variable depending on message, max 254 bytes None 40 | 41 | RequestBattery Control -> Stubby Null Battery message 42 | SendBattery Stubby -> Control 1 byte None 43 | 44 | -- Universal Controller commands 45 | UCButtonPush Control -> Stubby Index of button which was just pressed (1 byte) None 46 | UCButtonRelease Control -> Stubby Index of button which was just released (1 byte) None 47 | UCJoystickMove Control -> Stubby Value of all joystick axis (4 bytes, order LX:LY:RX:RY) None 48 | 49 | -- Processing API commands 50 | RequestPowerOn Control -> Stubby Null Ack 51 | RequestPowerOff Control -> Stubby Null Ack 52 | 53 | RequestMove Control -> Stubby Angle (1 byte), Velocity (1 byte), Distance (2 bytes) Ack + Complete when done 54 | RequestTurn Control -> Stubby Angle (1 bytes), Velocity (1 byte) Ack + Complete when done 55 | RequestTranslate Control -> Stubby Amount (3 bytes, order X, Y, Z) Ack 56 | RequestRotate Control -> Stubby Axis (1 byte), Angle (1 byte) Ack 57 | 58 | RequestHeading Control -> Stubby Null Heading message 59 | SendHeading Stubby -> Control Angle (1 byte) None 60 | 61 | RequestDistance Control -> Stubby Null Distance message 62 | SendDistance Stubby -> Control Distance (2 bytes, MSB first) None 63 | 64 | RequestOptical Control -> Stubby Null Optical message 65 | SendOptical Stubby -> Control 1 byte / sensor (TODO: How many sensors?) None 66 | 67 | RequestSetLED Control -> Stubby 3 bytes (RGB value) Ack 68 | 69 | -- Calibration program commands 70 | RequestJointCalibration Control -> Stubby Null SendJointCalibration message 71 | SendJointCalibration Either -> Either 18 bytes, ordered LEG0_TIBIA, LEG0_FEMUR, LEG0_COXA, ... LEG5_COXA None 72 | 73 | RequestFootCalibration Control -> Stubby Null SendFootCalibration message 74 | SendFootCalibration Either -> Either 18 bytes, ordered LEG0_X, LEG0_Y, LEG0_Z, LEG1_X, ... LEG5_Z None 75 | 76 | RequestMagnetometerCalibration Control -> Stubby Null SendMagnetometerCalibration message 77 | SendMagnetometerCalibration Either -> Either 4 bytes (two 16 bit signed integers, MSB first): X offset, Y offset None 78 | StartMagnetometerCalibration Control -> Stubby Null Repeated SendMagnetometerCalibration messages -------------------------------------------------------------------------------- /avr/lib/universal_controller/client.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "../serial/serial.h" 4 | 5 | #include "client.h" 6 | 7 | //For X axis, 0 is neutral, negative numbers are left, positive are right. 8 | //For Y axis, 0 is neutral, negative numbers are down (back), positive are up (forward). 9 | static volatile uc_stick_t left_stick; 10 | static volatile uc_stick_t right_stick; 11 | static volatile uint16_t _pressed = 0x00; //Bit mask of any buttons pressed. Bits are cleared when read. 12 | static volatile uint16_t _released = 0x00; //Bit mask of any buttons released. Bits are cleared when read. 13 | static volatile uint16_t _held = 0x00; //Bit mask of any buttons held. Bits are cleared when released (in ISR). 14 | 15 | void uc_init(){ 16 | serial_init_b(38400); 17 | } 18 | 19 | void uc_configure_remote(){ 20 | //Every few seconds we will send these controller init messages. 21 | static uint8_t controller_init = 0x00; //Whenever this resets to 0, we re-send controler init messages 22 | controller_init++; 23 | if (controller_init >= 0xFF){ 24 | //NOTE: You should change these values depending on your application. These defaults make sense for most projects, though. 25 | serial_write_b(0x41); //Enable analog sticks 26 | serial_write_b(0xFF); //Set an analog repeat time of about 32ms. 27 | controller_init = 0x00; 28 | } 29 | } 30 | 31 | uint16_t uc_read_pressed_buttons(){ 32 | uc_configure_remote(); 33 | 34 | if (_pressed == 0x00) return 0x00; 35 | 36 | uint16_t result = _pressed; 37 | _pressed = 0x00; 38 | return result; 39 | } 40 | 41 | uint16_t uc_read_released_buttons(){ 42 | uc_configure_remote(); 43 | 44 | if (_released == 0x00) return 0x00; 45 | 46 | uint16_t result = _released; 47 | _released = 0x00; 48 | return result; 49 | } 50 | 51 | uint16_t uc_read_held_buttons(){ 52 | uc_configure_remote(); 53 | 54 | return _held; 55 | } 56 | 57 | uc_stick_t uc_read_left(){ 58 | uc_stick_t result; 59 | result.x = left_stick.x; 60 | result.y = left_stick.y; 61 | return result; 62 | } 63 | uc_stick_t uc_read_right(){ 64 | uc_stick_t result; 65 | result.x = right_stick.x; 66 | result.y = right_stick.y; 67 | return result; 68 | } 69 | 70 | void _serial_init_rx(){ 71 | //Enable RX interrupts 72 | UCSR0B |= _BV(RXCIE0); 73 | 74 | //Enable interrupts 75 | sei(); 76 | } 77 | 78 | ISR(USART0_RX_vect){ 79 | uint8_t b = UDR0; 80 | 81 | UCSR0B &= ~_BV(RXCIE0); //Disable RX interrupts; we want PWM interrupts, but not RX interrupts. 82 | sei(); //We want PWM to continue uninterrupted while processing serial data 83 | 84 | if ((b & CONTROLLER_MESSAGE_TYPE_MASK) == CONTROLLER_MESSAGE_TYPE_ANALOG){ //Analog stick event 85 | //The PSX controller sends a 5 bit message: 0x00 for all the way left / forward, 0x1F for all the 86 | // way right / back. We want to offset this so that neutral is 0, so we subtract 15 from the raw 87 | // value; for the Y axis, we negate the value to flip the orientation. 88 | uint8_t value = b & CONTROLLER_ANALOG_VALUE_MASK; 89 | 90 | if ((b & CONTROLLER_ANALOG_STICK) == CONTROLLER_ANALOG_STICK_LEFT){ //Left stick 91 | if ((b & CONTROLLER_ANALOG_AXIS) == CONTROLLER_ANALOG_AXIS_X){ //X Axis 92 | left_stick.x = value - 15; 93 | } 94 | else if ((b & CONTROLLER_ANALOG_AXIS) == CONTROLLER_ANALOG_AXIS_Y){ //Y Axis 95 | left_stick.y = 15 - value; 96 | } 97 | } 98 | else { //Right stick 99 | if ((b & CONTROLLER_ANALOG_AXIS) == CONTROLLER_ANALOG_AXIS_X){ //X Axis 100 | right_stick.x = value - 15; 101 | } 102 | else if ((b & CONTROLLER_ANALOG_AXIS) == CONTROLLER_ANALOG_AXIS_Y){ //Y Axis 103 | right_stick.y = 15 - value; 104 | } 105 | } 106 | } 107 | else { //Digital button event 108 | uint16_t value = _BV((b & CONTROLLER_BUTTON_VALUE_MASK)); 109 | if ((b & CONTROLLER_BUTTON_PRESS_MASK) == CONTROLLER_BUTTON_PRESS){ //Pressed 110 | _pressed |= value; //Mark as pressed; this is cleared when they are read. 111 | _held |= value; //Mark as held; this is cleared when released 112 | } 113 | else { //Released 114 | _released |= value; //Mark as released; this is cleared when they are read. 115 | _held &= ~value; //Mark as no longer held 116 | } 117 | } 118 | 119 | //Re-enable RX interrupts 120 | UCSR0B |= _BV(RXCIE0); 121 | } 122 | -------------------------------------------------------------------------------- /kicad/rev1/hex.mod: -------------------------------------------------------------------------------- 1 | PCBNEW-LibModule-V1 Sun 27 Apr 2014 07:19:08 PM MDT 2 | # encoding utf-8 3 | Units mm 4 | $INDEX 5 | HEX_LEG 6 | HEX_LEG_REVERSE 7 | R2 8 | $EndINDEX 9 | $MODULE HEX_LEG 10 | Po 0 0 0 15 534FF5BE 00000000 ~~ 11 | Li HEX_LEG 12 | Sc 0 13 | AR 14 | Op 0 0 0 15 | T0 -7.112 -1.524 1 1 900 0.15 N I 21 N "HEX_LEG" 16 | T1 -4.064 -2.032 0.762 0.762 900 0.15 N V 21 N "VAL**" 17 | DS -1.905 -3.175 3.175 -3.175 0.15 21 18 | DS -1.905 -0.635 3.175 -0.635 0.15 21 19 | T2 5.588 0.508 0.762 0.762 900 0.127 N V 21 N "TIB" 20 | T2 5.588 -2.032 0.762 0.762 900 0.127 N V 21 N "FEM" 21 | T2 5.588 -4.572 0.762 0.762 900 0.127 N V 21 N "COX" 22 | $PAD 23 | Sh "1" C 1.905 1.905 0 0 2700 24 | Dr 1.27 0 0 25 | At STD N 00E0FFFF 26 | Ne 0 "" 27 | Po -1.905 -4.445 28 | $EndPAD 29 | $PAD 30 | Sh "2" C 1.905 1.905 0 0 2700 31 | Dr 1.27 0 0 32 | At STD N 00E0FFFF 33 | Ne 0 "" 34 | Po -1.905 -1.905 35 | $EndPAD 36 | $PAD 37 | Sh "3" C 1.905 1.905 0 0 2700 38 | Dr 1.27 0 0 39 | At STD N 00E0FFFF 40 | Ne 0 "" 41 | Po -1.905 0.635 42 | $EndPAD 43 | $PAD 44 | Sh "4" C 1.905 1.905 0 0 2700 45 | Dr 1.27 0 0 46 | At STD N 00E0FFFF 47 | Ne 0 "" 48 | Po 0.635 -4.445 49 | $EndPAD 50 | $PAD 51 | Sh "5" C 1.905 1.905 0 0 2700 52 | Dr 1.27 0 0 53 | At STD N 00E0FFFF 54 | Ne 0 "" 55 | Po 0.635 -1.905 56 | $EndPAD 57 | $PAD 58 | Sh "6" C 1.905 1.905 0 0 2700 59 | Dr 1.27 0 0 60 | At STD N 00E0FFFF 61 | Ne 0 "" 62 | Po 0.635 0.635 63 | $EndPAD 64 | $PAD 65 | Sh "7" C 1.905 1.905 0 0 2700 66 | Dr 1.27 0 0 67 | At STD N 00E0FFFF 68 | Ne 0 "" 69 | Po 3.175 -4.445 70 | $EndPAD 71 | $PAD 72 | Sh "8" C 1.905 1.905 0 0 2700 73 | Dr 1.27 0 0 74 | At STD N 00E0FFFF 75 | Ne 0 "" 76 | Po 3.175 -1.905 77 | $EndPAD 78 | $PAD 79 | Sh "9" C 1.905 1.905 0 0 2700 80 | Dr 1.27 0 0 81 | At STD N 00E0FFFF 82 | Ne 0 "" 83 | Po 3.175 0.635 84 | $EndPAD 85 | $EndMODULE HEX_LEG 86 | $MODULE HEX_LEG_REVERSE 87 | Po 0 0 0 15 534FF5F4 00000000 ~~ 88 | Li HEX_LEG_REVERSE 89 | Sc 0 90 | AR 91 | Op 0 0 0 92 | T0 -8 -3 1 1 900 0.15 N I 21 N "HEX_LEG_REVERSE" 93 | T1 -4 -2 0.762 0.762 900 0.15 N V 21 N "VAL**" 94 | DS 3.175 -0.635 -1.905 -0.635 0.15 21 95 | DS -1.905 -3.175 3.175 -3.175 0.15 21 96 | T2 5.588 -4.572 0.762 0.762 900 0.127 N V 21 N "TIB" 97 | T2 5.588 -2.032 0.762 0.762 900 0.127 N V 21 N "FEM" 98 | T2 5.588 0.508 0.762 0.762 900 0.127 N V 21 N "COX" 99 | $PAD 100 | Sh "1" C 1.905 1.905 0 0 2700 101 | Dr 1.27 0 0 102 | At STD N 00E0FFFF 103 | Ne 0 "" 104 | Po -1.905 -4.445 105 | $EndPAD 106 | $PAD 107 | Sh "2" C 1.905 1.905 0 0 2700 108 | Dr 1.27 0 0 109 | At STD N 00E0FFFF 110 | Ne 0 "" 111 | Po -1.905 -1.905 112 | $EndPAD 113 | $PAD 114 | Sh "3" C 1.905 1.905 0 0 2700 115 | Dr 1.27 0 0 116 | At STD N 00E0FFFF 117 | Ne 0 "" 118 | Po -1.905 0.635 119 | $EndPAD 120 | $PAD 121 | Sh "4" C 1.905 1.905 0 0 2700 122 | Dr 1.27 0 0 123 | At STD N 00E0FFFF 124 | Ne 0 "" 125 | Po 0.635 -4.445 126 | $EndPAD 127 | $PAD 128 | Sh "5" C 1.905 1.905 0 0 2700 129 | Dr 1.27 0 0 130 | At STD N 00E0FFFF 131 | Ne 0 "" 132 | Po 0.635 -1.905 133 | $EndPAD 134 | $PAD 135 | Sh "6" C 1.905 1.905 0 0 2700 136 | Dr 1.27 0 0 137 | At STD N 00E0FFFF 138 | Ne 0 "" 139 | Po 0.635 0.635 140 | $EndPAD 141 | $PAD 142 | Sh "7" C 1.905 1.905 0 0 2700 143 | Dr 1.27 0 0 144 | At STD N 00E0FFFF 145 | Ne 0 "" 146 | Po 3.175 -4.445 147 | $EndPAD 148 | $PAD 149 | Sh "8" C 1.905 1.905 0 0 2700 150 | Dr 1.27 0 0 151 | At STD N 00E0FFFF 152 | Ne 0 "" 153 | Po 3.175 -1.905 154 | $EndPAD 155 | $PAD 156 | Sh "9" C 1.905 1.905 0 0 2700 157 | Dr 1.27 0 0 158 | At STD N 00E0FFFF 159 | Ne 0 "" 160 | Po 3.175 0.635 161 | $EndPAD 162 | $EndMODULE HEX_LEG_REVERSE 163 | $MODULE R2 164 | Po 0 0 0 15 533996E9 00000000 ~~ 165 | Li R2 166 | Sc 0 167 | AR 168 | Op 0 0 0 169 | T0 0 -1.27 0.508 0.508 0 0.127 N V 21 N "R2" 170 | T1 0 0 0.508 0.508 0 0.127 N V 21 N "VAL**" 171 | DS 1.27 0 1.905 0 0.15 21 172 | DS -1.27 0 -1.905 0 0.15 21 173 | DS -1.27 -0.635 1.27 -0.635 0.15 21 174 | DS 1.27 -0.635 1.27 0.635 0.15 21 175 | DS 1.27 0.635 -1.27 0.635 0.15 21 176 | DS -1.27 0.635 -1.27 -0.635 0.15 21 177 | $PAD 178 | Sh "1" C 1.524 1.524 0 0 0 179 | Dr 1.016 0 0 180 | At STD N 00E0FFFF 181 | Ne 0 "" 182 | Po -2.54 0 183 | $EndPAD 184 | $PAD 185 | Sh "2" C 1.524 1.524 0 0 0 186 | Dr 1.016 0 0 187 | At STD N 00E0FFFF 188 | Ne 0 "" 189 | Po 2.54 0 190 | $EndPAD 191 | $EndMODULE R2 192 | $EndLIBRARY 193 | -------------------------------------------------------------------------------- /kicad/rev2/hex.mod: -------------------------------------------------------------------------------- 1 | PCBNEW-LibModule-V1 Sun 27 Apr 2014 07:19:08 PM MDT 2 | # encoding utf-8 3 | Units mm 4 | $INDEX 5 | HEX_LEG 6 | HEX_LEG_REVERSE 7 | R2 8 | $EndINDEX 9 | $MODULE HEX_LEG 10 | Po 0 0 0 15 534FF5BE 00000000 ~~ 11 | Li HEX_LEG 12 | Sc 0 13 | AR 14 | Op 0 0 0 15 | T0 -7.112 -1.524 1 1 900 0.15 N I 21 N "HEX_LEG" 16 | T1 -4.064 -2.032 0.762 0.762 900 0.15 N V 21 N "VAL**" 17 | DS -1.905 -3.175 3.175 -3.175 0.15 21 18 | DS -1.905 -0.635 3.175 -0.635 0.15 21 19 | T2 5.588 0.508 0.762 0.762 900 0.127 N V 21 N "TIB" 20 | T2 5.588 -2.032 0.762 0.762 900 0.127 N V 21 N "FEM" 21 | T2 5.588 -4.572 0.762 0.762 900 0.127 N V 21 N "COX" 22 | $PAD 23 | Sh "1" C 1.905 1.905 0 0 2700 24 | Dr 1.27 0 0 25 | At STD N 00E0FFFF 26 | Ne 0 "" 27 | Po -1.905 -4.445 28 | $EndPAD 29 | $PAD 30 | Sh "2" C 1.905 1.905 0 0 2700 31 | Dr 1.27 0 0 32 | At STD N 00E0FFFF 33 | Ne 0 "" 34 | Po -1.905 -1.905 35 | $EndPAD 36 | $PAD 37 | Sh "3" C 1.905 1.905 0 0 2700 38 | Dr 1.27 0 0 39 | At STD N 00E0FFFF 40 | Ne 0 "" 41 | Po -1.905 0.635 42 | $EndPAD 43 | $PAD 44 | Sh "4" C 1.905 1.905 0 0 2700 45 | Dr 1.27 0 0 46 | At STD N 00E0FFFF 47 | Ne 0 "" 48 | Po 0.635 -4.445 49 | $EndPAD 50 | $PAD 51 | Sh "5" C 1.905 1.905 0 0 2700 52 | Dr 1.27 0 0 53 | At STD N 00E0FFFF 54 | Ne 0 "" 55 | Po 0.635 -1.905 56 | $EndPAD 57 | $PAD 58 | Sh "6" C 1.905 1.905 0 0 2700 59 | Dr 1.27 0 0 60 | At STD N 00E0FFFF 61 | Ne 0 "" 62 | Po 0.635 0.635 63 | $EndPAD 64 | $PAD 65 | Sh "7" C 1.905 1.905 0 0 2700 66 | Dr 1.27 0 0 67 | At STD N 00E0FFFF 68 | Ne 0 "" 69 | Po 3.175 -4.445 70 | $EndPAD 71 | $PAD 72 | Sh "8" C 1.905 1.905 0 0 2700 73 | Dr 1.27 0 0 74 | At STD N 00E0FFFF 75 | Ne 0 "" 76 | Po 3.175 -1.905 77 | $EndPAD 78 | $PAD 79 | Sh "9" C 1.905 1.905 0 0 2700 80 | Dr 1.27 0 0 81 | At STD N 00E0FFFF 82 | Ne 0 "" 83 | Po 3.175 0.635 84 | $EndPAD 85 | $EndMODULE HEX_LEG 86 | $MODULE HEX_LEG_REVERSE 87 | Po 0 0 0 15 534FF5F4 00000000 ~~ 88 | Li HEX_LEG_REVERSE 89 | Sc 0 90 | AR 91 | Op 0 0 0 92 | T0 -8 -3 1 1 900 0.15 N I 21 N "HEX_LEG_REVERSE" 93 | T1 -4 -2 0.762 0.762 900 0.15 N V 21 N "VAL**" 94 | DS 3.175 -0.635 -1.905 -0.635 0.15 21 95 | DS -1.905 -3.175 3.175 -3.175 0.15 21 96 | T2 5.588 -4.572 0.762 0.762 900 0.127 N V 21 N "TIB" 97 | T2 5.588 -2.032 0.762 0.762 900 0.127 N V 21 N "FEM" 98 | T2 5.588 0.508 0.762 0.762 900 0.127 N V 21 N "COX" 99 | $PAD 100 | Sh "1" C 1.905 1.905 0 0 2700 101 | Dr 1.27 0 0 102 | At STD N 00E0FFFF 103 | Ne 0 "" 104 | Po -1.905 -4.445 105 | $EndPAD 106 | $PAD 107 | Sh "2" C 1.905 1.905 0 0 2700 108 | Dr 1.27 0 0 109 | At STD N 00E0FFFF 110 | Ne 0 "" 111 | Po -1.905 -1.905 112 | $EndPAD 113 | $PAD 114 | Sh "3" C 1.905 1.905 0 0 2700 115 | Dr 1.27 0 0 116 | At STD N 00E0FFFF 117 | Ne 0 "" 118 | Po -1.905 0.635 119 | $EndPAD 120 | $PAD 121 | Sh "4" C 1.905 1.905 0 0 2700 122 | Dr 1.27 0 0 123 | At STD N 00E0FFFF 124 | Ne 0 "" 125 | Po 0.635 -4.445 126 | $EndPAD 127 | $PAD 128 | Sh "5" C 1.905 1.905 0 0 2700 129 | Dr 1.27 0 0 130 | At STD N 00E0FFFF 131 | Ne 0 "" 132 | Po 0.635 -1.905 133 | $EndPAD 134 | $PAD 135 | Sh "6" C 1.905 1.905 0 0 2700 136 | Dr 1.27 0 0 137 | At STD N 00E0FFFF 138 | Ne 0 "" 139 | Po 0.635 0.635 140 | $EndPAD 141 | $PAD 142 | Sh "7" C 1.905 1.905 0 0 2700 143 | Dr 1.27 0 0 144 | At STD N 00E0FFFF 145 | Ne 0 "" 146 | Po 3.175 -4.445 147 | $EndPAD 148 | $PAD 149 | Sh "8" C 1.905 1.905 0 0 2700 150 | Dr 1.27 0 0 151 | At STD N 00E0FFFF 152 | Ne 0 "" 153 | Po 3.175 -1.905 154 | $EndPAD 155 | $PAD 156 | Sh "9" C 1.905 1.905 0 0 2700 157 | Dr 1.27 0 0 158 | At STD N 00E0FFFF 159 | Ne 0 "" 160 | Po 3.175 0.635 161 | $EndPAD 162 | $EndMODULE HEX_LEG_REVERSE 163 | $MODULE R2 164 | Po 0 0 0 15 533996E9 00000000 ~~ 165 | Li R2 166 | Sc 0 167 | AR 168 | Op 0 0 0 169 | T0 0 -1.27 0.508 0.508 0 0.127 N V 21 N "R2" 170 | T1 0 0 0.508 0.508 0 0.127 N V 21 N "VAL**" 171 | DS 1.27 0 1.905 0 0.15 21 172 | DS -1.27 0 -1.905 0 0.15 21 173 | DS -1.27 -0.635 1.27 -0.635 0.15 21 174 | DS 1.27 -0.635 1.27 0.635 0.15 21 175 | DS 1.27 0.635 -1.27 0.635 0.15 21 176 | DS -1.27 0.635 -1.27 -0.635 0.15 21 177 | $PAD 178 | Sh "1" C 1.524 1.524 0 0 0 179 | Dr 1.016 0 0 180 | At STD N 00E0FFFF 181 | Ne 0 "" 182 | Po -2.54 0 183 | $EndPAD 184 | $PAD 185 | Sh "2" C 1.524 1.524 0 0 0 186 | Dr 1.016 0 0 187 | At STD N 00E0FFFF 188 | Ne 0 "" 189 | Po 2.54 0 190 | $EndPAD 191 | $EndMODULE R2 192 | $EndLIBRARY 193 | -------------------------------------------------------------------------------- /kicad/rev2.1/stubby.mod: -------------------------------------------------------------------------------- 1 | PCBNEW-LibModule-V1 Sun 27 Apr 2014 07:19:08 PM MDT 2 | # encoding utf-8 3 | Units mm 4 | $INDEX 5 | HEX_LEG 6 | HEX_LEG_REVERSE 7 | R2 8 | $EndINDEX 9 | $MODULE HEX_LEG 10 | Po 0 0 0 15 534FF5BE 00000000 ~~ 11 | Li HEX_LEG 12 | Sc 0 13 | AR 14 | Op 0 0 0 15 | T0 -7.112 -1.524 1 1 900 0.15 N I 21 N "HEX_LEG" 16 | T1 -4.064 -2.032 0.762 0.762 900 0.15 N V 21 N "VAL**" 17 | DS -1.905 -3.175 3.175 -3.175 0.15 21 18 | DS -1.905 -0.635 3.175 -0.635 0.15 21 19 | T2 5.588 0.508 0.762 0.762 900 0.127 N V 21 N "TIB" 20 | T2 5.588 -2.032 0.762 0.762 900 0.127 N V 21 N "FEM" 21 | T2 5.588 -4.572 0.762 0.762 900 0.127 N V 21 N "COX" 22 | $PAD 23 | Sh "1" C 1.905 1.905 0 0 2700 24 | Dr 1.27 0 0 25 | At STD N 00E0FFFF 26 | Ne 0 "" 27 | Po -1.905 -4.445 28 | $EndPAD 29 | $PAD 30 | Sh "2" C 1.905 1.905 0 0 2700 31 | Dr 1.27 0 0 32 | At STD N 00E0FFFF 33 | Ne 0 "" 34 | Po -1.905 -1.905 35 | $EndPAD 36 | $PAD 37 | Sh "3" C 1.905 1.905 0 0 2700 38 | Dr 1.27 0 0 39 | At STD N 00E0FFFF 40 | Ne 0 "" 41 | Po -1.905 0.635 42 | $EndPAD 43 | $PAD 44 | Sh "4" C 1.905 1.905 0 0 2700 45 | Dr 1.27 0 0 46 | At STD N 00E0FFFF 47 | Ne 0 "" 48 | Po 0.635 -4.445 49 | $EndPAD 50 | $PAD 51 | Sh "5" C 1.905 1.905 0 0 2700 52 | Dr 1.27 0 0 53 | At STD N 00E0FFFF 54 | Ne 0 "" 55 | Po 0.635 -1.905 56 | $EndPAD 57 | $PAD 58 | Sh "6" C 1.905 1.905 0 0 2700 59 | Dr 1.27 0 0 60 | At STD N 00E0FFFF 61 | Ne 0 "" 62 | Po 0.635 0.635 63 | $EndPAD 64 | $PAD 65 | Sh "7" C 1.905 1.905 0 0 2700 66 | Dr 1.27 0 0 67 | At STD N 00E0FFFF 68 | Ne 0 "" 69 | Po 3.175 -4.445 70 | $EndPAD 71 | $PAD 72 | Sh "8" C 1.905 1.905 0 0 2700 73 | Dr 1.27 0 0 74 | At STD N 00E0FFFF 75 | Ne 0 "" 76 | Po 3.175 -1.905 77 | $EndPAD 78 | $PAD 79 | Sh "9" C 1.905 1.905 0 0 2700 80 | Dr 1.27 0 0 81 | At STD N 00E0FFFF 82 | Ne 0 "" 83 | Po 3.175 0.635 84 | $EndPAD 85 | $EndMODULE HEX_LEG 86 | $MODULE HEX_LEG_REVERSE 87 | Po 0 0 0 15 534FF5F4 00000000 ~~ 88 | Li HEX_LEG_REVERSE 89 | Sc 0 90 | AR 91 | Op 0 0 0 92 | T0 -8 -3 1 1 900 0.15 N I 21 N "HEX_LEG_REVERSE" 93 | T1 -4 -2 0.762 0.762 900 0.15 N V 21 N "VAL**" 94 | DS 3.175 -0.635 -1.905 -0.635 0.15 21 95 | DS -1.905 -3.175 3.175 -3.175 0.15 21 96 | T2 5.588 -4.572 0.762 0.762 900 0.127 N V 21 N "TIB" 97 | T2 5.588 -2.032 0.762 0.762 900 0.127 N V 21 N "FEM" 98 | T2 5.588 0.508 0.762 0.762 900 0.127 N V 21 N "COX" 99 | $PAD 100 | Sh "1" C 1.905 1.905 0 0 2700 101 | Dr 1.27 0 0 102 | At STD N 00E0FFFF 103 | Ne 0 "" 104 | Po -1.905 -4.445 105 | $EndPAD 106 | $PAD 107 | Sh "2" C 1.905 1.905 0 0 2700 108 | Dr 1.27 0 0 109 | At STD N 00E0FFFF 110 | Ne 0 "" 111 | Po -1.905 -1.905 112 | $EndPAD 113 | $PAD 114 | Sh "3" C 1.905 1.905 0 0 2700 115 | Dr 1.27 0 0 116 | At STD N 00E0FFFF 117 | Ne 0 "" 118 | Po -1.905 0.635 119 | $EndPAD 120 | $PAD 121 | Sh "4" C 1.905 1.905 0 0 2700 122 | Dr 1.27 0 0 123 | At STD N 00E0FFFF 124 | Ne 0 "" 125 | Po 0.635 -4.445 126 | $EndPAD 127 | $PAD 128 | Sh "5" C 1.905 1.905 0 0 2700 129 | Dr 1.27 0 0 130 | At STD N 00E0FFFF 131 | Ne 0 "" 132 | Po 0.635 -1.905 133 | $EndPAD 134 | $PAD 135 | Sh "6" C 1.905 1.905 0 0 2700 136 | Dr 1.27 0 0 137 | At STD N 00E0FFFF 138 | Ne 0 "" 139 | Po 0.635 0.635 140 | $EndPAD 141 | $PAD 142 | Sh "7" C 1.905 1.905 0 0 2700 143 | Dr 1.27 0 0 144 | At STD N 00E0FFFF 145 | Ne 0 "" 146 | Po 3.175 -4.445 147 | $EndPAD 148 | $PAD 149 | Sh "8" C 1.905 1.905 0 0 2700 150 | Dr 1.27 0 0 151 | At STD N 00E0FFFF 152 | Ne 0 "" 153 | Po 3.175 -1.905 154 | $EndPAD 155 | $PAD 156 | Sh "9" C 1.905 1.905 0 0 2700 157 | Dr 1.27 0 0 158 | At STD N 00E0FFFF 159 | Ne 0 "" 160 | Po 3.175 0.635 161 | $EndPAD 162 | $EndMODULE HEX_LEG_REVERSE 163 | $MODULE R2 164 | Po 0 0 0 15 533996E9 00000000 ~~ 165 | Li R2 166 | Sc 0 167 | AR 168 | Op 0 0 0 169 | T0 0 -1.27 0.508 0.508 0 0.127 N V 21 N "R2" 170 | T1 0 0 0.508 0.508 0 0.127 N V 21 N "VAL**" 171 | DS 1.27 0 1.905 0 0.15 21 172 | DS -1.27 0 -1.905 0 0.15 21 173 | DS -1.27 -0.635 1.27 -0.635 0.15 21 174 | DS 1.27 -0.635 1.27 0.635 0.15 21 175 | DS 1.27 0.635 -1.27 0.635 0.15 21 176 | DS -1.27 0.635 -1.27 -0.635 0.15 21 177 | $PAD 178 | Sh "1" C 1.524 1.524 0 0 0 179 | Dr 1.016 0 0 180 | At STD N 00E0FFFF 181 | Ne 0 "" 182 | Po -2.54 0 183 | $EndPAD 184 | $PAD 185 | Sh "2" C 1.524 1.524 0 0 0 186 | Dr 1.016 0 0 187 | At STD N 00E0FFFF 188 | Ne 0 "" 189 | Po 2.54 0 190 | $EndPAD 191 | $EndMODULE R2 192 | $EndLIBRARY 193 | -------------------------------------------------------------------------------- /avr/Makefile: -------------------------------------------------------------------------------- 1 | # Copied from variables.mk 2 | OS=$(shell uname -s) 3 | 4 | AVRDUDE=avrdude 5 | OBJCOPY=avr-objcopy 6 | OBJDUMP=avr-objdump 7 | AVRSIZE=avr-size 8 | 9 | CFLAGS=-mmcu=$(MMCU) -pedantic -Os -Wall 10 | LFLAGS=-Wl,-u,vfprintf -lprintf_flt -lc 11 | 12 | # End of variables.mk 13 | 14 | # Most user configurable options are in this file. 15 | include hardware.mk 16 | 17 | ifndef AVRDUDE_PROGRAMMER 18 | AVRDUDE_PROGRAMMER=usbtiny 19 | endif 20 | 21 | ifneq 'usbtiny' '$(AVRDUDE_PROGRAMMER)' 22 | ifndef AVRDUDE_PORT 23 | ifeq ($(OS),Linux) 24 | AVRDUDE_PORT=/dev/ttyUSB0 25 | else 26 | $(error Please define AVRDUDE_PORT in hardware.mk) 27 | endif 28 | endif 29 | endif 30 | 31 | ifndef PCB_REVISION 32 | PCB_REVISION=2 33 | endif 34 | 35 | ifndef MAGNETOMETER 36 | MAGNETOMETER=1 37 | ifndef MAGNETOMETER_ORIENTATION_OFFSET 38 | MAGNETOMETER_ORIENTATION_OFFSET=3.1415926 39 | endif 40 | endif 41 | 42 | ifndef DISTANCE_SENSOR 43 | DISTANCE_SENSOR=1 44 | endif 45 | 46 | ifndef F_CPU 47 | ifeq '2' '$(PCB_REVISION)' 48 | F_CPU=20000000 49 | else 50 | F_CPU=12000000 51 | endif 52 | endif 53 | 54 | ifndef TWI_FREQ 55 | TWI_FREQ=400000L 56 | endif 57 | 58 | 59 | CDEFS=-DF_CPU=$(F_CPU) -lc -lm -DTWI_FREQ=$(TWI_FREQ) -DPCB_REVISION=$(PCB_REVISION) -DMAGNETOMETER=$(MAGNETOMETER) -DMAGNETOMETER_ORIENTATION_OFFSET=$(MAGNETOMETER_ORIENTATION_OFFSET) -DDISTANCE_SENSOR=$(DISTANCE_SENSOR) -DSERIAL_BUFFER_SIZE=255 -DPWM_MAX_PINS=21 60 | PROJECT=stubby 61 | MMCU=atmega1284p 62 | COMPILER=avr-g++ 63 | SOURCES=Stubby.cpp Leg.cpp \ 64 | controllers/calibration.c controllers/processing.c controllers/universal_controller.c \ 65 | gait/gait_tripod.c \ 66 | hardware/battery.c hardware/distance_HCSR04.c hardware/magnetometer_HMC5883L.c hardware/servo.c hardware/status.c hardware/timer2.c \ 67 | types/Point.cpp \ 68 | util/convert.c util/delays.c util/math.c \ 69 | lib/protocol/protocol.c lib/protocol/protocol_serial.c lib/pwm/pwm.c lib/pwm/pwm.S lib/serial/serial.c lib/serial/serial_async_tx.c lib/twi/twi.c lib/Ring/Ring.cpp 70 | 71 | LFUSE=0xF7 72 | HFUSE=0xD1 73 | EFUSE=0xFE 74 | 75 | 76 | # Copied from targets.mk 77 | 78 | ifndef COMPILER 79 | COMPILER=avr-gcc 80 | endif 81 | ifeq 'avr-gcc' '$(COMPILER)' 82 | CFLAGS += -std=gnu99 83 | endif 84 | 85 | ifeq 'stk500v1' '$(AVRDUDE_PROGRAMMER)' 86 | ifeq ($(OS),Linux) 87 | AVRDUDE_PREP_COMMANDS=stty -F $(AVRDUDE_PORT) hupcl 88 | endif 89 | AVRDUDE_ARGS += -P $(AVRDUDE_PORT) -b 19200 90 | endif 91 | #We use the arduino programmer to mean 'Arduino Uno with Optiboot'; this is stk500v1 compatible at 115200 baud. 92 | ifeq 'arduino' '$(AVRDUDE_PROGRAMMER)' 93 | ifeq ($(OS),Linux) 94 | AVRDUDE_PREP_COMMANDS=stty -F $(AVRDUDE_PORT) hupcl 95 | endif 96 | AVRDUDE_ARGS += -P $(AVRDUDE_PORT) -b 115200 97 | endif 98 | 99 | ifeq 'stk500v2' '$(AVRDUDE_PROGRAMMER)' 100 | AVRDUDE_ARGS += -P $(AVRDUDE_PORT) -b 115200 101 | endif 102 | 103 | ifeq 'usbtiny' '$(AVRDUDE_PROGRAMMER)' 104 | AVRDUDE_SPEED = -B 1 105 | endif 106 | 107 | #If an EFUSE variable has been set, we program the extended fuses too 108 | ifeq '' '$(EFUSE)' 109 | EXTENDED_FUSE_WRITE= 110 | else 111 | EXTENDED_FUSE_WRITE=-U efuse:w:$(EFUSE):m 112 | endif 113 | 114 | # Default target. 115 | all: clean build 116 | 117 | build: $(PROJECT).hex 118 | 119 | assembly: $(PROJECT).asm 120 | 121 | $(PROJECT).hex: $(PROJECT).out 122 | $(OBJCOPY) -j .text -j .data -O ihex $(PROJECT).out $(PROJECT).hex 123 | $(AVRSIZE) -d -C --mcu=$(MMCU) $(PROJECT).out 124 | @rm -f $(PROJECT).out 125 | 126 | $(PROJECT).asm: $(PROJECT).out 127 | $(OBJDUMP) -C -d $(PROJECT).out 128 | 129 | $(PROJECT).out: $(SOURCES) 130 | $(COMPILER) $(CDEFS) $(CFLAGS) -I./ -o $(PROJECT).out $(CLIBS) $(SOURCES) $(LFLAGS) 131 | 132 | 133 | program: all 134 | $(AVRDUDE_PREP_COMMANDS) 135 | $(AVRDUDE) -F -p $(MMCU) -c $(AVRDUDE_PROGRAMMER) \ 136 | $(AVRDUDE_ARGS) $(AVRDUDE_SPEED)\ 137 | -U flash:w:$(PROJECT).hex 138 | 139 | fuse: 140 | $(AVRDUDE) -F -p $(MMCU) -c $(AVRDUDE_PROGRAMMER) \ 141 | $(AVRDUDE_ARGS) \ 142 | -U lfuse:w:$(LFUSE):m -U hfuse:w:$(HFUSE):m $(EXTENDED_FUSE_WRITE) 143 | 144 | readfuse: 145 | $(AVRDUDE) -V -F -p $(MMCU) -c $(AVRDUDE_PROGRAMMER) \ 146 | $(AVRDUDE_ARGS) \ 147 | -U lfuse:r:-:h -U hfuse:r:-:h -U efuse:r:-:h 148 | 149 | readeeprom: 150 | $(AVRDUDE) -V -F -p $(MMCU) -c $(AVRDUDE_PROGRAMMER) \ 151 | $(AVRDUDE_ARGS) \ 152 | -U eeprom:r:-:h 153 | 154 | clean: 155 | rm -f *.o 156 | rm -f $(PROJECT).hex 157 | 158 | #End of targets.mk 159 | -------------------------------------------------------------------------------- /python/controller.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import pygame 3 | import serial 4 | import time 5 | import os 6 | 7 | #Center the pygame window on screen 8 | os.environ['SDL_VIDEO_CENTERED'] = '1' 9 | 10 | ser = None 11 | 12 | MESSAGE_ANNOUNCE_CONTROL_ID = 0x00 13 | 14 | MESSAGE_UC_BUTTON_PUSH = 0x10 15 | MESSAGE_UC_BUTTON_RELEASE = 0x11 16 | MESSAGE_UC_BUTTON_NONE = 0x12 17 | MESSAGE_UC_JOYSTICK_MOVE = 0x13 18 | 19 | CONTROLLER_BUTTON_VALUE_SELECT = 0x00 20 | CONTROLLER_BUTTON_VALUE_LEFT3 = 0x01 21 | CONTROLLER_BUTTON_VALUE_RIGHT3 = 0x02 22 | CONTROLLER_BUTTON_VALUE_START = 0x03 23 | CONTROLLER_BUTTON_VALUE_PADUP = 0x04 24 | CONTROLLER_BUTTON_VALUE_PADRIGHT= 0x05 25 | CONTROLLER_BUTTON_VALUE_PADDOWN = 0x06 26 | CONTROLLER_BUTTON_VALUE_PADLEFT = 0x07 27 | CONTROLLER_BUTTON_VALUE_LEFT2 = 0x08 28 | CONTROLLER_BUTTON_VALUE_RIGHT2 = 0x09 29 | CONTROLLER_BUTTON_VALUE_LEFT1 = 0x0A 30 | CONTROLLER_BUTTON_VALUE_RIGHT1 = 0x0B 31 | CONTROLLER_BUTTON_VALUE_TRIANGLE= 0x0C 32 | CONTROLLER_BUTTON_VALUE_CIRCLE = 0x0D 33 | CONTROLLER_BUTTON_VALUE_CROSS = 0x0E 34 | CONTROLLER_BUTTON_VALUE_SQUARE = 0x0F 35 | 36 | def write(command, message): 37 | ''' 38 | Write the given command + message to the serial port. The command MUST be a single unsigned byte between 0 and 255. 39 | The message MUST be a list of numbers between 0x00 and 0xFF. 40 | ''' 41 | START = 0x7e 42 | ESCAPE = 0x7d 43 | 44 | def append_byte(data, b, escape=True): 45 | if (escape and (b == chr(START) or b == chr(ESCAPE))): 46 | data.append(chr(ESCAPE)) 47 | data.append(chr(b ^ 0x20)) 48 | else: 49 | data.append(chr(b)) 50 | 51 | data = [chr(START)] 52 | append_byte(data, len(message) + 1) 53 | append_byte(data, command) 54 | 55 | checksum = command 56 | for b in message: 57 | checksum = (checksum + b) & 0xFF 58 | data.append(chr(b)) 59 | 60 | append_byte(data, (0xFF - checksum)) 61 | 62 | print(data) 63 | for b in data: 64 | print(hex(ord(b))) 65 | ser.write(b) 66 | 67 | def keyboard_listener(): 68 | keys = {} 69 | 70 | while True: 71 | for event in pygame.event.get(): 72 | # determine if X was clicked, or Ctrl+W or Alt+F4 was used 73 | if event.type == pygame.QUIT: 74 | print("Quitting...") 75 | return 76 | 77 | if event.type == pygame.KEYDOWN or event.type == pygame.KEYUP: 78 | if event.key in [pygame.K_t, pygame.K_RCTRL, pygame.K_LCTRL]: 79 | if event.key == pygame.K_t: 80 | button_id = CONTROLLER_BUTTON_VALUE_START 81 | elif event.key == pygame.K_RCTRL: 82 | button_id = CONTROLLER_BUTTON_VALUE_RIGHT2 83 | elif event.key == pygame.K_LCTRL: 84 | button_id = CONTROLLER_BUTTON_VALUE_LEFT2 85 | 86 | if event.type == pygame.KEYDOWN: 87 | write(MESSAGE_UC_BUTTON_PUSH, [button_id]) 88 | else: 89 | write(MESSAGE_UC_BUTTON_RELEASE, [button_id]) 90 | 91 | 92 | if event.key in [pygame.K_LEFT, pygame.K_RIGHT, pygame.K_UP, pygame.K_DOWN, pygame.K_a, pygame.K_s, pygame.K_d, pygame.K_w]: 93 | if event.type == pygame.KEYDOWN: 94 | keys[event.key] = True 95 | else: 96 | del keys[event.key] 97 | 98 | if pygame.K_a in keys: 99 | left_x = 0 100 | elif pygame.K_d in keys: 101 | left_x = 255 102 | else: 103 | left_x = 128 104 | 105 | if pygame.K_w in keys: 106 | left_y = 0 107 | elif pygame.K_s in keys: 108 | left_y = 255 109 | else: 110 | left_y = 128 111 | 112 | if pygame.K_LEFT in keys: 113 | right_x = 0 114 | elif pygame.K_RIGHT in keys: 115 | right_x = 255 116 | else: 117 | right_x = 128 118 | 119 | if pygame.K_UP in keys: 120 | right_y = 0 121 | elif pygame.K_DOWN in keys: 122 | right_y = 255 123 | else: 124 | right_y = 128 125 | 126 | write(MESSAGE_ANNOUNCE_CONTROL_ID, [ord('U')]) 127 | 128 | analog = [left_x, left_y, right_x, right_y] 129 | write(MESSAGE_UC_JOYSTICK_MOVE, analog) 130 | print(keys) 131 | 132 | 133 | 134 | 135 | ########## Main startup hooks here ########## 136 | if (__name__=="__main__"): 137 | if (len(sys.argv) != 2): 138 | print("Usage: " + sys.argv[0] + " ") 139 | sys.exit(0) 140 | 141 | ser = serial.Serial(sys.argv[1], 38400, timeout=1) 142 | 143 | pygame.init() 144 | pygame.display.set_mode([320, 200]) 145 | pygame.key.set_repeat(1, 500) #No delay, 500ms interval 146 | 147 | write(MESSAGE_ANNOUNCE_CONTROL_ID, [ord('U')]) 148 | 149 | keyboard_listener() 150 | -------------------------------------------------------------------------------- /kicad/rev1/hex.cmp: -------------------------------------------------------------------------------- 1 | Cmp-Mod V01 Created by CvPcb (2013-dec-23)-stable date = Sun 01 Jun 2014 06:59:54 PM MDT 2 | 3 | BeginCmp 4 | TimeStamp = /532EDCAD; 5 | Reference = C1; 6 | ValeurCmp = 1uF; 7 | IdModule = SM0805; 8 | EndCmp 9 | 10 | BeginCmp 11 | TimeStamp = /53335945; 12 | Reference = C2; 13 | ValeurCmp = 1uF; 14 | IdModule = SM0805; 15 | EndCmp 16 | 17 | BeginCmp 18 | TimeStamp = /53335959; 19 | Reference = C3; 20 | ValeurCmp = 1uF; 21 | IdModule = SM0805; 22 | EndCmp 23 | 24 | BeginCmp 25 | TimeStamp = /5342F877; 26 | Reference = C4; 27 | ValeurCmp = 20pF; 28 | IdModule = SM0603_Capa; 29 | EndCmp 30 | 31 | BeginCmp 32 | TimeStamp = /5342F86B; 33 | Reference = C5; 34 | ValeurCmp = 20pF; 35 | IdModule = SM0603_Capa; 36 | EndCmp 37 | 38 | BeginCmp 39 | TimeStamp = /538BCD08; 40 | Reference = C6; 41 | ValeurCmp = 2200uF; 42 | IdModule = C1V5; 43 | EndCmp 44 | 45 | BeginCmp 46 | TimeStamp = /538BCD1F; 47 | Reference = C7; 48 | ValeurCmp = 470uF; 49 | IdModule = C1V5; 50 | EndCmp 51 | 52 | BeginCmp 53 | TimeStamp = /538BD17A; 54 | Reference = C8; 55 | ValeurCmp = 2200uF; 56 | IdModule = C1V5; 57 | EndCmp 58 | 59 | BeginCmp 60 | TimeStamp = /53483946; 61 | Reference = D1; 62 | ValeurCmp = LED; 63 | IdModule = LED-5MM; 64 | EndCmp 65 | 66 | BeginCmp 67 | TimeStamp = /535B2302; 68 | Reference = D2; 69 | ValeurCmp = RGB_LED; 70 | IdModule = LED_RGB; 71 | EndCmp 72 | 73 | BeginCmp 74 | TimeStamp = /532C95F4; 75 | Reference = IC1; 76 | ValeurCmp = ATMEGA1284P-P; 77 | IdModule = DIP-40__600; 78 | EndCmp 79 | 80 | BeginCmp 81 | TimeStamp = /532EDC04; 82 | Reference = K1; 83 | ValeurCmp = XTAL; 84 | IdModule = PIN_ARRAY_3X1; 85 | EndCmp 86 | 87 | BeginCmp 88 | TimeStamp = /532FA2B1; 89 | Reference = P1; 90 | ValeurCmp = PROG; 91 | IdModule = pin_array_3x2; 92 | EndCmp 93 | 94 | BeginCmp 95 | TimeStamp = /53337638; 96 | Reference = P2; 97 | ValeurCmp = BATT (4.8V-6V); 98 | IdModule = PIN_ARRAY_2X1; 99 | EndCmp 100 | 101 | BeginCmp 102 | TimeStamp = /5346FA4A; 103 | Reference = P3; 104 | ValeurCmp = CONN_8; 105 | IdModule = PIN_ARRAY_8x1; 106 | EndCmp 107 | 108 | BeginCmp 109 | TimeStamp = /532C9BB8; 110 | Reference = R1; 111 | ValeurCmp = 1k; 112 | IdModule = SM0805; 113 | EndCmp 114 | 115 | BeginCmp 116 | TimeStamp = /533780B6; 117 | Reference = R2; 118 | ValeurCmp = 1k; 119 | IdModule = SM0805; 120 | EndCmp 121 | 122 | BeginCmp 123 | TimeStamp = /533780C5; 124 | Reference = R3; 125 | ValeurCmp = 1k; 126 | IdModule = SM0805; 127 | EndCmp 128 | 129 | BeginCmp 130 | TimeStamp = /533781D4; 131 | Reference = R4; 132 | ValeurCmp = 1k; 133 | IdModule = SM0805; 134 | EndCmp 135 | 136 | BeginCmp 137 | TimeStamp = /533781DA; 138 | Reference = R5; 139 | ValeurCmp = 1k; 140 | IdModule = SM0805; 141 | EndCmp 142 | 143 | BeginCmp 144 | TimeStamp = /533781E0; 145 | Reference = R6; 146 | ValeurCmp = 1k; 147 | IdModule = SM0805; 148 | EndCmp 149 | 150 | BeginCmp 151 | TimeStamp = /533AEE5B; 152 | Reference = R7; 153 | ValeurCmp = 1k; 154 | IdModule = SM0805; 155 | EndCmp 156 | 157 | BeginCmp 158 | TimeStamp = /533AEE68; 159 | Reference = R8; 160 | ValeurCmp = 1k; 161 | IdModule = SM0805; 162 | EndCmp 163 | 164 | BeginCmp 165 | TimeStamp = /533C55EC; 166 | Reference = R9; 167 | ValeurCmp = 1k; 168 | IdModule = SM0805; 169 | EndCmp 170 | 171 | BeginCmp 172 | TimeStamp = /53483955; 173 | Reference = R10; 174 | ValeurCmp = 1k; 175 | IdModule = SM0805; 176 | EndCmp 177 | 178 | BeginCmp 179 | TimeStamp = /5333430A; 180 | Reference = S1; 181 | ValeurCmp = FRNT LFT; 182 | IdModule = HEX_LEG; 183 | EndCmp 184 | 185 | BeginCmp 186 | TimeStamp = /53334670; 187 | Reference = S2; 188 | ValeurCmp = FRNT RGT; 189 | IdModule = HEX_LEG_REVERSE; 190 | EndCmp 191 | 192 | BeginCmp 193 | TimeStamp = /5333467E; 194 | Reference = S3; 195 | ValeurCmp = MID LFT; 196 | IdModule = HEX_LEG; 197 | EndCmp 198 | 199 | BeginCmp 200 | TimeStamp = /5333468C; 201 | Reference = S4; 202 | ValeurCmp = MID RGT; 203 | IdModule = HEX_LEG_REVERSE; 204 | EndCmp 205 | 206 | BeginCmp 207 | TimeStamp = /5333469A; 208 | Reference = S5; 209 | ValeurCmp = REAR LFT; 210 | IdModule = HEX_LEG; 211 | EndCmp 212 | 213 | BeginCmp 214 | TimeStamp = /533346A8; 215 | Reference = S6; 216 | ValeurCmp = REAR RGT; 217 | IdModule = HEX_LEG_REVERSE; 218 | EndCmp 219 | 220 | BeginCmp 221 | TimeStamp = /533C3A10; 222 | Reference = SW1; 223 | ValeurCmp = DPDT; 224 | IdModule = DPDT; 225 | EndCmp 226 | 227 | BeginCmp 228 | TimeStamp = /535B0A48; 229 | Reference = VR1; 230 | ValeurCmp = MCP1702; 231 | IdModule = TO92; 232 | EndCmp 233 | 234 | BeginCmp 235 | TimeStamp = /5340FC50; 236 | Reference = X1; 237 | ValeurCmp = XBee (Sparkfun Breakout); 238 | IdModule = XBEE_SPARKFUN_BREAKOUT; 239 | EndCmp 240 | 241 | BeginCmp 242 | TimeStamp = /533C55FC; 243 | Reference = ZD1; 244 | ValeurCmp = ZD 3v3; 245 | IdModule = SOT23; 246 | EndCmp 247 | 248 | EndListe 249 | -------------------------------------------------------------------------------- /kicad/rev1/gerber/hex-B_Mask.gbs: -------------------------------------------------------------------------------- 1 | G04 (created by PCBNEW (2013-dec-23)-stable) date Thu 03 Jul 2014 07:48:38 AM MDT* 2 | %MOIN*% 3 | G04 Gerber Fmt 3.4, Leading zero omitted, Abs format* 4 | %FSLAX34Y34*% 5 | G01* 6 | G70* 7 | G90* 8 | G04 APERTURE LIST* 9 | %ADD10C,0.00590551*% 10 | %ADD11C,0.055*% 11 | %ADD12C,0.066*% 12 | %ADD13C,0.07*% 13 | %ADD14C,0.06*% 14 | %ADD15C,0.16*% 15 | %ADD16R,0.035X0.045*% 16 | %ADD17C,0.075*% 17 | %ADD18R,0.055X0.055*% 18 | G04 APERTURE END LIST* 19 | G54D10* 20 | G54D11* 21 | X32866Y-15738D03* 22 | X31866Y-15738D03* 23 | X31866Y-14738D03* 24 | G54D12* 25 | X24116Y-23918D03* 26 | X23116Y-23918D03* 27 | G54D11* 28 | X28163Y-6850D03* 29 | X27456Y-7557D03* 30 | X26748Y-8264D03* 31 | X26041Y-8971D03* 32 | X25334Y-9678D03* 33 | X24627Y-10385D03* 34 | X23920Y-11092D03* 35 | X23213Y-11799D03* 36 | X22506Y-12506D03* 37 | X21799Y-13214D03* 38 | X21092Y-13921D03* 39 | X20384Y-14628D03* 40 | X19677Y-15335D03* 41 | X18970Y-16042D03* 42 | X18263Y-16749D03* 43 | X17556Y-17456D03* 44 | X16849Y-18163D03* 45 | X16142Y-18870D03* 46 | X15435Y-19578D03* 47 | X14728Y-20285D03* 48 | X18970Y-24527D03* 49 | X19677Y-23820D03* 50 | X20384Y-23113D03* 51 | X21092Y-22406D03* 52 | X21799Y-21699D03* 53 | X22506Y-20992D03* 54 | X23213Y-20285D03* 55 | X23920Y-19578D03* 56 | X24627Y-18870D03* 57 | X25334Y-18163D03* 58 | X26041Y-17456D03* 59 | X26748Y-16749D03* 60 | X27456Y-16042D03* 61 | X28163Y-15335D03* 62 | X28870Y-14628D03* 63 | X29577Y-13921D03* 64 | X30284Y-13214D03* 65 | X30991Y-12506D03* 66 | X31698Y-11799D03* 67 | X32405Y-11092D03* 68 | G54D13* 69 | X24216Y-6538D03* 70 | X23216Y-6538D03* 71 | G54D14* 72 | X18659Y-14296D03* 73 | X19366Y-13588D03* 74 | X20074Y-12881D03* 75 | X27998Y-25067D03* 76 | X28705Y-24360D03* 77 | X29412Y-23653D03* 78 | X30119Y-22946D03* 79 | X30826Y-22238D03* 80 | X31534Y-21531D03* 81 | X32241Y-20824D03* 82 | X32948Y-20117D03* 83 | G54D15* 84 | X31889Y-24015D03* 85 | X15354Y-7480D03* 86 | G54D14* 87 | X30706Y-7085D03* 88 | X31413Y-6378D03* 89 | X31413Y-7792D03* 90 | X32120Y-7085D03* 91 | X32120Y-8499D03* 92 | X32827Y-7792D03* 93 | X16327Y-24442D03* 94 | X15620Y-25149D03* 95 | X15620Y-23735D03* 96 | X14913Y-24442D03* 97 | X14913Y-23028D03* 98 | X14206Y-23735D03* 99 | G54D16* 100 | X15716Y-15588D03* 101 | X15216Y-15588D03* 102 | X14716Y-15588D03* 103 | X14216Y-15588D03* 104 | G54D14* 105 | X19867Y-18353D03* 106 | X20574Y-17646D03* 107 | X21281Y-16938D03* 108 | X21988Y-16231D03* 109 | X22695Y-15524D03* 110 | X23402Y-14817D03* 111 | X24109Y-14110D03* 112 | X24816Y-13403D03* 113 | X25524Y-12696D03* 114 | X26231Y-11989D03* 115 | X29766Y-15524D03* 116 | X29059Y-16231D03* 117 | X28352Y-16938D03* 118 | X27645Y-17646D03* 119 | X26938Y-18353D03* 120 | X26231Y-19060D03* 121 | X25524Y-19767D03* 122 | X24816Y-20474D03* 123 | X24109Y-21181D03* 124 | X23402Y-21888D03* 125 | G54D10* 126 | G36* 127 | X19433Y-14229D02* 128 | X19751Y-14547D01* 129 | X19574Y-14724D01* 130 | X19256Y-14406D01* 131 | X19433Y-14229D01* 132 | X19433Y-14229D01* 133 | G37* 134 | G36* 135 | X19009Y-14653D02* 136 | X19327Y-14971D01* 137 | X19150Y-15148D01* 138 | X18832Y-14830D01* 139 | X19009Y-14653D01* 140 | X19009Y-14653D01* 141 | G37* 142 | G36* 143 | X20200Y-14098D02* 144 | X19882Y-13780D01* 145 | X20059Y-13603D01* 146 | X20377Y-13921D01* 147 | X20200Y-14098D01* 148 | X20200Y-14098D01* 149 | G37* 150 | G36* 151 | X20624Y-13674D02* 152 | X20306Y-13356D01* 153 | X20483Y-13179D01* 154 | X20801Y-13497D01* 155 | X20624Y-13674D01* 156 | X20624Y-13674D01* 157 | G37* 158 | G54D17* 159 | X18579Y-11936D03* 160 | X19286Y-11229D03* 161 | X19994Y-10522D03* 162 | X17872Y-11229D03* 163 | X18579Y-10522D03* 164 | X19286Y-9815D03* 165 | X17165Y-10522D03* 166 | X17872Y-9815D03* 167 | X18579Y-9108D03* 168 | X20699Y-9816D03* 169 | X21406Y-9109D03* 170 | X22114Y-8402D03* 171 | X19992Y-9109D03* 172 | X20699Y-8402D03* 173 | X21406Y-7695D03* 174 | X19285Y-8402D03* 175 | X19992Y-7695D03* 176 | X20699Y-6988D03* 177 | X16459Y-14056D03* 178 | X17166Y-13349D03* 179 | X17874Y-12642D03* 180 | X15752Y-13349D03* 181 | X16459Y-12642D03* 182 | X17166Y-11935D03* 183 | X15045Y-12642D03* 184 | X15752Y-11935D03* 185 | X16459Y-11228D03* 186 | X30804Y-16831D03* 187 | X30096Y-17538D03* 188 | X29389Y-18245D03* 189 | X31511Y-17538D03* 190 | X30804Y-18245D03* 191 | X30096Y-18952D03* 192 | X32218Y-18245D03* 193 | X31511Y-18952D03* 194 | X30804Y-19659D03* 195 | X28684Y-18951D03* 196 | X27976Y-19658D03* 197 | X27269Y-20365D03* 198 | X29391Y-19658D03* 199 | X28684Y-20365D03* 200 | X27976Y-21072D03* 201 | X30098Y-20365D03* 202 | X29391Y-21072D03* 203 | X28684Y-21779D03* 204 | X26564Y-21071D03* 205 | X25856Y-21778D03* 206 | X25149Y-22485D03* 207 | X27271Y-21778D03* 208 | X26564Y-22485D03* 209 | X25856Y-23192D03* 210 | X27978Y-22485D03* 211 | X27271Y-23192D03* 212 | X26564Y-23899D03* 213 | G54D18* 214 | X31900Y-16600D03* 215 | G54D11* 216 | X32900Y-16600D03* 217 | G54D10* 218 | G36* 219 | X18453Y-19257D02* 220 | X18842Y-19646D01* 221 | X18453Y-20035D01* 222 | X18064Y-19646D01* 223 | X18453Y-19257D01* 224 | X18453Y-19257D01* 225 | G37* 226 | G54D11* 227 | X17746Y-20353D03* 228 | G54D10* 229 | G36* 230 | X15542Y-14353D02* 231 | X15153Y-14742D01* 232 | X14764Y-14353D01* 233 | X15153Y-13964D01* 234 | X15542Y-14353D01* 235 | X15542Y-14353D01* 236 | G37* 237 | G54D11* 238 | X14446Y-13646D03* 239 | M02* 240 | -------------------------------------------------------------------------------- /avr/Stubby.cpp: -------------------------------------------------------------------------------- 1 | #include "Stubby.h" 2 | 3 | #include "controllers/calibration.h" 4 | #include "controllers/processing.h" 5 | #include "controllers/universal_controller.h" 6 | #include "gait/gait.h" 7 | #include "hardware/magnetometer.h" 8 | #include "hardware/servo.h" 9 | #include "hardware/status.h" 10 | #include "hardware/timer2.h" 11 | #include "types/Point.h" 12 | #include "util/math.h" 13 | 14 | #include "Leg.h" 15 | 16 | 17 | //Set up the leg objects, including servo details and mounting angle. Leg indices follows 18 | // // DIP numbering format, starting at front left and proceeding counter clockwise around. 19 | Leg legs[LEG_COUNT] = { 20 | #if PCB_REVISION == 1 21 | Leg(FRONT_LEFT, &PORTB, PORTB0, &PORTB, PORTB1, &PORTB, PORTB2, 2 * LEG_MOUNTING_ANGLE, Point(-60, 104, 0)), 22 | Leg(MIDDLE_LEFT, &PORTB, PORTB3, &PORTA, PORTA3, &PORTB, PORTB4, 3 * LEG_MOUNTING_ANGLE, Point(-120, 0, 0)), 23 | Leg(REAR_LEFT, &PORTD, PORTD2, &PORTD, PORTD3, &PORTC, PORTC4, 4 * LEG_MOUNTING_ANGLE, Point(-60, -104, 0)), 24 | Leg(REAR_RIGHT, &PORTC, PORTC3, &PORTC, PORTC2, &PORTD, PORTD7, 5 * LEG_MOUNTING_ANGLE, Point(60, -104, 0)), 25 | Leg(MIDDLE_RIGHT, &PORTC, PORTC7, &PORTC, PORTC6, &PORTC, PORTC5, 0 * LEG_MOUNTING_ANGLE, Point(120, 0, 0)), 26 | Leg(FRONT_RIGHT, &PORTA, PORTA4, &PORTA, PORTA5, &PORTA, PORTA6, 1 * LEG_MOUNTING_ANGLE, Point(60, 104, 0)) 27 | #elif PCB_REVISION == 2 28 | Leg(FRONT_LEFT, &PORTA, PORTA6, &PORTA, PORTA5, &PORTA, PORTA4, 2 * LEG_MOUNTING_ANGLE, Point(-60, 104, 0)), 29 | Leg(MIDDLE_LEFT, &PORTA, PORTA3, &PORTB, PORTB0, &PORTB, PORTB1, 3 * LEG_MOUNTING_ANGLE, Point(-120, 0, 0)), 30 | Leg(REAR_LEFT, &PORTB, PORTB2, &PORTB, PORTB3, &PORTB, PORTB4, 4 * LEG_MOUNTING_ANGLE, Point(-60, -104, 0)), 31 | Leg(REAR_RIGHT, &PORTD, PORTD4, &PORTD, PORTD3, &PORTD, PORTD2, 5 * LEG_MOUNTING_ANGLE, Point(60, -104, 0)), 32 | Leg(MIDDLE_RIGHT, &PORTD, PORTD7, &PORTD, PORTD6, &PORTD, PORTD5, 0 * LEG_MOUNTING_ANGLE, Point(120, 0, 0)), 33 | Leg(FRONT_RIGHT, &PORTC, PORTC4, &PORTC, PORTC3, &PORTC, PORTC2, 1 * LEG_MOUNTING_ANGLE, Point(60, 104, 0)) 34 | #else 35 | #error Unsupported PCB_REVISION value. 36 | #endif 37 | }; 38 | 39 | //Stubby state variables 40 | static volatile uint8_t power = 0x00; //0 is off, 1 is on 41 | volatile uint8_t controller = 0x00; //0 is no controller, 1 is Universal Controller, 2 is Processing API, 3 is Calibration API 42 | volatile uint8_t debug = 0x00; //0 is no debug, 1 is show debug 43 | volatile uint8_t pending_acknowledge = 0x00; //When set, we will send a message in the delay code 44 | volatile uint8_t pending_complete = 0x00; //When set, we will send a message in the delay code 45 | 46 | int main (void){ 47 | wdt_enable(WDTO_2S); 48 | 49 | servo_init(legs); 50 | serial_init_b(38400); 51 | battery_init(); 52 | magnetometer_init(); 53 | distance_init(); 54 | timer2_init(); 55 | 56 | while (1){ 57 | wdt_reset(); 58 | 59 | if (controller == CONTROLLER_UC){ 60 | uc_command_executor(); 61 | } 62 | else if (controller == CONTROLLER_PROCESSING){ 63 | processing_command_executor(); 64 | } 65 | else if (controller == CONTROLLER_CALIBRATION){ 66 | calibration_command_executor(); 67 | } 68 | 69 | delay_ms(10); 70 | } 71 | } 72 | 73 | uint8_t get_power(){ 74 | return power; 75 | } 76 | void set_power(uint8_t _power){ 77 | power = _power; 78 | } 79 | uint8_t get_controller(){ 80 | return controller; 81 | } 82 | 83 | void doAcknowledgeCommand(uint8_t command){ 84 | pending_acknowledge = command; 85 | } 86 | 87 | void doCompleteCommand(uint8_t command){ 88 | pending_complete = command; 89 | } 90 | 91 | void doSendDebug(char* message){ 92 | if (debug){ 93 | protocol_send_message(MESSAGE_SEND_DEBUG, (uint8_t*) message, strlen(message)); 94 | } 95 | } 96 | 97 | void doResetLegs(){ 98 | for (uint8_t l = 0; l < LEG_COUNT; l+=2){ 99 | legs[l].setOffset(Point(0,0,30)); 100 | } 101 | pwm_apply_batch(); 102 | delay_ms(200); 103 | 104 | for (uint8_t l = 0; l < LEG_COUNT; l+=2){ 105 | legs[l].setOffset(Point(0,0,0)); 106 | } 107 | pwm_apply_batch(); 108 | delay_ms(200); 109 | 110 | for (uint8_t l = 1; l < LEG_COUNT; l+=2){ 111 | legs[l].setOffset(Point(0,0,30)); 112 | } 113 | pwm_apply_batch(); 114 | delay_ms(200); 115 | 116 | for (uint8_t l = 1; l < LEG_COUNT; l+=2){ 117 | legs[l].setOffset(Point(0,0,0)); 118 | } 119 | pwm_apply_batch(); 120 | delay_ms(200); 121 | } 122 | 123 | /* 124 | * Called from ISR; keep things as short as possible. 125 | */ 126 | void protocol_dispatch_message(uint8_t cmd, uint8_t *message, uint8_t length){ 127 | if ((cmd& 0xF0) == 0x00){ 128 | //General messages; do nothing here. 129 | } 130 | else if ((cmd& 0xF0) == 0x10){ 131 | controller = CONTROLLER_UC; 132 | } 133 | else if ((cmd& 0xF0) == 0x20){ 134 | controller = CONTROLLER_PROCESSING; 135 | } 136 | else if ((cmd& 0xF0) == 0x30){ 137 | controller = CONTROLLER_CALIBRATION; 138 | } 139 | //TODO Put any other supported control modes here. 140 | else { 141 | controller = CONTROLLER_NONE; 142 | } 143 | 144 | if (cmd == MESSAGE_REQUEST_ENABLE_DEBUG){ 145 | debug = 0x01; 146 | doAcknowledgeCommand(MESSAGE_REQUEST_ENABLE_DEBUG); 147 | } 148 | else if (cmd == MESSAGE_REQUEST_DISABLE_DEBUG){ 149 | debug = 0x00; 150 | doAcknowledgeCommand(MESSAGE_REQUEST_DISABLE_DEBUG); 151 | } 152 | //This is a Universal Controller message (namespace 0x1X) 153 | else if (controller == CONTROLLER_UC && (cmd & 0xF0) == 0x10){ 154 | uc_dispatch_message(cmd, message, length); 155 | } 156 | //This is a Processing API message (namespace 0x2X) 157 | else if (controller == CONTROLLER_PROCESSING && (cmd & 0xF0) == 0x20){ 158 | processing_dispatch_message(cmd, message, length); 159 | } 160 | //This is a Calibration API message (namespace 0x3X) 161 | else if (controller == CONTROLLER_CALIBRATION && (cmd & 0xF0) == 0x30){ 162 | calibration_dispatch_message(cmd, message, length); 163 | } 164 | else { 165 | //TODO Send debug message 'unknown command' or similar 166 | } 167 | } -------------------------------------------------------------------------------- /avr/lib/twi/twi.h: -------------------------------------------------------------------------------- 1 | /* 2 | twi.h - TWI/I2C library for Wiring & Arduino 3 | Copyright (c) 2006 Nicholas Zambetti. All right reserved. 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version. 9 | 10 | This library is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | Lesser General Public License for more details. 14 | 15 | You should have received a copy of the GNU Lesser General Public 16 | License along with this library; if not, write to the Free Software 17 | Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 18 | */ 19 | 20 | //You can set the following defines (either in code or in the Makefile) to modify the behaviour of the system: 21 | //TWI_FREQ Set the TWI frequency, in Hz. Defaults to 400000L (400KHz) 22 | //TWI_BUFFER_LENGTH Maximum buffer length, in bytes; even needed if you provide custom buffers. Defaults to 32 23 | //TWI_DISABLE_MASTER Completely disable master mode 24 | //TWI_DISABLE_SLAVE Completely disable slave mode 25 | //TWI_DISABLE_SLAVE_RX Disable RX in slave mode 26 | //TWI_DISABLE_SLAVE_TX Disable TX in slave mode 27 | //TWI_CUSTOM_BUFFERS Set a custom buffer for master, slave rx, and slave tx. See twi_set_*_buffer 28 | //TWI_SLAVE_RX_READER Set a reader function to handle each byte as it comes in. See twi_attach_slave_rx_reader 29 | //TWI_MASTER_RX_READER Set a reader function to handle each byte as it comes in. See twi_attach_master_rx_reader 30 | //TWI_SLAVE_TX_WRITER Set a writer function to supply each byte as it is sent. See twi_attach_slave_tx_writer 31 | //TWI_MASTER_TX_WRITER Set a writer function to supply each byte as it is sent. See twi_attach_slave_tx_writer 32 | 33 | 34 | #ifndef twi_h 35 | #define twi_h 36 | 37 | #include 38 | 39 | #ifndef TWI_FREQ 40 | #define TWI_FREQ 400000L 41 | #endif 42 | 43 | #ifndef TWI_BUFFER_LENGTH 44 | #define TWI_BUFFER_LENGTH 32 45 | #endif 46 | 47 | //Constants used for read / write methods: send / no send stop bit, 48 | // block on function / no block on function 49 | #define TWI_STOP 1 50 | #define TWI_NO_STOP 0 51 | #define TWI_BLOCK 1 52 | #define TWO_NO_BLOCK 0 53 | 54 | #define TWI_READY 0 55 | #define TWI_MRX 1 56 | #define TWI_MTX 2 57 | #define TWI_SRX 3 58 | #define TWI_STX 4 59 | 60 | /* 61 | * Override the default buffers with your own buffers. This gives you more control over how the 62 | * buffers are constructed. 63 | */ 64 | void twi_set_master_buffer(uint8_t* buffer); 65 | void twi_set_rx_buffer(uint8_t* buffer); 66 | void twi_set_tx_buffer(uint8_t* buffer); 67 | 68 | /* 69 | * For even more control, you can attach readers / writers to the master / slave; this lets 70 | * you programatically construct the data to send / handle the data received, without just 71 | * relying on a sequential buffer. 72 | */ 73 | //Attach a slave rx reader function. This takes a uint8_t (incoming data), and uint16_t (incoming data's index). 74 | void twi_attach_slave_rx_reader( void (*function)(uint8_t, uint16_t) ); 75 | 76 | //Attach a slave tx writer function. This takes a uint16_t (outgoing data's index), and returns a uint8_t (the 77 | // byte to be transmitted). 78 | void twi_attach_slave_tx_writer( uint8_t (*function)(uint16_t) ); 79 | 80 | //Attach a master rx reader. This takes a uint8_t (incoming data), and uint16_t (incoming data's index). 81 | void twi_attach_master_rx_reader( void (*function)(uint8_t, uint16_t) ); 82 | 83 | //Attach a master tx writer. This takes a uint16_t (outgoing data's index), and returns a uint8_t (the 84 | // byte to be transmitted. 85 | void twi_attach_master_tx_writer( uint8_t (*function)(uint16_t) ); 86 | 87 | /* 88 | * The core TWI functions: send and receive data, from either master or slave. 89 | */ 90 | //Initialize the TWI hardware 91 | void twi_init(); 92 | 93 | //Master mode, read from the given address, to the given data buffer, for the given number of 94 | // bytes, and either TWI_STOP or TWI_NO_STOP when completed. 95 | uint8_t twi_read_from(uint8_t address, uint8_t* data, uint16_t length, uint8_t send_stop); 96 | 97 | //Master mode, write to the given slave address, from the given data buffer, for the given number of 98 | // bytes. If block is TWI_BLOCK (not TWI_NO_BLOCK) then this function blocks until the write is 99 | // completed. If send_stop is TWI_STOP then we send the stop byte when completed. 100 | uint8_t twi_write_to(uint8_t address, uint8_t* data, uint16_t length, uint8_t block, uint8_t send_stop); 101 | 102 | //Slave mode, start listening on the specified address. 103 | void twi_set_slave_address(uint8_t); 104 | 105 | //Transmit is called from the slave tx event callback, and will start the transmission back to the master. 106 | uint8_t twi_transmit(const uint8_t*, uint16_t); 107 | 108 | //Attach the supplied callback function, which is called when the slave rx is completed. The 109 | // callback's arguments are the daat buffer which was just read, and the length of that buffer. 110 | // This function is most likely redundant if TWI_SLAVE_RX_READER is defined (that callback will 111 | // be called once for each byte in the incoming message, whereas this one will not be called 112 | // until the entire message is buffered, but there are probably not many scenarios when both 113 | // approaches are valid at the same time.) 114 | void twi_attach_slave_rx_callback( void (*)(uint8_t*, uint16_t) ); 115 | 116 | //Attach the supplied callback function, which is called when the slave receives a tx request. 117 | // There are no arguments to the callback function; the callback must assemble the data buffer 118 | // and submit it via the twi_transmit() function. This function is most likely redundant if 119 | // TWI_SLAVE_TX_WRITER is defined (that callback will write one byte at a time, whereas this 120 | // one will assemble the entire message, but the end result is essentially the same.) 121 | void twi_attach_slave_tx_callback( void (*)(void) ); 122 | 123 | #endif 124 | 125 | -------------------------------------------------------------------------------- /avr/Leg.h: -------------------------------------------------------------------------------- 1 | #ifndef LEG_H 2 | #define LEG_H 3 | 4 | #ifndef DEBUG_SIMULATION 5 | #include 6 | #include "lib/pwm/pwm.h" 7 | #else 8 | #include 9 | #include 10 | #include 11 | #include "../simulation/debug.h" 12 | #endif 13 | #include "types/Point.h" 14 | 15 | #include "Stubby.h" 16 | 17 | //*** See doc/diagrams.pdf for figure x.y references ***// 18 | 19 | 20 | //The distance from 0,0 at which legs are mounted. This assumes a radial symmetry of all legs (i.e. the distance 21 | // is the same for all the legs); if this does not apply to your design, you will need to pass in an x,y offset for 22 | // each leg in its constructor, and modify the translation function to use it. See figure 1.1, 'leg offset' 23 | #define LEG_OFFSET 45 24 | //The angle offset for a single leg. All legs are defined as a multiple of this number. For the current design 25 | // (radial symmetry, with the middle leg sticking straight out the side) this should be 60 degrees. See 26 | // figure 1.1, 'mounting angle'. 27 | #define LEG_MOUNTING_ANGLE (60 * M_PI / 180) 28 | 29 | //The height from ground to the center of the coxa joint. See figure 1.2, 'coxa height'. 30 | #define COXA_HEIGHT 55.0 31 | //The length of the leg segment between coxa and femur joints (measured between center of each joint). See 32 | // figure 1.2, 'coxa length' 33 | #define COXA_LENGTH 49.0 34 | //The height from the center of the coxa joint to the center of the femur joint. See figure 1.2, 'femur height'. 35 | #define FEMUR_HEIGHT 17.825 36 | //The length of the leg segment between femur and tibia joints (measured between center of each joint). See 37 | // figure 1.2, 'femur length' 38 | #define FEMUR_LENGTH 40.0 39 | //The length of the leg segment between tibia joint and end of the foot (measured from the center of the joint) 40 | // See figure 1.2, 'tibia length' 41 | #define TIBIA_LENGTH 57.0 42 | 43 | 44 | //The lengths of the four segments in the Tibia drive system. See figure 1.3, segments a, b, c and d. 45 | #define TIBIA_A 30.0 46 | #define TIBIA_B 16.3 47 | #define TIBIA_C 54.0 48 | #define TIBIA_D 34.0 49 | //The difference in angle between the desired angle and the angle between segments d and a. See 50 | // figure 1.3, angle 'E' 51 | #define TIBIA_E_OFFSET_ANGLE (20 * M_PI / 180) 52 | //The angle at which the servo horn extends from the servo when a neutral PWM signal is applied. See 53 | // figure 1.3, angle 'N' 54 | #define TIBIA_NEUTRAL_SERVO_ANGLE (135 * M_PI / 180) 55 | 56 | //The lengths of the four segments in the Femur drive system. See figure 1.4, segments a, b, c and d. 57 | #define FEMUR_A 27.7 58 | #define FEMUR_B 16.5 59 | #define FEMUR_C 38.5 60 | #define FEMUR_D 34.0 61 | //The difference in angle between the desired angle and the angle between segments d and a. See 62 | // figure 1.4, angle 'E' 63 | #define FEMUR_E_OFFSET_ANGLE (31.5 * M_PI / 180) 64 | //The angle at which the servo horn extends from the servo when a neutral PWM signal is applied. See 65 | // figure 1.4, angle 'N' 66 | #define FEMUR_NEUTRAL_SERVO_ANGLE (135 * M_PI / 180) 67 | 68 | //For the Coxa joint we just use a linear equation to determine the offsets, since it is close enough for 69 | // most purposes and eliminates a lot of math needed for the drive system calculations. To determine 70 | // this value, set the coxa joint on a single leg to various phases (for instance, 1500 + 300, 1500 - 300, 71 | // 1500 + 600, etc) and measure the resulting angles. On my robot, I get approx. 16 degrees for each 72 | // phase offset of 300us. By dividing this out (300 / (16 * PI / 180)) I get the value 1074.296, which 73 | // means that for each radian I want to move, I adjust the phase by 1074.296. 74 | #define COXA_PHASE_MULTIPLIER -1074.296 75 | 76 | //Servo travel information. We assume a neutral offset of 1500, with even amounts on either side. We also assume that the servo has 77 | // a linear travel between one end and the other. 78 | #define PHASE_MIN 700 //Clockwise from neutral 79 | #define PHASE_NEUTRAL 1500 80 | #define PHASE_MAX 2300 //Counter clockwise from neutral 81 | 82 | //Maximum angle of travel for the servo, in radians (between MIN_PHASE and MAX_PHASE). Therefore, the maximum 83 | // travel in each direction from neutral is half of this number. 84 | #define SERVO_TRAVEL (150 * M_PI / 180) 85 | 86 | /* 87 | * C++ implementation of Stubby the Hexapod leg. 88 | */ 89 | class Leg { 90 | private: 91 | uint8_t index; 92 | 93 | volatile uint8_t *port[JOINT_COUNT]; 94 | uint8_t pin[JOINT_COUNT]; 95 | 96 | Point p; //Foot co-ordinates 97 | Point neutralP; //Neutral foot co-ordinates 98 | double mounting_angle; //The angle at which the leg is mounted, in degrees, relative to the X axis of a standard cartesian plane. 99 | int8_t calibration[CALIBRATION_COUNT]; //Calibration offset (in degrees for 0..2, and in mm for 3..5) 100 | 101 | //Set the angle for each servo. This includes the servo abstraction code. 102 | void setTibiaAngle(double angle); 103 | void setFemurAngle(double angle); 104 | void setCoxaAngle(double angle); 105 | 106 | public: 107 | /* 108 | * Initializes the leg, given the specified mounting angle describing it's radial position in degrees. 109 | */ 110 | Leg(uint8_t index, volatile uint8_t *tibia_port, uint8_t tibia_pin, volatile uint8_t *femur_port, uint8_t femur_pin, volatile uint8_t *coxa_port, uint8_t coxa_pin, double mounting_angle, Point neutralP); 111 | 112 | /* 113 | * Set the foot position, relative to neutralP. 114 | */ 115 | void setOffset(Point offset); 116 | 117 | /* 118 | * Resets the foot position to neutral (as defined from the constructor). 119 | */ 120 | void resetPosition(); 121 | 122 | /* 123 | * Returns the last set foot position. 124 | */ 125 | Point getPosition(); 126 | 127 | /* 128 | * Sets the foot position, in absolute x, y, z co-ordinates. Performs the IK calculations, the absolute angle to servo angle calculations, and 129 | * sets the servo position for each of the three joints. 130 | */ 131 | void setPosition(Point point); 132 | 133 | /* 134 | * Gets the offset for the given joint 135 | */ 136 | int8_t getCalibration(uint8_t joint); 137 | 138 | /* 139 | * Sets the offset for the given joint 140 | */ 141 | void setCalibration(uint8_t joint, int8_t offset); 142 | 143 | /* 144 | * Returns the index 145 | */ 146 | uint8_t getIndex(); 147 | 148 | /* 149 | * Returns the mounting angle 150 | */ 151 | double getMountingAngle(); 152 | 153 | /* 154 | * Gets the specified port 155 | */ 156 | volatile uint8_t* getPort(uint8_t joint); 157 | 158 | /* 159 | * Gets the specified pin 160 | */ 161 | uint8_t getPin(uint8_t joint); 162 | }; 163 | #endif 164 | -------------------------------------------------------------------------------- /avr/Leg.cpp: -------------------------------------------------------------------------------- 1 | #include "Leg.h" 2 | 3 | /* 4 | * Returns the angle which the servo needs to move from neutral. Used for both tibia and femur. 5 | */ 6 | double solveServoTrapezoid(double desired_angle, double length_a, double length_b, double length_c, double length_d, double angle_E_offset, double angle_E_offset2, double angle_N){ 7 | //See diagrams in doc/diagrams.pdf for description of sides and angles 8 | //Use law of cosines to find the length of the line between the control rod connection point and the servo shaft 9 | double length_e = sqrt(length_d * length_d + length_a * length_a - 2 * length_d * length_a * cos_f(desired_angle + angle_E_offset)); 10 | 11 | //Use law of cosines to find the angle between the line we just calculated (e) and the line between the servo shaft and servo horn / control rod connection (b) 12 | double angle_C = acos_f((length_e * length_e + length_b * length_b - length_c * length_c) / (2 * length_e * length_b)); 13 | //Use law of cosines to find the angle between the line we just calculated (e) and the line between the joint and the servo shaft (d) 14 | double angle_D = acos_f((length_e * length_e + length_a * length_a - length_d * length_d) / (2 * length_e * length_a)); 15 | 16 | #ifdef DEBUG_SIMULATION 17 | printf("e: %3.1f mm; C: %3.1f°; D: %3.1f°", length_e, angle_C * 180 / M_PI, angle_D * 180 / M_PI); 18 | #endif 19 | 20 | return (angle_C + angle_D + angle_E_offset2) - angle_N; 21 | } 22 | 23 | Leg::Leg(uint8_t index, volatile uint8_t *tibia_port, uint8_t tibia_pin, volatile uint8_t *femur_port, uint8_t femur_pin, volatile uint8_t *coxa_port, uint8_t coxa_pin, double mounting_angle, Point neutralP){ 24 | this->index = index; 25 | this->port[TIBIA] = tibia_port; 26 | this->port[FEMUR] = femur_port; 27 | this->port[COXA] = coxa_port; 28 | this->pin[TIBIA] = tibia_pin; 29 | this->pin[FEMUR] = femur_pin; 30 | this->pin[COXA] = coxa_pin; 31 | this->mounting_angle = mounting_angle; 32 | this->neutralP = neutralP; 33 | } 34 | 35 | void Leg::setPosition(Point p){ 36 | this->p = p; 37 | 38 | //Rotate leg around 0, 0 such that the leg is pointing straight out at angle 0 (straight right). 39 | p.rotateXY(this->mounting_angle * -1); 40 | //Translate the leg according to the leg offset, to put the coxa joint at co-ordinates 0,0. 41 | p.x -= LEG_OFFSET; 42 | 43 | //Find the angle of the leg, used to set the coxa joint. See figure 2.1, 'coxa angle'. 44 | double coxa_angle = atan2(p.y, p.x); 45 | 46 | //Find the length of the leg, from coxa joint to end of tibia, on the x,y plane (to be later 47 | // used for X,Z inverse kinematics). See figure 2.1, 'leg length', 'p.x', and 'p.y'. 48 | double leg_length = sqrt((p.x * p.x) + (p.y * p.y)); 49 | 50 | //Find the distance between the femur joint and the end of the tibia. Do this using the 51 | // right triangle of (FEMUR_HEIGHT + COXA_HEIGHT - z), (leg_length - COXA_LENGTH). See figure 52 | // 2.2, 'leg extension' 53 | double leg_extension = sqrt((FEMUR_HEIGHT + COXA_HEIGHT - p.z) * (FEMUR_HEIGHT + COXA_HEIGHT - p.z) + (leg_length - COXA_LENGTH) * (leg_length - COXA_LENGTH)); 54 | //Find the first part of the femur angle using law of cosines. See figure 2.2 for a diagram of this. 55 | double femur_angle_a = acos_f((((FEMUR_HEIGHT + COXA_HEIGHT - p.z) * (FEMUR_HEIGHT + COXA_HEIGHT - p.z)) + (leg_extension * leg_extension) - ((leg_length - COXA_LENGTH) * (leg_length - COXA_LENGTH))) / (2 * (FEMUR_HEIGHT + COXA_HEIGHT - p.z) * leg_extension)); 56 | //Find the second part of the femur angle using law of cosines. See figure 2.3 for a diagram of this. 57 | double femur_angle_b = acos_f(((FEMUR_LENGTH * FEMUR_LENGTH) + (leg_extension * leg_extension) - (TIBIA_LENGTH * TIBIA_LENGTH)) / (2 * FEMUR_LENGTH * leg_extension)); 58 | double femur_angle = femur_angle_a + femur_angle_b; 59 | 60 | //Find the desired tibia angle using law of cosines. See figure 2.4 for a diagram of this. 61 | double tibia_angle = acos_f(((FEMUR_LENGTH * FEMUR_LENGTH) + (TIBIA_LENGTH * TIBIA_LENGTH) - (leg_extension * leg_extension)) / (2 * FEMUR_LENGTH * TIBIA_LENGTH)); 62 | 63 | #ifdef DEBUG_SIMULATION 64 | printf(" IK: x,y,z: %d,%d,%d; leg_length: %3.1f mm; leg_extension: %3.1f mm\n", p.x,p.y,p.z, leg_length, leg_extension); 65 | #endif 66 | 67 | this->setTibiaAngle(tibia_angle); 68 | this->setFemurAngle(femur_angle); 69 | this->setCoxaAngle(coxa_angle); 70 | 71 | //TODO If the servo angles are out of bounds (either NaN or an integer outside of the valid PWM range), then re-calculate 72 | // the x,y,z co-ordinates based on valid numbers. This will be quite doable for out of bounds angles, but will be very 73 | // difficult for NaN, since it is impossible to say whether it is a small or large angle. 74 | } 75 | 76 | uint8_t Leg::getIndex(){ 77 | return this->index; 78 | } 79 | 80 | double Leg::getMountingAngle(){ 81 | return this->mounting_angle; 82 | } 83 | 84 | void Leg::setOffset(Point offset){ 85 | Point foot(0,0,0); 86 | foot.x = this->getCalibration(CALIBRATION_X); 87 | foot.y = this->getCalibration(CALIBRATION_Y); 88 | foot.z = this->getCalibration(CALIBRATION_Z); 89 | offset.add(foot); 90 | 91 | if (offset.x > 30) offset.x = 30; 92 | else if (offset.x < -30) offset.x = -30; 93 | 94 | if (offset.y > 30) offset.y = 30; 95 | else if (offset.y < -30) offset.y = -30; 96 | 97 | if (offset.z > 15) offset.z = 15; 98 | else if (offset.z < -15) offset.z = -15; 99 | 100 | offset.add(this->neutralP); 101 | this->setPosition(offset); 102 | } 103 | 104 | void Leg::resetPosition(){ 105 | this->setOffset(Point(0,0,0)); 106 | } 107 | 108 | Point Leg::getPosition(){ 109 | return this->p; 110 | } 111 | 112 | volatile uint8_t* Leg::getPort(uint8_t joint){ 113 | return this->port[joint]; 114 | } 115 | 116 | uint8_t Leg::getPin(uint8_t joint){ 117 | return this->pin[joint]; 118 | } 119 | 120 | int8_t Leg::getCalibration(uint8_t i){ 121 | if (i < CALIBRATION_COUNT) return this->calibration[i]; 122 | else return 0; 123 | } 124 | 125 | void Leg::setCalibration(uint8_t i, int8_t calibration){ 126 | if (i < CALIBRATION_COUNT) this->calibration[i] = calibration; 127 | } 128 | 129 | void Leg::setTibiaAngle(double desired_angle){ 130 | //See diagrams in doc/diagrams.pdf for description of sides and angles 131 | double angle_S = solveServoTrapezoid(desired_angle, TIBIA_A, TIBIA_B, TIBIA_C, TIBIA_D, TIBIA_E_OFFSET_ANGLE, TIBIA_E_OFFSET_ANGLE, TIBIA_NEUTRAL_SERVO_ANGLE); 132 | #ifdef DEBUG_SIMULATION 133 | printf(" Tibia: desired angle: %3.1f°; servo angle: %3.1f°\n", desired_angle * 180 / M_PI, angle_S * 180 / M_PI); 134 | #endif 135 | pwm_set_phase_batch((this->index * JOINT_COUNT) + TIBIA, (uint16_t) PHASE_NEUTRAL + (this->calibration[TIBIA] * 10) + angle_S * ((PHASE_MAX - PHASE_MIN) / SERVO_TRAVEL)); 136 | } 137 | 138 | void Leg::setFemurAngle(double desired_angle){ 139 | //See diagrams in doc/diagrams.pdf for description of sides and angles 140 | double angle_S = solveServoTrapezoid(desired_angle, FEMUR_A, FEMUR_B, FEMUR_C, FEMUR_D, FEMUR_E_OFFSET_ANGLE, FEMUR_E_OFFSET_ANGLE, FEMUR_NEUTRAL_SERVO_ANGLE); 141 | #ifdef DEBUG_SIMULATION 142 | printf(" Femur: desired angle: %3.1f°; servo angle: %3.1f°\n", desired_angle * 180 / M_PI, angle_S * 180 / M_PI); 143 | #endif 144 | pwm_set_phase_batch((this->index * JOINT_COUNT) + FEMUR, (uint16_t) PHASE_NEUTRAL + (this->calibration[FEMUR] * 10) + angle_S * ((PHASE_MAX - PHASE_MIN) / SERVO_TRAVEL)); 145 | } 146 | 147 | void Leg::setCoxaAngle(double desired_angle){ 148 | pwm_set_phase_batch((this->index * JOINT_COUNT) + COXA, (uint16_t) PHASE_NEUTRAL + (this->calibration[COXA] * 10) + desired_angle * COXA_PHASE_MULTIPLIER); 149 | } 150 | 151 | -------------------------------------------------------------------------------- /kicad/rev1/gerber/hex-F_Mask.gts: -------------------------------------------------------------------------------- 1 | G04 (created by PCBNEW (2013-dec-23)-stable) date Thu 03 Jul 2014 07:48:38 AM MDT* 2 | %MOIN*% 3 | G04 Gerber Fmt 3.4, Leading zero omitted, Abs format* 4 | %FSLAX34Y34*% 5 | G01* 6 | G70* 7 | G90* 8 | G04 APERTURE LIST* 9 | %ADD10C,0.00590551*% 10 | %ADD11C,0.055*% 11 | %ADD12C,0.066*% 12 | %ADD13C,0.07*% 13 | %ADD14C,0.06*% 14 | %ADD15R,0.035X0.055*% 15 | %ADD16R,0.055X0.035*% 16 | %ADD17C,0.16*% 17 | %ADD18R,0.0394X0.0315*% 18 | %ADD19R,0.035X0.045*% 19 | %ADD20C,0.075*% 20 | %ADD21R,0.055X0.055*% 21 | G04 APERTURE END LIST* 22 | G54D10* 23 | G54D11* 24 | X32866Y-15738D03* 25 | X31866Y-15738D03* 26 | X31866Y-14738D03* 27 | G54D12* 28 | X24116Y-23918D03* 29 | X23116Y-23918D03* 30 | G54D11* 31 | X28163Y-6850D03* 32 | X27456Y-7557D03* 33 | X26748Y-8264D03* 34 | X26041Y-8971D03* 35 | X25334Y-9678D03* 36 | X24627Y-10385D03* 37 | X23920Y-11092D03* 38 | X23213Y-11799D03* 39 | X22506Y-12506D03* 40 | X21799Y-13214D03* 41 | X21092Y-13921D03* 42 | X20384Y-14628D03* 43 | X19677Y-15335D03* 44 | X18970Y-16042D03* 45 | X18263Y-16749D03* 46 | X17556Y-17456D03* 47 | X16849Y-18163D03* 48 | X16142Y-18870D03* 49 | X15435Y-19578D03* 50 | X14728Y-20285D03* 51 | X18970Y-24527D03* 52 | X19677Y-23820D03* 53 | X20384Y-23113D03* 54 | X21092Y-22406D03* 55 | X21799Y-21699D03* 56 | X22506Y-20992D03* 57 | X23213Y-20285D03* 58 | X23920Y-19578D03* 59 | X24627Y-18870D03* 60 | X25334Y-18163D03* 61 | X26041Y-17456D03* 62 | X26748Y-16749D03* 63 | X27456Y-16042D03* 64 | X28163Y-15335D03* 65 | X28870Y-14628D03* 66 | X29577Y-13921D03* 67 | X30284Y-13214D03* 68 | X30991Y-12506D03* 69 | X31698Y-11799D03* 70 | X32405Y-11092D03* 71 | G54D13* 72 | X24216Y-6538D03* 73 | X23216Y-6538D03* 74 | G54D14* 75 | X18659Y-14296D03* 76 | X19366Y-13588D03* 77 | X20074Y-12881D03* 78 | G54D10* 79 | G36* 80 | X28022Y-18722D02* 81 | X27633Y-18333D01* 82 | X27881Y-18085D01* 83 | X28269Y-18474D01* 84 | X28022Y-18722D01* 85 | X28022Y-18722D01* 86 | G37* 87 | G36* 88 | X28552Y-18192D02* 89 | X28163Y-17803D01* 90 | X28411Y-17555D01* 91 | X28800Y-17944D01* 92 | X28552Y-18192D01* 93 | X28552Y-18192D01* 94 | G37* 95 | G36* 96 | X21331Y-23285D02* 97 | X21720Y-23674D01* 98 | X21472Y-23922D01* 99 | X21083Y-23533D01* 100 | X21331Y-23285D01* 101 | X21331Y-23285D01* 102 | G37* 103 | G36* 104 | X20801Y-23815D02* 105 | X21189Y-24204D01* 106 | X20942Y-24452D01* 107 | X20553Y-24063D01* 108 | X20801Y-23815D01* 109 | X20801Y-23815D01* 110 | G37* 111 | G54D14* 112 | X27998Y-25067D03* 113 | X28705Y-24360D03* 114 | X29412Y-23653D03* 115 | X30119Y-22946D03* 116 | X30826Y-22238D03* 117 | X31534Y-21531D03* 118 | X32241Y-20824D03* 119 | X32948Y-20117D03* 120 | G54D15* 121 | X32091Y-12988D03* 122 | X32841Y-12988D03* 123 | G54D10* 124 | G36* 125 | X30702Y-14612D02* 126 | X30313Y-14223D01* 127 | X30561Y-13975D01* 128 | X30949Y-14364D01* 129 | X30702Y-14612D01* 130 | X30702Y-14612D01* 131 | G37* 132 | G36* 133 | X31232Y-14082D02* 134 | X30843Y-13693D01* 135 | X31091Y-13445D01* 136 | X31480Y-13834D01* 137 | X31232Y-14082D01* 138 | X31232Y-14082D01* 139 | G37* 140 | G36* 141 | X32681Y-21405D02* 142 | X33070Y-21794D01* 143 | X32822Y-22042D01* 144 | X32433Y-21653D01* 145 | X32681Y-21405D01* 146 | X32681Y-21405D01* 147 | G37* 148 | G36* 149 | X32151Y-21935D02* 150 | X32539Y-22324D01* 151 | X32292Y-22572D01* 152 | X31903Y-22183D01* 153 | X32151Y-21935D01* 154 | X32151Y-21935D01* 155 | G37* 156 | G36* 157 | X29792Y-25232D02* 158 | X29403Y-24843D01* 159 | X29651Y-24595D01* 160 | X30039Y-24984D01* 161 | X29792Y-25232D01* 162 | X29792Y-25232D01* 163 | G37* 164 | G36* 165 | X30322Y-24702D02* 166 | X29933Y-24313D01* 167 | X30181Y-24065D01* 168 | X30570Y-24454D01* 169 | X30322Y-24702D01* 170 | X30322Y-24702D01* 171 | G37* 172 | G36* 173 | X23954Y-8996D02* 174 | X24343Y-9385D01* 175 | X24095Y-9633D01* 176 | X23706Y-9244D01* 177 | X23954Y-8996D01* 178 | X23954Y-8996D01* 179 | G37* 180 | G36* 181 | X23424Y-9526D02* 182 | X23813Y-9915D01* 183 | X23565Y-10163D01* 184 | X23176Y-9774D01* 185 | X23424Y-9526D01* 186 | X23424Y-9526D01* 187 | G37* 188 | G36* 189 | X22585Y-11123D02* 190 | X22196Y-10734D01* 191 | X22444Y-10486D01* 192 | X22833Y-10875D01* 193 | X22585Y-11123D01* 194 | X22585Y-11123D01* 195 | G37* 196 | G36* 197 | X23115Y-10593D02* 198 | X22726Y-10204D01* 199 | X22974Y-9956D01* 200 | X23363Y-10345D01* 201 | X23115Y-10593D01* 202 | X23115Y-10593D01* 203 | G37* 204 | G36* 205 | X21894Y-11016D02* 206 | X22283Y-11405D01* 207 | X22035Y-11653D01* 208 | X21646Y-11264D01* 209 | X21894Y-11016D01* 210 | X21894Y-11016D01* 211 | G37* 212 | G36* 213 | X21364Y-11546D02* 214 | X21753Y-11935D01* 215 | X21505Y-12183D01* 216 | X21116Y-11794D01* 217 | X21364Y-11546D01* 218 | X21364Y-11546D01* 219 | G37* 220 | G54D16* 221 | X16760Y-16935D03* 222 | X16760Y-16185D03* 223 | X14366Y-18763D03* 224 | X14366Y-18013D03* 225 | X15166Y-18163D03* 226 | X15166Y-17413D03* 227 | X15966Y-17563D03* 228 | X15966Y-16813D03* 229 | G54D17* 230 | X31889Y-24015D03* 231 | G54D18* 232 | X16437Y-15140D03* 233 | X17303Y-14765D03* 234 | X17303Y-15515D03* 235 | G54D17* 236 | X15354Y-7480D03* 237 | G54D14* 238 | X30706Y-7085D03* 239 | X31413Y-6378D03* 240 | X31413Y-7792D03* 241 | X32120Y-7085D03* 242 | X32120Y-8499D03* 243 | X32827Y-7792D03* 244 | X16327Y-24442D03* 245 | X15620Y-25149D03* 246 | X15620Y-23735D03* 247 | X14913Y-24442D03* 248 | X14913Y-23028D03* 249 | X14206Y-23735D03* 250 | G54D19* 251 | X15716Y-15588D03* 252 | X15216Y-15588D03* 253 | X14716Y-15588D03* 254 | X14216Y-15588D03* 255 | G54D14* 256 | X19867Y-18353D03* 257 | X20574Y-17646D03* 258 | X21281Y-16938D03* 259 | X21988Y-16231D03* 260 | X22695Y-15524D03* 261 | X23402Y-14817D03* 262 | X24109Y-14110D03* 263 | X24816Y-13403D03* 264 | X25524Y-12696D03* 265 | X26231Y-11989D03* 266 | X29766Y-15524D03* 267 | X29059Y-16231D03* 268 | X28352Y-16938D03* 269 | X27645Y-17646D03* 270 | X26938Y-18353D03* 271 | X26231Y-19060D03* 272 | X25524Y-19767D03* 273 | X24816Y-20474D03* 274 | X24109Y-21181D03* 275 | X23402Y-21888D03* 276 | G54D20* 277 | X18579Y-11936D03* 278 | X19286Y-11229D03* 279 | X19994Y-10522D03* 280 | X17872Y-11229D03* 281 | X18579Y-10522D03* 282 | X19286Y-9815D03* 283 | X17165Y-10522D03* 284 | X17872Y-9815D03* 285 | X18579Y-9108D03* 286 | X20699Y-9816D03* 287 | X21406Y-9109D03* 288 | X22114Y-8402D03* 289 | X19992Y-9109D03* 290 | X20699Y-8402D03* 291 | X21406Y-7695D03* 292 | X19285Y-8402D03* 293 | X19992Y-7695D03* 294 | X20699Y-6988D03* 295 | X16459Y-14056D03* 296 | X17166Y-13349D03* 297 | X17874Y-12642D03* 298 | X15752Y-13349D03* 299 | X16459Y-12642D03* 300 | X17166Y-11935D03* 301 | X15045Y-12642D03* 302 | X15752Y-11935D03* 303 | X16459Y-11228D03* 304 | X30804Y-16831D03* 305 | X30096Y-17538D03* 306 | X29389Y-18245D03* 307 | X31511Y-17538D03* 308 | X30804Y-18245D03* 309 | X30096Y-18952D03* 310 | X32218Y-18245D03* 311 | X31511Y-18952D03* 312 | X30804Y-19659D03* 313 | X28684Y-18951D03* 314 | X27976Y-19658D03* 315 | X27269Y-20365D03* 316 | X29391Y-19658D03* 317 | X28684Y-20365D03* 318 | X27976Y-21072D03* 319 | X30098Y-20365D03* 320 | X29391Y-21072D03* 321 | X28684Y-21779D03* 322 | X26564Y-21071D03* 323 | X25856Y-21778D03* 324 | X25149Y-22485D03* 325 | X27271Y-21778D03* 326 | X26564Y-22485D03* 327 | X25856Y-23192D03* 328 | X27978Y-22485D03* 329 | X27271Y-23192D03* 330 | X26564Y-23899D03* 331 | G54D21* 332 | X31900Y-16600D03* 333 | G54D11* 334 | X32900Y-16600D03* 335 | G54D10* 336 | G36* 337 | X18453Y-19257D02* 338 | X18842Y-19646D01* 339 | X18453Y-20035D01* 340 | X18064Y-19646D01* 341 | X18453Y-19257D01* 342 | X18453Y-19257D01* 343 | G37* 344 | G54D11* 345 | X17746Y-20353D03* 346 | G54D10* 347 | G36* 348 | X15542Y-14353D02* 349 | X15153Y-14742D01* 350 | X14764Y-14353D01* 351 | X15153Y-13964D01* 352 | X15542Y-14353D01* 353 | X15542Y-14353D01* 354 | G37* 355 | G54D11* 356 | X14446Y-13646D03* 357 | M02* 358 | -------------------------------------------------------------------------------- /processing/src/ca/digitalcave/stubby/Stubby.java: -------------------------------------------------------------------------------- 1 | package ca.digitalcave.stubby; 2 | 3 | import processing.core.PApplet; 4 | import processing.serial.Serial; 5 | 6 | public class Stubby { 7 | //private final PApplet parent; 8 | private final Protocol protocol; 9 | 10 | public Stubby(PApplet parent) { 11 | //this.parent = parent; 12 | 13 | for (String port : Serial.list()){ 14 | if (port.toLowerCase().contains("stubby")){ 15 | final StubbySerial serial = new StubbySerial(parent, port, 38400); 16 | protocol = new Protocol(serial); 17 | serial.setProtocol(protocol); 18 | return; 19 | } 20 | } 21 | 22 | throw new IllegalArgumentException("No serial port containing text 'stubby' could be found. Please connect Stubby using a port which name contains text 'stubby', or specify the proper port using the Stubby(PApplet, String) constructor."); 23 | } 24 | 25 | public Stubby(PApplet parent, String port) { 26 | final StubbySerial serial = new StubbySerial(parent, port, 38400); 27 | protocol = new Protocol(serial); 28 | serial.setProtocol(protocol); 29 | } 30 | 31 | public boolean turnOn(){ 32 | return protocol.sendMessageAndBlockForAck(new Message(Protocol.REQUEST_POWER_ON, new int[0]), 1000, 2); 33 | } 34 | public boolean turnOff(){ 35 | return protocol.sendMessageAndBlockForAck(new Message(Protocol.REQUEST_POWER_OFF, new int[0]), 1000, 2); 36 | } 37 | public boolean enableDebug(){ 38 | return protocol.sendMessageAndBlockForAck(new Message(Protocol.REQUEST_ENABLE_DEBUG, new int[0]), 1000, 2); 39 | } 40 | public boolean disableDebug(){ 41 | return protocol.sendMessageAndBlockForAck(new Message(Protocol.REQUEST_ENABLE_DEBUG, new int[0]), 1000, 2); 42 | } 43 | 44 | /** 45 | * Move the specified distance in the indicated direction at the given speed 46 | * 47 | * @param linearAngle Angle to move. 0 is straight forward, negative angles are right, and positive angles are left. Angle in degrees. 48 | * @param rotationalAngle Rotational angle to turn. Measured the same as linear angle. 49 | * @param linearVelocity Speed to move; unsigned 8 bit integer value between 255 (fastest) and 0 (slowest). 50 | * @param rotationalVelocity Speed to turn; same range as linearVelocity. 51 | * @param distance Distance to move (in mm). 52 | * @return 53 | */ 54 | public boolean move(int angle, int velocity, int distance){ 55 | if (distance == 0) return true; 56 | double angleRadians = angle * Math.PI / 180; 57 | angleRadians += (float) Math.PI / 2; //Internally, Stubby uses X axis for steps; here we want to use Y. 58 | if (distance < 0){ 59 | angleRadians = angleRadians + Math.PI; //Support going backwards 60 | distance = distance * -1; 61 | } 62 | int[] data = new int[4]; 63 | data[0] = Protocol.radianToByte(angleRadians); 64 | data[1] = velocity & 0xFF; 65 | data[2] = (distance >> 8) & 0xFF; 66 | data[3] = distance & 0xFF; 67 | if (!protocol.sendMessageAndBlockForAck(new Message(Protocol.REQUEST_MOVE, data), 1000, 2)){ 68 | return false; 69 | } 70 | return protocol.waitForComplete(Protocol.REQUEST_MOVE); 71 | } 72 | 73 | public boolean move(int linearAngle, int distance){ 74 | return move(linearAngle, 255, distance); 75 | } 76 | 77 | /** 78 | * Convenience method to walk forward. 79 | * @param steps 80 | */ 81 | public boolean moveForward(int distance){ 82 | return move(0, distance); 83 | } 84 | 85 | /** 86 | * Convenience method to walk backward. 87 | * @param steps 88 | */ 89 | public boolean moveBackward(int distance){ 90 | return move(180, distance); 91 | } 92 | 93 | /** 94 | * Convenience method to walk right. 95 | * @param steps 96 | */ 97 | public boolean moveRight(int distance){ 98 | return move(-90, distance); 99 | } 100 | 101 | /** 102 | * Convenience method to walk left. 103 | * @param steps 104 | */ 105 | public boolean moveLeft(int distance){ 106 | return move(90, distance); 107 | } 108 | 109 | /** 110 | * Rotates to the specified angle. Positive values rotate 111 | * clockwise, negative rotate counter clockwise. 112 | * @param angle In degrees, with 0 being the direction the robot is currently facing. Negative angles are to the right (clockwise), positive to the left (counter clockwise) 113 | * @param rotationalVelocity Speed to turn, between 0 and 255 114 | */ 115 | public boolean turn(int angle, int velocity){ 116 | if (angle == 0) return true; 117 | double angleRadians = angle * Math.PI / -180; 118 | int[] data = new int[2]; 119 | data[0] = Protocol.radianToByte(angleRadians); 120 | data[1] = velocity & 0xFF; 121 | if (!protocol.sendMessageAndBlockForAck(new Message(Protocol.REQUEST_TURN, data), 1000, 2)){ 122 | return false; 123 | } 124 | return protocol.waitForComplete(Protocol.REQUEST_TURN); 125 | } 126 | /** 127 | * Convenience method to turn right 128 | * @return 129 | */ 130 | public boolean turnRight(){ 131 | return turn(-90, 255); 132 | } 133 | /** 134 | * Convenience method to turn left 135 | * @return 136 | */ 137 | public boolean turnLeft(){ 138 | return turn(90, 255); 139 | } 140 | /** 141 | * Convenience method to turn around 180 degrees (counter clockwise) 142 | * @return 143 | */ 144 | public boolean turnAround(){ 145 | return turn(179, 255); 146 | } 147 | /** 148 | * Convenience method to turn around 180 degrees (clockwise) 149 | * @return 150 | */ 151 | public boolean turnAroundClockwise(){ 152 | return turn(-179, 255); 153 | } 154 | 155 | /** 156 | * Sets a translation offset on Stubby's body, in mm, for each axis. 157 | * Valid offsets are between -128 and 127 (although Stubby may not physically 158 | * be able to move to all these values). 159 | * @param x Positive X values shift the body to the right 160 | * @param y Positive Y values shift the body forward 161 | * @param z Positive Z values shift the body up 162 | * @return 163 | */ 164 | public boolean translate(int x, int y, int z){ 165 | int[] data = new int[3]; 166 | data[0] = (x * -1) & 0xFF; 167 | data[1] = (y * -1) & 0xFF; 168 | data[2] = (z * -1) & 0xFF; 169 | return protocol.sendMessageAndBlockForAck(new Message(Protocol.REQUEST_TRANSLATE, data), 1000, 2); 170 | } 171 | 172 | /** 173 | * Reads the distance sensor and returns the value in mm; returns -1 if there was an error. 174 | * @return 175 | */ 176 | public int getDistance(){ 177 | final Message reply = protocol.sendMessageAndBlockForReply(new Message(Protocol.REQUEST_DISTANCE, new int[0]), Protocol.SEND_DISTANCE, 100, 2); 178 | if (reply == null) return -1; 179 | int distance = (reply.getData()[0] << 8) + reply.getData()[1]; 180 | return distance; 181 | } 182 | 183 | /** 184 | * Reads the current heading, and returns the value in degrees. 0 is north, negative numbers are rotating 185 | * westward (counter clockwise) from north, and positive numbers are rotating eastward (clockwise) from north. 186 | * Returns NaN if there was an error. 187 | * @return 188 | */ 189 | public int getHeading(){ 190 | final Message reply = protocol.sendMessageAndBlockForReply(new Message(Protocol.REQUEST_HEADING, new int[0]), Protocol.SEND_HEADING, 100, 2); 191 | if (reply == null) return Integer.MAX_VALUE; 192 | return (int) Math.toDegrees(Protocol.byteToRadian(reply.getData()[0])); 193 | } 194 | 195 | 196 | // /** 197 | // * Sets the distance servo angle (in radians). 0 is straight forward; negative 198 | // * values are looking down, positive values are looking up. 199 | // * @param angle 200 | // */ 201 | // public void setDistanceAngle(double angle){ 202 | // 203 | // } 204 | // 205 | // /** 206 | // * Reads the specified optical sensor (return value will be a byte between 0 and 255) 207 | // * @param sensor The sensor index; 0 is furthest left, and increments going right. 208 | // * @return 209 | // */ 210 | // public int readOpticalSensor(int sensor){ 211 | // return 0; 212 | // } 213 | 214 | public void dispose(){ 215 | protocol.getSerial().stop(); 216 | } 217 | } 218 | --------------------------------------------------------------------------------