Mercurial > tote
changeset 202:a75e02f9c15a
Add code for version 5.
| author | Radomir Dopieralski <sheep-devel@sheep.art.pl> |
|---|---|
| date | Wed, 07 Dec 2016 21:10:22 +0100 |
| parents | 2e3dc44ed08c |
| children | c4a2d90ad57f |
| files | v5/IRLremote.cpp v5/IRLremote.h v5/Makefile v5/battery.h v5/battery.ino v5/beep.h v5/beep.ino v5/clock.h v5/clock.ino v5/creep.h v5/creep.ino v5/ir.h v5/ir.ino v5/leg.h v5/leg.ino v5/misc.h v5/misc.ino v5/servos.h v5/servos.ino v5/v5.ino |
| diffstat | 20 files changed, 1558 insertions(+), 0 deletions(-) [+] |
line wrap: on
line diff
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/v5/IRLremote.cpp Wed Dec 07 21:10:22 2016 +0100 @@ -0,0 +1,50 @@ +/* +Copyright (c) 2014 NicoHood +See the readme for credit to other people. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +*/ + +#include "IRLremote.h" + +//================================================================================ +// User function helper +//================================================================================ + +// ensure available() returns false +uint8_t IRLProtocol = 0; +uint16_t IRLAddress = 0; +uint32_t IRLCommand = 0; + +// Called when directly received correct IR Signal +// Do not use Serial inside, it can crash your Arduino! +void IREvent(uint8_t protocol, uint16_t address, uint32_t command) { + // For no user function set we need to pause IR + // to not overwrite the actual values until they are read. + if (IRLProtocol) return; + IRLAddress = address; + IRLCommand = command; + IRLProtocol = protocol; +} + +void decodeIR(const uint16_t duration){ + // add your own protocols here or create your own decoding function + // you can overwrite this function in your .ino file + decodeAll(duration); +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/v5/IRLremote.h Wed Dec 07 21:10:22 2016 +0100 @@ -0,0 +1,778 @@ +/* +Copyright (c) 2014 NicoHood +See the readme for credit to other people. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +*/ + +#ifndef IRLREMOTE_H +#define IRLREMOTE_H + +#include <Arduino.h> + +//missing 1.0.6 definition workaround +#if ARDUINO == 106 +#define NOT_AN_INTERRUPT -1 +#endif + +// board type detection +#if defined(__MK20DX128__) || defined(__MK20DX256__) +#define IRLREMOTE_ARM +#define IRLREMOTE_TEENSY3 +#elif defined(__SAM3X8E__) +#define IRLREMOTE_ARM +#define IRLREMOTE_DUE +#else +// default to assuming everything else is avr for now +#define IRLREMOTE_AVR +#endif + +// delay_basic is only for avrs. with ARM sending is currently not possible +#ifdef IRLREMOTE_AVR +#include <util/delay_basic.h> +#endif + +//================================================================================ +// Definitions +//================================================================================ + +//NEC +//IRP notation: {38.4k,564}<1,-1|1,-3>(16,-8,D:8,S:8,F:8,~F:8,1,-78,(16,-4,1,-173)*) +// Lead + Space logic +#define NEC_HZ 38400 +#define NEC_PULSE 564UL +#define NEC_BLOCKS 4 +#define NEC_ADDRESS_LENGTH 16 +#define NEC_COMMAND_LENGTH 16 +#define NEC_LENGTH 2 + NEC_BLOCKS*8*2 // 2 for lead + space, each block has 8bits: mark and space +#define NEC_TIMEOUT NEC_PULSE*173 +#define NEC_MARK_LEAD NEC_PULSE*16 +#define NEC_SPACE_LEAD NEC_PULSE*8 +#define NEC_SPACE_HOLDING NEC_PULSE*4 +#define NEC_MARK_ZERO NEC_PULSE*1 +#define NEC_MARK_ONE NEC_PULSE*1 +#define NEC_SPACE_ZERO NEC_PULSE*1 +#define NEC_SPACE_ONE NEC_PULSE*3 + +//PANASONIC +//IRP notation: {37k,432}<1,-1|1,-3>(8,-4,3:8,1:8,D:8,S:8,F:8,(D^S^F):8,1,-173)+ +// Lead + Space logic +#define PANASONIC_HZ 37000 +#define PANASONIC_PULSE 432UL +#define PANASONIC_BLOCKS 6 +#define PANASONIC_ADDRESS_LENGTH 16 +#define PANASONIC_COMMAND_LENGTH 32 +#define PANASONIC_LENGTH 2 + PANASONIC_BLOCKS*8*2 // 2 for lead + space, each block has 8bits: mark and space +#define PANASONIC_TIMEOUT PANASONIC_PULSE*173 +#define PANASONIC_MARK_LEAD PANASONIC_PULSE*8 +#define PANASONIC_SPACE_LEAD PANASONIC_PULSE*4 +#define PANASONIC_SPACE_HOLDING 0 // no holding function in this protocol +#define PANASONIC_MARK_ZERO PANASONIC_PULSE*1 +#define PANASONIC_MARK_ONE PANASONIC_PULSE*1 +#define PANASONIC_SPACE_ZERO PANASONIC_PULSE*1 +#define PANASONIC_SPACE_ONE PANASONIC_PULSE*3 + +/* +Panasonic pulse demonstration: + +*---------| |------------| |---------| |---| ... -| |--------- +* | | | | | | | ... | | +* | | | | | | | ... | | +* |------------------------| |---| |---| |- ... |---| +* | Lead Mark | Lead Space | Logical 1 | Log 0 | Data |End| + +*/ + +//SONY 8, 12, 15, 20 +//IRP notation: {40k,600}<1,-1|2,-1>(4,-1,F:8,^22200) +//IRP notation: {40k,600}<1,-1|2,-1>(4,-1,F:7,D:5,^45m)+ +//IRP notation: {40k,600}<1,-1|2,-1>(4,-1,F:7,D:8,^45m)+ +//IRP notation: {40k,600}<1,-1|2,-1>(4,-1,F:7,D:5,S:8,^45m) + +// Lead + Mark logic +#define SONY_HZ 40000 +#define SONY_PULSE 600UL +#define SONY_BLOCKS_8 1 +#define SONY_BLOCKS_12 2 +#define SONY_BLOCKS_15 2 +#define SONY_BLOCKS_20 3 +#define SONY_ADDRESS_LENGTH_8 0 +#define SONY_ADDRESS_LENGTH_12 5 +#define SONY_ADDRESS_LENGTH_15 8 +#define SONY_ADDRESS_LENGTH_20 13 +#define SONY_COMMAND_LENGTH_8 8 +#define SONY_COMMAND_LENGTH_12 7 +#define SONY_COMMAND_LENGTH_15 7 +#define SONY_COMMAND_LENGTH_20 7 +#define SONY_LENGTH_8 2 + (8-1)*2 // 2 for lead + space, -1 for mark end, 8 bit +#define SONY_LENGTH_12 2 + (7+5-1)*2 // 2 for lead + space, -1 for mark end, 12 bit +#define SONY_LENGTH_15 2 + (7+8-1)*2 // 2 for lead + space, -1 for mark end, 15 bit +#define SONY_LENGTH_20 2 + (7+5+8-1)*2 // 2 for lead + space, -1 for mark end, 20 bit +#define SONY_TIMEOUT_8 22200 +#define SONY_TIMEOUT 45000 // 12, 15, 20 have the same timeout +#define SONY_MARK_LEAD SONY_PULSE*4 +#define SONY_SPACE_LEAD SONY_PULSE*1 +#define SONY_SPACE_HOLDING 0 // no holding function in this protocol +#define SONY_MARK_ZERO SONY_PULSE*1 +#define SONY_MARK_ONE SONY_PULSE*2 +#define SONY_SPACE_ZERO SONY_PULSE*1 +#define SONY_SPACE_ONE SONY_PULSE*1 + +typedef enum IRType{ + IR_NO_PROTOCOL, // 0 + IR_USER, // 1 + IR_ALL, // 2 + IR_NEC, // ... + IR_PANASONIC, + IR_SONY8, + IR_SONY12, + IR_SONY15, + IR_SONY20, + // add new protocols here + IR_RAW, +}; + +// definitions to use decoding functions with extra accuracy +#define IR_EXTRA_ACCURACY true +#define IR_NO_EXTRA_ACCURACY false + +// definition for sending to determine what should be send first +#define IR_ADDRESS_FIRST true +#define IR_COMMAND_FIRST false + +// definition to convert an uint8_t array to an uint16_t/uint32_t at any position (thx timeage!) +#define UINT16_AT_OFFSET(p_to_8, offset) ((uint16_t)*((const uint16_t *)((p_to_8)+(offset)))) +#define UINT32_AT_OFFSET(p_to_8, offset) ((uint32_t)*((const uint32_t *)((p_to_8)+(offset)))) + +// definition to get the higher value +#define MAX(a,b) \ + ({ __typeof__ (a) _a = (a); \ + __typeof__ (b) _b = (b); \ + _a > _b ? _a : _b; }) + +// definition to get the lower value +#define MIN(a,b) \ + ({ __typeof__ (a) _a = (a); \ + __typeof__ (b) _b = (b); \ + _a < _b ? _a : _b; }) + +//================================================================================ +// Prototypes +//================================================================================ + +// attach the interrupt function +template <IRType irType> +inline void IRLbegin(const uint8_t interrupt) __attribute__((always_inline)); + +// dettach the interrupt function +inline void IRLend(const uint8_t interrupt) __attribute__((always_inline)); + +// variables for IR processing if no user function was set +extern uint8_t IRLProtocol; +extern uint16_t IRLAddress; +extern uint32_t IRLCommand; + +// functions to use if no user function was set +inline bool IRLavailable(void) __attribute__((always_inline)); +inline void IRLreset(void) __attribute__((always_inline)); +inline uint8_t IRLgetProtocol(void) __attribute__((always_inline)); +inline uint16_t IRLgetAddress(void) __attribute__((always_inline)); +inline uint32_t IRLgetCommand(void) __attribute__((always_inline)); + +// function called on a valid IR event, must be overwritten by the user +void __attribute__((weak)) IREvent(uint8_t protocol, uint16_t address, uint32_t command); + +// optinal user function called when the user wants to implement its own decoding function +void __attribute__((weak)) decodeIR(const uint16_t duration); + +// called by interrupt CHANGE +template <IRType irType> +inline void IRLinterrupt(void) __attribute__((always_inline)); + +// special decode function for each protocol +inline void decodeAll(const uint16_t duration) __attribute__((always_inline)); +template <bool extraAccuracy> inline void decodeNec(const uint16_t duration); +template <bool extraAccuracy> inline void decodePanasonic(const uint16_t duration) __attribute__((always_inline)); +template <bool extraAccuracy> inline void decodeSony8(const uint16_t duration); +template <bool extraAccuracy> inline void decodeSony12(const uint16_t duration); +template <bool extraAccuracy> inline void decodeSony15(const uint16_t duration); +template <bool extraAccuracy> inline void decodeSony20(const uint16_t duration); +inline void decodeRaw(const uint16_t duration) __attribute__((always_inline)); + +// functions to check if the received data is valid with the protocol checksums +inline bool IRLcheckInverse0(uint8_t data[]) __attribute__((always_inline)); +inline bool IRLcheckInverse1(uint8_t data[]) __attribute__((always_inline)); +inline bool IRLcheckHolding(uint8_t data[]) __attribute__((always_inline)); +inline bool IRLcheckXOR0(uint8_t data[]) __attribute__((always_inline)); + +// default decoder helper function +template <uint8_t irLength, uint16_t timeoutThreshold, uint16_t markLeadThreshold, uint16_t spaceLeadThreshold, + uint16_t spaceLeadHoldingThreshold, uint16_t markThreshold, uint16_t spaceThreshold, + uint16_t markTimeout, uint16_t spaceTimeout> + inline bool IRLdecode(uint16_t duration, uint8_t data[]) __attribute__((always_inline)); + +// functions to send the protocol +template <IRType irType> +inline void IRLwrite(const uint8_t pin, uint16_t address, uint32_t command) __attribute__((always_inline)); + +template <uint8_t addressLength, uint8_t commandLength, + uint16_t Hz, bool addressFirst, + uint16_t markLead, uint16_t spaceLead, + uint16_t markZero, uint16_t markOne, + uint16_t spaceZero, uint16_t spaceOne> + inline void IRLsend(volatile uint8_t * outPort, uint8_t bitmask, uint16_t address, uint32_t command) __attribute__((always_inline)); + +// functions to set the pin high or low (with bitbang pwm) +inline void IRLmark(const uint16_t Hz, volatile uint8_t * outPort, uint8_t bitMask, uint16_t time) __attribute__((always_inline)); +inline void IRLspace(volatile uint8_t * outPort, uint8_t bitMask, uint16_t time) __attribute__((always_inline)); + +//================================================================================ +// Inline Implementations (receive) +//================================================================================ + +template <IRType irType> +void IRLbegin(const uint8_t interrupt){ + // attach the function that decodes the signals + attachInterrupt(interrupt, IRLinterrupt<irType>, CHANGE); +} + +void IRLend(const uint8_t interrupt){ + // release the interrupt, if its NOT_AN_INTERRUPT the detach function does nothing + detachInterrupt(interrupt); + // also make sure to call reset() after end() if you use the intern reading functions +} + +bool IRLavailable(void){ + return IRLProtocol; +} + +void IRLreset(void){ + IRLProtocol = 0; +} + +uint8_t IRLgetProtocol(void){ + return IRLProtocol; +} + +uint16_t IRLgetAddress(void){ + return IRLAddress; +} + +uint32_t IRLgetCommand(void){ + return IRLCommand; +} + +template <IRType irType> +void IRLinterrupt(void){ + //save the duration between the last reading + static uint32_t lastTime = 0; + uint32_t time = micros(); + uint32_t duration_32 = time - lastTime; + lastTime = time; + + // calculate 16 bit duration. On overflow set duration to a clear timeout + uint16_t duration = 0xFFFF; + if (duration_32 <= 0xFFFF) + duration = duration_32; + + // determinate which decode function must be called + switch (irType){ + case IR_USER: + // userfunction that can be overwritten + decodeIR(duration); + break; + case IR_ALL: + decodeAll(duration); + break; + case IR_NEC: + decodeNec<IR_NO_EXTRA_ACCURACY>(duration); + break; + case IR_PANASONIC: + decodePanasonic<IR_NO_EXTRA_ACCURACY>(duration); + break; + //case IR_SONY8: + // decodeSony8<IR_NO_EXTRA_ACCURACY>(duration); + // break; + case IR_SONY12: + decodeSony12<IR_NO_EXTRA_ACCURACY>(duration); + break; + //case IR_SONY15: + // decodeSony15<IR_NO_EXTRA_ACCURACY>(duration); + // break; + //case IR_SONY20: + // decodeSony20<IR_NO_EXTRA_ACCURACY>(duration); + // break; + case IR_RAW: + decodeRaw(duration); + break; + } +} + +void decodeAll(const uint16_t duration){ + // go through all known protocols and decode with more (resource unfriendly) accuration + decodeNec<IR_EXTRA_ACCURACY>(duration); + decodePanasonic<IR_EXTRA_ACCURACY>(duration); + decodeSony12<IR_EXTRA_ACCURACY>(duration); +} + +template <bool extraAccuracy> void decodeNec(const uint16_t duration){ + // temporary buffer to hold bytes for decoding this protocol + static uint8_t data[NEC_BLOCKS]; + + // pass the duration to the decoding function + bool newInput; + if (extraAccuracy) // no accuracy set at the moment, no conflict detected yet + newInput = IRLdecode <NEC_LENGTH, (NEC_TIMEOUT + NEC_MARK_LEAD) / 2, // irLength, timeoutThreshold + (NEC_MARK_LEAD + NEC_SPACE_ONE) / 2, (NEC_SPACE_LEAD + NEC_SPACE_HOLDING) / 2, // markLeadThreshold, spaceLeadThreshold + (NEC_SPACE_HOLDING + NEC_SPACE_ONE) / 2, 0, // spaceLeadHoldingThreshold, markThreshold + (NEC_SPACE_ONE + NEC_SPACE_ZERO) / 2, // spaceThreshold + 0, 0>// markTimeout, spaceTimeout + (duration, data); + else + newInput = IRLdecode <NEC_LENGTH, (NEC_TIMEOUT + NEC_MARK_LEAD) / 2, // irLength, timeoutThreshold + (NEC_MARK_LEAD + NEC_SPACE_ONE) / 2, (NEC_SPACE_LEAD + NEC_SPACE_HOLDING) / 2, // markLeadThreshold, spaceLeadThreshold + (NEC_SPACE_HOLDING + NEC_SPACE_ONE) / 2, 0, // spaceLeadHoldingThreshold, markThreshold + (NEC_SPACE_ONE + NEC_SPACE_ZERO) / 2, // spaceThreshold + 0, 0>// markTimeout, spaceTimeout + (duration, data); + + if (newInput){ + // Check if the protcol's checksum is correct + bool holding; + if (IRLcheckInverse1(data) || (holding = IRLcheckHolding(data))){ + // normally NEC also check for the inverse of the address. + // newer remotes dont have this because of the wide used protocol all addresses were already used + // to make it less complicated its left out and the user can check the command inverse himself if needed + //if (!holding && !extended && !IRLcheckInverse0(data)) + // return; + + // save address + command and trigger event + uint16_t address = UINT16_AT_OFFSET(data, 0); + uint32_t command = UINT16_AT_OFFSET(data, 2); + IREvent(IR_NEC, address, command); + } + } +} + +template <bool extraAccuracy> void decodePanasonic(const uint16_t duration){ + // temporary buffer to hold bytes for decoding this protocol + static uint8_t data[PANASONIC_BLOCKS]; + + // pass the duration to the decoding function + bool newInput; + if (extraAccuracy) // no accuracy set at the moment, no conflict detected yet + newInput = IRLdecode <PANASONIC_LENGTH, (PANASONIC_TIMEOUT + PANASONIC_MARK_LEAD) / 2, // irLength, timeoutThreshold + (PANASONIC_MARK_LEAD + PANASONIC_SPACE_ONE) / 2, (PANASONIC_SPACE_LEAD + PANASONIC_SPACE_ONE) / 2, // markLeadThreshold, spaceLeadThreshold + 0, 0, // spaceLeadHoldingThreshold, markThreshold + (PANASONIC_SPACE_ONE + PANASONIC_SPACE_ZERO) / 2, // spaceThreshold + 0, 0>// markTimeout, spaceTimeout + (duration, data); + else + newInput = IRLdecode <PANASONIC_LENGTH, (PANASONIC_TIMEOUT + PANASONIC_MARK_LEAD) / 2, // irLength, timeoutThreshold + (PANASONIC_MARK_LEAD + PANASONIC_SPACE_ONE) / 2, (PANASONIC_SPACE_LEAD + PANASONIC_SPACE_ONE) / 2, // markLeadThreshold, spaceLeadThreshold + 0, 0, // spaceLeadHoldingThreshold, markThreshold + (PANASONIC_SPACE_ONE + PANASONIC_SPACE_ZERO) / 2, // spaceThreshold + 0, 0>// markTimeout, spaceTimeout + (duration, data); + + if (newInput){ + // Check if the protcol's checksum is correct + if (IRLcheckXOR0(data)){ + uint16_t address = UINT16_AT_OFFSET(data, 0); + uint32_t command = UINT32_AT_OFFSET(data, 2); IREvent(IR_PANASONIC, address, command); + return; + } + } +} + +template <bool extraAccuracy> void decodeSony12(const uint16_t duration){ + // temporary buffer to hold bytes for decoding this protocol + static uint8_t data[SONY_BLOCKS_12]; + + // pass the duration to the decoding function + bool newInput; + if (extraAccuracy) + newInput = IRLdecode <SONY_LENGTH_12, (SONY_TIMEOUT + SONY_MARK_LEAD) / 2, // irLength, timeoutThreshold + (SONY_MARK_LEAD + SONY_MARK_ONE) / 2, 0, // markLeadThreshold, spaceLeadThreshold + 0, (SONY_MARK_ONE + SONY_MARK_ZERO) / 2, // spaceLeadHoldingThreshold, markThreshold + 0, // spaceThreshold + (SONY_MARK_LEAD + SONY_MARK_ONE) / 2, SONY_MARK_ONE>// markTimeout, spaceTimeout + (duration, data); + else + newInput = IRLdecode <SONY_LENGTH_12, (SONY_TIMEOUT + SONY_MARK_LEAD) / 2, // irLength, timeoutThreshold + (SONY_MARK_LEAD + SONY_MARK_ONE) / 2, 0, // markLeadThreshold, spaceLeadThreshold + 0, (SONY_MARK_ONE + SONY_MARK_ZERO) / 2, // spaceLeadHoldingThreshold, markThreshold + 0, // spaceThreshold + 0, 0>// markTimeout, spaceTimeout + (duration, data); + + if (newInput){ + // protocol has no checksum + uint16_t address = UINT16_AT_OFFSET(data, 0) >> 7; + uint32_t command = data[0] & 0x7F; + IREvent(IR_SONY12, address, command); + return; + } +} + +void decodeRaw(const uint16_t duration){ + //TODO, use advanced raw example instead +} + +bool IRLcheckInverse0(uint8_t data[]){ + // check if byte 2 and is the inverse of byte 3 + if (uint8_t((data[0] ^ (~data[1]))) == 0) + return true; + else return false; +} + +bool IRLcheckInverse1(uint8_t data[]){ + // check if byte 0 and is the inverse of byte 1 + if (uint8_t((data[2] ^ (~data[3]))) == 0) + return true; + else return false; +} + +bool IRLcheckHolding(uint8_t data[]){ + // check if at least the command is always 1 + if (data[2] == 0xFF && data[3] == 0xFF) + return true; + else return false; +} + +bool IRLcheckXOR0(uint8_t data[]){ + // this function is used for panasonic checksum + if (uint8_t(data[2] ^ data[3] ^ data[4]) == data[5]) + return true; + else return false; +} + +// multifunctional template for receiving +template <uint8_t irLength, uint16_t timeoutThreshold, uint16_t markLeadThreshold, uint16_t spaceLeadThreshold, + uint16_t spaceLeadHoldingThreshold, uint16_t markThreshold, uint16_t spaceThreshold, + uint16_t markTimeout, uint16_t spaceTimeout> + bool IRLdecode(uint16_t duration, uint8_t data[]){ + + // variables for ir processing + static uint8_t count = 0; + + // if timeout always start next possible reading and abort any pending readings + if (duration >= timeoutThreshold) + count = 1; + + // on a reset (error in decoding) we are waiting for a timeout to start a new reading again + // this is to not conflict with other protocols while they are sending 0/1 which might be similar to a lead in another protocol + else if (count == 0) + return false; + + // check pulses for mark/space and lead + logical 0/1 seperate + else{ + // Mark pulses (odd numbers) + if (count % 2 == 1){ + // check Mark Lead (needs a timeout or a correct signal) + if (markLeadThreshold && count == 1){ + // lead is okay// wrong lead + if (duration <= markLeadThreshold){ + count = 0; + return false; + } + } + + else{ + // check for timeout if needed (might be a different protocol) + if (markTimeout && duration > markTimeout){ + count = 0; + return false; + } + + // only check values if the protocol has different logical space pulses + else if (markThreshold){ + + // get number of the Mark Bits (starting from zero) + uint8_t length; + // only save every 2nd value, substract the first two lead pulses + if (!spaceThreshold) + length = (count / 2) - 1; + // special case: spaces and marks both have data in the pulse + else length = count - 2; + + // move bits and write 1 or 0 depending on the duration + // 1.7: changed from MSB to LSB. somehow takes a bit more flash but is correct and easier to handle. + data[length / 8] >>= 1; + if (duration > markThreshold) + data[length / 8] |= 0x80; + //else // normally not needed through the bitshift + // data[length / 8] &= ~0x80; + } + + // check last input (always a mark) + if (count > irLength){ + count = 0; + return true; + } + } + } + + // Space pulses (even numbers) + else{ + //check Space Lead/Space Holding + if (spaceLeadThreshold && count == 2){ + // normal Space, next reading + if (duration > spaceLeadThreshold); + + // Button holding (if supported by protocol) + else if (spaceLeadHoldingThreshold && duration > spaceLeadHoldingThreshold){ + // set command to 0xFF if button is held down + if (irLength == (4 * 8 * 2 + 2)){ + data[0] = data[1] = 0x00; + data[2] = data[3] = 0xFF; + } + count = 0; + return true; + } + // wrong space + else { + count = 0; + return false; + } + } + else{ + // check for timeout if needed (might be a different protocol) + if (spaceTimeout && duration > spaceTimeout){ + count = 0; + return false; + } + + // only check values if the protocol has different logical space pulses + else if (spaceThreshold){ + + // get number of the Space Bits (starting from zero) + uint8_t length; + // only save every 2nd value, substract the first two lead pulses + if (!markThreshold) + length = (count / 2) - 2; + // special case: spaces and marks both have data in the pulse + else length = count - 2; + + // move bits and write 1 or 0 depending on the duration + // 1.7: changed from MSB to LSB. somehow takes a bit more flash but is correct and easier to handle. + data[length / 8] >>= 1; + if (duration > spaceThreshold) + data[length / 8] |= 0x80; + //else // normally not needed through the bitshift + // data[length / 8] &= ~0x80; + } + } + } + + // next reading, no errors + count++; + } + + // no valid input (yet) + return false; +} + +//================================================================================ +// Inline Implementations (send) +//================================================================================ + +template <IRType irType> +void IRLwrite(const uint8_t pin, uint16_t address, uint32_t command) +{ + // get the port mask and the pointers to the out/mode registers for faster access + uint8_t bitMask = digitalPinToBitMask(pin); + uint8_t port = digitalPinToPort(pin); + volatile uint8_t * outPort = portOutputRegister(port); + volatile uint8_t * modePort = portModeRegister(port); + + // set pin to OUTPUT and LOW + *modePort |= bitMask; + *outPort &= ~bitMask; + + // disable interrupts + uint8_t oldSREG = SREG; + cli(); + + switch (irType){ + + case IR_NEC: + // NEC only sends the data once + if (command == 0xFFF) + // send holding indicator + IRLsend<0, 0, NEC_HZ, 0, NEC_MARK_LEAD, NEC_SPACE_HOLDING, + 0, 0, 0, 0> + (outPort, bitMask, address, command); + else + // send data + IRLsend<NEC_ADDRESS_LENGTH, NEC_COMMAND_LENGTH, NEC_HZ, IR_ADDRESS_FIRST, NEC_MARK_LEAD, NEC_SPACE_LEAD, + NEC_MARK_ZERO, NEC_MARK_ONE, NEC_SPACE_ZERO, NEC_SPACE_ONE> + (outPort, bitMask, address, command); + break; + + case IR_PANASONIC: //TODO test + // send data + IRLsend<PANASONIC_ADDRESS_LENGTH, PANASONIC_COMMAND_LENGTH, PANASONIC_HZ, IR_ADDRESS_FIRST, PANASONIC_MARK_LEAD, PANASONIC_SPACE_LEAD, + PANASONIC_MARK_ZERO, PANASONIC_MARK_ONE, PANASONIC_SPACE_ZERO, PANASONIC_SPACE_ONE> + (outPort, bitMask, address, command); + break; + + case IR_SONY12: //TODO test, address -1? + // repeat 3 times + for (uint8_t i = 0; i < 3; i++) + // send data + IRLsend<SONY_ADDRESS_LENGTH_12, SONY_COMMAND_LENGTH_12, SONY_HZ, IR_COMMAND_FIRST, SONY_MARK_LEAD, SONY_SPACE_LEAD, + SONY_MARK_ZERO, SONY_MARK_ONE, SONY_SPACE_ZERO, SONY_SPACE_ONE> + (outPort, bitMask, address, command); + break; + } + + // enable interrupts + SREG = oldSREG; + + // set pin to INPUT again to be save + *modePort &= ~bitMask; +} + +// multifunctional template for sending +template <uint8_t addressLength, uint8_t commandLength, + uint16_t Hz, bool addressFirst, + uint16_t markLead, uint16_t spaceLead, + uint16_t markZero, uint16_t markOne, + uint16_t spaceZero, uint16_t spaceOne> + void IRLsend(volatile uint8_t * outPort, uint8_t bitmask, uint16_t address, uint32_t command){ + + // send lead mark + if (markLead) + IRLmark(Hz, outPort, bitmask, markLead); + + // send lead space + if (spaceLead) + IRLspace(outPort, bitmask, spaceLead); + + // abort if input values are both the same (sending a holding signal for example) + if (markOne != markZero || spaceOne != spaceZero) + { + // go through all bits + for (uint8_t i = 0; i < (addressLength + commandLength); i++){ + // determine if its a logical one or zero, starting with address or command + bool bitToSend; + if (addressFirst && i < addressLength || !addressFirst && i >= commandLength){ + bitToSend = address & 0x01; + address >>= 1; + } + else{ + bitToSend = command & 0x01; + command >>= 1; + } + + // send logic mark bits if needed + if (markOne != markZero){ + // modulate if spaces dont have logic, else only every even number + if (spaceOne == spaceZero || spaceOne != spaceZero && i % 2 == 0) + { + if (bitToSend) + IRLmark(Hz, outPort, bitmask, markOne); + else + IRLmark(Hz, outPort, bitmask, markZero); + } + } + else + IRLmark(Hz, outPort, bitmask, markOne); + + // send logic space bits if needed + if (spaceOne != spaceZero){ + // modulate if marks dont have logic, else only every odd number + if (markOne == markZero || markOne != markZero && i % 2 == 1){ + if (bitToSend) + IRLspace(outPort, bitmask, spaceOne); + else + IRLspace(outPort, bitmask, spaceZero); + } + } + else + IRLspace(outPort, bitmask, spaceOne); + } + } + + // finish mark + IRLmark(Hz, outPort, bitmask, markZero); //TODO zero or one for sony? + IRLspace(outPort, bitmask, 0); +} + +void IRLmark(const uint16_t Hz, volatile uint8_t * outPort, uint8_t bitMask, uint16_t time) { + /* + Bitbangs PWM in the given Hz number for the given time + ________________________________________________________________________________ + Delay calculation: + F_CPU/1.000.000 to get number of cycles/uS + /3 to get the number of loops needed for 1ms (1loop = 3 cycles) + + Multiply with the number of ms delay: + 1/kHz to get the seconds + * 1.000.000 to get it in uS + /2 to get half of a full pulse + + Substract the while, portmanipulation, loop overhead /3 loop cycles + + F_CPU(16.000.000) 1 * 1.000.000(pulse in uS) 12(overhead) + ========================== * ========================== - ============== + 1.000.000 * 3(loop cycles) Hz * 2(half of a pulse) 3(loop cycles) + + <==> + + F_CPU(16.000.000) - (12(overhead) * Hz * 2(half of a pulse)) + =========================================================== + Hz * 2(half of a on/off pulse) * 3(loop cycles) + + ________________________________________________________________________________ + Iterations calculation: + Devide time with cycles in while loop + Multiply this with the cycles per uS + cycles per while loop: 3(loop cycles) * delay + overhead + + time * (F_CPU(16.000.000) / 1.000.000) + ====================================== + delay*3(loop cycles) + overhead + */ + + const uint32_t loopCycles = 3; + const uint32_t overHead = 12; // just a guess from try + error + uint8_t delay = (F_CPU - (overHead * Hz * 2UL)) / (Hz * 2UL * loopCycles); + uint16_t iterations = (time*(F_CPU / 1000000UL)) / (delay * loopCycles + overHead); + + while (iterations--){ + // flip pin state and wait for the calculated time + *outPort ^= bitMask; +#ifdef IRLREMOTE_AVR + _delay_loop_1(delay); +#else + // ARM TODO? +#endif + } +} + +void IRLspace(volatile uint8_t * outPort, uint8_t bitMask, uint16_t time) { + // Sends an IRLspace for the specified number of microseconds. + // A IRLspace is no output, so the PWM output is disabled. + *outPort &= ~bitMask; // write pin LOW + delayMicroseconds(time); + //_delay_loop_2(time*(F_CPU/1000000UL)/4UL); +} + +#endif \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/v5/Makefile Wed Dec 07 21:10:22 2016 +0100 @@ -0,0 +1,11 @@ +.phony: verify upload + +ARDUINO=arduino-1.6.4 +ARDUINO_OPTS=--board arduino:avr:pro:cpu=8MHzatmega328 --port /dev/ttyUSB0 +SKETCH=start.ino + +verify: start.ino + $(ARDUINO) --verify $(ARDUINO_OPTS) $(SKETCH) + +upload: start.ino + $(ARDUINO) --upload $(ARDUINO_OPTS) $(SKETCH)
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/v5/battery.h Wed Dec 07 21:10:22 2016 +0100 @@ -0,0 +1,8 @@ +#ifndef BATTERY_H +#define BATTERY_H + +void battery_setup(void); +int get_battery(void); +void battery_loop(void); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/v5/battery.ino Wed Dec 07 21:10:22 2016 +0100 @@ -0,0 +1,31 @@ +#include "beep.h" + +#define BATTERY_PIN A3 +// #define MIN_BATTERY 557 // About 3.59V. +#define MIN_BATTERY 367 // About 3.59V. + +extern int robot_mode; + +void battery_setup() { + pinMode(BATTERY_PIN, INPUT); +} + +int get_battery() { + return analogRead(BATTERY_PIN); +} + +void battery_loop() { + static int battery_fail = 0; + int reading = get_battery(); + + if (reading < MIN_BATTERY) { + battery_fail += 1; + beep(reading, 200); + } else { + battery_fail = 0; + } + if (battery_fail > 40) { + /* If it stays bad for 4s, shut down. */ + robot_mode = 0; + } +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/v5/beep.h Wed Dec 07 21:10:22 2016 +0100 @@ -0,0 +1,9 @@ +#ifndef BEEP_H +#define BEEP_H + +extern bool beep_mute; + +void beep_setup(void); +void beep(int frequency, int duration); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/v5/beep.ino Wed Dec 07 21:10:22 2016 +0100 @@ -0,0 +1,20 @@ +#define BEEP_PIN 13 + +bool beep_mute = false; + + +void beep_setup() { + pinMode(BEEP_PIN, OUTPUT); + beep(1760, 50); + delay(100); + beep(1975, 50); + delay(50); + beep(1760, 50); +} + +void beep(int frequency, int duration) { + if (beep_mute) { + return; + } + tone(BEEP_PIN, frequency, duration); +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/v5/clock.h Wed Dec 07 21:10:22 2016 +0100 @@ -0,0 +1,9 @@ +#ifndef CLOCK_H +#define CLOCK_H + +#define FPS 25 + +const unsigned char TICK = 1000 / FPS; +void tick(unsigned int ms); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/v5/clock.ino Wed Dec 07 21:10:22 2016 +0100 @@ -0,0 +1,14 @@ +#include "clock.h" + +void tick(unsigned int ms) { + // Wait remaining time to delay ms milliseconds since last call. + static unsigned long last = 0; + unsigned long now = millis(); + long wait = last + ms - now; + if (wait > 0) { + delay(wait); + } else { + // Somehow engage a time machine here to go back in time. + } + last = now; +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/v5/creep.h Wed Dec 07 21:10:22 2016 +0100 @@ -0,0 +1,14 @@ +#ifndef CREEP_H +#define CREEP_H + +/* Walking speed. */ +extern double creep_dx; // Sideways. +extern double creep_dy; // Forward. +extern double creep_spread; +extern double creep_rotation; +extern double creep_height; + +/* Perform one step of the creep gait with the current speed. */ +void walk(); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/v5/creep.ino Wed Dec 07 21:10:22 2016 +0100 @@ -0,0 +1,206 @@ +#include "creep.h" +#include "misc.h" +#include "leg.h" +#include "clock.h" + + +/* Creep gait parameters. */ +#ifdef TOTE2 +#define SPEED 1 +/* How far forward to move them when making a step? */ +#define STRIDE 14.0 +/* How much to shift the body? */ +#define SHIFT 8.0 +/* In how many steps should the body be shifted? */ +#define SHIFT_STEPS 2 +/* How fast to raise the leg. */ +#define RAISE 10.0 +/* In how many steps. */ +#define RAISE_STEPS 3 +#else +#define SPEED 0.5 +/* How far forward to move them when making a step? */ +#define STRIDE 12.0 +/* How much to shift the body? */ +#define SHIFT 8.0 +/* In how many steps should the body be shifted? */ +#define SHIFT_STEPS 4 +/* How fast to raise the leg. */ +#define RAISE 10.0 +/* In how many steps. */ +#define RAISE_STEPS 3 +#endif + +/* Given current leg, which leg to move next? */ +const unsigned char _NEXT_LEG[6][4] = { + {2, 0, 3, 1}, // forward + {1, 3, 0, 2}, // backward + {1, 2, 3, 0}, // clockwise + {3, 0, 1, 2}, // counter-clockwise + {2, 3, 1, 0}, // left + {3, 2, 0, 1}, // right +}; + +/* Body position. */ +static double _shift_x = 0.0; +static double _shift_y = 0.0; + +/* Walking speed. */ +double creep_dx = 0.0; // Sideways. +double creep_dy = 1.0; // Forward. +double creep_rotation = 0.0; // Rotation. +double creep_height = TIBIA * 0.75; // Body height. +double creep_spread = 0.0; // Leg spread. + +/* Which legs are touching the ground? */ +static bool _on_ground[4] = {true, true, true, true}; + +/* +Move all legs that are on the ground backwards, pushing the robot forward with +the current walking speed. This is called every tick, to make the robot move +continuously. +*/ +void _creep_move() { + for (unsigned char leg=0; leg < 4; ++leg) { + if (_on_ground[leg]) { + move_leg( + leg, + leg_position[leg][0] - creep_dx * LEG_X[leg] * SPEED, + leg_position[leg][1] - creep_dy * LEG_Y[leg] * SPEED, + -creep_height + ); + rotate_leg_by(leg, -creep_rotation * LEG_X[leg] * LEG_Y[leg] / 2); + } + } +} + +/* Called every frame. */ +void _creep_tick() { + _creep_move(); + tick(TICK); +} + +/* Shifts the whole body by defined amount. */ +void _shift_body_by(double dx, double dy) { + for (unsigned char leg=0; leg < 4; ++leg) { + move_leg_by(leg, dx * LEG_X[leg], dy * LEG_Y[leg], 0); + } + _shift_x += dx; + _shift_y += dy; +} + +/* Shifts the body away from the given leg, for balance. */ +void _shift_body(unsigned char leg) { + double dx = (LEG_X[leg] * SHIFT - _shift_x) / SHIFT_STEPS; + double dy = (LEG_Y[leg] * SHIFT - _shift_y) / SHIFT_STEPS; + for (unsigned char i = 0; i < SHIFT_STEPS; ++i) { + _shift_body_by(dx, dy); + _creep_tick(); + } +} + +/* Perform a full step with a single leg. */ +void _creep_step(unsigned char leg) { + _shift_body(leg); + _on_ground[leg] = false; + for (unsigned char step = 0; step < RAISE_STEPS; ++step) { + move_leg_by(leg, 0, 0, RAISE); + _creep_tick(); + } + move_leg( + leg, + HOME + (_shift_x + creep_dx * STRIDE) * LEG_X[leg] + creep_spread, + HOME + (_shift_y + creep_dy * STRIDE) * LEG_Y[leg] + creep_spread, + RAISE * RAISE_STEPS - creep_height + ); + _creep_tick(); + _creep_tick(); + for (unsigned char step = 0; step < RAISE_STEPS - 1; ++step) { + move_leg_by(leg, 0, 0, -RAISE); + _creep_tick(); + } + _on_ground[leg] = true; + _creep_tick(); +} + + +/* Called every frame. */ +void _trot_tick() { + _creep_move(); + _creep_move(); + tick(TICK); +} + +/* Perform a full trot step. */ +void _trot_step(unsigned char leg) { + unsigned char other_leg = (leg + 2) % 4; + _on_ground[leg] = false; + _on_ground[other_leg] = false; + move_leg( + leg, + HOME + creep_dx * STRIDE * LEG_X[leg] + creep_spread, + HOME + creep_dy * STRIDE * LEG_Y[leg] + creep_spread, + RAISE * RAISE_STEPS - creep_height + ); + move_leg( + other_leg, + HOME + creep_dx * STRIDE * LEG_X[other_leg] + creep_spread, + HOME + creep_dy * STRIDE * LEG_Y[other_leg] + creep_spread, + RAISE * RAISE_STEPS - creep_height + ); + _trot_tick(); + _trot_tick(); + _trot_tick(); + _trot_tick(); + _on_ground[leg] = true; + _on_ground[other_leg] = true; + _trot_tick(); + _trot_tick(); + _trot_tick(); + _trot_tick(); +} + + +unsigned char step_order() { + // Decide on the best leg order. + if (abs(creep_dy) > abs(creep_dx)) { + // forward/backward + if (creep_dy > 0) { + return 0; // forward + } else { + return 1; // backward + } + } else if (abs(creep_dx) > abs(creep_dy)) { + // sideways + if (creep_dx < 0) { + return 4; // left + } else { + return 5; // right + } + } else if (abs(creep_rotation) > 0.001) { + // rotate + if (creep_rotation > 0) { + return 2; // clockwise + } else { + return 3; // counter-clockwise + } + } + return 0; // default to forward +} + +/* Perform one step of a walk, selected depending on the speed. */ +void walk() { + static unsigned char leg = 0; + double max_speed = max(abs(creep_rotation), + max(abs(creep_dx), abs(creep_dy))); + + if (max_speed > 1.5) { + _trot_step(leg); + leg = (leg + 1) % 2; + } else if (max_speed > 0.01) { + _creep_step(leg); + leg = _NEXT_LEG[step_order()][leg]; + } else { + _creep_tick(); + } +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/v5/ir.h Wed Dec 07 21:10:22 2016 +0100 @@ -0,0 +1,7 @@ +#ifndef IR_H +#define IR_H + +void ir_setup(); +void ir_loop(); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/v5/ir.ino Wed Dec 07 21:10:22 2016 +0100 @@ -0,0 +1,116 @@ +#include "creep.h" +#include "leg.h" +#include "servos.h" +#include "misc.h" +#include "beep.h" +#include "IRLremote.h" + + +#define IR_INTERRUPT 0 // Sensor on PIN 2 +#define IR_PROTOCOL IR_NEC +#define IR_ADDRESS 0xFF00 + +#define KEY_UP 0xB946 // Up arrow. +#define KEY_DOWN 0xEA15 // Down arrow. +#define KEY_LEFT 0xBB44 // Left arrow. +#define KEY_RIGHT 0xBC43 // Right arrow. +#define KEY_OK 0xBF40 // OK. + +#define KEY_0 0xF30C // Rewind. +#define KEY_1 0xE718 // Play/pause. +#define KEY_2 0xA15E // Fast forward. +#define KEY_3 0xF708 // Channel minus. +#define KEY_4 0xA55A // Channel plus. +#define KEY_5 0xE31C +#define KEY_6 0xBD42 // Volume down. +#define KEY_7 0xAD52 // Mute. +#define KEY_8 0xB54A // Volume up. +#define KEY_9 0xBA45 // Power. +#define KEY_AST 0xAF50 // Asterisk. +#define KEY_HASH 0xBB44 // Hash. + + +extern int robot_mode; + + +void ir_setup() { + IRLbegin<IR_ALL>(IR_INTERRUPT); + pinMode(3, OUTPUT); +} + +void ir_loop() { + static uint32_t last=0; + uint32_t IRcommand = 0; + + if (!IRLavailable()) { + return; + } + + IRcommand = IRLgetCommand(); + + if (IRcommand == 0xFFFF) { + IRcommand = last; + } else if (IRLgetAddress() != IR_ADDRESS) { + IRLreset(); + return; // React only to my remote. + } + last = IRcommand; + + beep(1319, 50); + + switch (IRcommand) { + case KEY_UP: // Up arrow. + creep_dy = min(3, creep_dy + 0.25); + break; + case KEY_DOWN: // Down arrow. + creep_dy = max(-3, creep_dy - 0.25); + break; + case KEY_LEFT: // Left arrow. + creep_rotation = min(PI/90, creep_rotation + PI/360); + break; + case KEY_RIGHT: // Right arrow. + creep_rotation = max(-PI/90, creep_rotation - PI/360); + break; + case KEY_1: // Spread -. + creep_spread = max(-10.0, creep_spread - 1); + break; + case KEY_2: // Spread default. + creep_spread = 0.0; + break; + case KEY_3: // Spread +. + creep_spread = min(10.0, creep_spread + 1); + break; + case KEY_4: // Height -. + creep_height = max(TIBIA * 0.25, creep_height - 1); + break; + case KEY_6: // Height +. + creep_height = min(TIBIA * 1.25, creep_height + 1); + break; + case KEY_5: // Height default. + creep_height = TIBIA * 0.80; + break; + case KEY_7: // Strafe left. + creep_dx = max(-1.0, creep_dx - 0.25); + break; + case KEY_0: // Mute. + beep_mute = !beep_mute; + break; + case KEY_9: // Strafe right. + creep_dx = min(1.0, creep_dx + 0.25); + break; + case KEY_AST: // Power. + robot_mode = 0; + break; + case KEY_HASH: // Home. + for (unsigned char leg=0; leg < 4; ++leg) { + move_leg(leg, HOME, HOME, TIBIA); + } + break; + case KEY_OK: // OK. + creep_dx = 0.0; + creep_dy = 0.0; + creep_rotation = 0.0; + break; + } + IRLreset(); +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/v5/leg.h Wed Dec 07 21:10:22 2016 +0100 @@ -0,0 +1,27 @@ +#ifndef LEG_H +#define LEG_H + +#ifdef TOTE2 +// For version 2: +#define COXA 12.0 +#define FEMUR 42.0 +#define TIBIA 38.5 +#define BASE 21.0 +#else +// For version 1: +#define COXA 8.0 +#define FEMUR 20.5 +#define TIBIA 28.5 +#define BASE 18.5 +#endif + +const signed char LEG_X[4] = {-1, -1, 1, 1}; +const signed char LEG_Y[4] = {1, -1, -1, 1}; +extern double leg_position[4][3]; +const double HOME = (COXA + FEMUR) / SQRT2; + +bool move_leg(unsigned char leg, double x, double y, double z); +bool move_leg_by(unsigned char leg, double dx, double dy, double dz); +bool rotate_leg_by(unsigned char leg, double angle); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/v5/leg.ino Wed Dec 07 21:10:22 2016 +0100 @@ -0,0 +1,84 @@ +#include "misc.h" +#include "leg.h" + +const unsigned char LEG_JOINT[4][3] = { + {0, 1, 2}, + {3, 4, 5}, + {6, 7, 8}, + {9, 10, 11}, +}; + +double leg_position[4][3] = { + {HOME, LEG_Y[0] * HOME, -TIBIA}, + {HOME, LEG_Y[0] * HOME, -TIBIA}, + {HOME, LEG_Y[0] * HOME, -TIBIA}, + {HOME, LEG_Y[0] * HOME, -TIBIA}, +}; + + +double _solve_triangle(double a, double b, double c) { + // Calculate the angle between a and b, opposite to c. + a = abs(a); + b = abs(b); + c = abs(c); + c = min(c, a + b); + b = min(b, a + c); + a = min(a, b + c); + return acos((a * a + b * b - c * c) / (2 * a * b)); +} + +double _norm(double a, double b) { + // Calculate the norm of a vector. + return sqrt(a * a + b * b); +} + +bool _inverse_kinematics(double x, double y, double z, + double *ankle, double *knee, double *hip) { + // Calculate angles for knee and ankle, and put them in those variables. + // Return true on success, and false if x and y are out of range. + double f = _norm(x, y) - COXA; + double d = _norm(f, z); + d = min(d, FEMUR + TIBIA); + *hip = atan2(y, x); + if (isnan(*hip)) { return false; } + *knee = _solve_triangle(FEMUR, d, TIBIA) - atan2(-z, f); + if (isnan(*knee)) { return false; } + *ankle = _solve_triangle(FEMUR, TIBIA, d) - PI2; + if (isnan(*ankle)) { return false; } + return true; +} + +bool move_leg(unsigned char leg, double x, double y, double z) { + // Move the tip of the leg to x, y. Return false when out of range. + double ankle = NAN; + double knee = NAN; + double hip = NAN; + leg_position[leg][0] = x; + leg_position[leg][1] = y; + leg_position[leg][2] = z; + if (!_inverse_kinematics(x, y, z, &ankle, &knee, &hip)) { + return false; + } + hip -= PI4; + servo_move(LEG_JOINT[leg][0], ankle); + servo_move(LEG_JOINT[leg][1], knee); + servo_move(LEG_JOINT[leg][2], hip); + return true; +} + +bool move_leg_by(unsigned char leg, double dx, double dy, double dz) { + // Move the tip of the leg by dx, dy. Return false when out of range. + return move_leg(leg, + leg_position[leg][0] + dx, + leg_position[leg][1] + dy, + leg_position[leg][2] + dz); +} + +bool rotate_leg_by(unsigned char leg, double angle) { + // Rotate the tip of the leg around the center of robot's body. + double x = leg_position[leg][0] + BASE; + double y = leg_position[leg][1] + BASE; + double nx = x * cos(angle) - y * sin(angle) - BASE; + double ny = x * sin(angle) + y * cos(angle) - BASE; + return move_leg(leg, nx, ny, leg_position[leg][2]); +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/v5/misc.h Wed Dec 07 21:10:22 2016 +0100 @@ -0,0 +1,12 @@ +#ifndef MISC_H +#define MISC_H + +extern const double PI2; +extern const double PI4; +extern const double TAU; +extern const double SQRT2; + +// Use the larger SG90 servos +#define TOTE2 + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/v5/misc.ino Wed Dec 07 21:10:22 2016 +0100 @@ -0,0 +1,6 @@ +#include "misc.h" + +const double PI2 = PI / 2; +const double PI4 = PI / 4; +const double TAU = PI * 2; +const double SQRT2 = 1.4142135623730951;
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/v5/servos.h Wed Dec 07 21:10:22 2016 +0100 @@ -0,0 +1,11 @@ +#ifndef SERVOS_H +#define SERVOS_H + +// Number of servos. +#define SERVOS 12 + +void servo_move(unsigned char servo, double rads); +void servo_setup(void); +void servo_shutdown(void); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/v5/servos.ino Wed Dec 07 21:10:22 2016 +0100 @@ -0,0 +1,102 @@ +#include "misc.h" +#include "servos.h" +#include <Servo.h> + +// Range of the servos. +#define SERVO_FREQ_MIN 600 +#define SERVO_FREQ_MAX 2400 + +static const unsigned char SERVO_PINS[SERVOS] = { +// ankle, knee, hip + 7, 8, 9 // front left + 6, 5, 4, // hind left + 3, 1, 0, // hind right + 12, 11, 10, // front right +}; +static const signed char SERVO_REVERSE[SERVOS] = { +// ankle, knee, hip + 1, -1, -1 // front left + -1, 1, 1, // hind left + 1, -1, -1, // hind right + -1, 1, 1, // front right +}; +static const signed char SERVO_TRIM[SERVOS] = { +// ankle, knee, hip + 0, 0, 10 // front left + 0, 0, 10, // hind left + 10, -5, -10, // hind right + 0, 5, -10, // front right +}; +static Servo servos[SERVOS]; + +static const double HOME_POSITION[SERVOS] = { +// ankle, knee, hip + 0, 0, 0 // front right + 0, 0, 0, // hind right + 0, 0, 0, // hind left + 0, 0, 0, // front left +}; + +static const double FOLD_POSITION[SERVOS] = { +// ankle, knee, hip + -PI2, PI2, -PI4 // front right + -PI2, PI2, -PI4, // hind right + -PI2, PI2, -PI4, // hind left + -PI2, PI2, -PI4, // front left +}; + +void servo_move(unsigned char servo, double rads) { + // Move a servo to a position in radians between -PI/2 and PI/2. + if (servo >= SERVOS) { + return; + } + rads = rads * SERVO_REVERSE[servo] + PI2; + while (rads > TAU) { + rads -= TAU; + } + while (rads < 0) { + rads += TAU; + } + int freq = (SERVO_FREQ_MAX - + SERVO_FREQ_MIN) * rads / PI + SERVO_FREQ_MIN; + freq = min(SERVO_FREQ_MAX, + max(SERVO_FREQ_MIN, + freq + SERVO_TRIM[servo] * 10)); + servos[servo].writeMicroseconds(freq); +} + +void servo_setup() { + // Initialize all servos. + // Start with hips. + for (unsigned char servo = 2; servo < SERVOS; servo += 3) { + servos[servo].attach(SERVO_PINS[servo], + SERVO_FREQ_MIN, SERVO_FREQ_MAX); + servo_move(servo, HOME_POSITION[servo]); + delay(150); + } + // Then ankles. + for (unsigned char servo = 0; servo < SERVOS; servo += 3) { + servos[servo].attach(SERVO_PINS[servo], + SERVO_FREQ_MIN, SERVO_FREQ_MAX); + servo_move(servo, HOME_POSITION[servo]); + delay(150); + } + // Finally knees. + for (unsigned char servo = 1; servo < SERVOS; servo += 3) { + servos[servo].attach(SERVO_PINS[servo], + SERVO_FREQ_MIN, SERVO_FREQ_MAX); + servo_move(servo, HOME_POSITION[servo]); + delay(150); + } +} + +void servo_shutdown() { + // Power down all servos. + for (unsigned char servo = 0; servo < SERVOS; ++servo) { + servo_move(servo, FOLD_POSITION[servo]); + } + delay(500); + for (unsigned char servo = 0; servo < SERVOS; ++servo) { + servos[servo].detach(); + } +}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/v5/v5.ino Wed Dec 07 21:10:22 2016 +0100 @@ -0,0 +1,43 @@ +#include "misc.h" +#include "servos.h" +#include "beep.h" +#include "battery.h" +#include "creep.h" +#include "leg.h" + +#include <avr/sleep.h> + +// 0 -- Power off +// 1 -- Walk +// 2 -- Nothing +int robot_mode = 1; + + +void setup() { + // battery_setup(); + servo_setup(); + beep_setup(); + ir_setup(); +} + +void loop() { + switch (robot_mode) { + case 0: // Power off + servo_shutdown(); + set_sleep_mode(SLEEP_MODE_PWR_DOWN); + sleep_enable(); + sleep_mode(); + while (1) { + delay(100); + } + break; + case 1: + walk(); + break; + case 2: // Nothing + delay(100); + break; + } + ir_loop(); + // battery_loop(); +}