├── .gitattributes ├── .gitignore ├── README.md ├── controller ├── .ccsproject ├── .cdtbuild ├── .cdtproject ├── .launches │ └── controller.launch ├── .project ├── CapacitiveTouchLibrary │ ├── CTS_HAL.c │ ├── CTS_HAL.h │ ├── CTS_Layer.c │ ├── CTS_Layer.h │ ├── structure.c │ └── structure.h ├── MSP430G2452.ccxml ├── lnk_msp430g2452.cmd └── src │ ├── i2c.c │ ├── i2c.h │ ├── main.c │ ├── wheel.c │ └── wheel.h ├── kinematicstest ├── Makefile ├── common.h ├── derivative.c ├── kinematics.cpp ├── kinematics.h └── main.cpp ├── mbed-stepper ├── common.h ├── kinematics.cpp ├── kinematics.h ├── main.cpp ├── mbed.lib ├── patterns.cpp ├── patterns.h ├── plan-position.cpp ├── plan-position.h ├── stepper.cpp └── stepper.h ├── mbed ├── common.h ├── dials.cpp ├── dials.h ├── i2c.cpp ├── i2c.h ├── main.cpp ├── mbed.lib ├── patterns.c ├── patterns.cpp ├── patterns.h ├── planner.cpp ├── planner.h ├── positioner.cpp ├── positioner.h ├── servo.cpp └── servo.h ├── pythagoras_android ├── .classpath ├── .project ├── AndroidManifest.xml ├── bin │ ├── classes.dex │ ├── com │ │ └── aaronbot3000 │ │ │ └── pythagorasandroid │ │ │ ├── R$attr.class │ │ │ ├── R$drawable.class │ │ │ ├── R$id.class │ │ │ ├── R$layout.class │ │ │ ├── R$string.class │ │ │ ├── R.class │ │ │ ├── StageOne$ButtonCallback.class │ │ │ ├── StageOne$SeekbarChangeCallback.class │ │ │ └── StageOne.class │ └── resources.ap_ ├── default.properties ├── gen │ ├── com │ │ └── aaronbot3000 │ │ │ └── pythagorasandroid │ │ │ └── R.java │ └── org │ │ └── opencv │ │ └── R.java ├── proguard.cfg ├── res │ ├── drawable-hdpi │ │ └── icon.png │ ├── drawable-ldpi │ │ └── icon.png │ ├── drawable-mdpi │ │ └── icon.png │ ├── layout │ │ └── main.xml │ └── values │ │ └── strings.xml └── src │ └── com │ └── aaronbot3000 │ └── pythagorasandroid │ └── StageOne.java ├── solidworks_models ├── #4 Screw.sldprt ├── 4-40_125_screw.sldprt ├── 4-40_14_screw.sldprt ├── 4-40_38_screw.sldprt ├── 4-40_nut.sldprt ├── 4-40_nut_wide.sldprt ├── 4-40_spacer.sldprt ├── arm.sldasm ├── balljoint.sldasm ├── balljoint_ball.sldprt ├── balljoint_joint.sldprt ├── baseplate.sldprt ├── cut_prep.dxf ├── cut_prep.sldasm ├── cut_prep.slddrw ├── elbow.sldasm ├── elbow_rod.sldprt ├── hand.sldasm ├── hand_base.sldprt ├── knife.sldprt ├── lower_arm.sldprt ├── pen.sldprt ├── robot.sldasm ├── servo.sldprt ├── servo_cover.sldprt ├── servo_with_horn.sldasm ├── servohorn1.sldprt ├── servohorn2.sldprt ├── servohorn3.sldprt ├── shaft_collar.sldprt ├── support.sldasm ├── support_lower.sldprt ├── support_upper.sldprt ├── upperarm.sldprt ├── wrist.sldasm └── wrist_connector.sldprt ├── solidworks_models_v2 ├── 4-40_12_screw.sldprt ├── 4-40_38_screw.sldprt ├── 4-40_nut.sldprt ├── 4-40_nut_wide.sldprt ├── 4-40_spacer.sldprt ├── M3-6_screw.sldprt ├── arm_collar.dxf ├── arm_collar.slddrw ├── arm_collar.sldprt ├── arm_lower.sldasm ├── arm_lower_rod.sldprt ├── arm_upper.sldprt ├── arm_upper_complete.sldasm ├── balljoint.sldasm ├── balljoint_ball.sldprt ├── balljoint_joint.sldprt ├── baseplate.sldprt ├── cut_parts_14.dxf ├── cut_parts_14.sldasm ├── cut_parts_14.slddrw ├── cut_parts_18.dxf ├── cut_parts_18.sldasm ├── cut_parts_18.slddrw ├── elbow.sldasm ├── elbow_rod.sldprt ├── gear.sldprt ├── gear_pot.sldprt ├── gear_stepper.sldprt ├── gusset_assm.dxf ├── gusset_assm.sldasm ├── gusset_assm.slddrw ├── gusset_plate.sldprt ├── hand.sldasm ├── hand_base.sldprt ├── motor_plate.sldprt ├── motor_plate_support.sldprt ├── new_support.sldasm ├── new_support_lower.sldprt ├── new_support_upper.sldprt ├── pen holder.sldprt ├── potentiometer.sldprt ├── potentiometer2.sldprt ├── robot.sldasm ├── robot_base.sldasm ├── stepper.sldprt ├── support.sldasm ├── support_lower.sldprt ├── support_support.dxf ├── support_support.sldprt ├── support_upper.sldprt ├── upperarm.sldprt ├── wrist.sldasm └── wrist_connector.sldprt ├── sympy_scripts ├── inv_kinematics.py ├── inv_kinematics_plain.py ├── resolved_rate.py └── test_idea.py ├── vectorizer-cpp ├── Makefile └── send-serial.cpp └── vectorizer ├── lena.bmp ├── rasterize.py ├── send-serial.py └── vectorize.py /.gitattributes: -------------------------------------------------------------------------------- 1 | *.sld* -text 2 | *.dxf -text 3 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.swp 2 | *.pyc 3 | *.class 4 | *.apk 5 | *.gch 6 | *.out 7 | .metadata 8 | ~* 9 | *~ 10 | controller/Debug/* 11 | controller/Release/* 12 | controller/.settings* 13 | controller/.launches* 14 | pythagoras/Debug/* 15 | pythagoras/Release/* 16 | pythagoras/.settings* 17 | pythagoras/.launches* 18 | vectorizer/*.jpg 19 | vectorizer/*.png 20 | vectorizer/*.gif 21 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Pythagoras, the Drawing Delta Robot 2 | =================================== 3 | 4 | By Aaron Fan 5 | 6 | A drawing delta robot with edge detecting vectorizing software. Read more at: 7 | 8 | http://aaronbot3000.blogspot.com/p/pythagoras.html 9 | -------------------------------------------------------------------------------- /controller/.ccsproject: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /controller/.cdtbuild: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 17 | 50 | 51 | 52 | 53 | 62 | 97 | 98 | 99 | 100 | 101 | -------------------------------------------------------------------------------- /controller/.cdtproject: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /controller/.launches/controller.launch: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /controller/.project: -------------------------------------------------------------------------------- 1 | 2 | 3 | controller 4 | 5 | 6 | 7 | 8 | 9 | org.eclipse.cdt.managedbuilder.core.genmakebuilder 10 | 11 | 12 | 13 | 14 | 15 | org.eclipse.cdt.core.cnature 16 | org.eclipse.cdt.managedbuilder.core.managedBuildNature 17 | org.eclipse.cdt.core.ccnature 18 | com.ti.ccstudio.managedbuild.core.ccsNature 19 | 20 | 21 | -------------------------------------------------------------------------------- /controller/CapacitiveTouchLibrary/CTS_HAL.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * 3 | * CTS_HAL.h 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 12 | * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the 15 | * distribution. 16 | * 17 | * Neither the name of Texas Instruments Incorporated nor the names of 18 | * its contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 24 | * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 25 | * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 26 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 27 | * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 28 | * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 29 | * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 30 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | ******************************************************************************/ 34 | 35 | /***************************************************************************//** 36 | * @file CTS_HAL.h 37 | * 38 | * @brief 39 | * 40 | * @par Project: 41 | * MSP430 Capacitive Touch Library 42 | * 43 | * @par Developed using: 44 | * IAR Version : 5.10.6 [Kickstart] (5.10.6.30180) 45 | * CCS Version : 4.2.1.00004, w/support for GCC extensions (--gcc) 46 | * 47 | * 48 | * @version 1.0.0 Initial Release 49 | * 50 | * @par Supported Hardware Configurations: 51 | * - TI_CTS_RO_COMPAp_TA0_WDTp_HAL() 52 | * - TI_CTS_fRO_COMPAp_TA0_SW_HAL() 53 | * - TI_CTS_fRO_COMPAp_SW_TA0_HAL() 54 | * - TI_CTS_RO_COMPAp_TA1_WDTp_HAL() 55 | * - TI_CTS_fRO_COMPAp_TA1_SW_HAL() 56 | * - TI_CTS_RC_PAIR_TA0_HAL() 57 | * - TI_CTS_RO_PINOSC_TA0_WDTp_HAL() 58 | * - TI_CTS_RO_PINOSC_TA0_HAL() 59 | * - TI_CTS_fRO_PINOSC_TA0_SW_HAL() 60 | * - TI_CTS_RO_COMPB_TA0_WDTA_HAL() 61 | * - TI_CTS_RO_COMPB_TA1_WDTA_HAL() 62 | * - TI_CTS_fRO_COMPB_TA0_SW_HAL() 63 | * - TI_CTS_fRO_COMPB_TA1_SW_HAL() 64 | ******************************************************************************/ 65 | 66 | #ifndef CAP_TOUCH_HAL 67 | #define CAP_TOUCH_HAL 68 | 69 | #include "structure.h" 70 | 71 | void TI_CTS_RO_COMPAp_TA0_WDTp_HAL(const struct Sensor *, uint16_t *); 72 | 73 | void TI_CTS_fRO_COMPAp_TA0_SW_HAL(const struct Sensor *, uint16_t *); 74 | 75 | void TI_CTS_fRO_COMPAp_SW_TA0_HAL(const struct Sensor *, uint16_t *); 76 | 77 | void TI_CTS_RO_COMPAp_TA1_WDTp_HAL(const struct Sensor *, uint16_t *); 78 | 79 | void TI_CTS_fRO_COMPAp_TA1_SW_HAL(const struct Sensor *, uint16_t *); 80 | 81 | void TI_CTS_RC_PAIR_TA0_HAL(const struct Sensor *, uint16_t *); 82 | 83 | void TI_CTS_RO_PINOSC_TA0_WDTp_HAL(const struct Sensor *, uint16_t *); 84 | 85 | void TI_CTS_RO_PINOSC_TA0_HAL(const struct Sensor *, uint16_t *); 86 | 87 | void TI_CTS_fRO_PINOSC_TA0_SW_HAL(const struct Sensor *, uint16_t *); 88 | 89 | void TI_CTS_RO_COMPB_TA0_WDTA_HAL(const struct Sensor *, uint16_t *); 90 | 91 | void TI_CTS_fRO_COMPB_TA0_SW_HAL(const struct Sensor *, uint16_t *); 92 | 93 | void TI_CTS_RO_COMPB_TA1_WDTA_HAL(const struct Sensor *, uint16_t *); 94 | 95 | void TI_CTS_fRO_COMPB_TA1_SW_HAL(const struct Sensor *, uint16_t *); 96 | 97 | #endif 98 | -------------------------------------------------------------------------------- /controller/CapacitiveTouchLibrary/CTS_Layer.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * 3 | * CTS_HAL.h 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 12 | * Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in the 14 | * documentation and/or other materials provided with the 15 | * distribution. 16 | * 17 | * Neither the name of Texas Instruments Incorporated nor the names of 18 | * its contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 24 | * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 25 | * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 26 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 27 | * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 28 | * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 29 | * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 30 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 31 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | ******************************************************************************/ 34 | 35 | /***************************************************************************//** 36 | * @file CTS_Layer.h 37 | * 38 | * @brief 39 | * 40 | * @par Project: 41 | * MSP430 Capacitive Touch Library 42 | * 43 | * @par Developed using: 44 | * IAR Version : 5.10.6 [Kickstart] (5.10.6.30180) 45 | * CCS Version : 4.2.1.00004, w/support for GCC extensions (--gcc) 46 | * 47 | * 48 | * @version 1.0.0 Initial Release 49 | * 50 | * @par Supported API Calls: 51 | * - TI_CAPT_Init_Baseline() 52 | * - TI_CAPT_Update_Baseline() 53 | * - TI_CAPT_Reset_Tracking() 54 | * - TI_CAPT_Update_Tracking_DOI() 55 | * - TI_CAPT_Update_Tracking_Rate() 56 | * - TI_CAPT_Update_Baseline() 57 | * - TI_CAPT_Raw() 58 | * - TI_CAPT_Custom() 59 | * - TI_CAPT_Button() 60 | * - TI_CAPT_Buttons() 61 | * - TI_CAPT_Slider() 62 | * - TI_CAPT_Wheel() 63 | ******************************************************************************/ 64 | 65 | #ifndef CTS_LAYER 66 | #define CTS_LAYER 67 | 68 | #include "CTS_HAL.h" 69 | 70 | //! \name Status Register Definitions 71 | //! @{ 72 | // 73 | //! (bit 0): Event Flag: this indicates that a threshold crossing occured 74 | #define EVNT 0x01 75 | //! (bit 1): Direction of Interest: This indicates if the measurement is looking 76 | //! for an increasing (set) or decreasing (clr) capacitance. 77 | #define DOI_MASK 0x02 78 | #define DOI_INC 0x02 79 | #define DOI_DEC 0x00 80 | //! (bit 2): Past Event Flag: this indicates that a prior element within the 81 | //! sensor group has detected a threshold crossing 82 | #define PAST_EVNT 0x04 83 | //! (bits 4-5): Tracking Rate in Direction of Interest: this indicates at what 84 | //! rate the baseline will adjust to the current measurement when the 85 | //! when the measurement is changing in the direction of interst but does not 86 | //! result in a threshold crossing: 87 | //! \n Very Slow 88 | //! \n Slow 89 | //! \n Medium 90 | //! \n Fast 91 | #define TRIDOI_VSLOW 0x00 92 | #define TRIDOI_SLOW 0x10 93 | #define TRIDOI_MED 0x20 94 | #define TRIDOI_FAST 0x30 95 | //! (bits 5-6): Tracking Rate Against Direction of Interest: this indicates at 96 | //! what rate the baseline will adjust to the current measurement when the 97 | //! when the measurement is changing against the direction of interst: 98 | //! \n Fast 99 | //! \n Medium 100 | //! \n Slow 101 | //! \n Very Slow 102 | #define TRADOI_FAST 0x00 103 | #define TRADOI_MED 0x40 104 | #define TRADOI_SLOW 0x80 105 | #define TRADOI_VSLOW 0xC0 106 | 107 | //! @} 108 | 109 | 110 | // API Calls 111 | void TI_CAPT_Init_Baseline(const struct Sensor*); 112 | void TI_CAPT_Update_Baseline(const struct Sensor*, uint8_t); 113 | 114 | void TI_CAPT_Reset_Tracking(void); 115 | void TI_CAPT_Update_Tracking_DOI(uint8_t); 116 | void TI_CAPT_Update_Tracking_Rate(uint8_t); 117 | 118 | void TI_CAPT_Raw(const struct Sensor*, uint16_t*); 119 | 120 | void TI_CAPT_Custom(const struct Sensor *, uint16_t*); 121 | 122 | uint8_t TI_CAPT_Button(const struct Sensor *); 123 | const struct Element * TI_CAPT_Buttons(const struct Sensor *); 124 | uint16_t TI_CAPT_Slider(const struct Sensor*); 125 | uint16_t TI_CAPT_Wheel(const struct Sensor*); 126 | 127 | // Internal Calls 128 | uint8_t Dominant_Element (const struct Sensor*, uint16_t*); 129 | 130 | #endif 131 | -------------------------------------------------------------------------------- /controller/CapacitiveTouchLibrary/structure.c: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * 3 | * structure.c 4 | * 5 | * Copyright (C) 2011 Texas Instruments Incorporated - http://www.ti.com/ 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 14 | * Redistributions in binary form must reproduce the above copyright 15 | * notice, this list of conditions and the following disclaimer in the 16 | * documentation and/or other materials provided with the 17 | * distribution. 18 | * 19 | * Neither the name of Texas Instruments Incorporated nor the names of 20 | * its contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 26 | * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 27 | * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 28 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 29 | * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 30 | * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 31 | * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 32 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 33 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | * 36 | ******************************************************************************/ 37 | //****************************************************************************** 38 | // structure.c 39 | // 40 | // 4 elements configured as a wheel. 41 | // 1 element configured as a button. [Center Button] 42 | // 1 element configured as a proximity sensor. 43 | // This file contains the structure names and the variable assingments. 44 | // 45 | // Revision 1.00 Baseline 46 | // Developed with IAR 5.10.4 [Kickstart] (5.10.4.30168) 47 | // 48 | // 09/24/10 Rev1.00 Pre-Alpha Version. Added "max_cnts" slider variable in 49 | // CT_Handler object. 50 | // 51 | // 09/29/10 0.02 Update naming convention for elements and sensors. 52 | // 53 | // 10/11/10 0.03 Update 54 | // 55 | //****************************************************************************** 56 | 57 | #include "structure.h" 58 | 59 | //PinOsc Volume down P2.2 60 | const struct Element volume_down = { 61 | 62 | .inputPxselRegister = (uint8_t *)&P2SEL, 63 | .inputPxsel2Register = (uint8_t *)&P2SEL2, 64 | .inputBits = BIT2, 65 | // measured for a 1Mhz SMCLK 66 | .maxResponse = 121+655, // actual measure was 980 67 | .threshold = 121 68 | 69 | }; 70 | 71 | //PinOsc forward right P2.3 72 | const struct Element right = { 73 | 74 | .inputPxselRegister = (uint8_t *)&P2SEL, 75 | .inputPxsel2Register = (uint8_t *)&P2SEL2, 76 | .inputBits = BIT3, 77 | // 1Mhz SMCLK 78 | .maxResponse = 113+655, 79 | .threshold = 113 80 | }; 81 | 82 | //PinOsc Volume up, P2.4 83 | const struct Element volume_up = { 84 | 85 | .inputPxselRegister = (uint8_t *)&P2SEL, 86 | .inputPxsel2Register = (uint8_t *)&P2SEL2, 87 | .inputBits = BIT4, 88 | // 1Mhz SMCLK 89 | .maxResponse = 118+655, 90 | .threshold = 118 91 | }; 92 | 93 | //PinOsc reverse left P2.1 94 | const struct Element left = { 95 | 96 | .inputPxselRegister = (uint8_t *)&P2SEL, 97 | .inputPxsel2Register = (uint8_t *)&P2SEL2, 98 | .inputBits = BIT1, 99 | // 1Mhz SMCLK 100 | .maxResponse = 111+655, 101 | .threshold = 111 102 | }; 103 | 104 | //PinOsc Wheel: middle button P2.5 105 | const struct Element middle_element = { 106 | 107 | .inputPxselRegister = (uint8_t *)&P2SEL, 108 | .inputPxsel2Register = (uint8_t *)&P2SEL2, 109 | .inputBits = BIT5, 110 | // When using an abstracted function to measure the element 111 | // the 100*(maxResponse - threshold) < 0xFFFF 112 | // ie maxResponse - threshold < 655 113 | .maxResponse = 350+655, 114 | .threshold = 350 115 | }; 116 | 117 | //PinOsc proximity: P2.0 118 | const struct Element proximity_element = { 119 | 120 | .inputPxselRegister = (uint8_t *)&P2SEL, 121 | .inputPxsel2Register = (uint8_t *)&P2SEL2, 122 | .inputBits = BIT0, 123 | .maxResponse = 200, 124 | .threshold = 130 125 | }; 126 | 127 | //*** CAP TOUCH HANDLER *******************************************************/ 128 | // This defines the grouping of sensors, the method to measure change in 129 | // capacitance, and the function of the group 130 | 131 | const struct Sensor wheel = 132 | { 133 | //.halDefinition = fRO_PINOSC_TA0_SW, 134 | .halDefinition = RO_PINOSC_TA0_WDTp, 135 | .numElements = 4, 136 | .points = 64, 137 | .sensorThreshold = 75, 138 | .baseOffset = 0, 139 | // Pointer to elements 140 | .arrayPtr[0] = &volume_up, // point to first element 141 | .arrayPtr[1] = &right, 142 | .arrayPtr[2] = &volume_down, 143 | .arrayPtr[3] = &left, 144 | // Timer Information 145 | //.measGateSource= GATE_WDT_SMCLK, // 0->SMCLK, 1-> ACLK 146 | //.accumulationCycles= WDTp_GATE_32768 //32768 147 | .accumulationCycles= WDTp_GATE_8192 // 8192 148 | //.accumulationCycles= WDTp_GATE_512 //512 149 | //.accumulationCycles= WDTp_GATE_64 //64 150 | //.accumulationCycles = 32 151 | }; 152 | 153 | const struct Sensor middle_button = 154 | { 155 | .halDefinition = RO_PINOSC_TA0_WDTp, 156 | .numElements = 1, 157 | .baseOffset = 4, 158 | // Pointer to elements 159 | .arrayPtr[0] = &middle_element, // point to first element 160 | // Timer Information 161 | .measGateSource= GATE_WDT_SMCLK, // 0->SMCLK, 1-> ACLK 162 | //.accumulationCycles= WDTp_GATE_32768 //32768 163 | .accumulationCycles= WDTp_GATE_8192 // 8192 164 | //.accumulationCycles= WDTp_GATE_512 //512 165 | //.accumulationCycles= WDTp_GATE_64 //64 166 | }; 167 | 168 | const struct Sensor proximity_sensor = 169 | { 170 | .halDefinition = RO_PINOSC_TA0_WDTp, 171 | .numElements = 1, 172 | .baseOffset = 5, 173 | // Pointer to elements 174 | .arrayPtr[0] = &proximity_element, // point to first element 175 | // Timer Information 176 | .measGateSource= GATE_WDT_SMCLK, // 0->SMCLK, 1-> ACLK 177 | //.accumulationCycles= WDTp_GATE_32768 //32768 178 | .accumulationCycles= WDTp_GATE_8192 // 8192 179 | //.accumulationCycles= WDTp_GATE_512 //512 180 | //.accumulationCycles= WDTp_GATE_64 //64 181 | }; 182 | -------------------------------------------------------------------------------- /controller/CapacitiveTouchLibrary/structure.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/controller/CapacitiveTouchLibrary/structure.h -------------------------------------------------------------------------------- /controller/MSP430G2452.ccxml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /controller/lnk_msp430g2452.cmd: -------------------------------------------------------------------------------- 1 | /******************************************************************************/ 2 | /* lnk_msp430g2452.cmd - LINKER COMMAND FILE FOR LINKING MSP430G2452 PROGRAMS */ 3 | /* */ 4 | /* Usage: lnk430 -o -m lnk.cmd */ 5 | /* cl430 -z -o -m lnk.cmd */ 6 | /* */ 7 | /*----------------------------------------------------------------------------*/ 8 | /* These linker options are for command line linking only. For IDE linking, */ 9 | /* you should set your linker options in Project Properties */ 10 | /* -c LINK USING C CONVENTIONS */ 11 | /* -stack 0x0100 SOFTWARE STACK SIZE */ 12 | /* -heap 0x0100 HEAP AREA SIZE */ 13 | /* */ 14 | /*----------------------------------------------------------------------------*/ 15 | 16 | 17 | /****************************************************************************/ 18 | /* SPECIFY THE SYSTEM MEMORY MAP */ 19 | /****************************************************************************/ 20 | 21 | MEMORY 22 | { 23 | SFR : origin = 0x0000, length = 0x0010 24 | PERIPHERALS_8BIT : origin = 0x0010, length = 0x00F0 25 | PERIPHERALS_16BIT : origin = 0x0100, length = 0x0100 26 | RAM : origin = 0x0200, length = 0x0100 27 | INFOA : origin = 0x10C0, length = 0x0040 28 | INFOB : origin = 0x1080, length = 0x0040 29 | INFOC : origin = 0x1040, length = 0x0040 30 | INFOD : origin = 0x1000, length = 0x0040 31 | FLASH : origin = 0xE000, length = 0x1FE0 32 | INT00 : origin = 0xFFE0, length = 0x0002 33 | INT01 : origin = 0xFFE2, length = 0x0002 34 | INT02 : origin = 0xFFE4, length = 0x0002 35 | INT03 : origin = 0xFFE6, length = 0x0002 36 | INT04 : origin = 0xFFE8, length = 0x0002 37 | INT05 : origin = 0xFFEA, length = 0x0002 38 | INT06 : origin = 0xFFEC, length = 0x0002 39 | INT07 : origin = 0xFFEE, length = 0x0002 40 | INT08 : origin = 0xFFF0, length = 0x0002 41 | INT09 : origin = 0xFFF2, length = 0x0002 42 | INT10 : origin = 0xFFF4, length = 0x0002 43 | INT11 : origin = 0xFFF6, length = 0x0002 44 | INT12 : origin = 0xFFF8, length = 0x0002 45 | INT13 : origin = 0xFFFA, length = 0x0002 46 | INT14 : origin = 0xFFFC, length = 0x0002 47 | RESET : origin = 0xFFFE, length = 0x0002 48 | } 49 | 50 | /****************************************************************************/ 51 | /* SPECIFY THE SECTIONS ALLOCATION INTO MEMORY */ 52 | /****************************************************************************/ 53 | 54 | SECTIONS 55 | { 56 | .bss : {} > RAM /* GLOBAL & STATIC VARS */ 57 | .sysmem : {} > RAM /* DYNAMIC MEMORY ALLOCATION AREA */ 58 | .stack : {} > RAM (HIGH) /* SOFTWARE SYSTEM STACK */ 59 | 60 | .text : {} > FLASH /* CODE */ 61 | .cinit : {} > FLASH /* INITIALIZATION TABLES */ 62 | .const : {} > FLASH /* CONSTANT DATA */ 63 | .cio : {} > RAM /* C I/O BUFFER */ 64 | 65 | .pinit : {} > FLASH /* C++ CONSTRUCTOR TABLES */ 66 | 67 | .infoA : {} > INFOA /* MSP430 INFO FLASH MEMORY SEGMENTS */ 68 | .infoB : {} > INFOB 69 | .infoC : {} > INFOC 70 | .infoD : {} > INFOD 71 | 72 | .int00 : {} > INT00 /* MSP430 INTERRUPT VECTORS */ 73 | .int01 : {} > INT01 74 | .int02 : {} > INT02 75 | .int03 : {} > INT03 76 | .int04 : {} > INT04 77 | .int05 : {} > INT05 78 | .int06 : {} > INT06 79 | .int07 : {} > INT07 80 | .int08 : {} > INT08 81 | .int09 : {} > INT09 82 | .int10 : {} > INT10 83 | .int11 : {} > INT11 84 | .int12 : {} > INT12 85 | .int13 : {} > INT13 86 | .int14 : {} > INT14 87 | .reset : {} > RESET /* MSP430 RESET VECTOR */ 88 | } 89 | 90 | /****************************************************************************/ 91 | /* INCLUDE PERIPHERALS MEMORY MAP */ 92 | /****************************************************************************/ 93 | 94 | -l msp430g2452.cmd 95 | 96 | -------------------------------------------------------------------------------- /controller/src/i2c.c: -------------------------------------------------------------------------------- 1 | #include "i2c.h" 2 | 3 | static const char SLV_Addr = 0x80; 4 | 5 | static volatile int I2C_State = 0; 6 | volatile unsigned char current_data = 0; 7 | 8 | void i2c_setup() { 9 | USICTL0 = USIPE6+USIPE7+USISWRST; // Port & USI mode setup 10 | USICTL1 = USII2C+USIIE+USISTTIE; // Enable I2C mode & USI interrupts 11 | USICKCTL = USICKPL; // Setup clock polarity 12 | USICNT |= USIIFGCC; // Disable automatic clear control 13 | USICTL0 &= ~USISWRST; // Enable USI 14 | USICTL1 &= ~USIIFG; // Clear pending flag 15 | _EINT(); 16 | } 17 | 18 | void set_i2c_data(char in) { 19 | current_data = in + 128; 20 | return; 21 | } 22 | 23 | //****************************************************** 24 | // USI interrupt service routine 25 | //****************************************************** 26 | #pragma vector = USI_VECTOR 27 | __interrupt void USI_TXRX (void) { 28 | if (USICTL1 & USISTTIFG) { // Start entry? 29 | I2C_State = 2; // Enter 1st state on start 30 | P1OUT &= ~0x01; 31 | } 32 | 33 | switch(I2C_State) { 34 | case 0: //Idle, should not get here 35 | break; 36 | 37 | case 2: //RX Address 38 | USICNT = (USICNT & 0xE0) + 0x08; // Bit counter = 8, RX Address 39 | USICTL1 &= ~USISTTIFG; // Clear start flag 40 | I2C_State = 4; // Go to next state: check address 41 | break; 42 | 43 | case 4: // Process Address and send (N)Ack 44 | USICTL0 |= USIOE; // SDA = output 45 | if (USISRL == (SLV_Addr | 0x1)) { // Address match? 46 | USISRL = 0x00; // Send Ack 47 | P1OUT |= 0x01; // LED on 48 | I2C_State = 8; // Go to next state: TX data 49 | } 50 | else { 51 | USISRL = 0xFF; // Send NAck 52 | P1OUT |= 0x01; // LED on: error 53 | I2C_State = 6; // Go to next state: prep for next Start 54 | } 55 | USICNT |= 0x01; // Bit counter = 1, send (N)Ack bit 56 | break; 57 | 58 | case 6: // Prep for Start condition 59 | USICTL0 &= ~USIOE; // SDA = input 60 | I2C_State = 0; // Reset state machine 61 | break; 62 | 63 | case 8: // Send Data byte 64 | USICTL0 |= USIOE; // SDA = output 65 | USISRL = current_data; 66 | current_data = 128; 67 | USICNT |= 0x08; // Bit counter = 8, TX data 68 | I2C_State = 10; // Go to next state: receive (N)Ack 69 | break; 70 | 71 | case 10:// Receive Data (N)Ack 72 | USICTL0 &= ~USIOE; // SDA = input 73 | USICNT |= 0x01; // Bit counter = 1, receive (N)Ack 74 | I2C_State = 12; // Go to next state: check (N)Ack 75 | break; 76 | 77 | case 12:// Process Data Ack/NAck 78 | if (USISRL & 0x01) { // If Nack received... 79 | P1OUT |= 0x01; // LED on: error 80 | } 81 | else { // Ack received 82 | P1OUT &= ~0x01; // LED off 83 | } 84 | // Prep for Start condition 85 | USICTL0 &= ~USIOE; // SDA = input 86 | I2C_State = 0; // Reset state machine 87 | P1OUT &= ~0x01; // LED off 88 | break; 89 | } 90 | 91 | USICTL1 &= ~USIIFG; // Clear pending flags 92 | } 93 | -------------------------------------------------------------------------------- /controller/src/i2c.h: -------------------------------------------------------------------------------- 1 | #ifndef I2C_H_ 2 | #define I2C_H_ 3 | 4 | #include 5 | 6 | #define INVALID_POS 0x7F 7 | 8 | void i2c_setup(); 9 | void set_i2c_data(char in); 10 | 11 | #endif /*I2C_H_*/ 12 | -------------------------------------------------------------------------------- /controller/src/main.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "i2c.h" 4 | #include "wheel.h" 5 | 6 | volatile char I2C_data; 7 | 8 | void setup() { 9 | // Stop watchdog 10 | WDTCTL = WDTPW + WDTHOLD; 11 | if (CALBC1_8MHZ ==0xFF || CALDCO_8MHZ == 0xFF) { 12 | // If calibration constants erased 13 | // do not load, trap CPU!! 14 | while(1) P1OUT ^= 0x01; 15 | } 16 | 17 | BCSCTL1 |= DIVA_0; // ACLK/(0:1,1:2,2:4,3:8) 18 | BCSCTL3 |= LFXT1S_2; // LFXT1 = VLO 19 | 20 | // Port init 21 | P1OUT &= ~(BIT3+BIT4+BIT5+BIT0); 22 | P1DIR |= BIT3+BIT4+BIT5+BIT0; 23 | P2SEL = 0x00; // No XTAL 24 | P2DIR |= (BIT0+BIT4+BIT2+BIT3+BIT1+BIT5); 25 | P2OUT &= ~(BIT0+BIT4+BIT2+BIT3+BIT1+BIT5); 26 | 27 | // Set DCO 28 | BCSCTL1 = CALBC1_8MHZ; 29 | DCOCTL = CALDCO_8MHZ; 30 | BCSCTL2 |= DIVS_3; 31 | 32 | TACCTL0 &= ~CCIE; 33 | } 34 | 35 | void main(void) { 36 | setup(); 37 | wheel_setup(); 38 | i2c_setup(); 39 | 40 | while(1) { 41 | // CPU off, await USI interrupt 42 | // LPM0; 43 | set_i2c_data(get_wheel_reading()); 44 | } 45 | } 46 | -------------------------------------------------------------------------------- /controller/src/wheel.c: -------------------------------------------------------------------------------- 1 | #include "wheel.h" 2 | 3 | static short pos_filt, last_pos_filt, last_pos, need_init; 4 | static int no_wrap; 5 | 6 | void wheel_setup() { 7 | P1OUT = BIT0; 8 | 9 | TI_CAPT_Init_Baseline(&wheel); 10 | TI_CAPT_Update_Baseline(&wheel,2); 11 | need_init = 1; 12 | } 13 | 14 | // Read the wheel position, filter it, and add hyteresis before returning 15 | // for super smooth etch-a-sketch operation 16 | // Returns a delta position, not absolute position 17 | char get_wheel_reading() { 18 | short ret_val = 0; 19 | short diff, reading, i; 20 | 21 | reading = TI_CAPT_Wheel(&wheel); 22 | 23 | 24 | if (reading != ILLEGAL_SLIDER_WHEEL_POSITION) { 25 | if (need_init) { 26 | no_wrap = reading >> 2; 27 | for (i = 0; i < 3; i++) { 28 | no_wrap += TI_CAPT_Wheel(&wheel) >> 2; 29 | } 30 | last_pos = no_wrap; 31 | pos_filt = no_wrap; 32 | last_pos_filt = ILLEGAL_SLIDER_WHEEL_POSITION; 33 | need_init = 0; 34 | } 35 | else { 36 | diff = reading - last_pos; 37 | if (diff > 32 && reading > 48 && last_pos < 16) 38 | diff -= 64; 39 | else if (diff < -32 && reading < 48 && last_pos > 16) 40 | diff += 64; 41 | 42 | no_wrap += diff; 43 | last_pos = reading; 44 | 45 | last_pos_filt = pos_filt; 46 | pos_filt = (3 * pos_filt) >> 2; 47 | pos_filt += no_wrap >> 2; 48 | 49 | if (pos_filt >= 255) { 50 | pos_filt -= 127; 51 | last_pos_filt -= 127; 52 | no_wrap -= 127; 53 | } 54 | if (pos_filt <= 0) { 55 | pos_filt += 128; 56 | last_pos_filt += 128; 57 | no_wrap += 128; 58 | } 59 | } 60 | 61 | // Indicate a touch recognized 62 | P1OUT |= MASK6 | MASK7; 63 | 64 | if (last_pos_filt != ILLEGAL_SLIDER_WHEEL_POSITION) { 65 | ret_val = pos_filt - last_pos_filt; 66 | } 67 | else 68 | ret_val = 0; 69 | } 70 | // no wheel position was detected 71 | else { 72 | P1OUT &= ~(MASK6 | MASK7); 73 | 74 | need_init = 1; 75 | } 76 | // saturate ret_val into a signed char, just in case 77 | if (ret_val > 127) 78 | ret_val = 127; 79 | if (ret_val < -128) 80 | ret_val = -128; 81 | return ret_val; 82 | } 83 | -------------------------------------------------------------------------------- /controller/src/wheel.h: -------------------------------------------------------------------------------- 1 | #ifndef WHEEL_H_ 2 | #define WHEEL_H_ 3 | 4 | #include "CTS_Layer.h" 5 | 6 | #define MASK7 BIT4 7 | #define MASK6 BIT5 8 | 9 | #define HYSTERESIS 3 10 | 11 | #define ABS(a) ((a) < 0 ? -(a) : (a)) 12 | 13 | void wheel_setup(); 14 | char get_wheel_reading(); 15 | 16 | #endif /*WHEEL_H_*/ 17 | -------------------------------------------------------------------------------- /kinematicstest/Makefile: -------------------------------------------------------------------------------- 1 | all: 2 | g++ -O3 *.h *.cpp 3 | -------------------------------------------------------------------------------- /kinematicstest/common.h: -------------------------------------------------------------------------------- 1 | #ifndef COMMON_H 2 | #define COMMON_H 3 | 4 | #define ARM_UPPER_LEN 2.489 5 | #define ARM_LOWER_LEN 10.546875 6 | 7 | //#define TESTVAL 8 | 9 | 10 | #ifndef TESTVAL 11 | 12 | #define HAND_XOFF 1.09725 13 | 14 | #define SERVO_XOFF -2.2085 15 | #define SERVO_ZOFF 0.51 16 | 17 | #else 18 | 19 | #define HAND_XOFF 0 20 | 21 | #define SERVO_XOFF -0 22 | #define SERVO_ZOFF 0 23 | 24 | #endif 25 | 26 | 27 | #define MAX_Z 11.5 28 | #define MIN_Z 10.15 29 | 30 | #define MAX_X 3.1 31 | #define MIN_X -3.1 32 | 33 | #define MAX_Y 3.1 34 | #define MIN_Y -3.1 35 | 36 | #define START_Z (MIN_Z - 0.5) 37 | 38 | #define MAX(a, b) ((a) > (b) ? (a) : (b)) 39 | #define MIN(a, b) ((a) < (b) ? (a) : (b)) 40 | #define RESTRICT(a, b1, b2) (MIN(b2, MAX(a, b1))) 41 | #define MAP(a, b1, b2, x1, x2) (((F32)((a) - (b1)) / (F32)((b2) - (b1))) * ((x2) - (x1)) + (x1)) 42 | 43 | #define DEBUG 44 | 45 | typedef enum { 46 | SUCCESS, 47 | FAILURE, 48 | END_PAT 49 | } Status; 50 | 51 | typedef unsigned char U08; 52 | typedef unsigned short U16; 53 | typedef unsigned int U32; 54 | typedef unsigned long long U64; 55 | 56 | typedef signed char S08; 57 | typedef short S16; 58 | typedef int S32; 59 | typedef long long S64; 60 | 61 | typedef double F32; 62 | typedef double F64; 63 | 64 | struct Point { 65 | F32 x; 66 | F32 y; 67 | F32 z; 68 | 69 | Point& operator=(const Point &rhs) { 70 | x = rhs.x; 71 | y = rhs.y; 72 | z = rhs.z; 73 | return *this; 74 | } 75 | 76 | /* 77 | Point& operator=(Point &rhs) { 78 | x = rhs.x; 79 | y = rhs.y; 80 | z = rhs.z; 81 | return *this; 82 | } 83 | */ 84 | 85 | /* 86 | Point(const Point& in) { 87 | x = in.x; 88 | y = in.y; 89 | z = in.z; 90 | } 91 | */ 92 | 93 | Point(volatile Point& in) { 94 | x = in.x; 95 | y = in.y; 96 | z = in.z; 97 | } 98 | 99 | Point() { 100 | x = y = z = 0; 101 | } 102 | }; 103 | 104 | #endif 105 | -------------------------------------------------------------------------------- /kinematicstest/derivative.c: -------------------------------------------------------------------------------- 1 | /* 2 | 30us per arm for geometric 3 | 50us per arm for trig 4 | */ 5 | inline float p2(float in) { 6 | return in * in; 7 | } 8 | 9 | inline float p3(float in) { 10 | return in * in * in; 11 | } 12 | 13 | float deriv(float x, float y, float z, float dx, float dy, float dz) { 14 | float l1_2 = p2(l1); 15 | float l2_2 = p2(l2); 16 | float x2 = p2(x); 17 | float y2 = p2(y); 18 | float z2 = p2(z); 19 | 20 | float a = l1_2 - l2_2 + x2 + y2 + z2; 21 | float b = sqrt(1 - p2(a) / (4 * l1_2 * (y2 + z2))); 22 | float c = sqrt(y2 + z2); 23 | float d = a / (2 * l1 * p3(c)); 24 | float e = (1 / (l1 * c) - d) / b; 25 | 26 | float xdot = -x / (l1 * b * c); 27 | float ydot = -z / (y2 + z2) - y * e; 28 | float zdot = y / (y2 + z2) - z * e; 29 | 30 | step xdot * dx + ydot * dy + zdot * dz; 31 | 32 | } 33 | 34 | /* 35 | respect to x 36 | -x_des/(l_1*(1 - (l_1**2 - l_2**2 + x_des**2 + y_des**2 + z_des**2)**2/(4*l_1**2*(y_des**2 + z_des**2)))**(1/2)*(y_des**2 + z_des**2)**(1/2)) 37 | 38 | respect to y 39 | -z_des/(y_des**2 + z_des**2) - 40 | (y_des/(l_1*(y_des**2 + z_des**2)**(1/2)) - y_des*(l_1**2 - l_2**2 + x_des**2 + y_des**2 + z_des**2)/(2*l_1*(y_des**2 + z_des**2)**(3/2)))/ 41 | (1 - (l_1**2 - l_2**2 + x_des**2 + y_des**2 + z_des**2)**2/(4*l_1**2*(y_des**2 + z_des**2)))**(1/2) 42 | 43 | respect to z 44 | y_des/(y_des**2 + z_des**2) - 45 | (z_des/(l_1*(y_des**2 + z_des**2)**(1/2)) - z_des*(l_1**2 - l_2**2 + x_des**2 + y_des**2 + z_des**2)/(2*l_1*(y_des**2 + z_des**2)**(3/2)))/ 46 | (1 - (l_1**2 - l_2**2 + x_des**2 + y_des**2 + z_des**2)**2/(4*l_1**2*(y_des**2 + z_des**2)))**(1/2) 47 | */ 48 | -------------------------------------------------------------------------------- /kinematicstest/kinematics.cpp: -------------------------------------------------------------------------------- 1 | #include "kinematics.h" 2 | 3 | Point rotate_xy(Point coord, F32 sin_theta, F32 cos_theta) { 4 | Point ret; 5 | ret.x = coord.x * cos_theta - coord.y * sin_theta; 6 | ret.y = coord.x * sin_theta + coord.y * cos_theta; 7 | ret.z = coord.z; 8 | return ret; 9 | } 10 | 11 | F32 r2(F32 in) { 12 | return in * in; 13 | } 14 | 15 | Status inv_kinematics(F32* result, Point target) { 16 | F32 dist, inv_dist, alpha; 17 | F32 angle; 18 | F32 x1, z1, x2, z2, h; 19 | F32 lower_radius; 20 | Point target_rot, trans; 21 | 22 | target_rot.x = target.x; 23 | target_rot.y = target.y; 24 | target_rot.z = target.z; 25 | 26 | for (int i = 0; i < 3; i++) { 27 | // rotate coordinates 28 | if (i == 1) 29 | target_rot = rotate_xy(target, SIN_120, COS_120); 30 | if (i == 2) 31 | target_rot = rotate_xy(target, SIN_240, COS_240); 32 | 33 | // Add servo offset and hand offset 34 | trans.x = target_rot.x + SERVO_XOFF + HAND_XOFF; 35 | trans.y = target_rot.y; 36 | // Add servo offset and tool offset 37 | trans.z = target_rot.z + SERVO_ZOFF; 38 | 39 | lower_radius = sqrt(r2(ARM_LOWER_LEN) - r2(trans.y)); 40 | 41 | dist = sqrt(r2(trans.x) + r2(trans.z)); 42 | // Inverse square root!!! 43 | inv_dist = 1 / dist; 44 | 45 | // Bounds checking 46 | if (dist > (ARM_UPPER_LEN + lower_radius) || 47 | dist < (lower_radius - ARM_UPPER_LEN)) { 48 | printf("OH FFFFFFUUUUUUUU Inv Failure %.5f, %.5f, %.5f\r\n", dist, ARM_UPPER_LEN, lower_radius); 49 | //led2 = 1; 50 | return FAILURE; 51 | } 52 | 53 | alpha = (r2(ARM_UPPER_LEN) - r2(lower_radius) + r2(dist)) * 0.5 * inv_dist; 54 | 55 | x1 = (trans.x * alpha * inv_dist); 56 | z1 = (trans.z * alpha * inv_dist); 57 | 58 | h = sqrt(r2(ARM_UPPER_LEN) - r2(alpha)); 59 | 60 | x2 = -trans.z * (h * inv_dist); 61 | z2 = trans.x * (h * inv_dist); 62 | 63 | angle = atan2(z1 - z2, x1 - x2); 64 | 65 | result[i] = angle; 66 | } 67 | return SUCCESS; 68 | } 69 | 70 | Status fwd_kinematics(Point* target, F32* angles) { 71 | F32 arm1 = ARM_UPPER_LEN * cos(angles[0]); 72 | F32 a1 = (arm1 - SERVO_XOFF - HAND_XOFF); 73 | F32 c1 = ARM_UPPER_LEN * sin(angles[0]) - SERVO_ZOFF; 74 | 75 | F32 arm2 = ARM_UPPER_LEN * cos(angles[2]); 76 | F32 a2 = (arm2 - SERVO_XOFF - HAND_XOFF) * COS_120; 77 | F32 b2 = (arm2 - SERVO_XOFF - HAND_XOFF) * SIN_120; 78 | F32 c2 = ARM_UPPER_LEN * sin(angles[2]) - SERVO_ZOFF; 79 | 80 | F32 arm3 = ARM_UPPER_LEN * cos(angles[1]); 81 | F32 a3 = (arm3 - SERVO_XOFF - HAND_XOFF) * COS_240; 82 | F32 b3 = (arm3 - SERVO_XOFF - HAND_XOFF) * SIN_240; 83 | F32 c3 = ARM_UPPER_LEN * sin(angles[1]) - SERVO_ZOFF; 84 | 85 | F32 p11 = 2 * (-a1 + a2); 86 | F32 p12 = 2 * b2; 87 | F32 p13 = 2 * (-c1 + c2); 88 | 89 | F32 d1 = -(r2(a1) + r2(c1)) + (r2(a2) + r2(b2) + r2(c2)); 90 | 91 | F32 p21 = 2 * (-a2 + a3); 92 | F32 p22 = 2 * (-b2 + b3); 93 | F32 p23 = 2 * (-c2 + c3); 94 | 95 | F32 d2 = -(r2(a2) + r2(b2) + r2(c2)) + (r2(a3) + r2(b3) + r2(c3)); 96 | 97 | F32 s1 = p12 * p23 - p13 * p22; 98 | F32 s2 = p13 * p21 - p11 * p23; 99 | F32 s3 = p11 * p22 - p12 * p21; 100 | 101 | //printf("p11: %f p12: %f p21 %f p22 %f\n", p11, p12, p21, p22); 102 | F32 py; 103 | F32 px; 104 | 105 | if (p21 < 1E-6 || -p21 < 1E-6) { 106 | py = d2 / p22; 107 | // TODO: if p11 < 1E-6 108 | px = (d1 - py * p12) / p11; 109 | } 110 | // TODO: if p11 < 1E-6 or (p12 * mult - p22) for some reason is zero 111 | else { 112 | F32 mult = p21 / p11; 113 | py = (d1 * mult - d2) / (p12 * mult - p22); 114 | px = (d2 - py * p22) / p21; 115 | } 116 | 117 | F32 A = r2(s1) + r2(s2) + r2(s3); 118 | F32 B = 2 * (s1 * (-a1 + px) + s2 * py - s3 * c1); 119 | F32 C = r2(-a1 + px) + r2(py) + r2(c1) - r2(ARM_LOWER_LEN); 120 | 121 | /* 122 | printf("%f\n", A); 123 | printf("%f\n", B); 124 | printf("%f\n", C); 125 | */ 126 | 127 | F32 t = (-B + sqrt(r2(B) - 4 * A * C)) / (2 * A); 128 | 129 | target->x = s1 * t + px; 130 | target->y = s2 * t + py; 131 | target->z = s3 * t; 132 | 133 | return SUCCESS; 134 | } 135 | 136 | 137 | -------------------------------------------------------------------------------- /kinematicstest/kinematics.h: -------------------------------------------------------------------------------- 1 | #ifndef KINEMATICS_H 2 | #define KINEMATICS_H 3 | 4 | #include "common.h" 5 | #include 6 | #include 7 | 8 | #define COS_30 0.866025404 9 | #define COS_60 0.5 10 | #define COS_120 -0.5 11 | #define COS_240 -0.5 12 | 13 | #define SIN_60 0.866025404 14 | #define SIN_120 0.866025404 15 | #define SIN_240 -0.866025404 16 | 17 | #define TAN_30 1.73205081 18 | #define TAN_60 0.577350269 19 | 20 | Status inv_kinematics(F32* result, Point target); 21 | Status fwd_kinematics(Point* target, F32* angles); 22 | 23 | #endif 24 | -------------------------------------------------------------------------------- /kinematicstest/main.cpp: -------------------------------------------------------------------------------- 1 | #include "common.h" 2 | #include "kinematics.h" 3 | #include 4 | 5 | #define TODEG (180.0 / 3.14159265) 6 | 7 | int main() { 8 | Point in; 9 | in.x = 0; 10 | in.y = 0; 11 | in.z = 11; 12 | F32 angles[3]; 13 | printf("point: %f %f %f\n", in.x, in.y, in.z); 14 | inv_kinematics(angles, in); 15 | printf("angles: %f %f %f\n", angles[0] * TODEG, angles[1] * TODEG, angles[2] * TODEG); 16 | 17 | fwd_kinematics(&in, angles); 18 | printf("point: %f %f %f\n", in.x, in.y, in.z); 19 | inv_kinematics(angles, in); 20 | printf("angles: %f %f %f\n", angles[0] * TODEG, angles[1] * TODEG, angles[2] * TODEG); 21 | 22 | in.x = 3; 23 | in.y = 4; 24 | in.z = 10; 25 | angles[3]; 26 | printf("point: %f %f %f\n", in.x, in.y, in.z); 27 | inv_kinematics(angles, in); 28 | printf("angles: %f %f %f\n", angles[0] * TODEG, angles[1] * TODEG, angles[2] * TODEG); 29 | 30 | fwd_kinematics(&in, angles); 31 | printf("point: %f %f %f\n", in.x, in.y, in.z); 32 | inv_kinematics(angles, in); 33 | printf("angles: %f %f %f\n", angles[0] * TODEG, angles[1] * TODEG, angles[2] * TODEG); 34 | 35 | in.x = 3; 36 | in.y = 4; 37 | in.z = 11; 38 | angles[3]; 39 | printf("point: %f %f %f\n", in.x, in.y, in.z); 40 | inv_kinematics(angles, in); 41 | printf("angles: %f %f %f\n", angles[0] * TODEG, angles[1] * TODEG, angles[2] * TODEG); 42 | 43 | fwd_kinematics(&in, angles); 44 | printf("point: %f %f %f\n", in.x, in.y, in.z); 45 | inv_kinematics(angles, in); 46 | printf("angles: %f %f %f\n", angles[0] * TODEG, angles[1] * TODEG, angles[2] * TODEG); 47 | return 0; 48 | } 49 | -------------------------------------------------------------------------------- /mbed-stepper/common.h: -------------------------------------------------------------------------------- 1 | #ifndef COMMON_H 2 | #define COMMON_H 3 | 4 | #include "mbed.h" 5 | #include "math.h" 6 | 7 | #define ARM_UPPER_LEN 2.75 8 | #define ARM_LOWER_LEN 13.125 9 | 10 | #define HAND_XOFF 1.09725 11 | #define HAND_ZOFF -0.20975 12 | 13 | #define TOOL_ZOFF -1.78000 14 | 15 | #define SERVO_XOFF -4.0096 16 | #define SERVO_ZOFF 0.7087 17 | 18 | #define MAX_Z 13 19 | #define MIN_Z 10.0 20 | 21 | #define MAX_X 3.85 22 | #define MIN_X -3.85 23 | 24 | #define MAX_Y 3.85 25 | #define MIN_Y -3.85 26 | 27 | #define START_Z (MIN_Z + 0.1) 28 | 29 | #define MAX(a, b) ((a) > (b) ? (a) : (b)) 30 | #define MIN(a, b) ((a) < (b) ? (a) : (b)) 31 | 32 | #define RESTRICT(a, b1, b2) (MIN(b2, MAX(a, b1))) 33 | 34 | #define MAP(a, b1, b2, x1, x2) (((F32)((a) - (b1)) / (F32)((b2) - (b1))) * ((x2) - (x1)) + (x1)) 35 | #define MAPEXP(a, b1, b2, x1, x2) (x1 * pow((float)1.5, -(((F32)((a) - (b1)) / (F32)((b2) - (b1))) * x2))) 36 | 37 | #define DEBUG 38 | 39 | typedef enum { 40 | SUCCESS, 41 | FAILURE, 42 | END_PAT, 43 | RUN_MORE 44 | } Status; 45 | 46 | typedef unsigned char U08; 47 | typedef unsigned short U16; 48 | typedef unsigned int U32; 49 | typedef unsigned long long U64; 50 | 51 | typedef signed char S08; 52 | typedef short S16; 53 | typedef int S32; 54 | typedef long long S64; 55 | 56 | typedef float F32; 57 | typedef double F64; 58 | 59 | struct Point { 60 | F32 x; 61 | F32 y; 62 | F32 z; 63 | bool is_traverse; 64 | 65 | Point& operator=(const Point &rhs) { 66 | x = rhs.x; 67 | y = rhs.y; 68 | z = rhs.z; 69 | is_traverse = rhs.is_traverse; 70 | return *this; 71 | } 72 | 73 | Point(volatile Point& in) { 74 | x = in.x; 75 | y = in.y; 76 | z = in.z; 77 | is_traverse = in.is_traverse; 78 | } 79 | 80 | Point() { 81 | x = y = z = 0; 82 | } 83 | 84 | Point(F32 a, F32 b, F32 c) { 85 | x = a; 86 | y = b; 87 | z = c; 88 | is_traverse = false; 89 | } 90 | 91 | Point(F32 a, F32 b, F32 c, bool t) { 92 | x = a; 93 | y = b; 94 | z = c; 95 | is_traverse = t; 96 | } 97 | }; 98 | 99 | #endif -------------------------------------------------------------------------------- /mbed-stepper/kinematics.cpp: -------------------------------------------------------------------------------- 1 | #include "kinematics.h" 2 | 3 | Point rotate_xy(Point coord, F32 sin_theta, F32 cos_theta) { 4 | Point ret; 5 | ret.x = coord.x * cos_theta - coord.y * sin_theta; 6 | ret.y = coord.x * sin_theta + coord.y * cos_theta; 7 | ret.z = coord.z; 8 | return ret; 9 | } 10 | 11 | inline F32 r2(F32 in) { 12 | return in * in; 13 | } 14 | 15 | //extern Serial pc; 16 | 17 | Status inv_kinematics(F32* result, Point target) { 18 | F32 dist, inv_dist, alpha; 19 | F32 angle; 20 | F32 x1, z1, x2, z2, h; 21 | F32 lower_radius; 22 | Point target_rot, trans; 23 | 24 | target_rot.x = target.x; 25 | target_rot.y = target.y; 26 | target_rot.z = target.z; 27 | 28 | for (int i = 0; i < 3; i++) { 29 | // rotate coordinates 30 | if (i == 1) 31 | target_rot = rotate_xy(target, SIN_120, COS_120); 32 | if (i == 2) 33 | target_rot = rotate_xy(target, SIN_240, COS_240); 34 | 35 | // Add servo offset and hand offset 36 | trans.x = target_rot.x + SERVO_XOFF + HAND_XOFF; 37 | trans.y = target_rot.y; 38 | // Add servo offset and tool offset 39 | trans.z = target_rot.z + SERVO_ZOFF; 40 | 41 | lower_radius = sqrt(r2(ARM_LOWER_LEN) - r2(trans.y)); 42 | 43 | dist = sqrt(r2(trans.x) + r2(trans.z)); 44 | // Inverse square root!!! 45 | inv_dist = 1 / dist; 46 | 47 | // Bounds checking 48 | if (dist > (ARM_UPPER_LEN + lower_radius) || 49 | dist < (lower_radius - ARM_UPPER_LEN)) { 50 | //pc.printf("OH FFFFFFUUUUUUUU Inv Failure %.5f, %.5f, %.5f\r\n", dist, ARM_UPPER_LEN, lower_radius); 51 | //led2 = 1; 52 | return FAILURE; 53 | } 54 | 55 | alpha = (r2(ARM_UPPER_LEN) - r2(lower_radius) + r2(dist)) * 0.5 * inv_dist; 56 | 57 | x1 = (trans.x * alpha * inv_dist); 58 | z1 = (trans.z * alpha * inv_dist); 59 | 60 | h = sqrt(r2(ARM_UPPER_LEN) - r2(alpha)); 61 | 62 | x2 = -trans.z * (h * inv_dist); 63 | z2 = trans.x * (h * inv_dist); 64 | 65 | angle = atan2(z1 - z2, x1 - x2); 66 | 67 | result[i] = angle; 68 | } 69 | return SUCCESS; 70 | } -------------------------------------------------------------------------------- /mbed-stepper/kinematics.h: -------------------------------------------------------------------------------- 1 | #ifndef KINEMATICS_H 2 | #define KINEMATICS_H 3 | 4 | #include "mbed.h" 5 | #include "common.h" 6 | 7 | #define COS_30 0.866025404 8 | #define TAN_60 0.577350269 9 | #define SIN_120 0.866025404 10 | #define COS_120 -0.5 11 | #define SIN_240 -0.866025404 12 | #define COS_240 -0.5 13 | 14 | Status inv_kinematics(F32* result, Point target); 15 | Status fwd_kinematics(Point* target, F32* angles); 16 | 17 | #endif 18 | -------------------------------------------------------------------------------- /mbed-stepper/main.cpp: -------------------------------------------------------------------------------- 1 | #include "mbed.h" 2 | 3 | #include "common.h" 4 | #include "plan-position.h" 5 | #include "patterns.h" 6 | 7 | #define moves_z (MAX(draw_z - 0.25, MIN_Z - 0.25)) 8 | 9 | #define START_TRANS 'B' 10 | #define END_TRANS 0xFFFF1111 11 | 12 | Serial pc(USBTX, USBRX); // tx, rx 13 | DigitalOut led1(LED1); 14 | DigitalOut led2(LED2); 15 | DigitalOut led3(LED3); 16 | DigitalOut led4(LED4); 17 | 18 | DigitalIn goto_draw_height(p16); 19 | DigitalIn troll_up(p15); 20 | DigitalIn troll_down(p14); 21 | DigitalIn start_pat(p13); 22 | 23 | DigitalOut enablers[] = {DigitalOut(p27), DigitalOut(p28), DigitalOut(p29)}; 24 | 25 | volatile U08 serial_buffer[16]; 26 | volatile S32 sbuffer_index; 27 | 28 | static F32 draw_z = MIN_Z + 0.6; // inches 29 | bool pen_needs_reset = false; 30 | 31 | Planner planner; 32 | 33 | void serial_callback() { 34 | serial_buffer[sbuffer_index++] = pc.getc(); 35 | } 36 | 37 | void startup_steppers() { 38 | enablers[0] = 0; 39 | for (int i = 1; i < 3; i++) { 40 | enablers[i] = 1; 41 | } 42 | wait_ms(100); 43 | enablers[1] = 0; 44 | wait_ms(100); 45 | enablers[2] = 0; 46 | wait_ms(100); 47 | } 48 | 49 | void setup() { 50 | startup_steppers(); 51 | 52 | sbuffer_index = 0; 53 | pc.baud(115200); 54 | pc.attach(serial_callback); 55 | 56 | //pc.printf("reset to %d %d %d\r\n", home.x, home.y, home.z); 57 | 58 | setup_planner(&planner); 59 | 60 | ///pc.printf("Setup\r\n"); 61 | } 62 | 63 | 64 | Status fill_buffer() { 65 | int i; 66 | Point in; 67 | 68 | led3 = 1; 69 | pause_steppers(&planner); 70 | pc.putc(START_TRANS); 71 | for (i = 0; i < BUFFER_SIZE - 2; i++) { 72 | while (sbuffer_index < 9); 73 | 74 | if ((*(U32*)(&serial_buffer[0])) == END_TRANS) { 75 | resume_steppers(&planner); 76 | return END_PAT; 77 | } 78 | sbuffer_index = 0; 79 | 80 | if (i < BUFFER_SIZE - 3) 81 | pc.putc(START_TRANS); 82 | 83 | in.x = *(F32*)(&serial_buffer[0]); 84 | in.y = *(F32*)(&serial_buffer[4]); 85 | 86 | if (serial_buffer[8] == 1) { 87 | in.z = moves_z; 88 | in.is_traverse = true; 89 | } 90 | else { 91 | in.z = draw_z; 92 | in.is_traverse = false; 93 | } 94 | 95 | if (!add_point_to_buffer(&planner, in)) { 96 | led2 = 1; 97 | } 98 | } 99 | 100 | //wait_ms(2000); 101 | led3 = 0; 102 | //update_pos(); 103 | resume_steppers(&planner); 104 | 105 | return SUCCESS; 106 | } 107 | 108 | void adj_z() { 109 | Point next_pos = planner.current_pos; 110 | next_pos.is_traverse = true; 111 | if (troll_up && draw_z > (MIN_Z + 0.6)) { 112 | //pc.printf("up\r\n"); 113 | draw_z -= 0.03; 114 | next_pos.z = draw_z; 115 | add_point_to_buffer(&planner, next_pos); 116 | } 117 | else if (troll_down && draw_z < (MAX_Z)) { 118 | //pc.printf("down\r\n"); 119 | draw_z += 0.03; 120 | next_pos.z = draw_z; 121 | add_point_to_buffer(&planner, next_pos); 122 | } 123 | else { 124 | next_pos.z = draw_z; 125 | add_point_to_buffer(&planner, next_pos); 126 | } 127 | wait_for_pattern(&planner); 128 | pen_needs_reset = true; 129 | wait_us(10000); 130 | } 131 | 132 | void reset_pen() { 133 | Point a(0, 0, START_Z, true); 134 | add_point_to_buffer(&planner, a); 135 | 136 | wait_for_pattern(&planner); 137 | } 138 | 139 | int main() { 140 | //Status status; 141 | setup(); 142 | 143 | // adjust z 144 | while (1) { 145 | if (goto_draw_height) { 146 | adj_z(); 147 | } 148 | else if (start_pat) { 149 | pen_needs_reset = true; 150 | calibrate(); 151 | //pc.printf("starting pattern\r\n"); 152 | Status status = SUCCESS; 153 | sbuffer_index = 0; 154 | while (status != END_PAT) { 155 | status = fill_buffer(); 156 | wait_for_pattern(&planner); 157 | } 158 | pc.putc('D'); 159 | } 160 | else if (pen_needs_reset) { 161 | reset_pen(); 162 | pen_needs_reset = false; 163 | } 164 | } 165 | } 166 | -------------------------------------------------------------------------------- /mbed-stepper/mbed.lib: -------------------------------------------------------------------------------- 1 | http://mbed.org/projects/libraries/svn/mbed/trunk@28 -------------------------------------------------------------------------------- /mbed-stepper/patterns.cpp: -------------------------------------------------------------------------------- 1 | #include "patterns.h" 2 | 3 | int draw_star(F32 moves_z, F32 draw_z, Planner* planner) { 4 | Point a, b, c, d, e, g; 5 | g.x = 0; 6 | g.y = 2; 7 | g.z = moves_z; 8 | a.x = 0; 9 | a.y = 2; 10 | a.z = draw_z; 11 | b.x = 2; 12 | b.y = -2; 13 | b.z = draw_z; 14 | c.x = -2; 15 | c.y = 1; 16 | c.z = draw_z; 17 | d.x = 2; 18 | d.y = 1; 19 | d.z = draw_z; 20 | e.x = -2; 21 | e.y = -2; 22 | e.z = draw_z; 23 | int index = 0; 24 | add_point_to_buffer(planner, g); 25 | add_point_to_buffer(planner, a); 26 | add_point_to_buffer(planner, b); 27 | add_point_to_buffer(planner, c); 28 | add_point_to_buffer(planner, d); 29 | add_point_to_buffer(planner, e); 30 | add_point_to_buffer(planner, a); 31 | add_point_to_buffer(planner, g); 32 | return index; 33 | } 34 | 35 | int draw_square_large(F32 moves_z, F32 draw_z, Planner* planner) { 36 | int index = 0; 37 | Point a, b, c, d, g; 38 | g.x = 0; 39 | g.y = 4; 40 | g.z = moves_z; 41 | a.x = 0; 42 | a.y = 4; 43 | a.z = draw_z; 44 | b.x = 4; 45 | b.y = 0; 46 | b.z = draw_z; 47 | c.x = 0; 48 | c.y = -4; 49 | c.z = draw_z; 50 | d.x = -4; 51 | d.y = 0; 52 | d.z = draw_z; 53 | 54 | add_point_to_buffer(planner, g); 55 | add_point_to_buffer(planner, a); 56 | add_point_to_buffer(planner, b); 57 | add_point_to_buffer(planner, c); 58 | add_point_to_buffer(planner, d); 59 | add_point_to_buffer(planner, a); 60 | add_point_to_buffer(planner, g); 61 | return index; 62 | } 63 | 64 | int draw_square_nn(F32 moves_z, F32 draw_z, Planner* planner) { 65 | int index = 0; 66 | Point a, b, c, d, g; 67 | g.x = -3; 68 | g.y = -3; 69 | g.z = moves_z; 70 | a.x = -3; 71 | a.y = -3; 72 | a.z = draw_z; 73 | b.x = -1; 74 | b.y = -3; 75 | b.z = draw_z; 76 | c.x = -1; 77 | c.y = -1; 78 | c.z = draw_z; 79 | d.x = -3; 80 | d.y = -1; 81 | d.z = draw_z; 82 | 83 | add_point_to_buffer(planner, g); 84 | add_point_to_buffer(planner, a); 85 | add_point_to_buffer(planner, b); 86 | add_point_to_buffer(planner, c); 87 | add_point_to_buffer(planner, d); 88 | add_point_to_buffer(planner, a); 89 | add_point_to_buffer(planner, g); 90 | return index; 91 | } 92 | 93 | int draw_ti(F32 moves_height, F32 draw_height, Point off, Planner* planner){ 94 | int index = 0; 95 | Point a; 96 | // H 97 | a.x = 0; a.y = 0; a.z = moves_height; 98 | a.x += off.x; a.y += off.y; 99 | add_point_to_buffer(planner, a); 100 | a.x = 0; a.y = 0; a.z = draw_height; 101 | a.x += off.x; a.y += off.y; 102 | add_point_to_buffer(planner, a); 103 | a.x = 0; a.y = 2; a.z = draw_height; 104 | a.x += off.x; a.y += off.y; 105 | add_point_to_buffer(planner, a); 106 | a.x = 0; a.y = 2; a.z = moves_height; 107 | a.x += off.x; a.y += off.y; 108 | add_point_to_buffer(planner, a); 109 | a.x = 0; a.y = 1; a.z = moves_height; 110 | a.x += off.x; a.y += off.y; 111 | add_point_to_buffer(planner, a); 112 | a.x = 0; a.y = 1; a.z = draw_height; 113 | a.x += off.x; a.y += off.y; 114 | add_point_to_buffer(planner, a); 115 | a.x = 0.5; a.y = 1; a.z = draw_height; 116 | a.x += off.x; a.y += off.y; 117 | add_point_to_buffer(planner, a); 118 | a.x = 0.5; a.y = 1; a.z = moves_height; 119 | a.x += off.x; a.y += off.y; 120 | add_point_to_buffer(planner, a); 121 | a.x = 0.5; a.y = 2; a.z = moves_height; 122 | a.x += off.x; a.y += off.y; 123 | add_point_to_buffer(planner, a); 124 | a.x = 0.5; a.y = 2; a.z = draw_height; 125 | a.x += off.x; a.y += off.y; 126 | add_point_to_buffer(planner, a); 127 | a.x = 0.5; a.y = 0; a.z = draw_height; 128 | a.x += off.x; a.y += off.y; 129 | add_point_to_buffer(planner, a); 130 | a.x = 0.5; a.y = 0; a.z = moves_height; 131 | a.x += off.x; a.y += off.y; 132 | add_point_to_buffer(planner, a); 133 | 134 | // I 135 | a.x = 0.75; a.y = 2; a.z = moves_height; 136 | a.x += off.x; a.y += off.y; 137 | add_point_to_buffer(planner, a); 138 | a.x = 0.75; a.y = 2; a.z = draw_height; 139 | a.x += off.x; a.y += off.y; 140 | add_point_to_buffer(planner, a); 141 | a.x = 0.75; a.y = 1.5; a.z = draw_height; 142 | a.x += off.x; a.y += off.y; 143 | add_point_to_buffer(planner, a); 144 | a.x = 0.75; a.y = 1.5; a.z = moves_height; 145 | a.x += off.x; a.y += off.y; 146 | add_point_to_buffer(planner, a); 147 | a.x = 0.75; a.y = 1; a.z = moves_height; 148 | a.x += off.x; a.y += off.y; 149 | add_point_to_buffer(planner, a); 150 | a.x = 0.75; a.y = 1; a.z = draw_height; 151 | a.x += off.x; a.y += off.y; 152 | add_point_to_buffer(planner, a); 153 | a.x = 0.75; a.y = 0; a.z = draw_height; 154 | a.x += off.x; a.y += off.y; 155 | add_point_to_buffer(planner, a); 156 | a.x = 0.75; a.y = 0; a.z = moves_height; 157 | a.x += off.x; a.y += off.y; 158 | add_point_to_buffer(planner, a); 159 | 160 | // T 161 | a.x = 1.5; a.y = 2; a.z = moves_height; 162 | a.x += off.x; a.y += off.y; 163 | add_point_to_buffer(planner, a); 164 | a.x = 1.5; a.y = 2; a.z = draw_height; 165 | a.x += off.x; a.y += off.y; 166 | add_point_to_buffer(planner, a); 167 | a.x = 2; a.y = 2; a.z = draw_height; 168 | a.x += off.x; a.y += off.y; 169 | add_point_to_buffer(planner, a); 170 | a.x = 1.75; a.y = 2; a.z = draw_height; 171 | a.x += off.x; a.y += off.y; 172 | add_point_to_buffer(planner, a); 173 | a.x = 1.75; a.y = 0; a.z = draw_height; 174 | a.x += off.x; a.y += off.y; 175 | add_point_to_buffer(planner, a); 176 | a.x = 1.75; a.y = 0; a.z = moves_height; 177 | a.x += off.x; a.y += off.y; 178 | add_point_to_buffer(planner, a); 179 | 180 | // I 181 | a.x = 2.25; a.y = 0; a.z = moves_height; 182 | a.x += off.x; a.y += off.y; 183 | add_point_to_buffer(planner, a); 184 | a.x = 2.25; a.y = 0; a.z = draw_height; 185 | a.x += off.x; a.y += off.y; 186 | add_point_to_buffer(planner, a); 187 | a.x = 2.75; a.y = 0; a.z = draw_height; 188 | a.x += off.x; a.y += off.y; 189 | add_point_to_buffer(planner, a); 190 | a.x = 2.5; a.y = 0; a.z = draw_height; 191 | a.x += off.x; a.y += off.y; 192 | add_point_to_buffer(planner, a); 193 | a.x = 2.5; a.y = 2; a.z = draw_height; 194 | a.x += off.x; a.y += off.y; 195 | add_point_to_buffer(planner, a); 196 | a.x = 2.25; a.y = 2; a.z = draw_height; 197 | a.x += off.x; a.y += off.y; 198 | add_point_to_buffer(planner, a); 199 | a.x = 2.75; a.y = 2; a.z = draw_height; 200 | a.x += off.x; a.y += off.y; 201 | add_point_to_buffer(planner, a); 202 | a.x = 2.75; a.y = 2; a.z = moves_height; 203 | a.x += off.x; a.y += off.y; 204 | add_point_to_buffer(planner, a); 205 | return index; 206 | } 207 | -------------------------------------------------------------------------------- /mbed-stepper/patterns.h: -------------------------------------------------------------------------------- 1 | #ifndef PATTERNS_H 2 | #define PATTERNS_H 3 | 4 | #include "common.h" 5 | #include "plan-position.h" 6 | 7 | int draw_star(F32 moves_height, F32 draw_height, Planner* planner); 8 | int draw_square_large(F32 moves_height, F32 draw_height, Planner* planner); 9 | int draw_square_nn(F32 moves_height, F32 draw_height, Planner* planner); 10 | int draw_ti(F32 moves_height, F32 draw_height, Point off, Planner* planner); 11 | 12 | #endif 13 | -------------------------------------------------------------------------------- /mbed-stepper/plan-position.cpp: -------------------------------------------------------------------------------- 1 | #include "plan-position.h" 2 | 3 | //extern Serial pc; 4 | extern DigitalOut led1; 5 | extern DigitalOut led2; 6 | extern DigitalOut led4; 7 | 8 | static Planner* cur_plan; 9 | 10 | Status get_next_steps(Planner* planner, bool reset_dist_flag); 11 | 12 | F32 dist_between(Point a, Point b) { 13 | return sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y) + (a.z - b.z) * (a.z - b.z)); 14 | } 15 | 16 | void conform_goal(Point* in) { 17 | in->x = RESTRICT(in->x, MIN_X, MAX_X); 18 | in->y = RESTRICT(in->y, MIN_Y, MAX_Y); 19 | in->z = RESTRICT(in->z, MIN_Z, MAX_Z); 20 | } 21 | 22 | F32 max(F32 a, F32 b) { 23 | return (a > b ? a : b); 24 | } 25 | 26 | F32 max(F32 a, F32 b, F32 c) { 27 | a = fabs(a); 28 | b = fabs(b); 29 | c = fabs(c); 30 | if (a > b) { 31 | return b > c ? a : a > c ? a : c; 32 | } 33 | else { // b > a 34 | return a > c ? b : b > c ? b : c; 35 | } 36 | } 37 | 38 | Status wait_for_pattern(Planner* planner) { 39 | while (true) { 40 | if (planner->finished) { 41 | return SUCCESS; 42 | } 43 | if (planner->errored) { 44 | //pc.printf("FAILURE"); 45 | led2 = 1; 46 | return FAILURE; 47 | } 48 | } 49 | } 50 | 51 | void setup_planner(Planner* planner) { 52 | calibrate(); 53 | 54 | planner->angles_actual = get_angles(); 55 | 56 | planner->buf_ind = 0; 57 | planner->buf_next = 1; 58 | 59 | planner->steps_to_next = 0; 60 | 61 | planner->buffer[0].x = 0; 62 | planner->buffer[0].y = 0; 63 | planner->buffer[0].z = START_Z; 64 | planner->buffer[0].is_traverse = 1; 65 | 66 | planner->current_pos.x = 0; 67 | planner->current_pos.y = 0; 68 | planner->current_pos.z = START_Z + 0.01; 69 | 70 | cur_plan = planner; 71 | 72 | planner->finished = false; 73 | planner->errored = false; 74 | 75 | get_next_steps(planner, 1); 76 | 77 | resume_steppers(planner); 78 | planner->state = PLR_ACCL; 79 | } 80 | 81 | bool add_point_to_buffer(Planner* planner, Point in) { 82 | if (INC_ONE(planner->buf_next) == planner->buf_ind) 83 | return false; 84 | 85 | planner->buffer[planner->buf_next].x = in.x; 86 | planner->buffer[planner->buf_next].y = in.y; 87 | planner->buffer[planner->buf_next].z = in.z; 88 | planner->buffer[planner->buf_next].is_traverse = in.is_traverse; 89 | 90 | planner->buf_next = INC_ONE(planner->buf_next); 91 | 92 | planner->finished = false; 93 | return true; 94 | } 95 | 96 | void pause_steppers(Planner* planner) { 97 | planner->runner.detach(); 98 | } 99 | 100 | void resume_steppers(Planner* planner) { 101 | planner->runner.attach_us(take_step, TIMING_INTERVAL); 102 | } 103 | 104 | Status get_next_steps(Planner* planner, bool reset_dist_flag) { 105 | Point goal = planner->buffer[planner->buf_ind]; 106 | bool is_traverse = goal.is_traverse; 107 | Point cur_pos = planner->current_pos; 108 | 109 | /* 110 | pc.printf("goal: %f %f %f\r\n", goal.x, goal.y, goal.z); 111 | pc.printf("curr: %f %f %f\r\n", cur_pos.x, cur_pos.y, cur_pos.z); 112 | */ 113 | 114 | conform_goal(&goal); 115 | 116 | F32 dx = goal.x - cur_pos.x; 117 | F32 dy = goal.y - cur_pos.y; 118 | F32 dz = goal.z - cur_pos.z; 119 | 120 | F32 dist = dist_between(cur_pos, goal); 121 | F32 step = 0; 122 | Point test; 123 | 124 | if (reset_dist_flag) { 125 | planner->full_dist = dist; 126 | planner->prev_dist = dist; 127 | pause_steppers(planner); 128 | resume_steppers(planner); 129 | for (int i = 0; i < 3; i++) 130 | planner->angles_ideal[i] = planner->angles_actual[i]; 131 | } 132 | F32 full_dist = planner->full_dist; 133 | F32 prev_dist = planner->prev_dist; 134 | 135 | F32 new_angles[3]; 136 | 137 | if (dist < MIN_DIST) { 138 | return END_PAT; 139 | } 140 | 141 | // Inverse square root!!! 142 | F32 inv_vec_mag = 1 / dist; 143 | dx *= inv_vec_mag; 144 | dy *= inv_vec_mag; 145 | dz *= inv_vec_mag; 146 | 147 | // Apply acceleration and deceleration and 148 | // acquire next step size 149 | if (planner->state == PLR_ACCL) { 150 | if (full_dist - prev_dist > ACCL_ZONE) 151 | planner->state = PLR_FULL; 152 | 153 | else if (prev_dist < ACCL_ZONE && prev_dist * 2 < full_dist) 154 | planner->state = PLR_DECL; 155 | 156 | else { 157 | if (is_traverse) 158 | step = MAP(fabs(full_dist - prev_dist), 0, ACCL_ZONE, MIN_STEP_SIZE, MAX_TRAV_SIZE); 159 | else 160 | step = MAP(fabs(full_dist - prev_dist), 0, ACCL_ZONE, MIN_STEP_SIZE, MAX_STEP_SIZE); 161 | } 162 | } 163 | if (planner->state == PLR_FULL) { 164 | if (prev_dist < ACCL_ZONE) 165 | planner->state = PLR_DECL; 166 | 167 | else { 168 | if (is_traverse) 169 | step = MAX_TRAV_SIZE; 170 | else 171 | step = MAX_STEP_SIZE; 172 | } 173 | } 174 | if (planner->state == PLR_DECL) { 175 | Point test; 176 | if (is_traverse) 177 | step = MAP(prev_dist, 0, ACCL_ZONE, MIN_STEP_SIZE, MAX_TRAV_SIZE); 178 | else 179 | step = MAP(prev_dist, 0, ACCL_ZONE, MIN_STEP_SIZE, MAX_STEP_SIZE); 180 | 181 | test.x = cur_pos.x + dx * step; 182 | test.y = cur_pos.y + dy * step; 183 | test.z = cur_pos.z + dz * step; 184 | 185 | F32 tdist = dist_between(test, goal); 186 | if (tdist >= prev_dist || tdist < MIN_DIST) { 187 | planner->state = PLR_ACCL; 188 | return END_PAT; 189 | } 190 | } 191 | 192 | // Generate a step 193 | cur_pos.x += dx * step; 194 | cur_pos.y += dy * step; 195 | cur_pos.z += dz * step; 196 | planner->current_pos = cur_pos; 197 | 198 | /* 199 | pc.printf("goal: %f %f %f\r\n", goal.x, goal.y, goal.z); 200 | pc.printf("curr: %f %f %f\r\n", cur_pos.x, cur_pos.y, cur_pos.z); 201 | pc.printf("d,step: %f %f %f %f\r\n", dx * step, dy, dz, step); 202 | pc.printf("dist, prev: %f %f\r\n", dist, prev_dist); 203 | */ 204 | 205 | if (inv_kinematics(new_angles, cur_pos) != SUCCESS) { 206 | led2 = 1; 207 | return FAILURE; 208 | } 209 | /* 210 | new_angles[0] = -.1184589; 211 | new_angles[1] = -.1184589; 212 | new_angles[2] = -.1184589; 213 | */ 214 | /* 215 | pc.printf("actaangles: %f %f %f\r\n", planner->angles_actual[0], planner->angles_actual[1], planner->angles_actual[2]); 216 | pc.printf("newangles: %f %f %f\r\n", new_angles[0], new_angles[1], new_angles[2]); 217 | */ 218 | 219 | // Get the difference in angles 220 | for (int i = 0; i < 3; i++) { 221 | //planner->angles_ideal[i] = planner->angles_actual[i]; 222 | new_angles[i] -= planner->angles_actual[i]; 223 | } 224 | 225 | F32 max_diff = max(new_angles[0], new_angles[1], new_angles[2]); 226 | 227 | // Number of steps to move max_diff amount 228 | planner->steps_to_next = floor(max_diff / STEPPER_STEP_SIZE + 0.500001); 229 | if (planner->steps_to_next <= 0) { 230 | return SUCCESS; 231 | } 232 | 233 | //pc.printf("max, steps, size, raw %f %d %f %f\r\n", max_diff, planner->steps_to_next, STEPPER_STEP_SIZE, max_diff / STEPPER_STEP_SIZE + 0.500001); 234 | 235 | for (int i = 0; i < 3; i++) { 236 | planner->angles_step[i] = new_angles[i] / planner->steps_to_next; 237 | } 238 | 239 | planner->prev_dist = dist; 240 | 241 | return SUCCESS; 242 | } 243 | 244 | Status make_next_step(Planner* planner) { 245 | S32 make_step = 0; 246 | S32 direction = 0; 247 | if (planner->steps_to_next == 0) 248 | return SUCCESS; 249 | //static int counter = 0; 250 | //if (counter % 128 == 0) { 251 | // counter = 1; 252 | //} 253 | 254 | // For all steppers, if the ideal angles are 1 step or greater from the actual angles, 255 | // move the steppers to them. 256 | for (int i = 0; i < 3; i++) { 257 | planner->angles_ideal[i] += planner->angles_step[i]; 258 | 259 | // Set the bits for the stepper mover 260 | if (fabs(planner->angles_ideal[i] - planner->angles_actual[i]) >= STEPPER_STEP_SIZE) { 261 | make_step |= 1 << i; 262 | direction |= (planner->angles_ideal[i] > planner->angles_actual[i]) << i; 263 | } 264 | } 265 | 266 | if (move_steppers(make_step, direction) != SUCCESS) { 267 | /* 268 | pc.printf("FFFUUUUU Stepper failure\r\n"); 269 | pc.printf("anglesa: %f %f %f\r\n", planner->angles_actual[0], planner->angles_actual[1], planner->angles_actual[2]); 270 | pc.printf("anglesi: %f %f %f\r\n", planner->angles_ideal[0], planner->angles_ideal[1], planner->angles_ideal[2]); 271 | pc.printf("position: %f %f %f\r\n", planner->current_pos.x, planner->current_pos.y, planner->current_pos.z); 272 | */ 273 | led2 = 1; 274 | return FAILURE; 275 | } 276 | planner->steps_to_next--; 277 | return SUCCESS; 278 | } 279 | 280 | Timer timer; 281 | void take_step() { 282 | #ifdef TIMER_ON 283 | Timer t; 284 | t.start(); 285 | #endif 286 | bool reset_dist_flag, exit_flag = false; 287 | 288 | static int counter = 0; 289 | if (counter++ > 200) { 290 | led4 = led4 ^ 1; 291 | counter = 0; 292 | } 293 | 294 | led1 = 0; 295 | make_next_step(cur_plan); 296 | if (cur_plan->steps_to_next == 0 && !exit_flag) { 297 | 298 | reset_dist_flag = 0; 299 | 300 | while (get_next_steps(cur_plan, reset_dist_flag) == END_PAT) { 301 | led1 = 1; 302 | reset_dist_flag = 1; 303 | 304 | if (INC_ONE(cur_plan->buf_ind) == cur_plan->buf_next) { 305 | cur_plan->finished = true; 306 | led1 = 0; 307 | exit_flag = true; 308 | break; 309 | } 310 | cur_plan->buf_ind = INC_ONE(cur_plan->buf_ind); 311 | } 312 | } 313 | #ifdef TIMER_ON 314 | t.stop(); 315 | pc.printf("Timer: %d\r\n", t.read_us()); 316 | #endif 317 | } 318 | -------------------------------------------------------------------------------- /mbed-stepper/plan-position.h: -------------------------------------------------------------------------------- 1 | #ifndef PLAN_POSITION_H 2 | #define PLAN_POSITION_H 3 | 4 | #include "common.h" 5 | #include "kinematics.h" 6 | #include "stepper.h" 7 | #include 8 | 9 | #define BUFFER_SIZE 256 10 | #define INC_ONE(a) (((a) + 1) % BUFFER_SIZE) 11 | 12 | #define MAX_TRAV_SIZE 0.003 13 | //#define MAX_STEP_SIZE 0.0006 14 | //#define MAX_STEP_SIZE 0.001 15 | #define MAX_STEP_SIZE 0.002 16 | //#define MIN_STEP_SIZE 0.00008 17 | //#define MIN_STEP_SIZE 0.00016 18 | #define MIN_STEP_SIZE 0.00012 19 | 20 | #define MIN_STEP_FRAC 3 21 | 22 | 23 | #define MIN_DIST 0.00025 24 | 25 | #define ACCL_ZONE 0.4 // inches 26 | //#define TIMER_ON 27 | 28 | #ifdef TIMER_ON 29 | #define TIMING_INTERVAL 100000 30 | #else 31 | #define TIMING_INTERVAL 200 32 | #endif 33 | 34 | 35 | enum Planner_State { 36 | PLR_ACCL, 37 | PLR_FULL, 38 | PLR_DECL, 39 | PLR_NEXT, 40 | PLR_WAIT 41 | }; 42 | 43 | struct Planner { 44 | volatile Point buffer[BUFFER_SIZE]; 45 | volatile U32 buf_ind; 46 | volatile U32 buf_next; 47 | 48 | Planner_State state; 49 | S32 steps_to_next; 50 | 51 | Ticker runner; 52 | 53 | F32 angles_step[3]; 54 | F32* angles_actual; 55 | F32 angles_ideal[3]; 56 | Point current_pos; 57 | 58 | F32 full_dist; 59 | F32 prev_dist; 60 | 61 | volatile bool finished; 62 | volatile bool errored; 63 | }; 64 | 65 | void go_to_home(Planner* planner); 66 | Status wait_for_pattern(Planner* planner); 67 | void setup_planner(Planner* planner); 68 | bool add_point_to_buffer(Planner* planner, Point in); 69 | void pause_steppers(Planner* planner); 70 | void resume_steppers(Planner* planner); 71 | void take_step(); 72 | 73 | #endif 74 | -------------------------------------------------------------------------------- /mbed-stepper/stepper.cpp: -------------------------------------------------------------------------------- 1 | #include "stepper.h" 2 | 3 | static DigitalOut dir[] = {DigitalOut(p23), DigitalOut(p22), DigitalOut(p21)}; 4 | static DigitalOut stepper[] = {DigitalOut(p26), DigitalOut(p25), DigitalOut(p24)}; 5 | static AnalogIn posIn[] = {AnalogIn(p18), AnalogIn(p17), AnalogIn(p20)}; 6 | 7 | extern DigitalOut led1; 8 | extern DigitalOut led2; 9 | extern DigitalOut led3; 10 | extern DigitalOut led4; 11 | 12 | static F32 angles[3]; 13 | 14 | //extern Serial pc; 15 | 16 | /* 17 | 45 top: -.90386 rad 18 | 19 | 1: 13440 20 | 2: 13731 21 | 3: 16307 22 | 23 | angle: 24 | 13.5744 deg or 6.7872 deg 25 | */ 26 | 27 | static float raw_angles[3]; 28 | static const float calib_angles[3] = {13440, 13731, 14800}; 29 | static const float calib_angle = -0.90386; 30 | 31 | static const S32 samples = 24; 32 | 33 | void update_pos() { 34 | int i; 35 | S32 ext[3][samples]; 36 | //static int counter = 0; 37 | 38 | // Wait for LPF to settle 39 | wait_us(100); 40 | for (int x = 0; x < 3; x++) { 41 | for (int j = 1; j < samples; j++) { 42 | ext[x][j] = posIn[x].read_u16(); 43 | S32 index = ext[x][j]; 44 | for (i = j; i > 0 && ext[x][i] > ext[x][i - 1]; i--) { 45 | ext[x][i] = ext[x][i-1]; 46 | } 47 | ext[x][i] = index; 48 | wait_us(100); 49 | } 50 | raw_angles[x] = 0; 51 | for (int i = 8; i < 16; i++) 52 | raw_angles[x] += ext[x][i]; 53 | 54 | raw_angles[x] /= 8; 55 | //if (counter == 10) 56 | // pc.printf("rawangles: %f\r\n", raw_angles[x]); 57 | } 58 | //counter= (counter + 1) % 11; 59 | //pc.printf("actaangles: %f %f %f\r\n", angles[0], angles[1], angles[2]); 60 | } 61 | 62 | F32* get_angles() { 63 | return angles; 64 | } 65 | 66 | extern Serial pc; 67 | Status move_steppers(int steppers, int direction) { 68 | Status retcode = SUCCESS; 69 | for (int i = 0; i < 3; i++) { 70 | // Step to larger angle (down) 71 | if ((steppers >> i) & 0x1) { 72 | if ((direction >> i) & 0x1) { 73 | if (angles[i] > STEPPER_MAX_ANGLE) { 74 | retcode = FAILURE; 75 | steppers &= ~(0x1 << i); 76 | led2 = 1; 77 | } 78 | else { 79 | dir[i] = 0; 80 | angles[i] += STEPPER_STEP_SIZE; 81 | } 82 | } 83 | // Step to smaller angle (up) 84 | else { 85 | if (angles[i] < STEPPER_MIN_ANGLE) { 86 | retcode = FAILURE; 87 | steppers &= ~(0x1 << i); 88 | led2 = 1; 89 | } 90 | else { 91 | dir[i] = 1; 92 | angles[i] -= STEPPER_STEP_SIZE; 93 | } 94 | } 95 | } 96 | } 97 | wait_us(5); 98 | 99 | for (int i = 0; i < 3; i++) { 100 | stepper[i] = (steppers >> i) & 0x1; 101 | } 102 | wait_us(5); 103 | 104 | for (int i = 0; i < 3; i++) { 105 | stepper[i] = 0; 106 | } 107 | return retcode; 108 | } 109 | 110 | void calib_arm(int arm_no) { 111 | int make_step = 0; 112 | while (true) { 113 | update_pos(); 114 | angles[arm_no] = 0; 115 | make_step = 0; 116 | 117 | make_step |= (raw_angles[arm_no] > calib_angles[arm_no]) << arm_no; 118 | if (!make_step) 119 | break; 120 | 121 | move_steppers(make_step, 0); 122 | 123 | if (raw_angles[arm_no] - calib_angles[arm_no] > 500) { 124 | for (int i = 0; i < 5; i++) { 125 | wait_us(300); 126 | move_steppers(make_step, 0); 127 | } 128 | } 129 | } 130 | angles[arm_no] = calib_angle; 131 | for (int i = 0; i < 300; i++) { 132 | move_steppers(1 << arm_no, 1 << arm_no); 133 | wait_us(400); 134 | } 135 | } 136 | 137 | void calibrate() { 138 | for (int i = 0; i < 3; i++) { 139 | calib_arm(i); 140 | } 141 | } 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | -------------------------------------------------------------------------------- /mbed-stepper/stepper.h: -------------------------------------------------------------------------------- 1 | #ifndef STEPPER_H 2 | #define STEPPER_H 3 | 4 | #include "mbed.h" 5 | #include "common.h" 6 | 7 | #define STEPPER_MIN_ANGLE -1.01922088 8 | #define STEPPER_MAX_ANGLE 1.49077498 9 | //#define STEPPER_STEP_SIZE 0.000763581548 10 | #define STEPPER_STEP_SIZE 0.000757840333 11 | 12 | #define STEPPER_UP 1 13 | #define STEPPER_DN 0 14 | 15 | #ifdef DEBUG 16 | extern Serial pc; 17 | #endif 18 | 19 | void reset_steppers(); 20 | void calibrate(); 21 | F32* get_angles(); 22 | Status move_steppers(int steppers, int directions); 23 | 24 | #endif 25 | -------------------------------------------------------------------------------- /mbed/common.h: -------------------------------------------------------------------------------- 1 | #ifndef COMMON_H 2 | #define COMMON_H 3 | 4 | #include "mbed.h" 5 | 6 | #define ARM_UPPER_LEN 2.489 7 | #define ARM_LOWER_LEN 10.546875 8 | 9 | #define HAND_XOFF 1.09725 10 | #define HAND_ZOFF -0.20975 11 | 12 | #define TOOL_ZOFF -1.78000 13 | 14 | #define SERVO_XOFF -2.2085 15 | #define SERVO_ZOFF 0.51 16 | 17 | #define MAX_Z 11.5 18 | #define MIN_Z 10.15 19 | 20 | #define MAX_X 3.1 21 | #define MIN_X -3.1 22 | 23 | #define MAX_Y 3.1 24 | #define MIN_Y -3.1 25 | 26 | #define START_Z (MIN_Z - 0.5) 27 | 28 | #define MAX(a, b) ((a) > (b) ? (a) : (b)) 29 | #define MIN(a, b) ((a) < (b) ? (a) : (b)) 30 | #define RESTRICT(a, b1, b2) (MIN(b2, MAX(a, b1))) 31 | #define MAP(a, b1, b2, x1, x2) (((F32)((a) - (b1)) / (F32)((b2) - (b1))) * ((x2) - (x1)) + (x1)) 32 | 33 | #define DEBUG 34 | 35 | typedef enum { 36 | SUCCESS, 37 | FAILURE, 38 | END_PAT 39 | } Status; 40 | 41 | typedef unsigned char U08; 42 | typedef unsigned short U16; 43 | typedef unsigned int U32; 44 | typedef unsigned long long U64; 45 | 46 | typedef signed char S08; 47 | typedef short S16; 48 | typedef int S32; 49 | typedef long long S64; 50 | 51 | typedef float F32; 52 | typedef double F64; 53 | 54 | struct Point { 55 | F32 x; 56 | F32 y; 57 | F32 z; 58 | }; 59 | 60 | #endif -------------------------------------------------------------------------------- /mbed/dials.cpp: -------------------------------------------------------------------------------- 1 | #include "dials.h" 2 | 3 | // trololololol 4 | extern DigitalIn go_troll_up; 5 | extern DigitalIn go_troll_dn; 6 | extern DigitalIn go_adj_z; 7 | extern DigitalIn go_run_pat; 8 | 9 | static S08 at_draw = 0, debounce = 0; 10 | void read_dials(Planner* planner, F32 moves_z, F32 draw_z) { 11 | F32 adj_x = 0, adj_y = 0; 12 | Point next; 13 | U08 in_x, in_y, zch = 0; 14 | 15 | next = planner->next_pos; 16 | 17 | // Debounce the switches 18 | if (go_adj_z && debounce < 10) { 19 | debounce++; 20 | if (debounce == 10) { 21 | if (at_draw) { 22 | next.z = moves_z; 23 | at_draw = 0; 24 | } 25 | else { 26 | next.z = draw_z; 27 | at_draw = 1; 28 | } 29 | zch = 1; 30 | } 31 | } 32 | if (!go_adj_z && debounce > 0) 33 | debounce--; 34 | 35 | in_x = i2c_read(PERIP_X); 36 | in_y = i2c_read(PERIP_Y); 37 | if (in_x != 128 && in_x != I2C_ERROR) { 38 | adj_x = ((F32)RESTRICT(in_x - 128, -16, 16))/64.0; 39 | //pc.printf("input: %0.5f\n", adj_x); 40 | next.x += adj_x; 41 | } 42 | if (in_y != 128 && in_y != I2C_ERROR) { 43 | adj_y = ((F32)RESTRICT(in_y - 128, -16, 16))/64.0; 44 | //pc.printf("input: %0.5f\n", adj_y); 45 | next.y += adj_y; 46 | } 47 | if ((adj_x != 0 ||adj_y != 0 || zch) && get_num_in_buffer(planner) < 1) { 48 | //pc.printf("added to buffer"); 49 | add_point_to_buffer(planner, next); 50 | } 51 | } 52 | -------------------------------------------------------------------------------- /mbed/dials.h: -------------------------------------------------------------------------------- 1 | #ifndef DIALS_H 2 | #define DIALS_H 3 | 4 | #include "i2c.h" 5 | #include "planner.h" 6 | #include "common.h" 7 | 8 | #define PERIP_X 0x90 9 | #define PERIP_Y 0x80 10 | 11 | void read_dials(Planner* planner, F32 moves_z, F32 draw_z); 12 | 13 | #endif -------------------------------------------------------------------------------- /mbed/i2c.cpp: -------------------------------------------------------------------------------- 1 | #include "i2c.h" 2 | 3 | static I2C i2c(p28, p27); // sda, scl 4 | 5 | S32 i2c_read(S32 addr) { 6 | S32 ret_val; 7 | i2c.start(); 8 | i2c.write(addr + 1); 9 | 10 | // Acknowledge the incoming byte 11 | ret_val = i2c.read(1); 12 | 13 | i2c.stop(); 14 | return ret_val; 15 | } -------------------------------------------------------------------------------- /mbed/i2c.h: -------------------------------------------------------------------------------- 1 | #ifndef I2C_H 2 | #define I2C_H 3 | 4 | #include "mbed.h" 5 | #include "common.h" 6 | 7 | #define I2C_ERROR 0xFF 8 | 9 | S32 i2c_read(S32 address); 10 | 11 | #endif -------------------------------------------------------------------------------- /mbed/main.cpp: -------------------------------------------------------------------------------- 1 | #include "mbed.h" 2 | 3 | #include "common.h" 4 | #include "servo.h" 5 | #include "positioner.h" 6 | #include "planner.h" 7 | #include "i2c.h" 8 | #include "patterns.h" 9 | #include "dials.h" 10 | 11 | #define moves_z (MAX(draw_z - 0.5, MIN_Z - 0.5)) 12 | #define UPDATE_INTERVAL 10000 13 | #define START_TRANS 'B' 14 | #define END_TRANS 0xFFFF1111 15 | 16 | Serial pc(USBTX, USBRX); // tx, rx 17 | Ticker runner; 18 | DigitalOut led1(LED1); 19 | DigitalOut led2(LED2); 20 | DigitalOut led3(LED3); 21 | DigitalOut led4(LED4); 22 | 23 | volatile U08 serial_buffer[16]; 24 | volatile S32 sbuffer_index; 25 | 26 | // trololololol 27 | DigitalIn go_troll_up(p20); 28 | DigitalIn go_troll_dn(p19); 29 | DigitalIn go_adj_z(p18); 30 | 31 | DigitalIn go_run_pat(p17); 32 | 33 | static Planner planner; 34 | static F32 draw_z = MIN_Z; // inches 35 | 36 | void serial_callback() { 37 | serial_buffer[sbuffer_index++] = pc.getc(); 38 | } 39 | 40 | void setup() { 41 | servos_setup(); 42 | int ret = planner_setup(&planner); 43 | pc.baud(115200); 44 | pc.attach(serial_callback); 45 | sbuffer_index = 0; 46 | //pc.printf("Setup: %d\n", ret); 47 | } 48 | 49 | void adj_z() { 50 | goto_point(&planner, 0, 0, draw_z); 51 | 52 | if (go_troll_up) { 53 | troll_up(&planner); 54 | draw_z = planner.current_pos.z; 55 | } 56 | if (go_troll_dn) { 57 | troll_down(&planner); 58 | draw_z = planner.current_pos.z; 59 | } 60 | } 61 | 62 | void read_dials_callback() { 63 | read_dials(&planner, moves_z, draw_z); 64 | } 65 | 66 | void run_pattern() { 67 | Status status = SUCCESS; 68 | while (status == SUCCESS) { 69 | status = planner_process(&planner); 70 | } 71 | } 72 | 73 | Status fill_buffer() { 74 | int i; 75 | Point in; 76 | 77 | pc.putc(START_TRANS); 78 | for (i = 0; i < PLANNER_BUFFER_SIZE - 5; i++) { 79 | while (sbuffer_index < 9); 80 | if ((*(U32*)(&serial_buffer[0])) == END_TRANS) 81 | return END_PAT; 82 | sbuffer_index = 0; 83 | pc.putc(START_TRANS); 84 | 85 | in.x = *(F32*)(&serial_buffer[0]); 86 | in.y = *(F32*)(&serial_buffer[4]); 87 | 88 | if (serial_buffer[8] == 1) 89 | in.z = moves_z; 90 | else 91 | in.z = draw_z; 92 | add_point_to_buffer(&planner, in); 93 | } 94 | 95 | return SUCCESS; 96 | } 97 | 98 | 99 | int main() { 100 | Status status; 101 | setup(); 102 | // adjust z 103 | while (1) { 104 | if (go_adj_z) { 105 | adj_z(); 106 | } 107 | else if (go_run_pat) { 108 | break; 109 | } 110 | else { 111 | reset_position(&planner); 112 | } 113 | } 114 | 115 | wait_ms(500); 116 | 117 | while (1) { 118 | 119 | runner.attach_us(read_dials_callback, UPDATE_INTERVAL); 120 | while(1) { 121 | run_pattern(); 122 | if (go_run_pat) 123 | break; 124 | } 125 | 126 | runner.detach(); 127 | 128 | status = SUCCESS; 129 | sbuffer_index = 0; 130 | while (status == SUCCESS) { 131 | status = fill_buffer(); 132 | run_pattern(); 133 | } 134 | pc.putc('D'); 135 | } 136 | } 137 | -------------------------------------------------------------------------------- /mbed/mbed.lib: -------------------------------------------------------------------------------- 1 | http://mbed.org/projects/libraries/svn/mbed/trunk@28 -------------------------------------------------------------------------------- /mbed/patterns.c: -------------------------------------------------------------------------------- 1 | #include "patterns.h" 2 | 3 | void draw_star(F32 moves_z, F32 draw_z, Planner* planner) { 4 | Point a, b, c, d, e, g; 5 | g.x = 0; 6 | g.y = 2; 7 | g.z = moves_z; 8 | a.x = 0; 9 | a.y = 2; 10 | a.z = draw_z; 11 | b.x = 2; 12 | b.y = -2; 13 | b.z = draw_z; 14 | c.x = -2; 15 | c.y = 1; 16 | c.z = draw_z; 17 | d.x = 2; 18 | d.y = 1; 19 | d.z = draw_z; 20 | e.x = -2; 21 | e.y = -2; 22 | e.z = draw_z; 23 | add_point_to_buffer(planner, g); 24 | add_point_to_buffer(planner, a); 25 | add_point_to_buffer(planner, b); 26 | add_point_to_buffer(planner, c); 27 | add_point_to_buffer(planner, d); 28 | add_point_to_buffer(planner, e); 29 | add_point_to_buffer(planner, a); 30 | add_point_to_buffer(planner, g); 31 | //add_point_to_buffer(planner, f); 32 | } 33 | 34 | void draw_square_large(F32 moves_z, F32 draw_z, Planner* planner) { 35 | Point a, b, c, d, g; 36 | g.x = 0; 37 | g.y = 4; 38 | g.z = moves_z; 39 | a.x = 0; 40 | a.y = 4; 41 | a.z = draw_z; 42 | b.x = 4; 43 | b.y = 0; 44 | b.z = draw_z; 45 | c.x = 0; 46 | c.y = -4; 47 | c.z = draw_z; 48 | d.x = -4; 49 | d.y = 0; 50 | d.z = draw_z; 51 | 52 | add_point_to_buffer(planner, g); 53 | add_point_to_buffer(planner, a); 54 | add_point_to_buffer(planner, b); 55 | add_point_to_buffer(planner, c); 56 | add_point_to_buffer(planner, d); 57 | add_point_to_buffer(planner, a); 58 | add_point_to_buffer(planner, g); 59 | } 60 | 61 | void draw_square_nn(F32 moves_z, F32 draw_z, Planner* planner) { 62 | Point a, b, c, d, g; 63 | g.x = -3; 64 | g.y = -3; 65 | g.z = moves_z; 66 | a.x = -3; 67 | a.y = -3; 68 | a.z = draw_z; 69 | b.x = -1; 70 | b.y = -3; 71 | b.z = draw_z; 72 | c.x = -1; 73 | c.y = -1; 74 | c.z = draw_z; 75 | d.x = -3; 76 | d.y = -1; 77 | d.z = draw_z; 78 | 79 | add_point_to_buffer(planner, g); 80 | add_point_to_buffer(planner, a); 81 | add_point_to_buffer(planner, b); 82 | add_point_to_buffer(planner, c); 83 | add_point_to_buffer(planner, d); 84 | add_point_to_buffer(planner, a); 85 | add_point_to_buffer(planner, g); 86 | } 87 | 88 | void draw_ti(F32 moves_height, F32 draw_height, Point off, Planner* planner){ 89 | Point a; 90 | // H 91 | a.x = 0; a.y = 0; a.z = moves_height; 92 | a.x += off.x; a.y += off.y; 93 | add_point_to_buffer(planner, a); 94 | a.x = 0; a.y = 0; a.z = draw_height; 95 | a.x += off.x; a.y += off.y; 96 | add_point_to_buffer(planner, a); 97 | a.x = 0; a.y = 2; a.z = draw_height; 98 | a.x += off.x; a.y += off.y; 99 | add_point_to_buffer(planner, a); 100 | a.x = 0; a.y = 2; a.z = moves_height; 101 | a.x += off.x; a.y += off.y; 102 | add_point_to_buffer(planner, a); 103 | a.x = 0; a.y = 1; a.z = moves_height; 104 | a.x += off.x; a.y += off.y; 105 | add_point_to_buffer(planner, a); 106 | a.x = 0; a.y = 1; a.z = draw_height; 107 | a.x += off.x; a.y += off.y; 108 | add_point_to_buffer(planner, a); 109 | a.x = 0.5; a.y = 1; a.z = draw_height; 110 | a.x += off.x; a.y += off.y; 111 | add_point_to_buffer(planner, a); 112 | a.x = 0.5; a.y = 1; a.z = moves_height; 113 | a.x += off.x; a.y += off.y; 114 | add_point_to_buffer(planner, a); 115 | a.x = 0.5; a.y = 2; a.z = moves_height; 116 | a.x += off.x; a.y += off.y; 117 | add_point_to_buffer(planner, a); 118 | a.x = 0.5; a.y = 2; a.z = draw_height; 119 | a.x += off.x; a.y += off.y; 120 | add_point_to_buffer(planner, a); 121 | a.x = 0.5; a.y = 0; a.z = draw_height; 122 | a.x += off.x; a.y += off.y; 123 | add_point_to_buffer(planner, a); 124 | a.x = 0.5; a.y = 0; a.z = moves_height; 125 | a.x += off.x; a.y += off.y; 126 | add_point_to_buffer(planner, a); 127 | 128 | // I 129 | a.x = 0.75; a.y = 2; a.z = moves_height; 130 | a.x += off.x; a.y += off.y; 131 | add_point_to_buffer(planner, a); 132 | a.x = 0.75; a.y = 2; a.z = draw_height; 133 | a.x += off.x; a.y += off.y; 134 | add_point_to_buffer(planner, a); 135 | a.x = 0.75; a.y = 1.5; a.z = draw_height; 136 | a.x += off.x; a.y += off.y; 137 | add_point_to_buffer(planner, a); 138 | a.x = 0.75; a.y = 1.5; a.z = moves_height; 139 | a.x += off.x; a.y += off.y; 140 | add_point_to_buffer(planner, a); 141 | a.x = 0.75; a.y = 1; a.z = moves_height; 142 | a.x += off.x; a.y += off.y; 143 | add_point_to_buffer(planner, a); 144 | a.x = 0.75; a.y = 1; a.z = draw_height; 145 | a.x += off.x; a.y += off.y; 146 | add_point_to_buffer(planner, a); 147 | a.x = 0.75; a.y = 0; a.z = draw_height; 148 | a.x += off.x; a.y += off.y; 149 | add_point_to_buffer(planner, a); 150 | a.x = 0.75; a.y = 0; a.z = moves_height; 151 | a.x += off.x; a.y += off.y; 152 | add_point_to_buffer(planner, a); 153 | 154 | // T 155 | a.x = 1.5; a.y = 2; a.z = moves_height; 156 | a.x += off.x; a.y += off.y; 157 | add_point_to_buffer(planner, a); 158 | a.x = 1.5; a.y = 2; a.z = draw_height; 159 | a.x += off.x; a.y += off.y; 160 | add_point_to_buffer(planner, a); 161 | a.x = 2; a.y = 2; a.z = draw_height; 162 | a.x += off.x; a.y += off.y; 163 | add_point_to_buffer(planner, a); 164 | a.x = 1.75; a.y = 2; a.z = draw_height; 165 | a.x += off.x; a.y += off.y; 166 | add_point_to_buffer(planner, a); 167 | a.x = 1.75; a.y = 0; a.z = draw_height; 168 | a.x += off.x; a.y += off.y; 169 | add_point_to_buffer(planner, a); 170 | a.x = 1.75; a.y = 0; a.z = moves_height; 171 | a.x += off.x; a.y += off.y; 172 | add_point_to_buffer(planner, a); 173 | 174 | // I 175 | a.x = 2.25; a.y = 0; a.z = moves_height; 176 | a.x += off.x; a.y += off.y; 177 | add_point_to_buffer(planner, a); 178 | a.x = 2.25; a.y = 0; a.z = draw_height; 179 | a.x += off.x; a.y += off.y; 180 | add_point_to_buffer(planner, a); 181 | a.x = 2.75; a.y = 0; a.z = draw_height; 182 | a.x += off.x; a.y += off.y; 183 | add_point_to_buffer(planner, a); 184 | a.x = 2.5; a.y = 0; a.z = draw_height; 185 | a.x += off.x; a.y += off.y; 186 | add_point_to_buffer(planner, a); 187 | a.x = 2.5; a.y = 2; a.z = draw_height; 188 | a.x += off.x; a.y += off.y; 189 | add_point_to_buffer(planner, a); 190 | a.x = 2.25; a.y = 2; a.z = draw_height; 191 | a.x += off.x; a.y += off.y; 192 | add_point_to_buffer(planner, a); 193 | a.x = 2.75; a.y = 2; a.z = draw_height; 194 | a.x += off.x; a.y += off.y; 195 | add_point_to_buffer(planner, a); 196 | a.x = 2.75; a.y = 2; a.z = moves_height; 197 | a.x += off.x; a.y += off.y; 198 | add_point_to_buffer(planner, a); 199 | } 200 | 201 | 202 | 203 | 204 | 205 | 206 | -------------------------------------------------------------------------------- /mbed/patterns.cpp: -------------------------------------------------------------------------------- 1 | #include "patterns.h" 2 | 3 | void draw_star(F32 moves_z, F32 draw_z, Planner* planner) { 4 | Point a, b, c, d, e, g; 5 | g.x = 0; 6 | g.y = 2; 7 | g.z = moves_z; 8 | a.x = 0; 9 | a.y = 2; 10 | a.z = draw_z; 11 | b.x = 2; 12 | b.y = -2; 13 | b.z = draw_z; 14 | c.x = -2; 15 | c.y = 1; 16 | c.z = draw_z; 17 | d.x = 2; 18 | d.y = 1; 19 | d.z = draw_z; 20 | e.x = -2; 21 | e.y = -2; 22 | e.z = draw_z; 23 | add_point_to_buffer(planner, g); 24 | add_point_to_buffer(planner, a); 25 | add_point_to_buffer(planner, b); 26 | add_point_to_buffer(planner, c); 27 | add_point_to_buffer(planner, d); 28 | add_point_to_buffer(planner, e); 29 | add_point_to_buffer(planner, a); 30 | add_point_to_buffer(planner, g); 31 | //add_point_to_buffer(planner, f); 32 | } 33 | 34 | void draw_square_large(F32 moves_z, F32 draw_z, Planner* planner) { 35 | Point a, b, c, d, g; 36 | g.x = 0; 37 | g.y = 4; 38 | g.z = moves_z; 39 | a.x = 0; 40 | a.y = 4; 41 | a.z = draw_z; 42 | b.x = 4; 43 | b.y = 0; 44 | b.z = draw_z; 45 | c.x = 0; 46 | c.y = -4; 47 | c.z = draw_z; 48 | d.x = -4; 49 | d.y = 0; 50 | d.z = draw_z; 51 | 52 | add_point_to_buffer(planner, g); 53 | add_point_to_buffer(planner, a); 54 | add_point_to_buffer(planner, b); 55 | add_point_to_buffer(planner, c); 56 | add_point_to_buffer(planner, d); 57 | add_point_to_buffer(planner, a); 58 | add_point_to_buffer(planner, g); 59 | } 60 | 61 | void draw_square_nn(F32 moves_z, F32 draw_z, Planner* planner) { 62 | Point a, b, c, d, g; 63 | g.x = -3; 64 | g.y = -3; 65 | g.z = moves_z; 66 | a.x = -3; 67 | a.y = -3; 68 | a.z = draw_z; 69 | b.x = -1; 70 | b.y = -3; 71 | b.z = draw_z; 72 | c.x = -1; 73 | c.y = -1; 74 | c.z = draw_z; 75 | d.x = -3; 76 | d.y = -1; 77 | d.z = draw_z; 78 | 79 | add_point_to_buffer(planner, g); 80 | add_point_to_buffer(planner, a); 81 | add_point_to_buffer(planner, b); 82 | add_point_to_buffer(planner, c); 83 | add_point_to_buffer(planner, d); 84 | add_point_to_buffer(planner, a); 85 | add_point_to_buffer(planner, g); 86 | } 87 | 88 | void draw_ti(F32 moves_height, F32 draw_height, Point off, Planner* planner){ 89 | Point a; 90 | // H 91 | a.x = 0; a.y = 0; a.z = moves_height; 92 | a.x += off.x; a.y += off.y; 93 | add_point_to_buffer(planner, a); 94 | a.x = 0; a.y = 0; a.z = draw_height; 95 | a.x += off.x; a.y += off.y; 96 | add_point_to_buffer(planner, a); 97 | a.x = 0; a.y = 2; a.z = draw_height; 98 | a.x += off.x; a.y += off.y; 99 | add_point_to_buffer(planner, a); 100 | a.x = 0; a.y = 2; a.z = moves_height; 101 | a.x += off.x; a.y += off.y; 102 | add_point_to_buffer(planner, a); 103 | a.x = 0; a.y = 1; a.z = moves_height; 104 | a.x += off.x; a.y += off.y; 105 | add_point_to_buffer(planner, a); 106 | a.x = 0; a.y = 1; a.z = draw_height; 107 | a.x += off.x; a.y += off.y; 108 | add_point_to_buffer(planner, a); 109 | a.x = 0.5; a.y = 1; a.z = draw_height; 110 | a.x += off.x; a.y += off.y; 111 | add_point_to_buffer(planner, a); 112 | a.x = 0.5; a.y = 1; a.z = moves_height; 113 | a.x += off.x; a.y += off.y; 114 | add_point_to_buffer(planner, a); 115 | a.x = 0.5; a.y = 2; a.z = moves_height; 116 | a.x += off.x; a.y += off.y; 117 | add_point_to_buffer(planner, a); 118 | a.x = 0.5; a.y = 2; a.z = draw_height; 119 | a.x += off.x; a.y += off.y; 120 | add_point_to_buffer(planner, a); 121 | a.x = 0.5; a.y = 0; a.z = draw_height; 122 | a.x += off.x; a.y += off.y; 123 | add_point_to_buffer(planner, a); 124 | a.x = 0.5; a.y = 0; a.z = moves_height; 125 | a.x += off.x; a.y += off.y; 126 | add_point_to_buffer(planner, a); 127 | 128 | // I 129 | a.x = 0.75; a.y = 2; a.z = moves_height; 130 | a.x += off.x; a.y += off.y; 131 | add_point_to_buffer(planner, a); 132 | a.x = 0.75; a.y = 2; a.z = draw_height; 133 | a.x += off.x; a.y += off.y; 134 | add_point_to_buffer(planner, a); 135 | a.x = 0.75; a.y = 1.5; a.z = draw_height; 136 | a.x += off.x; a.y += off.y; 137 | add_point_to_buffer(planner, a); 138 | a.x = 0.75; a.y = 1.5; a.z = moves_height; 139 | a.x += off.x; a.y += off.y; 140 | add_point_to_buffer(planner, a); 141 | a.x = 0.75; a.y = 1; a.z = moves_height; 142 | a.x += off.x; a.y += off.y; 143 | add_point_to_buffer(planner, a); 144 | a.x = 0.75; a.y = 1; a.z = draw_height; 145 | a.x += off.x; a.y += off.y; 146 | add_point_to_buffer(planner, a); 147 | a.x = 0.75; a.y = 0; a.z = draw_height; 148 | a.x += off.x; a.y += off.y; 149 | add_point_to_buffer(planner, a); 150 | a.x = 0.75; a.y = 0; a.z = moves_height; 151 | a.x += off.x; a.y += off.y; 152 | add_point_to_buffer(planner, a); 153 | 154 | // T 155 | a.x = 1.5; a.y = 2; a.z = moves_height; 156 | a.x += off.x; a.y += off.y; 157 | add_point_to_buffer(planner, a); 158 | a.x = 1.5; a.y = 2; a.z = draw_height; 159 | a.x += off.x; a.y += off.y; 160 | add_point_to_buffer(planner, a); 161 | a.x = 2; a.y = 2; a.z = draw_height; 162 | a.x += off.x; a.y += off.y; 163 | add_point_to_buffer(planner, a); 164 | a.x = 1.75; a.y = 2; a.z = draw_height; 165 | a.x += off.x; a.y += off.y; 166 | add_point_to_buffer(planner, a); 167 | a.x = 1.75; a.y = 0; a.z = draw_height; 168 | a.x += off.x; a.y += off.y; 169 | add_point_to_buffer(planner, a); 170 | a.x = 1.75; a.y = 0; a.z = moves_height; 171 | a.x += off.x; a.y += off.y; 172 | add_point_to_buffer(planner, a); 173 | 174 | // I 175 | a.x = 2.25; a.y = 0; a.z = moves_height; 176 | a.x += off.x; a.y += off.y; 177 | add_point_to_buffer(planner, a); 178 | a.x = 2.25; a.y = 0; a.z = draw_height; 179 | a.x += off.x; a.y += off.y; 180 | add_point_to_buffer(planner, a); 181 | a.x = 2.75; a.y = 0; a.z = draw_height; 182 | a.x += off.x; a.y += off.y; 183 | add_point_to_buffer(planner, a); 184 | a.x = 2.5; a.y = 0; a.z = draw_height; 185 | a.x += off.x; a.y += off.y; 186 | add_point_to_buffer(planner, a); 187 | a.x = 2.5; a.y = 2; a.z = draw_height; 188 | a.x += off.x; a.y += off.y; 189 | add_point_to_buffer(planner, a); 190 | a.x = 2.25; a.y = 2; a.z = draw_height; 191 | a.x += off.x; a.y += off.y; 192 | add_point_to_buffer(planner, a); 193 | a.x = 2.75; a.y = 2; a.z = draw_height; 194 | a.x += off.x; a.y += off.y; 195 | add_point_to_buffer(planner, a); 196 | a.x = 2.75; a.y = 2; a.z = moves_height; 197 | a.x += off.x; a.y += off.y; 198 | add_point_to_buffer(planner, a); 199 | } 200 | 201 | 202 | 203 | 204 | 205 | 206 | -------------------------------------------------------------------------------- /mbed/patterns.h: -------------------------------------------------------------------------------- 1 | #ifndef PATTERNS_H 2 | #define PATTERNS_H 3 | 4 | #include "planner.h" 5 | #include "common.h" 6 | 7 | void draw_square_large(F32 moves_height, F32 draw_height, Planner* planner); 8 | void draw_star(F32 moves_height, F32 draw_height, Planner* planner); 9 | void draw_square_nn(F32 moves_height, F32 draw_height, Planner* planner); 10 | void draw_ti(F32 moves_height, F32 draw_height, Point off, Planner* planner); 11 | 12 | #endif -------------------------------------------------------------------------------- /mbed/planner.cpp: -------------------------------------------------------------------------------- 1 | #include "planner.h" 2 | 3 | extern DigitalOut led1; 4 | extern DigitalOut led2; 5 | extern DigitalOut led3; 6 | extern DigitalOut led4; 7 | 8 | inline F32 dist_between(Point a, Point b) { 9 | return sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y) + (a.z - b.z) * (a.z - b.z)); 10 | } 11 | 12 | inline void conform_goal(Point* in) { 13 | in->x = RESTRICT(in->x, MIN_X, MAX_X); 14 | in->y = RESTRICT(in->y, MIN_Y, MAX_Y); 15 | in->z = RESTRICT(in->z, MIN_Z, MAX_Z); 16 | } 17 | 18 | Status planner_setup(Planner* planner) { 19 | // Initialize circular buffer 20 | planner->current = 0; 21 | planner->next = 0; 22 | 23 | planner->current_pos.x = 0; 24 | planner->current_pos.y = 0; 25 | planner->current_pos.z = START_Z; 26 | 27 | planner->next_pos = planner->current_pos; 28 | 29 | planner->prev_dist = 0; 30 | 31 | return set_position(planner->current_pos); 32 | } 33 | 34 | Status reset_position(Planner* planner) { 35 | Point goal; 36 | goal.x = 0; 37 | goal.y = 0; 38 | goal.z = START_Z; 39 | return goto_point(planner, goal); 40 | } 41 | 42 | Status add_point_to_buffer(Planner* planner, Point in) { 43 | if (INC_ONE(planner->next) == planner->current) 44 | return FAILURE; 45 | planner->buffer[planner->next] = in; 46 | planner->next = INC_ONE(planner->next); 47 | return SUCCESS; 48 | } 49 | 50 | void clear_buffer(Planner* planner) { 51 | // Initial location 52 | planner->next = planner->current; 53 | } 54 | 55 | int get_num_in_buffer(Planner* planner) { 56 | if (planner->next < planner->current) 57 | return planner->next + PLANNER_BUFFER_SIZE - planner->current - 1; 58 | else 59 | return planner->next - planner->current - 1; 60 | } 61 | 62 | Status goto_point(Planner* planner, F32 x, F32 y, F32 z) { 63 | Point goal; 64 | goal.x = x; 65 | goal.y = y; 66 | goal.z = z; 67 | return goto_point(planner, goal); 68 | } 69 | 70 | Status goto_point(Planner* planner, Point goal) { 71 | Point cur = planner->current_pos; 72 | 73 | F32 step = 0, inv_vec_mag; 74 | F32 dx, dy, dz; 75 | F32 dist, full_dist, prev_dist; 76 | 77 | Planner_State state = PLR_ACCL; 78 | 79 | conform_goal(&goal); 80 | planner->next_pos = goal; 81 | 82 | dx = goal.x - cur.x; 83 | dy = goal.y - cur.y; 84 | dz = goal.z - cur.z; 85 | 86 | dist = dist_between(cur, goal); 87 | full_dist = dist; 88 | prev_dist = dist; 89 | 90 | if (dist < MIN_STEP_SIZE) 91 | return SUCCESS; 92 | 93 | // Inverse square root!!! 94 | inv_vec_mag = 1 / dist; 95 | dx = dx * inv_vec_mag; 96 | dy = dy * inv_vec_mag; 97 | dz = dz * inv_vec_mag; 98 | 99 | // TODO: think of a better way to exit 100 | while (1) { 101 | if (state == PLR_ACCL) { 102 | if (full_dist - prev_dist > ACCL_ZONE) 103 | state = PLR_FULL; 104 | 105 | else if (prev_dist < ACCL_ZONE && prev_dist * 2 < full_dist) 106 | state = PLR_DECL; 107 | 108 | else 109 | step = MAP(full_dist - prev_dist, 0, ACCL_ZONE, MIN_STEP_SIZE, MAX_STEP_SIZE); 110 | } 111 | if (state == PLR_FULL) { 112 | if (prev_dist < ACCL_ZONE) 113 | state = PLR_DECL; 114 | 115 | else 116 | step = MAX_STEP_SIZE; 117 | } 118 | if (state == PLR_DECL) { 119 | Point test_step; 120 | step = MAP(prev_dist, 0, ACCL_ZONE, MIN_STEP_SIZE, MAX_STEP_SIZE); 121 | 122 | test_step.x = cur.x + dx * step; 123 | test_step.y = cur.y + dy * step; 124 | test_step.z = cur.z + dz * step; 125 | 126 | dist = dist_between(test_step, goal); 127 | 128 | // pc.printf("dist: %.15f, prev_dist: %.15f\n", dist, prev_dist); 129 | if (dist >= prev_dist || dist < MIN_STEP_SIZE) { 130 | // This corrects accumulated error 131 | cur.x = goal.x; 132 | cur.y = goal.y; 133 | cur.z = goal.z; 134 | break; 135 | } 136 | } 137 | 138 | cur.x += dx * step; 139 | cur.y += dy * step; 140 | cur.z += dz * step; 141 | 142 | if (set_position(cur) != SUCCESS) 143 | return FAILURE; 144 | 145 | prev_dist = dist_between(cur, goal); 146 | } 147 | planner->current_pos = cur; 148 | return SUCCESS; 149 | } 150 | 151 | Status planner_process(Planner* planner) { 152 | if (planner->current == planner->next) 153 | return SUCCESS; 154 | if (goto_point(planner, planner->buffer[planner->current]) == SUCCESS) { 155 | 156 | if (INC_ONE(planner->current) == planner->next) 157 | return END_PAT; 158 | 159 | planner->current = INC_ONE(planner->current); 160 | 161 | return SUCCESS; 162 | } 163 | led1 = 1; 164 | return FAILURE; 165 | } 166 | 167 | Status troll_up(Planner* planner) { 168 | if (planner->current_pos.z - 0.0001 > MIN_Z) { 169 | planner->current_pos.z -= MIN_STEP_SIZE; 170 | return set_position(planner->current_pos); 171 | } 172 | return FAILURE; 173 | } 174 | 175 | Status troll_down(Planner* planner) { 176 | if (planner->current_pos.z + MIN_STEP_SIZE < MAX_Z) { 177 | planner->current_pos.z += MIN_STEP_SIZE; 178 | return set_position(planner->current_pos); 179 | } 180 | return FAILURE; 181 | } 182 | 183 | Status nudge_x(Planner* planner, F32 amount) { 184 | Point goal = planner->current_pos; 185 | goal.x += amount; 186 | conform_goal(&goal); 187 | //pc.printf("X at: %.5f delta: %.5f\n", goal.x, amount); 188 | return goto_point(planner, goal); 189 | } 190 | -------------------------------------------------------------------------------- /mbed/planner.h: -------------------------------------------------------------------------------- 1 | #ifndef PLANNER_H 2 | #define PLANNER_H 3 | 4 | #include "common.h" 5 | #include "positioner.h" 6 | 7 | #define PLANNER_BUFFER_SIZE 256 8 | 9 | #define MAX_STEP_SIZE 0.0025 10 | #define MIN_STEP_SIZE 0.0001 11 | 12 | #define ACCL_ZONE .75 // inches 13 | 14 | #define INC_ONE(a) (((a) + 1) % PLANNER_BUFFER_SIZE) 15 | 16 | enum Planner_State { 17 | PLR_ACCL, 18 | PLR_FULL, 19 | PLR_DECL, 20 | PLR_NEXT, 21 | PLR_WAIT 22 | }; 23 | 24 | struct Planner { 25 | Point buffer[PLANNER_BUFFER_SIZE]; 26 | 27 | S32 current; 28 | S32 next; 29 | 30 | Point current_pos; 31 | Point next_pos; 32 | F32 dx; 33 | F32 dy; 34 | F32 dz; 35 | 36 | F32 prev_dist; 37 | F32 full_dist; 38 | }; 39 | 40 | #ifdef DEBUG 41 | extern Serial pc; 42 | #endif 43 | 44 | Status planner_setup(Planner* planner); 45 | Status reset_position(Planner* planner); 46 | 47 | Status add_point_to_buffer(Planner* planner, Point in); 48 | void clear_buffer(Planner* planner); 49 | S32 get_num_in_buffer(Planner* planner); 50 | 51 | Status goto_point(Planner* planner, F32 x, F32 y, F32 z); 52 | Status goto_point(Planner* planner, Point goal); 53 | Status planner_process(Planner* planner); 54 | 55 | Status troll_up(Planner* planner); 56 | Status troll_down(Planner* planner); 57 | 58 | Status nudge_x(Planner* planner, F32 amount); 59 | 60 | #endif 61 | -------------------------------------------------------------------------------- /mbed/positioner.cpp: -------------------------------------------------------------------------------- 1 | #include "positioner.h" 2 | 3 | extern DigitalOut led1; 4 | extern DigitalOut led2; 5 | extern DigitalOut led3; 6 | extern DigitalOut led4; 7 | 8 | static Point cur_position; 9 | 10 | inline F32 max(F32 a, F32 b) { 11 | return (a > b ? a : b); 12 | } 13 | 14 | inline Point rotate_xy(Point coord, F32 sin_theta, F32 cos_theta) { 15 | Point ret; 16 | ret.x = coord.x * cos_theta - coord.y * sin_theta; 17 | ret.y = coord.x * sin_theta + coord.y * cos_theta; 18 | ret.z = coord.z; 19 | return ret; 20 | } 21 | 22 | inline F32 r2(F32 in) { 23 | return in * in; 24 | } 25 | 26 | Status set_position(Point target) { 27 | F32 dist, inv_dist, alpha; 28 | F32 angle; 29 | F32 x2, z2, rx, rz, h; 30 | F32 lower_radius; 31 | Point target_rot, trans; 32 | F32 result[3]; 33 | 34 | // Bounds checking 35 | // Shaddup bout the Z offset 36 | if (target.x < MIN_X || target.x > MAX_X || 37 | target.y < MIN_Y || target.y > MAX_Y || 38 | target.z < MIN_Z - 0.75 || target.z > MAX_Z) { 39 | led3 = 1; 40 | return FAILURE; 41 | } 42 | 43 | target_rot.x = target.x; 44 | target_rot.y = target.y; 45 | target_rot.z = target.z; 46 | 47 | for (int servo = 0; servo < 3; servo++) { 48 | // rotate coordinates 49 | if (servo == SERVO_120) 50 | target_rot = rotate_xy(target, SIN_120, COS_120); 51 | if (servo == SERVO_240) 52 | target_rot = rotate_xy(target, SIN_240, COS_240); 53 | 54 | // Add servo offset and hand offset 55 | trans.x = target_rot.x + SERVO_XOFF + HAND_XOFF; 56 | trans.y = target_rot.y; 57 | // Add servo offset and tool offset 58 | trans.z = target_rot.z + SERVO_ZOFF + TOOL_ZOFF; 59 | 60 | lower_radius = sqrt(r2(ARM_LOWER_LEN) - r2(trans.y)); 61 | 62 | dist = hypot(trans.x, trans.z); 63 | // Inverse square root!!! 64 | inv_dist = 1 / dist; 65 | 66 | // Bounds checking 67 | if (dist > (ARM_UPPER_LEN + lower_radius) || 68 | dist < (lower_radius - ARM_UPPER_LEN)) { 69 | //pc.printf("OH FFFFFFUUUUUUUU %.5f, %.5f, %.5f\n", dist, ARM_UPPER_LEN, lower_radius); 70 | led2 = 1; 71 | return FAILURE; 72 | } 73 | 74 | alpha = (r2(ARM_UPPER_LEN) - r2(lower_radius) + r2(dist)) * 0.5 * inv_dist; 75 | 76 | x2 = (trans.x * alpha * inv_dist); 77 | z2 = (trans.z * alpha * inv_dist); 78 | 79 | h = sqrt(r2(ARM_UPPER_LEN) - r2(alpha)); 80 | 81 | rx = -trans.z * (h * inv_dist); 82 | rz = trans.x * (h * inv_dist); 83 | 84 | angle = atan2(z2 - rz, x2 - rx); 85 | 86 | result[servo] = angle; 87 | } 88 | 89 | servo_set_angle(SERVO_0, result[0]); 90 | servo_set_angle(SERVO_120, result[1]); 91 | servo_set_angle(SERVO_240, result[2]); 92 | 93 | return SUCCESS; 94 | } 95 | 96 | Point position() { 97 | return cur_position; 98 | } 99 | 100 | -------------------------------------------------------------------------------- /mbed/positioner.h: -------------------------------------------------------------------------------- 1 | #ifndef POSITIONER_H 2 | #define POSITIONER_H 3 | 4 | #include "common.h" 5 | #include "servo.h" 6 | 7 | #define SIN_120 0.866025404 8 | #define COS_120 -0.5 9 | #define SIN_240 -0.866025404 10 | #define COS_240 -0.5 11 | 12 | #ifdef DEBUG 13 | extern Serial pc; 14 | #endif 15 | 16 | // Returns if possible to set position to this location 17 | Status set_position(Point coord); 18 | Point position(); 19 | 20 | #endif -------------------------------------------------------------------------------- /mbed/servo.cpp: -------------------------------------------------------------------------------- 1 | #include "servo.h" 2 | 3 | static PwmOut servos[3] = {PwmOut(p21), PwmOut(p22), PwmOut(p23)}; 4 | 5 | void servos_setup() { 6 | servos[0].period(0.020); 7 | servos[1].period(0.020); 8 | servos[2].period(0.020); 9 | } 10 | 11 | Status servo_set_angle(Servo servo, F32 angle) { 12 | //pc.printf("Setting servo %d to %0.2f\n", servo, angle * 180.0 / 3.141592); 13 | if (angle > SERVO_MAX_ANGLE || angle < SERVO_MIN_ANGLE) 14 | return FAILURE; 15 | 16 | // Map the servo's value in degrees to the value in usec 17 | F32 us = MAP(angle, SERVO_MIN_ANGLE, SERVO_MAX_ANGLE, SERVO_MIN_PULSE, SERVO_MAX_PULSE); 18 | //((F32)(angle - SERVO_MIN_ANGLE) / (F32)(SERVO_MAX_ANGLE - SERVO_MIN_ANGLE)) * 19 | // (SERVO_MAX_PULSE - SERVO_MIN_PULSE) + SERVO_MIN_PULSE; 20 | servos[servo].pulsewidth_us((S32)us); 21 | return SUCCESS; 22 | } -------------------------------------------------------------------------------- /mbed/servo.h: -------------------------------------------------------------------------------- 1 | #ifndef SERVO_H 2 | #define SERVO_H 3 | 4 | #include "mbed.h" 5 | 6 | #include "common.h" 7 | 8 | #define SERVO_MIN_PULSE 760 9 | #define SERVO_MAX_PULSE 2200 10 | 11 | #define SERVO_MIN_ANGLE -1.01922088 12 | #define SERVO_MAX_ANGLE 1.49077498 13 | 14 | typedef enum { 15 | SERVO_0, 16 | SERVO_120, 17 | SERVO_240 18 | } Servo; 19 | 20 | #ifdef DEBUG 21 | extern Serial pc; 22 | #endif 23 | 24 | void servos_setup(); 25 | Status servo_set_angle(Servo servo, F32 angle); 26 | 27 | #endif -------------------------------------------------------------------------------- /pythagoras_android/.classpath: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /pythagoras_android/.project: -------------------------------------------------------------------------------- 1 | 2 | 3 | pythagoras_android 4 | 5 | 6 | 7 | 8 | 9 | com.android.ide.eclipse.adt.ResourceManagerBuilder 10 | 11 | 12 | 13 | 14 | com.android.ide.eclipse.adt.PreCompilerBuilder 15 | 16 | 17 | 18 | 19 | org.eclipse.jdt.core.javabuilder 20 | 21 | 22 | 23 | 24 | com.android.ide.eclipse.adt.ApkBuilder 25 | 26 | 27 | 28 | 29 | 30 | com.android.ide.eclipse.adt.AndroidNature 31 | org.eclipse.jdt.core.javanature 32 | 33 | 34 | 35 | org_opencv_src 36 | 2 37 | _android_org_opencv_231e4df8/src 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /pythagoras_android/AndroidManifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 8 | 9 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /pythagoras_android/bin/classes.dex: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/pythagoras_android/bin/classes.dex -------------------------------------------------------------------------------- /pythagoras_android/bin/com/aaronbot3000/pythagorasandroid/R$attr.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/pythagoras_android/bin/com/aaronbot3000/pythagorasandroid/R$attr.class -------------------------------------------------------------------------------- /pythagoras_android/bin/com/aaronbot3000/pythagorasandroid/R$drawable.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/pythagoras_android/bin/com/aaronbot3000/pythagorasandroid/R$drawable.class -------------------------------------------------------------------------------- /pythagoras_android/bin/com/aaronbot3000/pythagorasandroid/R$id.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/pythagoras_android/bin/com/aaronbot3000/pythagorasandroid/R$id.class -------------------------------------------------------------------------------- /pythagoras_android/bin/com/aaronbot3000/pythagorasandroid/R$layout.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/pythagoras_android/bin/com/aaronbot3000/pythagorasandroid/R$layout.class -------------------------------------------------------------------------------- /pythagoras_android/bin/com/aaronbot3000/pythagorasandroid/R$string.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/pythagoras_android/bin/com/aaronbot3000/pythagorasandroid/R$string.class -------------------------------------------------------------------------------- /pythagoras_android/bin/com/aaronbot3000/pythagorasandroid/R.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/pythagoras_android/bin/com/aaronbot3000/pythagorasandroid/R.class -------------------------------------------------------------------------------- /pythagoras_android/bin/com/aaronbot3000/pythagorasandroid/StageOne$ButtonCallback.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/pythagoras_android/bin/com/aaronbot3000/pythagorasandroid/StageOne$ButtonCallback.class -------------------------------------------------------------------------------- /pythagoras_android/bin/com/aaronbot3000/pythagorasandroid/StageOne$SeekbarChangeCallback.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/pythagoras_android/bin/com/aaronbot3000/pythagorasandroid/StageOne$SeekbarChangeCallback.class -------------------------------------------------------------------------------- /pythagoras_android/bin/com/aaronbot3000/pythagorasandroid/StageOne.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/pythagoras_android/bin/com/aaronbot3000/pythagorasandroid/StageOne.class -------------------------------------------------------------------------------- /pythagoras_android/bin/resources.ap_: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/pythagoras_android/bin/resources.ap_ -------------------------------------------------------------------------------- /pythagoras_android/default.properties: -------------------------------------------------------------------------------- 1 | # This file is automatically generated by Android Tools. 2 | # Do not modify this file -- YOUR CHANGES WILL BE ERASED! 3 | # 4 | # This file must be checked in Version Control Systems. 5 | # 6 | # To customize properties used by the Ant build system use, 7 | # "build.properties", and override values to adapt the script to your 8 | # project structure. 9 | 10 | # Project target. 11 | target=android-10 12 | android.library.reference.1=../../OpenCV-2.3.1 13 | -------------------------------------------------------------------------------- /pythagoras_android/gen/com/aaronbot3000/pythagorasandroid/R.java: -------------------------------------------------------------------------------- 1 | /* AUTO-GENERATED FILE. DO NOT MODIFY. 2 | * 3 | * This class was automatically generated by the 4 | * aapt tool from the resource data it found. It 5 | * should not be modified by hand. 6 | */ 7 | 8 | package com.aaronbot3000.pythagorasandroid; 9 | 10 | public final class R { 11 | public static final class attr { 12 | } 13 | public static final class drawable { 14 | public static final int icon=0x7f020000; 15 | } 16 | public static final class id { 17 | public static final int button_continue=0x7f050006; 18 | public static final int button_take_picture=0x7f050005; 19 | public static final int image_first_stage=0x7f050003; 20 | public static final int linearLayout1=0x7f050004; 21 | public static final int slider_blur=0x7f050000; 22 | public static final int slider_high_thresh=0x7f050001; 23 | public static final int slider_low_thresh=0x7f050002; 24 | } 25 | public static final class layout { 26 | public static final int main=0x7f030000; 27 | } 28 | public static final class string { 29 | public static final int app_name=0x7f040001; 30 | public static final int hello=0x7f040000; 31 | public static final int st_continue=0x7f040003; 32 | public static final int st_take_picture=0x7f040002; 33 | } 34 | } 35 | -------------------------------------------------------------------------------- /pythagoras_android/gen/org/opencv/R.java: -------------------------------------------------------------------------------- 1 | /* AUTO-GENERATED FILE. DO NOT MODIFY. 2 | * 3 | * This class was automatically generated by the 4 | * aapt tool from the resource data it found. It 5 | * should not be modified by hand. 6 | */ 7 | 8 | package org.opencv; 9 | 10 | public final class R { 11 | public static final class attr { 12 | } 13 | public static final class drawable { 14 | public static final int icon=0x7f020000; 15 | } 16 | public static final class id { 17 | public static final int button_continue=0x7f050006; 18 | public static final int button_take_picture=0x7f050005; 19 | public static final int image_first_stage=0x7f050003; 20 | public static final int linearLayout1=0x7f050004; 21 | public static final int slider_blur=0x7f050000; 22 | public static final int slider_high_thresh=0x7f050001; 23 | public static final int slider_low_thresh=0x7f050002; 24 | } 25 | public static final class layout { 26 | public static final int main=0x7f030000; 27 | } 28 | public static final class string { 29 | public static final int app_name=0x7f040001; 30 | public static final int hello=0x7f040000; 31 | public static final int st_continue=0x7f040003; 32 | public static final int st_take_picture=0x7f040002; 33 | } 34 | } 35 | -------------------------------------------------------------------------------- /pythagoras_android/proguard.cfg: -------------------------------------------------------------------------------- 1 | -optimizationpasses 5 2 | -dontusemixedcaseclassnames 3 | -dontskipnonpubliclibraryclasses 4 | -dontpreverify 5 | -verbose 6 | -optimizations !code/simplification/arithmetic,!field/*,!class/merging/* 7 | 8 | -keep public class * extends android.app.Activity 9 | -keep public class * extends android.app.Application 10 | -keep public class * extends android.app.Service 11 | -keep public class * extends android.content.BroadcastReceiver 12 | -keep public class * extends android.content.ContentProvider 13 | -keep public class * extends android.app.backup.BackupAgentHelper 14 | -keep public class * extends android.preference.Preference 15 | -keep public class com.android.vending.licensing.ILicensingService 16 | 17 | -keepclasseswithmembernames class * { 18 | native ; 19 | } 20 | 21 | -keepclasseswithmembers class * { 22 | public (android.content.Context, android.util.AttributeSet); 23 | } 24 | 25 | -keepclasseswithmembers class * { 26 | public (android.content.Context, android.util.AttributeSet, int); 27 | } 28 | 29 | -keepclassmembers class * extends android.app.Activity { 30 | public void *(android.view.View); 31 | } 32 | 33 | -keepclassmembers enum * { 34 | public static **[] values(); 35 | public static ** valueOf(java.lang.String); 36 | } 37 | 38 | -keep class * implements android.os.Parcelable { 39 | public static final android.os.Parcelable$Creator *; 40 | } 41 | -------------------------------------------------------------------------------- /pythagoras_android/res/drawable-hdpi/icon.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/pythagoras_android/res/drawable-hdpi/icon.png -------------------------------------------------------------------------------- /pythagoras_android/res/drawable-ldpi/icon.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/pythagoras_android/res/drawable-ldpi/icon.png -------------------------------------------------------------------------------- /pythagoras_android/res/drawable-mdpi/icon.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/pythagoras_android/res/drawable-mdpi/icon.png -------------------------------------------------------------------------------- /pythagoras_android/res/layout/main.xml: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /pythagoras_android/res/values/strings.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Hello World, PA_Activity! 4 | Pythagoras-Android 5 | Take Picture 6 | Continue 7 | 8 | -------------------------------------------------------------------------------- /pythagoras_android/src/com/aaronbot3000/pythagorasandroid/StageOne.java: -------------------------------------------------------------------------------- 1 | package com.aaronbot3000.pythagorasandroid; 2 | 3 | import java.io.File; 4 | import java.io.IOException; 5 | 6 | import org.opencv.android.Utils; 7 | import org.opencv.core.Mat; 8 | import org.opencv.core.Point; 9 | import org.opencv.core.Size; 10 | import org.opencv.imgproc.Imgproc; 11 | 12 | import android.app.Activity; 13 | import android.content.Intent; 14 | import android.graphics.Bitmap; 15 | import android.graphics.BitmapFactory; 16 | import android.net.Uri; 17 | import android.os.Bundle; 18 | import android.provider.MediaStore; 19 | import android.util.Log; 20 | import android.view.View; 21 | import android.widget.Button; 22 | import android.widget.ImageView; 23 | import android.widget.SeekBar; 24 | 25 | public class StageOne extends Activity { 26 | /** Called when the activity is first created. */ 27 | 28 | private final int CAMERA_INTENT_ID = 1; 29 | 30 | private ImageView display; 31 | private SeekBar seekbar_blur; 32 | private SeekBar seekbar_high_thresh; 33 | private SeekBar seekbar_low_thresh; 34 | 35 | private int blur_amt; 36 | private int high_amt; 37 | private int low_amt; 38 | 39 | private static boolean has_image = false; 40 | 41 | private static Bitmap imagedata, cvdisplay = null; 42 | 43 | private static File tempfile = null; 44 | 45 | private class SeekbarChangeCallback implements 46 | SeekBar.OnSeekBarChangeListener { 47 | public void onProgressChanged(SeekBar sb, int val, boolean fromUser) { 48 | if (sb.getId() == R.id.slider_blur) { 49 | blur_amt = val; 50 | } else if (sb.getId() == R.id.slider_high_thresh) { 51 | high_amt = val; 52 | } 53 | if (sb.getId() == R.id.slider_low_thresh) { 54 | low_amt = val; 55 | } 56 | if (has_image) 57 | updateDisplay(); 58 | } 59 | 60 | public void onStartTrackingTouch(SeekBar arg0) { 61 | } 62 | 63 | public void onStopTrackingTouch(SeekBar arg0) { 64 | } 65 | } 66 | 67 | private class ButtonCallback implements View.OnClickListener { 68 | public void onClick(View view) { 69 | if (view.getId() == R.id.button_continue) 70 | startStageTwo(); 71 | else if (view.getId() == R.id.button_take_picture) 72 | startCameraActivity(); 73 | } 74 | } 75 | 76 | public void onCreate(Bundle savedInstanceState) { 77 | super.onCreate(savedInstanceState); 78 | setContentView(R.layout.main); 79 | 80 | seekbar_blur = (SeekBar) findViewById(R.id.slider_blur); 81 | seekbar_blur.setMax(10); 82 | seekbar_high_thresh = (SeekBar) findViewById(R.id.slider_high_thresh); 83 | seekbar_low_thresh = (SeekBar) findViewById(R.id.slider_low_thresh); 84 | 85 | SeekbarChangeCallback scc = new SeekbarChangeCallback(); 86 | seekbar_blur.setOnSeekBarChangeListener(scc); 87 | seekbar_high_thresh.setOnSeekBarChangeListener(scc); 88 | seekbar_low_thresh.setOnSeekBarChangeListener(scc); 89 | 90 | display = (ImageView) findViewById(R.id.image_first_stage); 91 | //cvdisplay = Bitmap.createBitmap(width, height, config); 92 | 93 | ButtonCallback bc = new ButtonCallback(); 94 | ((Button) findViewById(R.id.button_continue)).setOnClickListener(bc); 95 | ((Button) findViewById(R.id.button_take_picture)) 96 | .setOnClickListener(bc); 97 | 98 | if (has_image) { 99 | display.setImageBitmap(imagedata); 100 | } 101 | 102 | if (tempfile == null) { 103 | try { 104 | tempfile = File.createTempFile("pythagorasinput", ".bmp"); 105 | } 106 | catch(IOException e) { 107 | Log.e("pythagoras", "Cannot create temp file! FFFFFFFUUUUUUUUUUUU"); 108 | e.printStackTrace(); 109 | } 110 | } 111 | } 112 | 113 | protected void onDestroy() { 114 | super.onDestroy(); 115 | 116 | if (imagedata != null) 117 | imagedata.recycle(); 118 | 119 | if (cvdisplay != null) { 120 | cvdisplay.recycle(); 121 | cvdisplay = null; 122 | } 123 | 124 | if (tempfile != null) { 125 | tempfile.delete(); 126 | tempfile = null; 127 | } 128 | } 129 | 130 | private void startCameraActivity() { 131 | Intent intent = new Intent( 132 | android.provider.MediaStore.ACTION_IMAGE_CAPTURE); 133 | intent.putExtra( MediaStore.EXTRA_OUTPUT, Uri.fromFile(tempfile)); 134 | startActivityForResult(intent, CAMERA_INTENT_ID); 135 | } 136 | 137 | private void startStageTwo() { 138 | } 139 | 140 | protected void onActivityResult(int requestCode, int resultCode, Intent data) { 141 | switch (resultCode) { 142 | case 0: 143 | Log.i("MakeMachine", "User cancelled"); 144 | break; 145 | 146 | case -1: 147 | BitmapFactory.Options options = new BitmapFactory.Options(); 148 | options.inSampleSize = 8; 149 | 150 | if (imagedata != null) 151 | imagedata.recycle(); 152 | imagedata = (Bitmap) BitmapFactory.decodeFile(tempfile.getAbsolutePath(), options); 153 | Log.i("pythagoras", "image in type: " + imagedata.getConfig()); 154 | imagedata = imagedata.copy(Bitmap.Config.ARGB_8888, false); 155 | has_image = true; 156 | updateDisplay(); 157 | break; 158 | } 159 | } 160 | 161 | private void updateDisplay() { 162 | //display.setImageBitmap(imagedata); 163 | synchronized (this) { 164 | Mat cvmat = Utils.bitmapToMat(imagedata); 165 | Log.i("pythagoras", "made input mat"); 166 | Mat gray = new Mat(), cvoutput = new Mat(), grayout = new Mat(); 167 | Log.i("pythagoras", "alloced interim mats"); 168 | Imgproc.cvtColor(cvmat, gray, Imgproc.COLOR_RGB2GRAY); 169 | Log.i("pythagoras", "conv to gray"); 170 | Imgproc.GaussianBlur(gray, gray, new Size(new Point(blur_amt * 2 + 1, 171 | blur_amt * 2 + 1)), 10d); 172 | Log.i("pythagoras", "gaussian blurred"); 173 | Imgproc.cvtColor(gray, cvoutput, Imgproc.COLOR_GRAY2RGBA); 174 | Log.i("pythagoras", "conv to color"); 175 | 176 | if (cvdisplay == null) 177 | cvdisplay = Bitmap.createBitmap(cvoutput.width(), cvoutput.height(), Bitmap.Config.ARGB_8888); 178 | 179 | Utils.matToBitmap(cvoutput, cvdisplay); 180 | Log.i("pythagoras", "conv to bmp"); 181 | display.setImageBitmap(cvdisplay); 182 | } 183 | } 184 | } -------------------------------------------------------------------------------- /solidworks_models/#4 Screw.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/#4 Screw.sldprt -------------------------------------------------------------------------------- /solidworks_models/4-40_125_screw.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/4-40_125_screw.sldprt -------------------------------------------------------------------------------- /solidworks_models/4-40_14_screw.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/4-40_14_screw.sldprt -------------------------------------------------------------------------------- /solidworks_models/4-40_38_screw.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/4-40_38_screw.sldprt -------------------------------------------------------------------------------- /solidworks_models/4-40_nut.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/4-40_nut.sldprt -------------------------------------------------------------------------------- /solidworks_models/4-40_nut_wide.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/4-40_nut_wide.sldprt -------------------------------------------------------------------------------- /solidworks_models/4-40_spacer.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/4-40_spacer.sldprt -------------------------------------------------------------------------------- /solidworks_models/arm.sldasm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/arm.sldasm -------------------------------------------------------------------------------- /solidworks_models/balljoint.sldasm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/balljoint.sldasm -------------------------------------------------------------------------------- /solidworks_models/balljoint_ball.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/balljoint_ball.sldprt -------------------------------------------------------------------------------- /solidworks_models/balljoint_joint.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/balljoint_joint.sldprt -------------------------------------------------------------------------------- /solidworks_models/baseplate.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/baseplate.sldprt -------------------------------------------------------------------------------- /solidworks_models/cut_prep.sldasm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/cut_prep.sldasm -------------------------------------------------------------------------------- /solidworks_models/cut_prep.slddrw: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/cut_prep.slddrw -------------------------------------------------------------------------------- /solidworks_models/elbow.sldasm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/elbow.sldasm -------------------------------------------------------------------------------- /solidworks_models/elbow_rod.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/elbow_rod.sldprt -------------------------------------------------------------------------------- /solidworks_models/hand.sldasm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/hand.sldasm -------------------------------------------------------------------------------- /solidworks_models/hand_base.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/hand_base.sldprt -------------------------------------------------------------------------------- /solidworks_models/knife.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/knife.sldprt -------------------------------------------------------------------------------- /solidworks_models/lower_arm.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/lower_arm.sldprt -------------------------------------------------------------------------------- /solidworks_models/pen.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/pen.sldprt -------------------------------------------------------------------------------- /solidworks_models/robot.sldasm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/robot.sldasm -------------------------------------------------------------------------------- /solidworks_models/servo.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/servo.sldprt -------------------------------------------------------------------------------- /solidworks_models/servo_cover.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/servo_cover.sldprt -------------------------------------------------------------------------------- /solidworks_models/servo_with_horn.sldasm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/servo_with_horn.sldasm -------------------------------------------------------------------------------- /solidworks_models/servohorn1.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/servohorn1.sldprt -------------------------------------------------------------------------------- /solidworks_models/servohorn2.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/servohorn2.sldprt -------------------------------------------------------------------------------- /solidworks_models/servohorn3.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/servohorn3.sldprt -------------------------------------------------------------------------------- /solidworks_models/shaft_collar.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/shaft_collar.sldprt -------------------------------------------------------------------------------- /solidworks_models/support.sldasm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/support.sldasm -------------------------------------------------------------------------------- /solidworks_models/support_lower.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/support_lower.sldprt -------------------------------------------------------------------------------- /solidworks_models/support_upper.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/support_upper.sldprt -------------------------------------------------------------------------------- /solidworks_models/upperarm.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/upperarm.sldprt -------------------------------------------------------------------------------- /solidworks_models/wrist.sldasm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/wrist.sldasm -------------------------------------------------------------------------------- /solidworks_models/wrist_connector.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models/wrist_connector.sldprt -------------------------------------------------------------------------------- /solidworks_models_v2/4-40_12_screw.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/4-40_12_screw.sldprt -------------------------------------------------------------------------------- /solidworks_models_v2/4-40_38_screw.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/4-40_38_screw.sldprt -------------------------------------------------------------------------------- /solidworks_models_v2/4-40_nut.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/4-40_nut.sldprt -------------------------------------------------------------------------------- /solidworks_models_v2/4-40_nut_wide.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/4-40_nut_wide.sldprt -------------------------------------------------------------------------------- /solidworks_models_v2/4-40_spacer.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/4-40_spacer.sldprt -------------------------------------------------------------------------------- /solidworks_models_v2/M3-6_screw.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/M3-6_screw.sldprt -------------------------------------------------------------------------------- /solidworks_models_v2/arm_collar.slddrw: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/arm_collar.slddrw -------------------------------------------------------------------------------- /solidworks_models_v2/arm_collar.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/arm_collar.sldprt -------------------------------------------------------------------------------- /solidworks_models_v2/arm_lower.sldasm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/arm_lower.sldasm -------------------------------------------------------------------------------- /solidworks_models_v2/arm_lower_rod.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/arm_lower_rod.sldprt -------------------------------------------------------------------------------- /solidworks_models_v2/arm_upper.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/arm_upper.sldprt -------------------------------------------------------------------------------- /solidworks_models_v2/arm_upper_complete.sldasm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/arm_upper_complete.sldasm -------------------------------------------------------------------------------- /solidworks_models_v2/balljoint.sldasm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/balljoint.sldasm -------------------------------------------------------------------------------- /solidworks_models_v2/balljoint_ball.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/balljoint_ball.sldprt -------------------------------------------------------------------------------- /solidworks_models_v2/balljoint_joint.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/balljoint_joint.sldprt -------------------------------------------------------------------------------- /solidworks_models_v2/baseplate.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/baseplate.sldprt -------------------------------------------------------------------------------- /solidworks_models_v2/cut_parts_14.sldasm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/cut_parts_14.sldasm -------------------------------------------------------------------------------- /solidworks_models_v2/cut_parts_14.slddrw: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/cut_parts_14.slddrw -------------------------------------------------------------------------------- /solidworks_models_v2/cut_parts_18.sldasm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/cut_parts_18.sldasm -------------------------------------------------------------------------------- /solidworks_models_v2/cut_parts_18.slddrw: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/cut_parts_18.slddrw -------------------------------------------------------------------------------- /solidworks_models_v2/elbow.sldasm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/elbow.sldasm -------------------------------------------------------------------------------- /solidworks_models_v2/elbow_rod.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/elbow_rod.sldprt -------------------------------------------------------------------------------- /solidworks_models_v2/gear.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/gear.sldprt -------------------------------------------------------------------------------- /solidworks_models_v2/gear_pot.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/gear_pot.sldprt -------------------------------------------------------------------------------- /solidworks_models_v2/gear_stepper.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/gear_stepper.sldprt -------------------------------------------------------------------------------- /solidworks_models_v2/gusset_assm.sldasm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/gusset_assm.sldasm -------------------------------------------------------------------------------- /solidworks_models_v2/gusset_assm.slddrw: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/gusset_assm.slddrw -------------------------------------------------------------------------------- /solidworks_models_v2/gusset_plate.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/gusset_plate.sldprt -------------------------------------------------------------------------------- /solidworks_models_v2/hand.sldasm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/hand.sldasm -------------------------------------------------------------------------------- /solidworks_models_v2/hand_base.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/hand_base.sldprt -------------------------------------------------------------------------------- /solidworks_models_v2/motor_plate.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/motor_plate.sldprt -------------------------------------------------------------------------------- /solidworks_models_v2/motor_plate_support.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/motor_plate_support.sldprt -------------------------------------------------------------------------------- /solidworks_models_v2/new_support.sldasm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/new_support.sldasm -------------------------------------------------------------------------------- /solidworks_models_v2/new_support_lower.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/new_support_lower.sldprt -------------------------------------------------------------------------------- /solidworks_models_v2/new_support_upper.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/new_support_upper.sldprt -------------------------------------------------------------------------------- /solidworks_models_v2/pen holder.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/pen holder.sldprt -------------------------------------------------------------------------------- /solidworks_models_v2/potentiometer.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/potentiometer.sldprt -------------------------------------------------------------------------------- /solidworks_models_v2/potentiometer2.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/potentiometer2.sldprt -------------------------------------------------------------------------------- /solidworks_models_v2/robot.sldasm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/robot.sldasm -------------------------------------------------------------------------------- /solidworks_models_v2/robot_base.sldasm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/robot_base.sldasm -------------------------------------------------------------------------------- /solidworks_models_v2/stepper.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/stepper.sldprt -------------------------------------------------------------------------------- /solidworks_models_v2/support.sldasm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/support.sldasm -------------------------------------------------------------------------------- /solidworks_models_v2/support_lower.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/support_lower.sldprt -------------------------------------------------------------------------------- /solidworks_models_v2/support_support.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/support_support.sldprt -------------------------------------------------------------------------------- /solidworks_models_v2/support_upper.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/support_upper.sldprt -------------------------------------------------------------------------------- /solidworks_models_v2/upperarm.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/upperarm.sldprt -------------------------------------------------------------------------------- /solidworks_models_v2/wrist.sldasm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/wrist.sldasm -------------------------------------------------------------------------------- /solidworks_models_v2/wrist_connector.sldprt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/solidworks_models_v2/wrist_connector.sldprt -------------------------------------------------------------------------------- /sympy_scripts/inv_kinematics.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python2 2 | 3 | from __future__ import division 4 | from sympy import * 5 | 6 | # Generates a range for floats 7 | def frange(start, stop, step): 8 | r = start 9 | while r < stop: 10 | yield r 11 | r += step 12 | 13 | # Arm lengths 14 | ul_val = 8 15 | ll_val = 10 16 | 17 | # Target location 18 | radius = 6 * sqrt(2) 19 | tz_val = 12 20 | 21 | offset_matrix = Matrix([-2, 0]) 22 | 23 | # Symbols 24 | ul = Symbol('U') 25 | ll = Symbol('L') 26 | 27 | tx = Symbol('TX') 28 | ty = Symbol('TY') 29 | tz = Symbol('TZ') 30 | 31 | arm_theta = Symbol('at') 32 | 33 | # Functions 34 | rot_matrix = Matrix([(cos(arm_theta), -sin(arm_theta)), (sin(arm_theta), cos(arm_theta))]) 35 | 36 | dist = sqrt(tx**2 + ty**2 + tz**2) 37 | alpha = (ll**2 - ul**2 - dist**2) / 2 38 | 39 | jx1 = (sqrt(tx**2 * tz**2 * ul**2 + tz**4 * ul**2 - tz**2 * alpha**2) 40 | - tx * alpha)/(tx**2 + tz**2) 41 | 42 | jx2 = (-sqrt(tx**2 * tz**2 * ul**2 + tz**4 * ul**2 - tz**2 * alpha**2) 43 | - tx * alpha)/(tx**2 + tz**2) 44 | 45 | jz1 = acos(jx1/ul) 46 | jz2 = acos(jx2/ul) 47 | 48 | # Initial substitution 49 | jx1 = jx1.subs([ 50 | (ul, ul_val), 51 | (ll, ll_val), 52 | (tz, tz_val)]) 53 | 54 | jx2 = jx2.subs([ 55 | (ul, ul_val), 56 | (ll, ll_val), 57 | (tz, tz_val)]) 58 | 59 | # Making the spiral 60 | for theta in frange(0, 2*pi.evalf(), 0.1): 61 | print 'theta %.2f' % (theta * 180 / pi).evalf() 62 | 63 | #target = Matrix([radius * theta / (2 * pi) * cos(theta), radius * theta / (2 * pi) * sin(theta)]) 64 | target = Matrix([radius * cos(theta), radius * sin(theta)]) 65 | 66 | flag = 0 67 | target_val = (rot_matrix * target + offset_matrix).subs([(arm_theta, 0)]) 68 | 69 | jx_val1 = jx1.subs([ 70 | (tx, target_val[0]), 71 | (ty, target_val[1])]) 72 | 73 | jx_val2 = jx2.subs([ 74 | (tx, target_val[0]), 75 | (ty, target_val[1])]) 76 | 77 | print 'Arm 1' 78 | print 'xpos %.2f' % target_val[0].evalf() 79 | print 'ypos %.2f' % target_val[1].evalf() 80 | 81 | if jx_val1.evalf().is_real: 82 | print 'jx1: %.2f' % jx_val1.evalf() 83 | flag += 1 84 | if jx_val2.evalf().is_real: 85 | print 'jx2: %.2f' % jx_val2.evalf() 86 | flag += 1 87 | if not flag: 88 | print 'NO REAL ANSWER' 89 | 90 | flag = 0 91 | target_val = (rot_matrix * target + offset_matrix).subs([(arm_theta, 2 * pi / 3)]) 92 | 93 | jx_val1 = jx1.subs([ 94 | (tx, target_val[0]), 95 | (ty, target_val[1])]) 96 | 97 | jx_val2 = jx2.subs([ 98 | (tx, target_val[0]), 99 | (ty, target_val[1])]) 100 | 101 | print 'Arm 2' 102 | print 'xpos %.2f' % target_val[0].evalf() 103 | print 'ypos %.2f' % target_val[1].evalf() 104 | 105 | if jx_val1.evalf().is_real: 106 | print 'jx1: %.2f' % jx_val1.evalf() 107 | flag += 1 108 | if jx_val2.evalf().is_real: 109 | print 'jx2: %.2f' % jx_val2.evalf() 110 | flag += 1 111 | if not flag: 112 | print 'NO REAL ANSWER' 113 | 114 | flag = 0 115 | target_val = (rot_matrix * target + offset_matrix).subs([(arm_theta, 4 * pi / 3)]) 116 | 117 | jx_val1 = jx1.subs([ 118 | (tx, target_val[0]), 119 | (ty, target_val[1])]) 120 | 121 | jx_val2 = jx2.subs([ 122 | (tx, target_val[0]), 123 | (ty, target_val[1])]) 124 | 125 | print 'Arm 3' 126 | print 'xpos %.2f' % target_val[0].evalf() 127 | print 'ypos %.2f' % target_val[1].evalf() 128 | 129 | if jx_val1.evalf().is_real: 130 | print 'jx1: %.2f' % jx_val1.evalf() 131 | flag += 1 132 | if jx_val2.evalf().is_real: 133 | print 'jx2: %.2f' % jx_val2.evalf() 134 | flag += 1 135 | if not flag: 136 | print 'NO REAL ANSWER' 137 | print '\n' 138 | -------------------------------------------------------------------------------- /sympy_scripts/inv_kinematics_plain.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python2 2 | 3 | from __future__ import division 4 | from sympy import * 5 | 6 | xd = Symbol('x_des') 7 | yd = Symbol('y_des') 8 | zd = Symbol('z_des') 9 | 10 | l1 = Symbol('l_1') 11 | l2 = Symbol('l_2') 12 | 13 | vals = [(xd, 0), (yd, 5), (zd, -10), (l1, 2.5), (l2, 10)] 14 | 15 | lower_radius = sqrt(l2**2 - xd**2) 16 | 17 | dist = sqrt(yd**2 + zd**2) 18 | inv_dist = 1 / dist; 19 | 20 | alpha = (l1**2 - l2**2 + dist**2) * 0.5 * inv_dist; 21 | 22 | y1 = (yd * alpha * inv_dist); 23 | z1 = (zd * alpha * inv_dist); 24 | 25 | h = sqrt(l1**2 - alpha**2); 26 | 27 | y2 = -zd * (h * inv_dist); 28 | z2 = yd * (h * inv_dist); 29 | 30 | angle = atan2(z1 + z2, y1 + y2); 31 | print(angle.subs(vals).evalf()) 32 | -------------------------------------------------------------------------------- /sympy_scripts/resolved_rate.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python2 2 | 3 | from __future__ import division 4 | from sympy import * 5 | 6 | xd = Symbol('x_des') 7 | yd = Symbol('y_des') 8 | zd = Symbol('z_des') 9 | 10 | dx = Symbol('dx') 11 | dy = Symbol('dy') 12 | dz = Symbol('dz') 13 | 14 | l1 = Symbol('l_1') 15 | l2 = Symbol('l_2') 16 | 17 | vals = [(xd, 0), (yd, 5), (zd, -10), (l1, 2.5), (l2, 10)] 18 | dvals = [(dx, 5), (dy, 0), (dz, 0)] 19 | 20 | r = sqrt(l2**2 - xd**2) 21 | d = sqrt(zd**2 + yd**2) 22 | 23 | psi = atan2(zd, yd) 24 | beta = acos((d**2 + l1**2 - r**2) / (2 * d * l1)) 25 | 26 | angle = psi + beta 27 | 28 | print(angle) 29 | 30 | print('respect to x') 31 | dangle_x = diff(angle, xd) 32 | print(dangle_x) 33 | 34 | print('respect to y') 35 | dangle_y = diff(angle, yd) 36 | print(dangle_y) 37 | 38 | print('respect to z') 39 | dangle_z = diff(angle, zd) 40 | print(dangle_z) 41 | 42 | dangle = dangle_x * dx + dangle_y * dy + dangle_z * dz 43 | 44 | subbed = angle.subs(vals) 45 | dsubbed = dangle.subs(vals) 46 | dsubbed = dsubbed.subs(dvals) 47 | 48 | 49 | print('total') 50 | print(dsubbed.evalf()) 51 | print(subbed.evalf()) 52 | -------------------------------------------------------------------------------- /sympy_scripts/test_idea.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python2 2 | 3 | from __future__ import division 4 | from sympy import * 5 | 6 | t = Symbol('t') 7 | sx = Symbol('sx') 8 | sy = Symbol('sy') 9 | sz = Symbol('sz') 10 | dx = Symbol('dx') 11 | dy = Symbol('dy') 12 | dz = Symbol('dz') 13 | lx = sx + t * dx 14 | ly = sy + t * dy 15 | lz = sz + t * dz 16 | 17 | arm_ul = 5 18 | arm_ll = 7 19 | 20 | lower_radius = sqrt(arm_ll ** 2 - ly ** 2) 21 | 22 | dist = sqrt(lx ** 2 + lz ** 2) 23 | inv_dist = 1 / dist 24 | 25 | alpha = (arm_ul - lower_radius ** 2 + dist ** 2) * 0.5 * inv_dist 26 | 27 | x1 = (lx * alpha * inv_dist) 28 | z1 = (lz * alpha * inv_dist) 29 | 30 | h = sqrt(arm_ul ** 2 - alpha ** 2) 31 | 32 | x2 = -lz * (h * inv_dist) 33 | z2 = lx * (h * inv_dist) 34 | 35 | angle = atan2(z1 - z2, x1 - x2) 36 | 37 | print (z1 - z2) 38 | -------------------------------------------------------------------------------- /vectorizer-cpp/Makefile: -------------------------------------------------------------------------------- 1 | PROJ = send-serial 2 | OBJS = send-serial 3 | 4 | CC = g++ 5 | CFLAGS = -O2 -Wall -Wextra 6 | LIBS = -lopencv_core -lopencv_highgui 7 | 8 | all: $(OBJS) 9 | 10 | send-serial: send-serial.cpp 11 | $(CC) $(LIBS) $(CFLAGS) $< -o $@ 12 | -------------------------------------------------------------------------------- /vectorizer-cpp/send-serial.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | using namespace std; 8 | using namespace cv; 9 | 10 | static int smooth_val = 3; 11 | static int apeture = 0; 12 | static int high_th = 70; 13 | static int low_th = 30; 14 | 15 | static int poly_acc = 300; 16 | 17 | static Mat res_smooth; 18 | static Mat resized; 19 | 20 | void smooth_val_callback(int pos, void* data) { 21 | } 22 | 23 | 24 | void setup_gui() { 25 | namedWindow("Canny", CV_WINDOW_NORMAL); 26 | namedWindow("Contours", CV_WINDOW_NORMAL); 27 | 28 | createTrackbar("smooth", "Canny", &smooth_val, 10, &smooth_val_callback); 29 | /* 30 | createTrackbar("apeture", "Canny", &apeture, 2, &smooth_val_callback); 31 | createTrackbar("lowThresh", "Canny", &high_th, 100, &smooth_val_callback); 32 | createTrackbar("highThresh", "Canny", &low_th, 100, &smooth_val_callback); 33 | */ 34 | 35 | createTrackbar("poly acc", "Contours", &poly_acc, 500, &smooth_val_callback); 36 | } 37 | 38 | void init_mats(int width, int height) { 39 | Size size(width, height); 40 | res_smooth.create(size, CV_8UC1); 41 | resized.create(size, CV_8UC1); 42 | } 43 | 44 | 45 | int main(int argc, char* argv[]) { 46 | setup_gui(); 47 | 48 | if (argc < 3) { 49 | cout << "Not enough arguments\n"; 50 | return 0; 51 | } 52 | 53 | int width = atoi(argv[1]); 54 | string filename(argv[2]); 55 | 56 | Mat source = imread(filename, 0); 57 | int height = (double)width / source.cols * source.rows; 58 | 59 | init_mats(width, height); 60 | 61 | Size newsize(width, height); 62 | resize(source, resized, newsize); 63 | 64 | 65 | while(waitKey() != 'e'); 66 | } 67 | -------------------------------------------------------------------------------- /vectorizer/lena.bmp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronbot3000/deltadraw/ea16c73fc33f5fe422921deabf831bd977202c64/vectorizer/lena.bmp -------------------------------------------------------------------------------- /vectorizer/rasterize.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python2 2 | 3 | import cv, sys 4 | 5 | TEST_IMAGE = '003Venusaur.png' 6 | MOVE_POINT = 1 7 | DRAW_POINT = 2 8 | 9 | OVERLAP = 1.03 10 | MAX_DIFF = 0.007 11 | 12 | WHITE_THRESH = 0.85 13 | AMP_THRESH = 0.7 14 | 15 | class Rasterizer: 16 | 17 | __nwidth = 120 18 | 19 | # inches 20 | __stroke_width = 0.0115 21 | __gamma = 2.2 22 | 23 | def __map_range(self, a, b1, b2, x1, x2): 24 | return (float(a - b1) / float(b2 - b1)) * (x2 - x1) + x1 25 | 26 | def get_lines(self, image_name, MIN_X, MAX_X, MIN_Y, MAX_Y): 27 | cv.NamedWindow('Output', cv.CV_WINDOW_NORMAL) 28 | 29 | self.__plotw = MAX_X - MIN_X 30 | self.__ploth = MAX_Y - MIN_Y 31 | self.__load_image(image_name) 32 | cv.CreateTrackbar('width', 'Output', self.__nwidth, self.__gray.cols, self.__width_callback) 33 | cv.CreateTrackbar('gamma', 'Output', 220, 400, self.__gamma_callback) 34 | 35 | self.__width_callback(self.__nwidth) 36 | 37 | key = cv.WaitKey() 38 | while not (key == ord('q') or key == ord('e')): 39 | key = cv.WaitKey() 40 | print key 41 | if key == ord('e'): 42 | sys.exit(0) 43 | 44 | return self.__points 45 | 46 | def __fill_pixels(self, r, c): 47 | fulld = self.__pixw / self.__stroke_width 48 | 49 | # Get the average of the pixels 50 | value = cv.Get2D(self.__resized, r, c)[0] 51 | actual = self.__map_range(value, 0, 255, 0, 1) ** (1 / self.__gamma) 52 | 53 | points = [] 54 | 55 | if actual > WHITE_THRESH: 56 | cv.Set2D(self.__output, r, c, 255) 57 | return [] 58 | elif actual > AMP_THRESH: 59 | up = self.__pixh * self.__map_range(actual, AMP_THRESH, WHITE_THRESH, (1 - OVERLAP), 0.5) 60 | down = self.__pixh * self.__map_range(actual, AMP_THRESH, WHITE_THRESH, OVERLAP, 0.5) 61 | 62 | cv.Set2D(self.__output, r, c, int(255 * actual)) 63 | 64 | points.append((self.__pixw / 2.0, down)) 65 | points.append((self.__pixw, up)) 66 | else: 67 | linecount = int(self.__map_range(actual, 0, AMP_THRESH, fulld, 1)) 68 | cv.Set2D(self.__output, r, c, 69 | int(AMP_THRESH*255 - ((linecount-1) / float(fulld-1)) ** (1/2.2) * AMP_THRESH*255)) 70 | 71 | gap = float(self.__pixw) / linecount 72 | up = self.__pixh * (1 - OVERLAP) 73 | down = self.__pixh * OVERLAP 74 | 75 | # Here is where the magic happens 76 | for i in range(linecount): 77 | points.append((gap * (i + 0.5), down)) 78 | points.append((gap * (i + 1), up)) 79 | return points 80 | 81 | def __fill_image(self): 82 | last_was_move = True 83 | points = [] 84 | for i in range(self.__resized.rows): 85 | #for i in range(10): 86 | j = 0 87 | for j in range(self.__resized.cols): 88 | value = cv.Get2D(self.__resized, i, j)[0] 89 | 90 | pixels = self.__fill_pixels(i, j) 91 | shifted_pixel = [] 92 | if pixels: 93 | if last_was_move: 94 | shifted_pixel.append((-self.__draww/2 + pixels[0][0] + j * self.__pixw, 95 | -(-self.__drawh/2 + pixels[0][1] + i * self.__pixh), 96 | MOVE_POINT)) 97 | last_was_move = False 98 | shifted = [(-self.__draww/2 + x[0] + j * self.__pixw, 99 | -(-self.__drawh/2 + x[1] + i * self.__pixh), 100 | DRAW_POINT) for x in pixels] 101 | shifted_pixel.extend(shifted) 102 | else: 103 | if points and not last_was_move: 104 | shifted_pixel = [(points[-1][0], points[-1][1], MOVE_POINT)] 105 | last_was_move = True 106 | points.extend(shifted_pixel) 107 | 108 | # Move pen to next line 109 | if not last_was_move: 110 | shifted_pixel = [(points[-1][0], points[-1][1], MOVE_POINT)] 111 | points.extend(shifted_pixel) 112 | last_was_move = True 113 | 114 | return points 115 | 116 | def __load_image(self, image_name): 117 | orig = cv.LoadImageM(image_name) 118 | self.__gray = cv.CreateMat(orig.rows, orig.cols, cv.CV_8UC1) 119 | cv.CvtColor(orig, self.__gray, cv.CV_RGB2GRAY) 120 | 121 | if orig.cols > orig.rows: 122 | self.__draww = float(self.__plotw) 123 | self.__drawh = float(orig.rows) / orig.cols * self.__plotw 124 | else: 125 | self.__draww = float(orig.cols) / orig.rows * self.__ploth 126 | self.__drawh = float(self.__ploth) 127 | 128 | def __resize_image(self, newX): 129 | newY = int(float(newX) / self.__gray.cols * self.__gray.rows) 130 | if '__resized' in dir(self): 131 | del self.__resized 132 | 133 | if '__output' in dir(self): 134 | del self.__output 135 | 136 | self.__resized = cv.CreateMat(newY, newX, cv.CV_8UC1) 137 | self.__output = cv.CreateMat(newY, newX, cv.CV_8UC1) 138 | cv.Resize(self.__gray, self.__resized) 139 | 140 | self.__pixw = self.__draww / self.__resized.cols 141 | self.__pixh = self.__drawh / self.__resized.rows 142 | 143 | self.__refill_image() 144 | 145 | def __refill_image(self): 146 | # Filling image here 147 | self.__points = self.__fill_image() 148 | 149 | cv.ShowImage('Output', self.__output) 150 | 151 | 152 | def __width_callback(self, value): 153 | self.__resize_image(value + 1) 154 | 155 | def __gamma_callback(self, value): 156 | self.__gamma = float(value) / 100 157 | self.__refill_image() 158 | 159 | def main(): 160 | a = Rasterizer() 161 | points = a.get_lines(TEST_IMAGE, -3.75, 3.75, -3.75, 3.75) 162 | #for i in range(100): 163 | #print points[i] 164 | 165 | if __name__ == '__main__': 166 | main() 167 | -------------------------------------------------------------------------------- /vectorizer/send-serial.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python2 2 | 3 | from serial import Serial 4 | from vectorize import Vectorizer 5 | from rasterize import Rasterizer 6 | import struct, sys 7 | 8 | #INPUT = '4.1.05.tiff' 9 | #INPUT = 'test2.jpg' 10 | #INPUT = 'test_text.png' 11 | #INPUT = 'test4.png' 12 | INPUT = 'test4.jpg' 13 | #INPUT = 'test5.jpg' 14 | #INPUT = 'test3.png' 15 | #INPUT = 'lena.bmp' 16 | #INPUT = 'test_pattern.jpg' 17 | RES_X = 160 18 | 19 | MAX_X = 3.75 20 | MIN_X = -3.75 21 | 22 | MAX_Y = 3.75 23 | MIN_Y = -3.75 24 | 25 | SERIAL_PORT = '/dev/ttyACM0' 26 | BAUD = 115200 27 | 28 | END_DATA = struct.pack(' self.__newY: 26 | MIN_Y *= float(self.__newY) / self.__newX 27 | MAX_Y *= float(self.__newY) / self.__newX 28 | elif self.__newX < self.__newY: 29 | MIN_X *= float(self.__newX) / self.__newY 30 | MAX_X *= float(self.__newX) / self.__newY 31 | 32 | cur_p = self.polys_out 33 | poly_count = 1 34 | while cur_p: 35 | # Move to the first point in the polygon 36 | next_x = self.map_range(cur_p[0][0], 0, self.__newX, MIN_X, MAX_X) 37 | next_y = -self.map_range(cur_p[0][1], 0, self.__newY, MIN_Y, MAX_Y) 38 | points.append((next_x, next_y, MOVE_POINT)) 39 | 40 | # Send the polygon 41 | for point in cur_p: 42 | next_x = self.map_range(point[0], 0, self.__newX, MIN_X, MAX_X) 43 | next_y = -self.map_range(point[1], 0, self.__newY, MIN_Y, MAX_Y) 44 | points.append((next_x, next_y, DRAW_POINT)) 45 | 46 | next_x = self.map_range(cur_p[0][0], 0, self.__newX, MIN_X, MAX_X) 47 | next_y = -self.map_range(cur_p[0][1], 0, self.__newY, MIN_Y, MAX_Y) 48 | points.append((next_x, next_y, DRAW_POINT)) 49 | 50 | next_x = self.map_range(cur_p[0][0], 0, self.__newX, MIN_X, MAX_X) 51 | next_y = -self.map_range(cur_p[0][1], 0, self.__newY, MIN_Y, MAX_Y) 52 | points.append((next_x, next_y, MOVE_POINT)) 53 | 54 | poly_count += 1 55 | cur_p = cur_p.h_next() 56 | return points 57 | 58 | def get_polygons(self, image_name, newX, MIN_X, MAX_X, MIN_Y, MAX_Y): 59 | cv.NamedWindow('Canny', cv.CV_WINDOW_NORMAL) 60 | cv.NamedWindow('Contours', cv.CV_WINDOW_NORMAL) 61 | cv.CreateTrackbar('smooth', 'Canny', self.__smooth_val, 10, self.__smooth_val_callback) 62 | cv.CreateTrackbar('apeture', 'Canny', self.__canny_apeture, 2, self.__canny_apeture_callback) 63 | cv.CreateTrackbar('lowThreshold', 'Canny', self.__canny_lo, 100, self.__canny_lo_callback) 64 | cv.CreateTrackbar('highThreshold', 'Canny', self.__canny_hi, 100, self.__canny_hi_callback) 65 | cv.CreateTrackbar('poly acc', 'Contours', self.__poly_acc, 1000, self.__poly_acc_callback) 66 | 67 | self.__load_image(image_name, newX) 68 | self.__smooth_val_callback(self.__smooth_val) 69 | 70 | key = cv.WaitKey() 71 | while not (key == ord('q') or key == ord('e')): 72 | key = cv.WaitKey() 73 | print key 74 | 75 | if key == ord('e'): 76 | sys.exit(0) 77 | 78 | return self.__prep_for_machine(MIN_X, MAX_X, MIN_Y, MAX_Y) 79 | 80 | def __load_image(self, image_name, newX): 81 | orig = cv.LoadImageM(image_name) 82 | self.__newX = newX 83 | self.__newY = int(float(newX) / orig.cols * orig.rows) 84 | self.__init_mem(newX, self.__newY) 85 | 86 | cv.Resize(orig, self.res) 87 | cv.CvtColor(self.res, self.__res_gray, cv.CV_RGB2GRAY) 88 | 89 | def __init_mem(self, newX, newY): 90 | self.res = cv.CreateMat(newY, newX, cv.CV_8UC3) 91 | self.__res_gray = cv.CreateMat(newY, newX, cv.CV_8UC1) 92 | self.res_smooth = cv.CreateMat(newY, newX, cv.CV_8UC1) 93 | self.canny = cv.CreateMat(newY, newX, cv.CV_8UC1) 94 | self.contour_in = cv.CreateMat(newY, newX, cv.CV_8UC1) 95 | self.contour_out = cv.CreateMat(newY, newX, cv.CV_8UC3) 96 | self.c_storage = cv.CreateMemStorage() 97 | self.a_storage = cv.CreateMemStorage() 98 | 99 | def del_mem(self): 100 | del self.res 101 | del self.__res_gray 102 | del self.res_smooth 103 | del self.canny 104 | del self.contour_in 105 | del self.contour_out 106 | del self.contours 107 | del self.c_storage 108 | del self.a_storage 109 | cv.DestroyAllWindows() 110 | 111 | def __refresh_poly(self): 112 | self.polys_out = cv.ApproxPoly(self.contours, self.a_storage, cv.CV_POLY_APPROX_DP, self.__poly_acc / 100.0, -1) 113 | 114 | # Prints a count of the number of polygons and points in the picture thingy 115 | con = self.polys_out 116 | self.pointc = 0 117 | self.polyc = 0 118 | while not con == None: 119 | self.pointc += len(con) 120 | self.polyc += 1 121 | con = con.h_next() 122 | print '\n%d polygons'%self.polyc 123 | print '%d points'%self.pointc 124 | 125 | cv.Set(self.contour_out, cv.ScalarAll(255)) 126 | cv.DrawContours(self.contour_out, self.polys_out, cv.Scalar(0, 0, 0), cv.Scalar(0, 0, 0), 99) 127 | 128 | cv.ShowImage('Contours', self.contour_out) 129 | 130 | def __refresh_canny(self): 131 | cv.Canny(self.res_smooth, self.canny, self.__canny_lo, self.__canny_hi, self.__canny_apeture * 2 + 3) 132 | #cv.Threshold(self.res_smooth, self.canny, self.__canny_lo * 2, 255, cv.CV_THRESH_BINARY) 133 | cv.ShowImage('Canny', self.canny) 134 | cv.Copy(self.canny, self.contour_in) 135 | self.contours = cv.FindContours(self.contour_in, self.c_storage, cv.CV_RETR_LIST, cv.CV_CHAIN_APPROX_NONE) 136 | self.__refresh_poly() 137 | 138 | def __poly_acc_callback(self, value): 139 | self.__poly_acc = value 140 | self.__refresh_poly() 141 | 142 | def __smooth_val_callback(self, value): 143 | self.__smooth_val = value 144 | cv.Smooth(self.__res_gray, self.res_smooth, cv.CV_GAUSSIAN, self.__smooth_val * 2 + 1, self.__smooth_val * 2 + 1) 145 | self.__refresh_canny() 146 | 147 | def __canny_lo_callback(self, value): 148 | self.__canny_lo = value 149 | self.__refresh_canny() 150 | 151 | def __canny_hi_callback(self, value): 152 | self.__canny_hi = value 153 | self.__refresh_canny() 154 | 155 | def __canny_apeture_callback(self, value): 156 | self.__canny_apeture = value 157 | self.__refresh_canny() 158 | 159 | def main(): 160 | a = Vectorizer() 161 | a.get_polygons(INPUT, RES_X) 162 | a.get_polygons(INPUT, RES_X) 163 | 164 | if __name__ == '__main__': 165 | main() 166 | --------------------------------------------------------------------------------