FireFly 2012: the robot code as an Arduino library 'hexapod' (v3.1): IK, SSC-32, IR, ultrasonic, GPS, 7-segment
From Electronics\Hexapod\Arduino library 2012.
This commit is contained in:
mangavalk committed
1 parent
f0b272a1d3
commit
304367a769
21 files changed
+3043
No files matched your search
@@ -0,0 +1,63 @@
|
||||
// Converter.cpp
|
||||
// V3.0
|
||||
|
||||
#include "Converter.h"
|
||||
|
||||
void Converter::setup()
|
||||
{
|
||||
//Serial.begin(9600);
|
||||
}
|
||||
|
||||
int ConverttoMS(int Position, int low, int high)
|
||||
{
|
||||
int tmp = ( Position * ( ( high - low ) / 180 ) );
|
||||
return low + tmp;
|
||||
}
|
||||
|
||||
void ConvertServos(struct Data *DataLine)
|
||||
{
|
||||
for (int i=0; i<6; i++)
|
||||
{
|
||||
DataLine->ServoData[i].t1 = ConverttoMS(DataLine->ServoData[i].o1, smin0, smax0);
|
||||
DataLine->ServoData[i].t2 = ConverttoMS(DataLine->ServoData[i].o2, smin1, smax1);
|
||||
DataLine->ServoData[i].t3 = ConverttoMS(DataLine->ServoData[i].o3, smin2, smax2);
|
||||
}
|
||||
|
||||
#ifdef DEBUGS
|
||||
for (int i=0; i<6; i++)
|
||||
{
|
||||
if (i != 0)
|
||||
Serial.print(", ");
|
||||
else
|
||||
Serial.print("### Servos->mSend ServosAngles Are: ");
|
||||
|
||||
Serial.print(DataLine->ServoData[i].o1);
|
||||
Serial.print(", ");
|
||||
Serial.print(DataLine->ServoData[i].o2);
|
||||
Serial.print(", ");
|
||||
Serial.print(DataLine->ServoData[i].o3);
|
||||
}
|
||||
Serial.println(";");
|
||||
#endif
|
||||
}
|
||||
|
||||
void Converter::ConvertHead(struct Data *DataLine)
|
||||
{
|
||||
DataLine->HeadPosition = ConverttoMS((90 + DataLine->HeadAngle), smin3, smax3);
|
||||
|
||||
#ifdef DEBUGC
|
||||
Serial.print("### Converter->HeadPosition is: ");
|
||||
Serial.println(DataLine->HeadPosition);
|
||||
#endif
|
||||
}
|
||||
|
||||
void Converter::Convert(Data *DataLine)
|
||||
{
|
||||
#ifdef DEBUGC
|
||||
Serial.println("### Converter->Converter ###");
|
||||
#endif
|
||||
|
||||
ConvertServos(DataLine);
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,27 @@
|
||||
// Converter.h
|
||||
// V3.0
|
||||
|
||||
#ifndef CONVERTER_H
|
||||
#define CONVERTER_H
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "Data.h"
|
||||
|
||||
class Converter
|
||||
{
|
||||
public:
|
||||
void setup();
|
||||
|
||||
void Convert(Data *DataLine);
|
||||
void ConvertHead(Data *DataLine);
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,58 @@
|
||||
//Data.h Data Structure
|
||||
|
||||
#ifndef DATA
|
||||
#define DATA
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "Def.h"
|
||||
#include "SerD.h"
|
||||
|
||||
// LAYOUT
|
||||
// _U_
|
||||
// 0==/ \==3
|
||||
// >***<
|
||||
// 1===|---|===4
|
||||
// >***<
|
||||
// 2==\___/==5
|
||||
|
||||
struct Data
|
||||
{
|
||||
int HeadTime;
|
||||
int WalkTime;
|
||||
SerD ServoData[6];
|
||||
float HeadAngle;
|
||||
|
||||
//IK
|
||||
int HeadPosition;
|
||||
float l3;
|
||||
float s;
|
||||
|
||||
//Init
|
||||
Data()
|
||||
{
|
||||
HeadAngle = 0;
|
||||
|
||||
ServoData[0](5, 3, 8);
|
||||
ServoData[1](8, 3, 0);
|
||||
ServoData[2](5, 3, -8);
|
||||
ServoData[3](5, 3, 8);
|
||||
ServoData[4](8, 3, 0);
|
||||
ServoData[5](5, 3, -8);
|
||||
}
|
||||
|
||||
void input (DataObject* R)
|
||||
{
|
||||
ServoData[0] = R->S0;
|
||||
ServoData[1] = R->S1;
|
||||
ServoData[2] = R->S2;
|
||||
ServoData[3] = R->S3;
|
||||
ServoData[4] = R->S4;
|
||||
ServoData[5] = R->S5;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,25 @@
|
||||
// Def.h
|
||||
// Define some Hard Coded Variables
|
||||
// V3.0
|
||||
|
||||
//DEBUG CODES
|
||||
//#define DEBUGC // Converter
|
||||
//#define DEBUGIK // InverseKinemetics
|
||||
//#define DEBUGS // Servos
|
||||
//#define DEBUGHS // ServosHead
|
||||
//#define DEBUGW // Watchdog
|
||||
//#define Freeze // Protect all the servos
|
||||
|
||||
//InverseKinemetics VARIABLES
|
||||
#define l2 15
|
||||
#define l1 10
|
||||
|
||||
//Converter VARIABLES
|
||||
#define smin0 600
|
||||
#define smin1 960
|
||||
#define smin2 600
|
||||
#define smin3 600
|
||||
#define smax0 2400
|
||||
#define smax1 2100
|
||||
#define smax2 2400
|
||||
#define smax3 2400
|
||||
@@ -0,0 +1,722 @@
|
||||
/*
|
||||
* IRremote
|
||||
* Version 0.11 August, 2009
|
||||
* Copyright 2009 Ken Shirriff
|
||||
* For details, see http://arcfn.com/2009/08/multi-protocol-infrared-remote-library.html
|
||||
*
|
||||
* Modified by Paul Stoffregen <paul@pjrc.com> to support other boards and timers
|
||||
*
|
||||
* Interrupt code based on NECIRrcv by Joe Knapp
|
||||
* http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1210243556
|
||||
* Also influenced by http://zovirl.com/2008/11/12/building-a-universal-remote-with-an-arduino/
|
||||
*/
|
||||
|
||||
#include "IRremote.h"
|
||||
#include "IRremoteInt.h"
|
||||
|
||||
// Provides ISR
|
||||
#include <avr/interrupt.h>
|
||||
|
||||
volatile irparams_t irparams;
|
||||
|
||||
// These versions of MATCH, MATCH_MARK, and MATCH_SPACE are only for debugging.
|
||||
// To use them, set DEBUG in IRremoteInt.h
|
||||
// Normally macros are used for efficiency
|
||||
#ifdef DEBUG
|
||||
int MATCH(int measured, int desired) {
|
||||
Serial.print("Testing: ");
|
||||
Serial.print(TICKS_LOW(desired), DEC);
|
||||
Serial.print(" <= ");
|
||||
Serial.print(measured, DEC);
|
||||
Serial.print(" <= ");
|
||||
Serial.println(TICKS_HIGH(desired), DEC);
|
||||
return measured >= TICKS_LOW(desired) && measured <= TICKS_HIGH(desired);
|
||||
}
|
||||
|
||||
int MATCH_MARK(int measured_ticks, int desired_us) {
|
||||
Serial.print("Testing mark ");
|
||||
Serial.print(measured_ticks * USECPERTICK, DEC);
|
||||
Serial.print(" vs ");
|
||||
Serial.print(desired_us, DEC);
|
||||
Serial.print(": ");
|
||||
Serial.print(TICKS_LOW(desired_us + MARK_EXCESS), DEC);
|
||||
Serial.print(" <= ");
|
||||
Serial.print(measured_ticks, DEC);
|
||||
Serial.print(" <= ");
|
||||
Serial.println(TICKS_HIGH(desired_us + MARK_EXCESS), DEC);
|
||||
return measured_ticks >= TICKS_LOW(desired_us + MARK_EXCESS) && measured_ticks <= TICKS_HIGH(desired_us + MARK_EXCESS);
|
||||
}
|
||||
|
||||
int MATCH_SPACE(int measured_ticks, int desired_us) {
|
||||
Serial.print("Testing space ");
|
||||
Serial.print(measured_ticks * USECPERTICK, DEC);
|
||||
Serial.print(" vs ");
|
||||
Serial.print(desired_us, DEC);
|
||||
Serial.print(": ");
|
||||
Serial.print(TICKS_LOW(desired_us - MARK_EXCESS), DEC);
|
||||
Serial.print(" <= ");
|
||||
Serial.print(measured_ticks, DEC);
|
||||
Serial.print(" <= ");
|
||||
Serial.println(TICKS_HIGH(desired_us - MARK_EXCESS), DEC);
|
||||
return measured_ticks >= TICKS_LOW(desired_us - MARK_EXCESS) && measured_ticks <= TICKS_HIGH(desired_us - MARK_EXCESS);
|
||||
}
|
||||
#endif
|
||||
|
||||
void IRsend::sendNEC(unsigned long data, int nbits)
|
||||
{
|
||||
enableIROut(38);
|
||||
mark(NEC_HDR_MARK);
|
||||
space(NEC_HDR_SPACE);
|
||||
for (int i = 0; i < nbits; i++) {
|
||||
if (data & TOPBIT) {
|
||||
mark(NEC_BIT_MARK);
|
||||
space(NEC_ONE_SPACE);
|
||||
}
|
||||
else {
|
||||
mark(NEC_BIT_MARK);
|
||||
space(NEC_ZERO_SPACE);
|
||||
}
|
||||
data <<= 1;
|
||||
}
|
||||
mark(NEC_BIT_MARK);
|
||||
space(0);
|
||||
}
|
||||
|
||||
void IRsend::sendSony(unsigned long data, int nbits) {
|
||||
enableIROut(40);
|
||||
mark(SONY_HDR_MARK);
|
||||
space(SONY_HDR_SPACE);
|
||||
data = data << (32 - nbits);
|
||||
for (int i = 0; i < nbits; i++) {
|
||||
if (data & TOPBIT) {
|
||||
mark(SONY_ONE_MARK);
|
||||
space(SONY_HDR_SPACE);
|
||||
}
|
||||
else {
|
||||
mark(SONY_ZERO_MARK);
|
||||
space(SONY_HDR_SPACE);
|
||||
}
|
||||
data <<= 1;
|
||||
}
|
||||
}
|
||||
|
||||
void IRsend::sendRaw(unsigned int buf[], int len, int hz)
|
||||
{
|
||||
enableIROut(hz);
|
||||
for (int i = 0; i < len; i++) {
|
||||
if (i & 1) {
|
||||
space(buf[i]);
|
||||
}
|
||||
else {
|
||||
mark(buf[i]);
|
||||
}
|
||||
}
|
||||
space(0); // Just to be sure
|
||||
}
|
||||
|
||||
// Note: first bit must be a one (start bit)
|
||||
void IRsend::sendRC5(unsigned long data, int nbits)
|
||||
{
|
||||
enableIROut(36);
|
||||
data = data << (32 - nbits);
|
||||
mark(RC5_T1); // First start bit
|
||||
space(RC5_T1); // Second start bit
|
||||
mark(RC5_T1); // Second start bit
|
||||
for (int i = 0; i < nbits; i++) {
|
||||
if (data & TOPBIT) {
|
||||
space(RC5_T1); // 1 is space, then mark
|
||||
mark(RC5_T1);
|
||||
}
|
||||
else {
|
||||
mark(RC5_T1);
|
||||
space(RC5_T1);
|
||||
}
|
||||
data <<= 1;
|
||||
}
|
||||
space(0); // Turn off at end
|
||||
}
|
||||
|
||||
// Caller needs to take care of flipping the toggle bit
|
||||
void IRsend::sendRC6(unsigned long data, int nbits)
|
||||
{
|
||||
enableIROut(36);
|
||||
data = data << (32 - nbits);
|
||||
mark(RC6_HDR_MARK);
|
||||
space(RC6_HDR_SPACE);
|
||||
mark(RC6_T1); // start bit
|
||||
space(RC6_T1);
|
||||
int t;
|
||||
for (int i = 0; i < nbits; i++) {
|
||||
if (i == 3) {
|
||||
// double-wide trailer bit
|
||||
t = 2 * RC6_T1;
|
||||
}
|
||||
else {
|
||||
t = RC6_T1;
|
||||
}
|
||||
if (data & TOPBIT) {
|
||||
mark(t);
|
||||
space(t);
|
||||
}
|
||||
else {
|
||||
space(t);
|
||||
mark(t);
|
||||
}
|
||||
|
||||
data <<= 1;
|
||||
}
|
||||
space(0); // Turn off at end
|
||||
}
|
||||
|
||||
void IRsend::mark(int time) {
|
||||
// Sends an IR mark for the specified number of microseconds.
|
||||
// The mark output is modulated at the PWM frequency.
|
||||
TIMER_ENABLE_PWM; // Enable pin 3 PWM output
|
||||
delayMicroseconds(time);
|
||||
}
|
||||
|
||||
/* Leave pin off for time (given in microseconds) */
|
||||
void IRsend::space(int time) {
|
||||
// Sends an IR space for the specified number of microseconds.
|
||||
// A space is no output, so the PWM output is disabled.
|
||||
TIMER_DISABLE_PWM; // Disable pin 3 PWM output
|
||||
delayMicroseconds(time);
|
||||
}
|
||||
|
||||
void IRsend::enableIROut(int khz) {
|
||||
// Enables IR output. The khz value controls the modulation frequency in kilohertz.
|
||||
// The IR output will be on pin 3 (OC2B).
|
||||
// This routine is designed for 36-40KHz; if you use it for other values, it's up to you
|
||||
// to make sure it gives reasonable results. (Watch out for overflow / underflow / rounding.)
|
||||
// TIMER2 is used in phase-correct PWM mode, with OCR2A controlling the frequency and OCR2B
|
||||
// controlling the duty cycle.
|
||||
// There is no prescaling, so the output frequency is 16MHz / (2 * OCR2A)
|
||||
// To turn the output on and off, we leave the PWM running, but connect and disconnect the output pin.
|
||||
// A few hours staring at the ATmega documentation and this will all make sense.
|
||||
// See my Secrets of Arduino PWM at http://arcfn.com/2009/07/secrets-of-arduino-pwm.html for details.
|
||||
|
||||
|
||||
// Disable the Timer2 Interrupt (which is used for receiving IR)
|
||||
TIMER_DISABLE_INTR; //Timer2 Overflow Interrupt
|
||||
|
||||
pinMode(TIMER_PWM_PIN, OUTPUT);
|
||||
digitalWrite(TIMER_PWM_PIN, LOW); // When not sending PWM, we want it low
|
||||
|
||||
// COM2A = 00: disconnect OC2A
|
||||
// COM2B = 00: disconnect OC2B; to send signal set to 10: OC2B non-inverted
|
||||
// WGM2 = 101: phase-correct PWM with OCRA as top
|
||||
// CS2 = 000: no prescaling
|
||||
// The top value for the timer. The modulation frequency will be SYSCLOCK / 2 / OCR2A.
|
||||
TIMER_CONFIG_KHZ(khz);
|
||||
}
|
||||
|
||||
IRrecv::IRrecv(int recvpin)
|
||||
{
|
||||
irparams.recvpin = recvpin;
|
||||
irparams.blinkflag = 0;
|
||||
}
|
||||
|
||||
// initialization
|
||||
void IRrecv::enableIRIn() {
|
||||
cli();
|
||||
// setup pulse clock timer interrupt
|
||||
//Prescale /8 (16M/8 = 0.5 microseconds per tick)
|
||||
// Therefore, the timer interval can range from 0.5 to 128 microseconds
|
||||
// depending on the reset value (255 to 0)
|
||||
TIMER_CONFIG_NORMAL();
|
||||
|
||||
//Timer2 Overflow Interrupt Enable
|
||||
TIMER_ENABLE_INTR;
|
||||
|
||||
TIMER_RESET;
|
||||
|
||||
sei(); // enable interrupts
|
||||
|
||||
// initialize state machine variables
|
||||
irparams.rcvstate = STATE_IDLE;
|
||||
irparams.rawlen = 0;
|
||||
|
||||
// set pin modes
|
||||
pinMode(irparams.recvpin, INPUT);
|
||||
}
|
||||
|
||||
// enable/disable blinking of pin 13 on IR processing
|
||||
void IRrecv::blink13(int blinkflag)
|
||||
{
|
||||
irparams.blinkflag = blinkflag;
|
||||
if (blinkflag)
|
||||
pinMode(BLINKLED, OUTPUT);
|
||||
}
|
||||
|
||||
// TIMER2 interrupt code to collect raw data.
|
||||
// Widths of alternating SPACE, MARK are recorded in rawbuf.
|
||||
// Recorded in ticks of 50 microseconds.
|
||||
// rawlen counts the number of entries recorded so far.
|
||||
// First entry is the SPACE between transmissions.
|
||||
// As soon as a SPACE gets long, ready is set, state switches to IDLE, timing of SPACE continues.
|
||||
// As soon as first MARK arrives, gap width is recorded, ready is cleared, and new logging starts
|
||||
ISR(TIMER_INTR_NAME)
|
||||
{
|
||||
TIMER_RESET;
|
||||
|
||||
uint8_t irdata = (uint8_t)digitalRead(irparams.recvpin);
|
||||
|
||||
irparams.timer++; // One more 50us tick
|
||||
if (irparams.rawlen >= RAWBUF) {
|
||||
// Buffer overflow
|
||||
irparams.rcvstate = STATE_STOP;
|
||||
}
|
||||
switch(irparams.rcvstate) {
|
||||
case STATE_IDLE: // In the middle of a gap
|
||||
if (irdata == MARK) {
|
||||
if (irparams.timer < GAP_TICKS) {
|
||||
// Not big enough to be a gap.
|
||||
irparams.timer = 0;
|
||||
}
|
||||
else {
|
||||
// gap just ended, record duration and start recording transmission
|
||||
irparams.rawlen = 0;
|
||||
irparams.rawbuf[irparams.rawlen++] = irparams.timer;
|
||||
irparams.timer = 0;
|
||||
irparams.rcvstate = STATE_MARK;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case STATE_MARK: // timing MARK
|
||||
if (irdata == SPACE) { // MARK ended, record time
|
||||
irparams.rawbuf[irparams.rawlen++] = irparams.timer;
|
||||
irparams.timer = 0;
|
||||
irparams.rcvstate = STATE_SPACE;
|
||||
}
|
||||
break;
|
||||
case STATE_SPACE: // timing SPACE
|
||||
if (irdata == MARK) { // SPACE just ended, record it
|
||||
irparams.rawbuf[irparams.rawlen++] = irparams.timer;
|
||||
irparams.timer = 0;
|
||||
irparams.rcvstate = STATE_MARK;
|
||||
}
|
||||
else { // SPACE
|
||||
if (irparams.timer > GAP_TICKS) {
|
||||
// big SPACE, indicates gap between codes
|
||||
// Mark current code as ready for processing
|
||||
// Switch to STOP
|
||||
// Don't reset timer; keep counting space width
|
||||
irparams.rcvstate = STATE_STOP;
|
||||
}
|
||||
}
|
||||
break;
|
||||
case STATE_STOP: // waiting, measuring gap
|
||||
if (irdata == MARK) { // reset gap timer
|
||||
irparams.timer = 0;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
if (irparams.blinkflag) {
|
||||
if (irdata == MARK) {
|
||||
BLINKLED_ON(); // turn pin 13 LED on
|
||||
}
|
||||
else {
|
||||
BLINKLED_OFF(); // turn pin 13 LED off
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void IRrecv::resume() {
|
||||
irparams.rcvstate = STATE_IDLE;
|
||||
irparams.rawlen = 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
// Decodes the received IR message
|
||||
// Returns 0 if no data ready, 1 if data ready.
|
||||
// Results of decoding are stored in results
|
||||
int IRrecv::decode(decode_results *results) {
|
||||
results->rawbuf = irparams.rawbuf;
|
||||
results->rawlen = irparams.rawlen;
|
||||
if (irparams.rcvstate != STATE_STOP) {
|
||||
return ERR;
|
||||
}
|
||||
#ifdef DEBUG
|
||||
Serial.println("Attempting NEC decode");
|
||||
#endif
|
||||
if (decodeNEC(results)) {
|
||||
return DECODED;
|
||||
}
|
||||
#ifdef DEBUG
|
||||
Serial.println("Attempting Sony decode");
|
||||
#endif
|
||||
if (decodeSony(results)) {
|
||||
return DECODED;
|
||||
}
|
||||
#ifdef DEBUG
|
||||
Serial.println("Attempting RC5 decode");
|
||||
#endif
|
||||
if (decodeRC5(results)) {
|
||||
return DECODED;
|
||||
}
|
||||
#ifdef DEBUG
|
||||
Serial.println("Attempting RC6 decode");
|
||||
#endif
|
||||
if (decodeRC6(results)) {
|
||||
return DECODED;
|
||||
}
|
||||
// decodeHash returns a hash on any input.
|
||||
// Thus, it needs to be last in the list.
|
||||
// If you add any decodes, add them before this.
|
||||
if (decodeHash(results)) {
|
||||
return DECODED;
|
||||
}
|
||||
// Throw away and start over
|
||||
resume();
|
||||
return ERR;
|
||||
}
|
||||
|
||||
long IRrecv::decodeNEC(decode_results *results) {
|
||||
long data = 0;
|
||||
int offset = 1; // Skip first space
|
||||
// Initial mark
|
||||
if (!MATCH_MARK(results->rawbuf[offset], NEC_HDR_MARK)) {
|
||||
return ERR;
|
||||
}
|
||||
offset++;
|
||||
// Check for repeat
|
||||
if (irparams.rawlen == 4 &&
|
||||
MATCH_SPACE(results->rawbuf[offset], NEC_RPT_SPACE) &&
|
||||
MATCH_MARK(results->rawbuf[offset+1], NEC_BIT_MARK)) {
|
||||
results->bits = 0;
|
||||
results->value = REPEAT;
|
||||
results->decode_type = NEC;
|
||||
return DECODED;
|
||||
}
|
||||
if (irparams.rawlen < 2 * NEC_BITS + 4) {
|
||||
return ERR;
|
||||
}
|
||||
// Initial space
|
||||
if (!MATCH_SPACE(results->rawbuf[offset], NEC_HDR_SPACE)) {
|
||||
return ERR;
|
||||
}
|
||||
offset++;
|
||||
for (int i = 0; i < NEC_BITS; i++) {
|
||||
if (!MATCH_MARK(results->rawbuf[offset], NEC_BIT_MARK)) {
|
||||
return ERR;
|
||||
}
|
||||
offset++;
|
||||
if (MATCH_SPACE(results->rawbuf[offset], NEC_ONE_SPACE)) {
|
||||
data = (data << 1) | 1;
|
||||
}
|
||||
else if (MATCH_SPACE(results->rawbuf[offset], NEC_ZERO_SPACE)) {
|
||||
data <<= 1;
|
||||
}
|
||||
else {
|
||||
return ERR;
|
||||
}
|
||||
offset++;
|
||||
}
|
||||
// Success
|
||||
results->bits = NEC_BITS;
|
||||
results->value = data;
|
||||
results->decode_type = NEC;
|
||||
return DECODED;
|
||||
}
|
||||
|
||||
long IRrecv::decodeSony(decode_results *results) {
|
||||
long data = 0;
|
||||
if (irparams.rawlen < 2 * SONY_BITS + 2) {
|
||||
return ERR;
|
||||
}
|
||||
int offset = 1; // Skip first space
|
||||
// Initial mark
|
||||
if (!MATCH_MARK(results->rawbuf[offset], SONY_HDR_MARK)) {
|
||||
return ERR;
|
||||
}
|
||||
offset++;
|
||||
|
||||
while (offset + 1 < irparams.rawlen) {
|
||||
if (!MATCH_SPACE(results->rawbuf[offset], SONY_HDR_SPACE)) {
|
||||
break;
|
||||
}
|
||||
offset++;
|
||||
if (MATCH_MARK(results->rawbuf[offset], SONY_ONE_MARK)) {
|
||||
data = (data << 1) | 1;
|
||||
}
|
||||
else if (MATCH_MARK(results->rawbuf[offset], SONY_ZERO_MARK)) {
|
||||
data <<= 1;
|
||||
}
|
||||
else {
|
||||
return ERR;
|
||||
}
|
||||
offset++;
|
||||
}
|
||||
|
||||
// Success
|
||||
results->bits = (offset - 1) / 2;
|
||||
if (results->bits < 12) {
|
||||
results->bits = 0;
|
||||
return ERR;
|
||||
}
|
||||
results->value = data;
|
||||
results->decode_type = SONY;
|
||||
return DECODED;
|
||||
}
|
||||
|
||||
// Gets one undecoded level at a time from the raw buffer.
|
||||
// The RC5/6 decoding is easier if the data is broken into time intervals.
|
||||
// E.g. if the buffer has MARK for 2 time intervals and SPACE for 1,
|
||||
// successive calls to getRClevel will return MARK, MARK, SPACE.
|
||||
// offset and used are updated to keep track of the current position.
|
||||
// t1 is the time interval for a single bit in microseconds.
|
||||
// Returns -1 for error (measured time interval is not a multiple of t1).
|
||||
int IRrecv::getRClevel(decode_results *results, int *offset, int *used, int t1) {
|
||||
if (*offset >= results->rawlen) {
|
||||
// After end of recorded buffer, assume SPACE.
|
||||
return SPACE;
|
||||
}
|
||||
int width = results->rawbuf[*offset];
|
||||
int val = ((*offset) % 2) ? MARK : SPACE;
|
||||
int correction = (val == MARK) ? MARK_EXCESS : - MARK_EXCESS;
|
||||
|
||||
int avail;
|
||||
if (MATCH(width, t1 + correction)) {
|
||||
avail = 1;
|
||||
}
|
||||
else if (MATCH(width, 2*t1 + correction)) {
|
||||
avail = 2;
|
||||
}
|
||||
else if (MATCH(width, 3*t1 + correction)) {
|
||||
avail = 3;
|
||||
}
|
||||
else {
|
||||
return -1;
|
||||
}
|
||||
|
||||
(*used)++;
|
||||
if (*used >= avail) {
|
||||
*used = 0;
|
||||
(*offset)++;
|
||||
}
|
||||
#ifdef DEBUG
|
||||
if (val == MARK) {
|
||||
Serial.println("MARK");
|
||||
}
|
||||
else {
|
||||
Serial.println("SPACE");
|
||||
}
|
||||
#endif
|
||||
return val;
|
||||
}
|
||||
|
||||
long IRrecv::decodeRC5(decode_results *results) {
|
||||
if (irparams.rawlen < MIN_RC5_SAMPLES + 2) {
|
||||
return ERR;
|
||||
}
|
||||
int offset = 1; // Skip gap space
|
||||
long data = 0;
|
||||
int used = 0;
|
||||
// Get start bits
|
||||
if (getRClevel(results, &offset, &used, RC5_T1) != MARK) return ERR;
|
||||
if (getRClevel(results, &offset, &used, RC5_T1) != SPACE) return ERR;
|
||||
if (getRClevel(results, &offset, &used, RC5_T1) != MARK) return ERR;
|
||||
int nbits;
|
||||
for (nbits = 0; offset < irparams.rawlen; nbits++) {
|
||||
int levelA = getRClevel(results, &offset, &used, RC5_T1);
|
||||
int levelB = getRClevel(results, &offset, &used, RC5_T1);
|
||||
if (levelA == SPACE && levelB == MARK) {
|
||||
// 1 bit
|
||||
data = (data << 1) | 1;
|
||||
}
|
||||
else if (levelA == MARK && levelB == SPACE) {
|
||||
// zero bit
|
||||
data <<= 1;
|
||||
}
|
||||
else {
|
||||
return ERR;
|
||||
}
|
||||
}
|
||||
|
||||
// Success
|
||||
results->bits = nbits;
|
||||
results->value = data;
|
||||
results->decode_type = RC5;
|
||||
return DECODED;
|
||||
}
|
||||
|
||||
long IRrecv::decodeRC6(decode_results *results) {
|
||||
if (results->rawlen < MIN_RC6_SAMPLES) {
|
||||
return ERR;
|
||||
}
|
||||
int offset = 1; // Skip first space
|
||||
// Initial mark
|
||||
if (!MATCH_MARK(results->rawbuf[offset], RC6_HDR_MARK)) {
|
||||
return ERR;
|
||||
}
|
||||
offset++;
|
||||
if (!MATCH_SPACE(results->rawbuf[offset], RC6_HDR_SPACE)) {
|
||||
return ERR;
|
||||
}
|
||||
offset++;
|
||||
long data = 0;
|
||||
int used = 0;
|
||||
// Get start bit (1)
|
||||
if (getRClevel(results, &offset, &used, RC6_T1) != MARK) return ERR;
|
||||
if (getRClevel(results, &offset, &used, RC6_T1) != SPACE) return ERR;
|
||||
int nbits;
|
||||
for (nbits = 0; offset < results->rawlen; nbits++) {
|
||||
int levelA, levelB; // Next two levels
|
||||
levelA = getRClevel(results, &offset, &used, RC6_T1);
|
||||
if (nbits == 3) {
|
||||
// T bit is double wide; make sure second half matches
|
||||
if (levelA != getRClevel(results, &offset, &used, RC6_T1)) return ERR;
|
||||
}
|
||||
levelB = getRClevel(results, &offset, &used, RC6_T1);
|
||||
if (nbits == 3) {
|
||||
// T bit is double wide; make sure second half matches
|
||||
if (levelB != getRClevel(results, &offset, &used, RC6_T1)) return ERR;
|
||||
}
|
||||
if (levelA == MARK && levelB == SPACE) { // reversed compared to RC5
|
||||
// 1 bit
|
||||
data = (data << 1) | 1;
|
||||
}
|
||||
else if (levelA == SPACE && levelB == MARK) {
|
||||
// zero bit
|
||||
data <<= 1;
|
||||
}
|
||||
else {
|
||||
return ERR; // Error
|
||||
}
|
||||
}
|
||||
// Success
|
||||
results->bits = nbits;
|
||||
results->value = data;
|
||||
results->decode_type = RC6;
|
||||
return DECODED;
|
||||
}
|
||||
|
||||
/* -----------------------------------------------------------------------
|
||||
* hashdecode - decode an arbitrary IR code.
|
||||
* Instead of decoding using a standard encoding scheme
|
||||
* (e.g. Sony, NEC, RC5), the code is hashed to a 32-bit value.
|
||||
*
|
||||
* The algorithm: look at the sequence of MARK signals, and see if each one
|
||||
* is shorter (0), the same length (1), or longer (2) than the previous.
|
||||
* Do the same with the SPACE signals. Hszh the resulting sequence of 0's,
|
||||
* 1's, and 2's to a 32-bit value. This will give a unique value for each
|
||||
* different code (probably), for most code systems.
|
||||
*
|
||||
* http://arcfn.com/2010/01/using-arbitrary-remotes-with-arduino.html
|
||||
*/
|
||||
|
||||
// Compare two tick values, returning 0 if newval is shorter,
|
||||
// 1 if newval is equal, and 2 if newval is longer
|
||||
// Use a tolerance of 20%
|
||||
int IRrecv::compare(unsigned int oldval, unsigned int newval) {
|
||||
if (newval < oldval * .8) {
|
||||
return 0;
|
||||
}
|
||||
else if (oldval < newval * .8) {
|
||||
return 2;
|
||||
}
|
||||
else {
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
// Use FNV hash algorithm: http://isthe.com/chongo/tech/comp/fnv/#FNV-param
|
||||
#define FNV_PRIME_32 16777619
|
||||
#define FNV_BASIS_32 2166136261
|
||||
|
||||
/* Converts the raw code values into a 32-bit hash code.
|
||||
* Hopefully this code is unique for each button.
|
||||
* This isn't a "real" decoding, just an arbitrary value.
|
||||
*/
|
||||
long IRrecv::decodeHash(decode_results *results) {
|
||||
// Require at least 6 samples to prevent triggering on noise
|
||||
if (results->rawlen < 6) {
|
||||
return ERR;
|
||||
}
|
||||
long hash = FNV_BASIS_32;
|
||||
for (int i = 1; i+2 < results->rawlen; i++) {
|
||||
int value = compare(results->rawbuf[i], results->rawbuf[i+2]);
|
||||
// Add value into the hash
|
||||
hash = (hash * FNV_PRIME_32) ^ value;
|
||||
}
|
||||
results->value = hash;
|
||||
results->bits = 32;
|
||||
results->decode_type = UNKNOWN;
|
||||
return DECODED;
|
||||
}
|
||||
|
||||
/* Sharp and DISH support by Todd Treece ( http://unionbridge.org/design/ircommand )
|
||||
|
||||
The Dish send function needs to be repeated 4 times, and the Sharp function
|
||||
has the necessary repeat built in because of the need to invert the signal.
|
||||
|
||||
Sharp protocol documentation:
|
||||
http://www.sbprojects.com/knowledge/ir/sharp.htm
|
||||
|
||||
Here are the LIRC files that I found that seem to match the remote codes
|
||||
from the oscilloscope:
|
||||
|
||||
Sharp LCD TV:
|
||||
http://lirc.sourceforge.net/remotes/sharp/GA538WJSA
|
||||
|
||||
DISH NETWORK (echostar 301):
|
||||
http://lirc.sourceforge.net/remotes/echostar/301_501_3100_5100_58xx_59xx
|
||||
|
||||
For the DISH codes, only send the last for characters of the hex.
|
||||
i.e. use 0x1C10 instead of 0x0000000000001C10 which is listed in the
|
||||
linked LIRC file.
|
||||
*/
|
||||
|
||||
void IRsend::sendSharp(unsigned long data, int nbits) {
|
||||
unsigned long invertdata = data ^ SHARP_TOGGLE_MASK;
|
||||
enableIROut(38);
|
||||
for (int i = 0; i < nbits; i++) {
|
||||
if (data & 0x4000) {
|
||||
mark(SHARP_BIT_MARK);
|
||||
space(SHARP_ONE_SPACE);
|
||||
}
|
||||
else {
|
||||
mark(SHARP_BIT_MARK);
|
||||
space(SHARP_ZERO_SPACE);
|
||||
}
|
||||
data <<= 1;
|
||||
}
|
||||
|
||||
mark(SHARP_BIT_MARK);
|
||||
space(SHARP_ZERO_SPACE);
|
||||
delay(46);
|
||||
for (int i = 0; i < nbits; i++) {
|
||||
if (invertdata & 0x4000) {
|
||||
mark(SHARP_BIT_MARK);
|
||||
space(SHARP_ONE_SPACE);
|
||||
}
|
||||
else {
|
||||
mark(SHARP_BIT_MARK);
|
||||
space(SHARP_ZERO_SPACE);
|
||||
}
|
||||
invertdata <<= 1;
|
||||
}
|
||||
mark(SHARP_BIT_MARK);
|
||||
space(SHARP_ZERO_SPACE);
|
||||
delay(46);
|
||||
}
|
||||
|
||||
void IRsend::sendDISH(unsigned long data, int nbits)
|
||||
{
|
||||
enableIROut(56);
|
||||
mark(DISH_HDR_MARK);
|
||||
space(DISH_HDR_SPACE);
|
||||
for (int i = 0; i < nbits; i++) {
|
||||
if (data & DISH_TOP_BIT) {
|
||||
mark(DISH_BIT_MARK);
|
||||
space(DISH_ONE_SPACE);
|
||||
}
|
||||
else {
|
||||
mark(DISH_BIT_MARK);
|
||||
space(DISH_ZERO_SPACE);
|
||||
}
|
||||
data <<= 1;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,101 @@
|
||||
/*
|
||||
* IRremote
|
||||
* Version 0.1 July, 2009
|
||||
* Copyright 2009 Ken Shirriff
|
||||
* For details, see http://arcfn.com/2009/08/multi-protocol-infrared-remote-library.htm http://arcfn.com
|
||||
*
|
||||
* Interrupt code based on NECIRrcv by Joe Knapp
|
||||
* http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1210243556
|
||||
* Also influenced by http://zovirl.com/2008/11/12/building-a-universal-remote-with-an-arduino/
|
||||
*/
|
||||
|
||||
#ifndef IRremote_h
|
||||
#define IRremote_h
|
||||
|
||||
// The following are compile-time library options.
|
||||
// If you change them, recompile the library.
|
||||
// If DEBUG is defined, a lot of debugging output will be printed during decoding.
|
||||
// TEST must be defined for the IRtest unittests to work. It will make some
|
||||
// methods virtual, which will be slightly slower, which is why it is optional.
|
||||
// #define DEBUG
|
||||
// #define TEST
|
||||
|
||||
// Results returned from the decoder
|
||||
class decode_results {
|
||||
public:
|
||||
int decode_type; // NEC, SONY, RC5, UNKNOWN
|
||||
unsigned long value; // Decoded value
|
||||
int bits; // Number of bits in decoded value
|
||||
volatile unsigned int *rawbuf; // Raw intervals in .5 us ticks
|
||||
int rawlen; // Number of records in rawbuf.
|
||||
};
|
||||
|
||||
// Values for decode_type
|
||||
#define NEC 1
|
||||
#define SONY 2
|
||||
#define RC5 3
|
||||
#define RC6 4
|
||||
#define DISH 5
|
||||
#define SHARP 6
|
||||
#define UNKNOWN -1
|
||||
|
||||
// Decoded value for NEC when a repeat code is received
|
||||
#define REPEAT 0xffffffff
|
||||
|
||||
// main class for receiving IR
|
||||
class IRrecv
|
||||
{
|
||||
public:
|
||||
IRrecv(int recvpin);
|
||||
void blink13(int blinkflag);
|
||||
int decode(decode_results *results);
|
||||
void enableIRIn();
|
||||
void resume();
|
||||
private:
|
||||
// These are called by decode
|
||||
int getRClevel(decode_results *results, int *offset, int *used, int t1);
|
||||
long decodeNEC(decode_results *results);
|
||||
long decodeSony(decode_results *results);
|
||||
long decodeRC5(decode_results *results);
|
||||
long decodeRC6(decode_results *results);
|
||||
long decodeHash(decode_results *results);
|
||||
int compare(unsigned int oldval, unsigned int newval);
|
||||
|
||||
}
|
||||
;
|
||||
|
||||
// Only used for testing; can remove virtual for shorter code
|
||||
#ifdef TEST
|
||||
#define VIRTUAL virtual
|
||||
#else
|
||||
#define VIRTUAL
|
||||
#endif
|
||||
|
||||
class IRsend
|
||||
{
|
||||
public:
|
||||
IRsend() {}
|
||||
void sendNEC(unsigned long data, int nbits);
|
||||
void sendSony(unsigned long data, int nbits);
|
||||
void sendRaw(unsigned int buf[], int len, int hz);
|
||||
void sendRC5(unsigned long data, int nbits);
|
||||
void sendRC6(unsigned long data, int nbits);
|
||||
void sendDISH(unsigned long data, int nbits);
|
||||
void sendSharp(unsigned long data, int nbits);
|
||||
// private:
|
||||
void enableIROut(int khz);
|
||||
VIRTUAL void mark(int usec);
|
||||
VIRTUAL void space(int usec);
|
||||
}
|
||||
;
|
||||
|
||||
// Some useful constants
|
||||
|
||||
#define USECPERTICK 50 // microseconds per clock interrupt tick
|
||||
#define RAWBUF 76 // Length of raw duration buffer
|
||||
|
||||
// Marks tend to be 100us too long, and spaces 100us too short
|
||||
// when received due to sensor lag.
|
||||
#define MARK_EXCESS 100
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,398 @@
|
||||
/*
|
||||
* IRremote
|
||||
* Version 0.1 July, 2009
|
||||
* Copyright 2009 Ken Shirriff
|
||||
* For details, see http://arcfn.com/2009/08/multi-protocol-infrared-remote-library.html
|
||||
*
|
||||
* Modified by Paul Stoffregen <paul@pjrc.com> to support other boards and timers
|
||||
*
|
||||
* Interrupt code based on NECIRrcv by Joe Knapp
|
||||
* http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1210243556
|
||||
* Also influenced by http://zovirl.com/2008/11/12/building-a-universal-remote-with-an-arduino/
|
||||
*/
|
||||
|
||||
#ifndef IRremoteint_h
|
||||
#define IRremoteint_h
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
// define which timer to use
|
||||
//
|
||||
// Uncomment the timer you wish to use on your board. If you
|
||||
// are using another library which uses timer2, you have options
|
||||
// to switch IRremote to use a different timer.
|
||||
|
||||
// Arduino Mega
|
||||
#if defined(__AVR_ATmega1280__)
|
||||
//#define IR_USE_TIMER1 // tx = pin 11
|
||||
#define IR_USE_TIMER2 // tx = pin 9
|
||||
//#define IR_USE_TIMER3 // tx = pin 5
|
||||
//#define IR_USE_TIMER4 // tx = pin 6
|
||||
//#define IR_USE_TIMER5 // tx = pin 46
|
||||
|
||||
// Teensy 1.0
|
||||
#elif defined(__AVR_AT90USB162__)
|
||||
#define IR_USE_TIMER1 // tx = pin 17
|
||||
|
||||
// Teensy 2.0
|
||||
#elif defined(__AVR_ATmega32U4__)
|
||||
//#define IR_USE_TIMER1 // tx = pin 14
|
||||
//#define IR_USE_TIMER3 // tx = pin 9
|
||||
#define IR_USE_TIMER4_HS // tx = pin 10
|
||||
|
||||
// Teensy++ 1.0 & 2.0
|
||||
#elif defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__)
|
||||
//#define IR_USE_TIMER1 // tx = pin 25
|
||||
#define IR_USE_TIMER2 // tx = pin 1
|
||||
//#define IR_USE_TIMER3 // tx = pin 16
|
||||
|
||||
// Sanguino
|
||||
#elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__)
|
||||
//#define IR_USE_TIMER1 // tx = pin 13
|
||||
#define IR_USE_TIMER2 // tx = pin 14
|
||||
|
||||
// Arduino Duemilanove, Diecimila, LilyPad, Mini, Fio, etc
|
||||
#else
|
||||
//#define IR_USE_TIMER1 // tx = pin 9
|
||||
#define IR_USE_TIMER2 // tx = pin 3
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
#ifdef F_CPU
|
||||
#define SYSCLOCK F_CPU // main Arduino clock
|
||||
#else
|
||||
#define SYSCLOCK 16000000 // main Arduino clock
|
||||
#endif
|
||||
|
||||
#define ERR 0
|
||||
#define DECODED 1
|
||||
|
||||
|
||||
// defines for setting and clearing register bits
|
||||
#ifndef cbi
|
||||
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
|
||||
#endif
|
||||
#ifndef sbi
|
||||
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
|
||||
#endif
|
||||
|
||||
|
||||
// pulse parameters in usec
|
||||
#define NEC_HDR_MARK 9000
|
||||
#define NEC_HDR_SPACE 4500
|
||||
#define NEC_BIT_MARK 560
|
||||
#define NEC_ONE_SPACE 1600
|
||||
#define NEC_ZERO_SPACE 560
|
||||
#define NEC_RPT_SPACE 2250
|
||||
|
||||
#define SONY_HDR_MARK 2400
|
||||
#define SONY_HDR_SPACE 600
|
||||
#define SONY_ONE_MARK 1200
|
||||
#define SONY_ZERO_MARK 600
|
||||
#define SONY_RPT_LENGTH 45000
|
||||
|
||||
#define RC5_T1 889
|
||||
#define RC5_RPT_LENGTH 46000
|
||||
|
||||
#define RC6_HDR_MARK 2666
|
||||
#define RC6_HDR_SPACE 889
|
||||
#define RC6_T1 444
|
||||
#define RC6_RPT_LENGTH 46000
|
||||
|
||||
#define SHARP_BIT_MARK 245
|
||||
#define SHARP_ONE_SPACE 1805
|
||||
#define SHARP_ZERO_SPACE 795
|
||||
#define SHARP_GAP 600000
|
||||
#define SHARP_TOGGLE_MASK 0x3FF
|
||||
#define SHARP_RPT_SPACE 3000
|
||||
|
||||
#define DISH_HDR_MARK 400
|
||||
#define DISH_HDR_SPACE 6100
|
||||
#define DISH_BIT_MARK 400
|
||||
#define DISH_ONE_SPACE 1700
|
||||
#define DISH_ZERO_SPACE 2800
|
||||
#define DISH_RPT_SPACE 6200
|
||||
#define DISH_TOP_BIT 0x8000
|
||||
|
||||
#define SHARP_BITS 15
|
||||
#define DISH_BITS 16
|
||||
|
||||
#define TOLERANCE 25 // percent tolerance in measurements
|
||||
#define LTOL (1.0 - TOLERANCE/100.)
|
||||
#define UTOL (1.0 + TOLERANCE/100.)
|
||||
|
||||
#define _GAP 5000 // Minimum map between transmissions
|
||||
#define GAP_TICKS (_GAP/USECPERTICK)
|
||||
|
||||
#define TICKS_LOW(us) (int) (((us)*LTOL/USECPERTICK))
|
||||
#define TICKS_HIGH(us) (int) (((us)*UTOL/USECPERTICK + 1))
|
||||
|
||||
#ifndef DEBUG
|
||||
#define MATCH(measured_ticks, desired_us) ((measured_ticks) >= TICKS_LOW(desired_us) && (measured_ticks) <= TICKS_HIGH(desired_us))
|
||||
#define MATCH_MARK(measured_ticks, desired_us) MATCH(measured_ticks, (desired_us) + MARK_EXCESS)
|
||||
#define MATCH_SPACE(measured_ticks, desired_us) MATCH((measured_ticks), (desired_us) - MARK_EXCESS)
|
||||
// Debugging versions are in IRremote.cpp
|
||||
#endif
|
||||
|
||||
// receiver states
|
||||
#define STATE_IDLE 2
|
||||
#define STATE_MARK 3
|
||||
#define STATE_SPACE 4
|
||||
#define STATE_STOP 5
|
||||
|
||||
// information for the interrupt handler
|
||||
typedef struct {
|
||||
uint8_t recvpin; // pin for IR data from detector
|
||||
uint8_t rcvstate; // state machine
|
||||
uint8_t blinkflag; // TRUE to enable blinking of pin 13 on IR processing
|
||||
unsigned int timer; // state timer, counts 50uS ticks.
|
||||
unsigned int rawbuf[RAWBUF]; // raw data
|
||||
uint8_t rawlen; // counter of entries in rawbuf
|
||||
}
|
||||
irparams_t;
|
||||
|
||||
// Defined in IRremote.cpp
|
||||
extern volatile irparams_t irparams;
|
||||
|
||||
// IR detector output is active low
|
||||
#define MARK 0
|
||||
#define SPACE 1
|
||||
|
||||
#define TOPBIT 0x80000000
|
||||
|
||||
#define NEC_BITS 32
|
||||
#define SONY_BITS 12
|
||||
#define MIN_RC5_SAMPLES 11
|
||||
#define MIN_RC6_SAMPLES 1
|
||||
|
||||
|
||||
|
||||
|
||||
// defines for timer2 (8 bits)
|
||||
#if defined(IR_USE_TIMER2)
|
||||
#define TIMER_RESET
|
||||
#define TIMER_ENABLE_PWM (TCCR2A |= _BV(COM2B1))
|
||||
#define TIMER_DISABLE_PWM (TCCR2A &= ~(_BV(COM2B1)))
|
||||
#define TIMER_ENABLE_INTR (TIMSK2 = _BV(OCIE2A))
|
||||
#define TIMER_DISABLE_INTR (TIMSK2 - 0)
|
||||
#define TIMER_INTR_NAME TIMER2_COMPA_vect
|
||||
#define TIMER_CONFIG_KHZ(val) ({ \
|
||||
const uint8_t pwmval = SYSCLOCK / 2000 / (val); \
|
||||
TCCR2A = _BV(WGM20); \
|
||||
TCCR2B = _BV(WGM22) | _BV(CS20); \
|
||||
OCR2A = pwmval; \
|
||||
OCR2B = pwmval / 3; \
|
||||
})
|
||||
#define TIMER_COUNT_TOP (SYSCLOCK * USECPERTICK / 1000000)
|
||||
#if (TIMER_COUNT_TOP < 256)
|
||||
#define TIMER_CONFIG_NORMAL() ({ \
|
||||
TCCR2A = _BV(WGM21); \
|
||||
TCCR2B = _BV(CS20); \
|
||||
OCR2A = TIMER_COUNT_TOP; \
|
||||
TCNT2 = 0; \
|
||||
})
|
||||
#else
|
||||
#define TIMER_CONFIG_NORMAL() ({ \
|
||||
TCCR2A = _BV(WGM21); \
|
||||
TCCR2B = _BV(CS21); \
|
||||
OCR2A = TIMER_COUNT_TOP / 8; \
|
||||
TCNT2 = 0; \
|
||||
})
|
||||
#endif
|
||||
#if defined(CORE_OC2B_PIN)
|
||||
#define TIMER_PWM_PIN CORE_OC2B_PIN /* Teensy */
|
||||
#elif defined(__AVR_ATmega1280__)
|
||||
#define TIMER_PWM_PIN 9 /* Arduino Mega */
|
||||
#elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__)
|
||||
#define TIMER_PWM_PIN 14 /* Sanguino */
|
||||
#else
|
||||
#define TIMER_PWM_PIN 3 /* Arduino Duemilanove, Diecimila, LilyPad, etc */
|
||||
#endif
|
||||
|
||||
|
||||
// defines for timer1 (16 bits)
|
||||
#elif defined(IR_USE_TIMER1)
|
||||
#define TIMER_RESET
|
||||
#define TIMER_ENABLE_PWM (TCCR1A |= _BV(COM1A1))
|
||||
#define TIMER_DISABLE_PWM (TCCR1A &= ~(_BV(COM1A1)))
|
||||
#define TIMER_ENABLE_INTR (TIMSK1 = _BV(OCIE1A))
|
||||
#define TIMER_DISABLE_INTR (TIMSK1 = 0)
|
||||
#define TIMER_INTR_NAME TIMER1_COMPA_vect
|
||||
#define TIMER_CONFIG_KHZ(val) ({ \
|
||||
const uint16_t pwmval = SYSCLOCK / 2000 / (val); \
|
||||
TCCR1A = _BV(WGM11); \
|
||||
TCCR1B = _BV(WGM13) | _BV(CS10); \
|
||||
ICR1 = pwmval; \
|
||||
OCR1A = pwmval / 3; \
|
||||
})
|
||||
#define TIMER_CONFIG_NORMAL() ({ \
|
||||
TCCR1A = 0; \
|
||||
TCCR1B = _BV(WGM12) | _BV(CS10); \
|
||||
OCR1A = SYSCLOCK * USECPERTICK / 1000000; \
|
||||
TCNT1 = 0; \
|
||||
})
|
||||
#if defined(CORE_OC1A_PIN)
|
||||
#define TIMER_PWM_PIN CORE_OC1A_PIN /* Teensy */
|
||||
#elif defined(__AVR_ATmega1280__)
|
||||
#define TIMER_PWM_PIN 11 /* Arduino Mega */
|
||||
#elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__)
|
||||
#define TIMER_PWM_PIN 13 /* Sanguino */
|
||||
#else
|
||||
#define TIMER_PWM_PIN 9 /* Arduino Duemilanove, Diecimila, LilyPad, etc */
|
||||
#endif
|
||||
|
||||
|
||||
// defines for timer3 (16 bits)
|
||||
#elif defined(IR_USE_TIMER3)
|
||||
#define TIMER_RESET
|
||||
#define TIMER_ENABLE_PWM (TCCR3A |= _BV(COM3A1))
|
||||
#define TIMER_DISABLE_PWM (TCCR3A &= ~(_BV(COM3A1)))
|
||||
#define TIMER_ENABLE_INTR (TIMSK3 = _BV(OCIE3A))
|
||||
#define TIMER_DISABLE_INTR (TIMSK3 = 0)
|
||||
#define TIMER_INTR_NAME TIMER3_COMPA_vect
|
||||
#define TIMER_CONFIG_KHZ(val) ({ \
|
||||
const uint16_t pwmval = SYSCLOCK / 2000 / (val); \
|
||||
TCCR3A = _BV(WGM31); \
|
||||
TCCR3B = _BV(WGM33) | _BV(CS30); \
|
||||
ICR3 = pwmval; \
|
||||
OCR3A = pwmval / 3; \
|
||||
})
|
||||
#define TIMER_CONFIG_NORMAL() ({ \
|
||||
TCCR3A = 0; \
|
||||
TCCR3B = _BV(WGM32) | _BV(CS30); \
|
||||
OCR3A = SYSCLOCK * USECPERTICK / 1000000; \
|
||||
TCNT3 = 0; \
|
||||
})
|
||||
#if defined(CORE_OC3A_PIN)
|
||||
#define TIMER_PWM_PIN CORE_OC3A_PIN /* Teensy */
|
||||
#elif defined(__AVR_ATmega1280__)
|
||||
#define TIMER_PWM_PIN 5 /* Arduino Mega */
|
||||
#else
|
||||
#error "Please add OC3A pin number here\n"
|
||||
#endif
|
||||
|
||||
|
||||
// defines for timer4 (10 bits, high speed option)
|
||||
#elif defined(IR_USE_TIMER4_HS)
|
||||
#define TIMER_RESET
|
||||
#define TIMER_ENABLE_PWM (TCCR4A |= _BV(COM4A1))
|
||||
#define TIMER_DISABLE_PWM (TCCR4A &= ~(_BV(COM4A1)))
|
||||
#define TIMER_ENABLE_INTR (TIMSK4 = _BV(TOIE4))
|
||||
#define TIMER_DISABLE_INTR (TIMSK4 = 0)
|
||||
#define TIMER_INTR_NAME TIMER4_OVF_vect
|
||||
#define TIMER_CONFIG_KHZ(val) ({ \
|
||||
const uint16_t pwmval = SYSCLOCK / 2000 / (val); \
|
||||
TCCR4A = (1<<PWM4A); \
|
||||
TCCR4B = _BV(CS40); \
|
||||
TCCR4C = 0; \
|
||||
TCCR4D = (1<<WGM40); \
|
||||
TCCR4E = 0; \
|
||||
TC4H = pwmval >> 8; \
|
||||
OCR4C = pwmval; \
|
||||
TC4H = (pwmval / 3) >> 8; \
|
||||
OCR4A = (pwmval / 3) & 255; \
|
||||
})
|
||||
#define TIMER_CONFIG_NORMAL() ({ \
|
||||
TCCR4A = 0; \
|
||||
TCCR4B = _BV(CS40); \
|
||||
TCCR4C = 0; \
|
||||
TCCR4D = 0; \
|
||||
TCCR4E = 0; \
|
||||
TC4H = (SYSCLOCK * USECPERTICK / 1000000) >> 8; \
|
||||
OCR4C = (SYSCLOCK * USECPERTICK / 1000000) & 255; \
|
||||
TC4H = 0; \
|
||||
TCNT4 = 0; \
|
||||
})
|
||||
#if defined(CORE_OC4A_PIN)
|
||||
#define TIMER_PWM_PIN CORE_OC4A_PIN /* Teensy */
|
||||
#else
|
||||
#error "Please add OC4A pin number here\n"
|
||||
#endif
|
||||
|
||||
|
||||
// defines for timer4 (16 bits)
|
||||
#elif defined(IR_USE_TIMER4)
|
||||
#define TIMER_RESET
|
||||
#define TIMER_ENABLE_PWM (TCCR4A |= _BV(COM4A1))
|
||||
#define TIMER_DISABLE_PWM (TCCR4A &= ~(_BV(COM4A1)))
|
||||
#define TIMER_ENABLE_INTR (TIMSK4 = _BV(OCIE4A))
|
||||
#define TIMER_DISABLE_INTR (TIMSK4 = 0)
|
||||
#define TIMER_INTR_NAME TIMER4_COMPA_vect
|
||||
#define TIMER_CONFIG_KHZ(val) ({ \
|
||||
const uint16_t pwmval = SYSCLOCK / 2000 / (val); \
|
||||
TCCR4A = _BV(WGM41); \
|
||||
TCCR4B = _BV(WGM43) | _BV(CS40); \
|
||||
ICR4 = pwmval; \
|
||||
OCR4A = pwmval / 3; \
|
||||
})
|
||||
#define TIMER_CONFIG_NORMAL() ({ \
|
||||
TCCR4A = 0; \
|
||||
TCCR4B = _BV(WGM42) | _BV(CS40); \
|
||||
OCR4A = SYSCLOCK * USECPERTICK / 1000000; \
|
||||
TCNT4 = 0; \
|
||||
})
|
||||
#if defined(CORE_OC4A_PIN)
|
||||
#define TIMER_PWM_PIN CORE_OC4A_PIN
|
||||
#elif defined(__AVR_ATmega1280__)
|
||||
#define TIMER_PWM_PIN 6 /* Arduino Mega */
|
||||
#else
|
||||
#error "Please add OC4A pin number here\n"
|
||||
#endif
|
||||
|
||||
|
||||
// defines for timer5 (16 bits)
|
||||
#elif defined(IR_USE_TIMER5)
|
||||
#define TIMER_RESET
|
||||
#define TIMER_ENABLE_PWM (TCCR5A |= _BV(COM5A1))
|
||||
#define TIMER_DISABLE_PWM (TCCR5A &= ~(_BV(COM5A1)))
|
||||
#define TIMER_ENABLE_INTR (TIMSK5 = _BV(OCIE5A))
|
||||
#define TIMER_DISABLE_INTR (TIMSK5 = 0)
|
||||
#define TIMER_INTR_NAME TIMER5_COMPA_vect
|
||||
#define TIMER_CONFIG_KHZ(val) ({ \
|
||||
const uint16_t pwmval = SYSCLOCK / 2000 / (val); \
|
||||
TCCR5A = _BV(WGM51); \
|
||||
TCCR5B = _BV(WGM53) | _BV(CS50); \
|
||||
ICR5 = pwmval; \
|
||||
OCR5A = pwmval / 3; \
|
||||
})
|
||||
#define TIMER_CONFIG_NORMAL() ({ \
|
||||
TCCR5A = 0; \
|
||||
TCCR5B = _BV(WGM52) | _BV(CS50); \
|
||||
OCR5A = SYSCLOCK * USECPERTICK / 1000000; \
|
||||
TCNT5 = 0; \
|
||||
})
|
||||
#if defined(CORE_OC5A_PIN)
|
||||
#define TIMER_PWM_PIN CORE_OC5A_PIN
|
||||
#elif defined(__AVR_ATmega1280__)
|
||||
#define TIMER_PWM_PIN 46 /* Arduino Mega */
|
||||
#else
|
||||
#error "Please add OC5A pin number here\n"
|
||||
#endif
|
||||
|
||||
|
||||
#else // unknown timer
|
||||
#error "Internal code configuration error, no known IR_USE_TIMER# defined\n"
|
||||
#endif
|
||||
|
||||
|
||||
// defines for blinking the LED
|
||||
#if defined(CORE_LED0_PIN)
|
||||
#define BLINKLED CORE_LED0_PIN
|
||||
#define BLINKLED_ON() (digitalWrite(CORE_LED0_PIN, HIGH))
|
||||
#define BLINKLED_OFF() (digitalWrite(CORE_LED0_PIN, LOW))
|
||||
#elif defined(__AVR_ATmega1280__)
|
||||
#define BLINKLED 13
|
||||
#define BLINKLED_ON() (PORTB |= B10000000)
|
||||
#define BLINKLED_OFF() (PORTB &= B01111111)
|
||||
#elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__)
|
||||
#define BLINKLED 0
|
||||
#define BLINKLED_ON() (PORTD |= B00000001)
|
||||
#define BLINKLED_OFF() (PORTD &= B11111110)
|
||||
#else
|
||||
#define BLINKLED 13
|
||||
#define BLINKLED_ON() (PORTB |= B00100000)
|
||||
#define BLINKLED_OFF() (PORTB &= B11011111)
|
||||
#endif
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,150 @@
|
||||
#include "InverseKinemetics.h"
|
||||
// V3.0
|
||||
|
||||
//Variables
|
||||
boolean error;
|
||||
|
||||
void RL3 (int i, Data *DataLine)
|
||||
{
|
||||
float x = DataLine->ServoData[i].x;
|
||||
float z = DataLine->ServoData[i].z;
|
||||
|
||||
if ((x == 0) && (z == 0))
|
||||
DataLine->l3 = 0;
|
||||
else DataLine->l3 = hypot(z, x);
|
||||
|
||||
if (x < 0)
|
||||
DataLine->l3 = DataLine->l3 * -1;
|
||||
}
|
||||
|
||||
float O1 (int i, Data *DataLine)
|
||||
{
|
||||
RL3(i, DataLine);
|
||||
|
||||
float x = DataLine->ServoData[i].x;
|
||||
float z = DataLine->ServoData[i].z;
|
||||
float r;
|
||||
|
||||
if ((x != 0) & (z != 0)){
|
||||
double a = (z / x);
|
||||
return atan(a);
|
||||
}
|
||||
else
|
||||
return atan(0.0); // Math Error Protection.
|
||||
}
|
||||
|
||||
float T1 (int i, Data *DataLine)
|
||||
{
|
||||
float x = DataLine->ServoData[i].x;
|
||||
float y = DataLine->ServoData[i].y;
|
||||
float z = DataLine->ServoData[i].z;
|
||||
float x2 = x * x;
|
||||
float y2 = y * y;
|
||||
float z2 = z * z;
|
||||
float l32 = DataLine->l3 * DataLine->l3;
|
||||
|
||||
DataLine->s = sqrt(x2 + y2 + z2);
|
||||
|
||||
float t1 = 0;
|
||||
|
||||
float s2 = DataLine->s * DataLine->s;
|
||||
|
||||
if ((y == 0) && (DataLine->l3 == 0))
|
||||
t1 = 0; // Math Error Protection.
|
||||
else
|
||||
t1 = (atan2(float(DataLine->l3), float(y)));
|
||||
|
||||
float a = (l1 * l1) - (l2*l2) + s2;
|
||||
float b = 2 * l1 * DataLine->s;
|
||||
float t2 = acos(a / b);
|
||||
|
||||
float ret = 0;
|
||||
|
||||
ret = t1 + abs(t2);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
float T3 (Data *DataLine)
|
||||
{
|
||||
return (acos(((l1*l1) + (l2*l2) - (DataLine->s*DataLine->s)) / (2 * l1 * l2)));
|
||||
}
|
||||
|
||||
void setleg (int i, Data *DataLine)
|
||||
{
|
||||
Segment Seg;
|
||||
|
||||
if (i < 3)
|
||||
{
|
||||
float a = ((float)90 - degrees(O1(i, DataLine)));
|
||||
float b = ((float)180 - degrees(T1(i, DataLine)));
|
||||
float c = degrees(T3(DataLine));
|
||||
|
||||
if ((a >= 0) & (a <= 180)
|
||||
& (b >= 0) & (b <= 180)
|
||||
& (c >= 0) & (c <= 180))
|
||||
{
|
||||
DataLine->ServoData[i].o1 = a;
|
||||
DataLine->ServoData[i].o2 = b;
|
||||
DataLine->ServoData[i].o3 = c;
|
||||
}
|
||||
else
|
||||
{
|
||||
error = true;
|
||||
Seg.sevenSegWrite('e');
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
float a = ((float)90 + degrees(O1(i, DataLine)));
|
||||
float b = degrees(T1(i, DataLine));
|
||||
float c = ((float)180 - degrees(T3(DataLine)));
|
||||
|
||||
if ((a >= 0) & (a <= 180)
|
||||
& (b >= 0) & (b <= 180)
|
||||
& (c >= 0) & (c <= 180))
|
||||
{
|
||||
DataLine->ServoData[i].o1 = a;
|
||||
DataLine->ServoData[i].o2 = b;
|
||||
DataLine->ServoData[i].o3 = c;
|
||||
}
|
||||
else
|
||||
{
|
||||
error = true;
|
||||
Seg.sevenSegWrite('e');
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void InverseKinemetics::IK(Data *DataLine)
|
||||
{
|
||||
Segment Seg;
|
||||
Seg.sevenSegWrite('o');
|
||||
|
||||
error = false;
|
||||
|
||||
for (int i=0; i<6; i++)
|
||||
setleg(i, DataLine);
|
||||
|
||||
#ifdef DEBUGIK
|
||||
for (int i=0; i<6; i++)
|
||||
{
|
||||
if (i != 0)
|
||||
Serial.print(", ");
|
||||
else
|
||||
Serial.print("### InverseKinemetics->ServosPosition's are: ");
|
||||
|
||||
Serial.print(DataLine->ServoData[i].x);
|
||||
Serial.print(", ");
|
||||
Serial.print(DataLine->ServoData[i].y);
|
||||
Serial.print(", ");
|
||||
Serial.print(DataLine->ServoData[i].z);
|
||||
}
|
||||
if (error == true)
|
||||
Serial.println("; ERROR");
|
||||
else
|
||||
Serial.println(";");
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,20 @@
|
||||
// InverseKinemetics.h
|
||||
// V3.0
|
||||
|
||||
#ifndef inverseKinemetics_H
|
||||
#define inverseKinemetics_H
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "hexapod.h"
|
||||
|
||||
class InverseKinemetics
|
||||
{
|
||||
public:
|
||||
void IK(Data *DataLine);
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
@@ -0,0 +1,84 @@
|
||||
// 7Segment
|
||||
#include "Arduino.h"
|
||||
#include "Segment.h"
|
||||
|
||||
byte seven_seg_digits[][8] = {
|
||||
{ 0,1,1,1,1,1,0,1 }, // = 0
|
||||
{ 0,1,1,0,0,0,0,0 }, // = 1
|
||||
{ 0,0,1,1,1,1,1,0 }, // = 2
|
||||
{ 0,1,1,1,1,0,1,0 }, // = 3
|
||||
{ 0,1,1,0,0,0,1,1 }, // = 4
|
||||
{ 0,1,0,1,1,0,1,1 }, // = 5
|
||||
{ 0,1,0,1,1,1,1,1 }, // = 6
|
||||
{ 0,1,1,1,0,0,0,0 }, // = 7
|
||||
{ 0,1,1,1,1,1,1,1 }, // = 8
|
||||
{ 0,1,1,1,1,0,1,1 }, // = 9
|
||||
{ 1,1,1,1,0,1,1,1 }, // = A
|
||||
{ 1,1,0,0,1,1,1,1 }, // = B
|
||||
{ 1,0,0,1,1,1,0,1 }, // = C
|
||||
{ 1,1,0,0,1,1,1,1 }, // = D
|
||||
{ 1,0,0,1,1,1,1,1 }, // = E
|
||||
{ 1,0,0,1,0,1,1,1 }, // = F
|
||||
{ 1,1,0,1,1,0,1,1 }, // = S
|
||||
{ 0,0,0,0,0,0,0,0 }, // = NULL
|
||||
{ 1,0,0,1,0,1,0,1 } // = R
|
||||
};
|
||||
|
||||
Segment::Segment() {
|
||||
for (int i=0; i<8; i++)
|
||||
pinMode(6+i, OUTPUT);
|
||||
}
|
||||
|
||||
/*void Segment::sevenSegWrite(byte digit) {
|
||||
byte pin = 6;
|
||||
for (byte segCount = 0; segCount < 8; ++segCount) {
|
||||
digitalWrite(pin, !seven_seg_digits[digit][segCount]);
|
||||
++pin;
|
||||
}
|
||||
}*/
|
||||
|
||||
void Segment::sevenSegWrite(char chr) {
|
||||
byte pin = 6;
|
||||
byte digit;
|
||||
if(chr <= 9) digit = chr;
|
||||
else if (chr <= 'f') digit = 10 + (chr -'a');
|
||||
else if (chr == 's') digit = 16;
|
||||
else if (chr == 'r') digit = 18;
|
||||
else digit = Cls;
|
||||
for (byte segCount = 0; segCount < 8; ++segCount) {
|
||||
digitalWrite(pin, !seven_seg_digits[digit][segCount]);
|
||||
++pin;
|
||||
}
|
||||
}
|
||||
|
||||
/*void Segment::BlinksevenSegWrite(byte digit) {
|
||||
byte pin = 6;
|
||||
for (byte segCount = 0; segCount < 8; ++segCount) {
|
||||
digitalWrite(pin, !seven_seg_digits[digit][segCount]);
|
||||
++pin;
|
||||
}
|
||||
delay(100);
|
||||
sevenSegWrite((byte)17);
|
||||
}*/
|
||||
|
||||
void Segment::BlinksevenSegWrite(char chr) {
|
||||
byte pin = 6;
|
||||
byte digit;
|
||||
if(chr <= 9) digit = chr;
|
||||
else if (chr <= 'f') digit = 10 + (chr -'a');
|
||||
else if (chr == 's') digit = 16;
|
||||
else digit = Cls;
|
||||
for (byte segCount = 0; segCount < 8; ++segCount) {
|
||||
digitalWrite(pin, !seven_seg_digits[digit][segCount]);
|
||||
++pin;
|
||||
}
|
||||
delay(100);
|
||||
sevenSegWrite(Cls);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,20 @@
|
||||
// 7 Segment driver.
|
||||
|
||||
#ifndef SEGMENT_H
|
||||
#define SEGMENT_H
|
||||
|
||||
#include "Arduino.h"
|
||||
#define Cls 17
|
||||
|
||||
class Segment
|
||||
{
|
||||
public:
|
||||
Segment();
|
||||
//void sevenSegWrite(byte digit);
|
||||
void sevenSegWrite(char chr);
|
||||
//void BlinksevenSegWrite(byte digit);
|
||||
void BlinksevenSegWrite(char chr);
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,100 @@
|
||||
// SerD.h
|
||||
|
||||
#ifndef SERD
|
||||
#define SERD
|
||||
|
||||
class SerD
|
||||
{
|
||||
public:
|
||||
|
||||
//Coordinate system
|
||||
int x;
|
||||
int y;
|
||||
int z;
|
||||
|
||||
//Angles
|
||||
int o1;
|
||||
int o2;
|
||||
int o3;
|
||||
|
||||
//Timings
|
||||
int t1;
|
||||
int t2;
|
||||
int t3;
|
||||
|
||||
//Input
|
||||
SerD& operator() (int X, int Y, int Z)
|
||||
{
|
||||
x = X;
|
||||
y = Y;
|
||||
z = Z;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
SerD& operator= (SerD& R)
|
||||
{
|
||||
x = R.x;
|
||||
y = R.y;
|
||||
z = R.z;
|
||||
|
||||
return *this;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
// DataObject
|
||||
|
||||
#ifndef DOB
|
||||
#define DOB
|
||||
|
||||
class DataObject
|
||||
{
|
||||
public:
|
||||
SerD S0;
|
||||
SerD S1;
|
||||
SerD S2;
|
||||
SerD S3;
|
||||
SerD S4;
|
||||
SerD S5;
|
||||
|
||||
int WalkTime;
|
||||
|
||||
DataObject& operator() (
|
||||
int a1, int a2, int a3,
|
||||
int b1, int b2, int b3,
|
||||
int c1, int c2, int c3,
|
||||
int d1, int d2, int d3,
|
||||
int e1, int e2, int e3,
|
||||
int f1, int f2, int f3, int speed=500)
|
||||
{
|
||||
S0(a1, a2, a3);
|
||||
S1(b1, b2, b3);
|
||||
S2(c1, c2, c3);
|
||||
S3(d1, d2, d3);
|
||||
S4(e1, e2, e3);
|
||||
S5(f1, f2, f3);
|
||||
WalkTime = speed;
|
||||
}
|
||||
|
||||
void input(
|
||||
int a1, int a2, int a3,
|
||||
int b1, int b2, int b3,
|
||||
int c1, int c2, int c3,
|
||||
int d1, int d2, int d3,
|
||||
int e1, int e2, int e3,
|
||||
int f1, int f2, int f3, int speed=500)
|
||||
{
|
||||
S0(a1, a2, a3);
|
||||
S1(b1, b2, b3);
|
||||
S2(c1, c2, c3);
|
||||
S3(d1, d2, d3);
|
||||
S4(e1, e2, e3);
|
||||
S5(f1, f2, f3);
|
||||
WalkTime = speed;
|
||||
}
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,102 @@
|
||||
// Servos.cpp
|
||||
// V3.0
|
||||
|
||||
#include "Servos.h"
|
||||
|
||||
void Servos::MoveHead (Data *DataLine)
|
||||
{
|
||||
Serial.begin(38400);
|
||||
Serial1.begin(38400);
|
||||
|
||||
#ifdef DEBUGHS
|
||||
Serial.print("### Servos->Head Time/Position: ");
|
||||
Serial.print(DataLine->HeadTime);
|
||||
Serial.print(", ");
|
||||
Serial.println(DataLine->HeadPosition);
|
||||
#endif
|
||||
|
||||
Serial1.print(" #");
|
||||
Serial1.print(9);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(DataLine->HeadPosition);
|
||||
Serial1.print(" T");
|
||||
Serial1.println(DataLine->HeadTime);
|
||||
}
|
||||
|
||||
void mSend(Data *DataLine)
|
||||
{
|
||||
int s=3;
|
||||
|
||||
for (int i=0; i<9; i += 3){
|
||||
Serial1.print(" #");
|
||||
Serial1.print(i);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(DataLine->ServoData[s].t1);
|
||||
Serial1.print(" #");
|
||||
Serial1.print(i+1);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(DataLine->ServoData[s].t2);
|
||||
Serial1.print(" #");
|
||||
Serial1.print(i+2);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(DataLine->ServoData[s].t3);
|
||||
s++;
|
||||
}
|
||||
|
||||
s = 0;
|
||||
|
||||
for (int i=16; i<25; i += 3){
|
||||
Serial1.print(" #");
|
||||
Serial1.print(i);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(DataLine->ServoData[s].t1);
|
||||
Serial1.print(" #");
|
||||
Serial1.print(i+1);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(DataLine->ServoData[s].t2);
|
||||
Serial1.print(" #");
|
||||
Serial1.print(i+2);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(DataLine->ServoData[s].t3);
|
||||
s++;
|
||||
}
|
||||
|
||||
Serial1.print(" T");
|
||||
Serial1.println(DataLine->WalkTime);
|
||||
|
||||
#ifdef DEBUGC
|
||||
for (int i=0; i<6; i++)
|
||||
{
|
||||
if (i != 0)
|
||||
Serial.print(", ");
|
||||
else
|
||||
{
|
||||
Serial.print("### Converter->ServosAnglesTime Are: ");
|
||||
}
|
||||
|
||||
Serial.print(DataLine->ServoData[i].t1);
|
||||
Serial.print(", ");
|
||||
Serial.print(DataLine->ServoData[i].t2);
|
||||
Serial.print(", ");
|
||||
Serial.print(DataLine->ServoData[i].t3);
|
||||
}
|
||||
Serial.println(";");
|
||||
#endif
|
||||
}
|
||||
|
||||
void Servos::Send(Data *DataLine)
|
||||
{
|
||||
#ifdef DEBUGS
|
||||
Serial.println("### Servos->Servos ###");
|
||||
Serial.println("### Servos Send ###");
|
||||
digitalWrite(13, HIGH);
|
||||
delay(10);
|
||||
digitalWrite(13, LOW);
|
||||
#endif
|
||||
|
||||
#ifndef Freeze
|
||||
mSend(DataLine);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,21 @@
|
||||
// Servos.h
|
||||
// V3.0
|
||||
|
||||
#ifndef SERVOS_H
|
||||
#define SERVOS_H
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "hexapod.h"
|
||||
|
||||
class Servos
|
||||
{
|
||||
public:
|
||||
void Send(Data *DataLine);
|
||||
void MoveHead (Data *DataLine);
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
@@ -0,0 +1,426 @@
|
||||
/*
|
||||
TinyGPS - a small GPS library for Arduino providing basic NMEA parsing
|
||||
Based on work by and "distance_to" and "course_to" courtesy of Maarten Lamers.
|
||||
Suggestion to add satellites(), course_to(), and cardinal(), by Matt Monson.
|
||||
Copyright (C) 2008-2012 Mikal Hart
|
||||
All rights reserved.
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
This library is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
Lesser General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU Lesser General Public
|
||||
License along with this library; if not, write to the Free Software
|
||||
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*/
|
||||
|
||||
#include "TinyGPS.h"
|
||||
|
||||
#define _GPRMC_TERM "GPRMC"
|
||||
#define _GPGGA_TERM "GPGGA"
|
||||
|
||||
TinyGPS::TinyGPS()
|
||||
: _time(GPS_INVALID_TIME)
|
||||
, _date(GPS_INVALID_DATE)
|
||||
, _latitude(GPS_INVALID_ANGLE)
|
||||
, _longitude(GPS_INVALID_ANGLE)
|
||||
, _altitude(GPS_INVALID_ALTITUDE)
|
||||
, _speed(GPS_INVALID_SPEED)
|
||||
, _course(GPS_INVALID_ANGLE)
|
||||
, _hdop(GPS_INVALID_HDOP)
|
||||
, _numsats(GPS_INVALID_SATELLITES)
|
||||
, _last_time_fix(GPS_INVALID_FIX_TIME)
|
||||
, _last_position_fix(GPS_INVALID_FIX_TIME)
|
||||
, _parity(0)
|
||||
, _is_checksum_term(false)
|
||||
, _sentence_type(_GPS_SENTENCE_OTHER)
|
||||
, _term_number(0)
|
||||
, _term_offset(0)
|
||||
, _gps_data_good(false)
|
||||
#ifndef _GPS_NO_STATS
|
||||
, _encoded_characters(0)
|
||||
, _good_sentences(0)
|
||||
, _failed_checksum(0)
|
||||
#endif
|
||||
{
|
||||
_term[0] = '\0';
|
||||
}
|
||||
|
||||
//
|
||||
// public methods
|
||||
//
|
||||
|
||||
bool TinyGPS::encode(char c)
|
||||
{
|
||||
bool valid_sentence = false;
|
||||
|
||||
#ifndef _GPS_NO_STATS
|
||||
++_encoded_characters;
|
||||
#endif
|
||||
switch(c)
|
||||
{
|
||||
case ',': // term terminators
|
||||
_parity ^= c;
|
||||
case '\r':
|
||||
case '\n':
|
||||
case '*':
|
||||
if (_term_offset < sizeof(_term))
|
||||
{
|
||||
_term[_term_offset] = 0;
|
||||
valid_sentence = term_complete();
|
||||
}
|
||||
++_term_number;
|
||||
_term_offset = 0;
|
||||
_is_checksum_term = c == '*';
|
||||
return valid_sentence;
|
||||
|
||||
case '$': // sentence begin
|
||||
_term_number = _term_offset = 0;
|
||||
_parity = 0;
|
||||
_sentence_type = _GPS_SENTENCE_OTHER;
|
||||
_is_checksum_term = false;
|
||||
_gps_data_good = false;
|
||||
return valid_sentence;
|
||||
}
|
||||
|
||||
// ordinary characters
|
||||
if (_term_offset < sizeof(_term) - 1)
|
||||
_term[_term_offset++] = c;
|
||||
if (!_is_checksum_term)
|
||||
_parity ^= c;
|
||||
|
||||
return valid_sentence;
|
||||
}
|
||||
|
||||
#ifndef _GPS_NO_STATS
|
||||
void TinyGPS::stats(unsigned long *chars, unsigned short *sentences, unsigned short *failed_cs)
|
||||
{
|
||||
if (chars) *chars = _encoded_characters;
|
||||
if (sentences) *sentences = _good_sentences;
|
||||
if (failed_cs) *failed_cs = _failed_checksum;
|
||||
}
|
||||
#endif
|
||||
|
||||
//
|
||||
// internal utilities
|
||||
//
|
||||
int TinyGPS::from_hex(char a)
|
||||
{
|
||||
if (a >= 'A' && a <= 'F')
|
||||
return a - 'A' + 10;
|
||||
else if (a >= 'a' && a <= 'f')
|
||||
return a - 'a' + 10;
|
||||
else
|
||||
return a - '0';
|
||||
}
|
||||
|
||||
unsigned long TinyGPS::parse_decimal()
|
||||
{
|
||||
char *p = _term;
|
||||
bool isneg = *p == '-';
|
||||
if (isneg) ++p;
|
||||
unsigned long ret = 100UL * gpsatol(p);
|
||||
while (gpsisdigit(*p)) ++p;
|
||||
if (*p == '.')
|
||||
{
|
||||
if (gpsisdigit(p[1]))
|
||||
{
|
||||
ret += 10 * (p[1] - '0');
|
||||
if (gpsisdigit(p[2]))
|
||||
ret += p[2] - '0';
|
||||
}
|
||||
}
|
||||
return isneg ? -ret : ret;
|
||||
}
|
||||
|
||||
unsigned long TinyGPS::parse_degrees()
|
||||
{
|
||||
char *p;
|
||||
unsigned long left = gpsatol(_term);
|
||||
unsigned long tenk_minutes = (left % 100UL) * 10000UL;
|
||||
for (p=_term; gpsisdigit(*p); ++p);
|
||||
if (*p == '.')
|
||||
{
|
||||
unsigned long mult = 1000;
|
||||
while (gpsisdigit(*++p))
|
||||
{
|
||||
tenk_minutes += mult * (*p - '0');
|
||||
mult /= 10;
|
||||
}
|
||||
}
|
||||
return (left / 100) * 100000 + tenk_minutes / 6;
|
||||
}
|
||||
|
||||
#define COMBINE(sentence_type, term_number) (((unsigned)(sentence_type) << 5) | term_number)
|
||||
|
||||
// Processes a just-completed term
|
||||
// Returns true if new sentence has just passed checksum test and is validated
|
||||
bool TinyGPS::term_complete()
|
||||
{
|
||||
if (_is_checksum_term)
|
||||
{
|
||||
byte checksum = 16 * from_hex(_term[0]) + from_hex(_term[1]);
|
||||
if (checksum == _parity)
|
||||
{
|
||||
if (_gps_data_good)
|
||||
{
|
||||
#ifndef _GPS_NO_STATS
|
||||
++_good_sentences;
|
||||
#endif
|
||||
_last_time_fix = _new_time_fix;
|
||||
_last_position_fix = _new_position_fix;
|
||||
|
||||
switch(_sentence_type)
|
||||
{
|
||||
case _GPS_SENTENCE_GPRMC:
|
||||
_time = _new_time;
|
||||
_date = _new_date;
|
||||
_latitude = _new_latitude;
|
||||
_longitude = _new_longitude;
|
||||
_speed = _new_speed;
|
||||
_course = _new_course;
|
||||
break;
|
||||
case _GPS_SENTENCE_GPGGA:
|
||||
_altitude = _new_altitude;
|
||||
_time = _new_time;
|
||||
_latitude = _new_latitude;
|
||||
_longitude = _new_longitude;
|
||||
_numsats = _new_numsats;
|
||||
_hdop = _new_hdop;
|
||||
break;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
#ifndef _GPS_NO_STATS
|
||||
else
|
||||
++_failed_checksum;
|
||||
#endif
|
||||
return false;
|
||||
}
|
||||
|
||||
// the first term determines the sentence type
|
||||
if (_term_number == 0)
|
||||
{
|
||||
if (!gpsstrcmp(_term, _GPRMC_TERM))
|
||||
_sentence_type = _GPS_SENTENCE_GPRMC;
|
||||
else if (!gpsstrcmp(_term, _GPGGA_TERM))
|
||||
_sentence_type = _GPS_SENTENCE_GPGGA;
|
||||
else
|
||||
_sentence_type = _GPS_SENTENCE_OTHER;
|
||||
return false;
|
||||
}
|
||||
|
||||
if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0])
|
||||
switch(COMBINE(_sentence_type, _term_number))
|
||||
{
|
||||
case COMBINE(_GPS_SENTENCE_GPRMC, 1): // Time in both sentences
|
||||
case COMBINE(_GPS_SENTENCE_GPGGA, 1):
|
||||
_new_time = parse_decimal();
|
||||
_new_time_fix = millis();
|
||||
break;
|
||||
case COMBINE(_GPS_SENTENCE_GPRMC, 2): // GPRMC validity
|
||||
_gps_data_good = _term[0] == 'A';
|
||||
break;
|
||||
case COMBINE(_GPS_SENTENCE_GPRMC, 3): // Latitude
|
||||
case COMBINE(_GPS_SENTENCE_GPGGA, 2):
|
||||
_new_latitude = parse_degrees();
|
||||
_new_position_fix = millis();
|
||||
break;
|
||||
case COMBINE(_GPS_SENTENCE_GPRMC, 4): // N/S
|
||||
case COMBINE(_GPS_SENTENCE_GPGGA, 3):
|
||||
if (_term[0] == 'S')
|
||||
_new_latitude = -_new_latitude;
|
||||
break;
|
||||
case COMBINE(_GPS_SENTENCE_GPRMC, 5): // Longitude
|
||||
case COMBINE(_GPS_SENTENCE_GPGGA, 4):
|
||||
_new_longitude = parse_degrees();
|
||||
break;
|
||||
case COMBINE(_GPS_SENTENCE_GPRMC, 6): // E/W
|
||||
case COMBINE(_GPS_SENTENCE_GPGGA, 5):
|
||||
if (_term[0] == 'W')
|
||||
_new_longitude = -_new_longitude;
|
||||
break;
|
||||
case COMBINE(_GPS_SENTENCE_GPRMC, 7): // Speed (GPRMC)
|
||||
_new_speed = parse_decimal();
|
||||
break;
|
||||
case COMBINE(_GPS_SENTENCE_GPRMC, 8): // Course (GPRMC)
|
||||
_new_course = parse_decimal();
|
||||
break;
|
||||
case COMBINE(_GPS_SENTENCE_GPRMC, 9): // Date (GPRMC)
|
||||
_new_date = gpsatol(_term);
|
||||
break;
|
||||
case COMBINE(_GPS_SENTENCE_GPGGA, 6): // Fix data (GPGGA)
|
||||
_gps_data_good = _term[0] > '0';
|
||||
break;
|
||||
case COMBINE(_GPS_SENTENCE_GPGGA, 7): // Satellites used (GPGGA)
|
||||
_new_numsats = (unsigned char)atoi(_term);
|
||||
break;
|
||||
case COMBINE(_GPS_SENTENCE_GPGGA, 8): // HDOP
|
||||
_new_hdop = parse_decimal();
|
||||
break;
|
||||
case COMBINE(_GPS_SENTENCE_GPGGA, 9): // Altitude (GPGGA)
|
||||
_new_altitude = parse_decimal();
|
||||
break;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
long TinyGPS::gpsatol(const char *str)
|
||||
{
|
||||
long ret = 0;
|
||||
while (gpsisdigit(*str))
|
||||
ret = 10 * ret + *str++ - '0';
|
||||
return ret;
|
||||
}
|
||||
|
||||
int TinyGPS::gpsstrcmp(const char *str1, const char *str2)
|
||||
{
|
||||
while (*str1 && *str1 == *str2)
|
||||
++str1, ++str2;
|
||||
return *str1;
|
||||
}
|
||||
|
||||
/* static */
|
||||
float TinyGPS::distance_between (float lat1, float long1, float lat2, float long2)
|
||||
{
|
||||
// returns distance in meters between two positions, both specified
|
||||
// as signed decimal-degrees latitude and longitude. Uses great-circle
|
||||
// distance computation for hypothetical sphere of radius 6372795 meters.
|
||||
// Because Earth is no exact sphere, rounding errors may be up to 0.5%.
|
||||
// Courtesy of Maarten Lamers
|
||||
float delta = radians(long1-long2);
|
||||
float sdlong = sin(delta);
|
||||
float cdlong = cos(delta);
|
||||
lat1 = radians(lat1);
|
||||
lat2 = radians(lat2);
|
||||
float slat1 = sin(lat1);
|
||||
float clat1 = cos(lat1);
|
||||
float slat2 = sin(lat2);
|
||||
float clat2 = cos(lat2);
|
||||
delta = (clat1 * slat2) - (slat1 * clat2 * cdlong);
|
||||
delta = sq(delta);
|
||||
delta += sq(clat2 * sdlong);
|
||||
delta = sqrt(delta);
|
||||
float denom = (slat1 * slat2) + (clat1 * clat2 * cdlong);
|
||||
delta = atan2(delta, denom);
|
||||
return delta * 6372795;
|
||||
}
|
||||
|
||||
float TinyGPS::course_to (float lat1, float long1, float lat2, float long2)
|
||||
{
|
||||
// returns course in degrees (North=0, West=270) from position 1 to position 2,
|
||||
// both specified as signed decimal-degrees latitude and longitude.
|
||||
// Because Earth is no exact sphere, calculated course may be off by a tiny fraction.
|
||||
// Courtesy of Maarten Lamers
|
||||
float dlon = radians(long2-long1);
|
||||
lat1 = radians(lat1);
|
||||
lat2 = radians(lat2);
|
||||
float a1 = sin(dlon) * cos(lat2);
|
||||
float a2 = sin(lat1) * cos(lat2) * cos(dlon);
|
||||
a2 = cos(lat1) * sin(lat2) - a2;
|
||||
a2 = atan2(a1, a2);
|
||||
if (a2 < 0.0)
|
||||
{
|
||||
a2 += TWO_PI;
|
||||
}
|
||||
return degrees(a2);
|
||||
}
|
||||
|
||||
const char *TinyGPS::cardinal (float course)
|
||||
{
|
||||
static const char* directions[] = {"N", "NNE", "NE", "ENE", "E", "ESE", "SE", "SSE", "S", "SSW", "SW", "WSW", "W", "WNW", "NW", "NNW"};
|
||||
|
||||
int direction = (int)((course + 11.25f) / 22.5f);
|
||||
return directions[direction % 16];
|
||||
}
|
||||
|
||||
// lat/long in hundred thousandths of a degree and age of fix in milliseconds
|
||||
void TinyGPS::get_position(long *latitude, long *longitude, unsigned long *fix_age)
|
||||
{
|
||||
if (latitude) *latitude = _latitude;
|
||||
if (longitude) *longitude = _longitude;
|
||||
if (fix_age) *fix_age = _last_position_fix == GPS_INVALID_FIX_TIME ?
|
||||
GPS_INVALID_AGE : millis() - _last_position_fix;
|
||||
}
|
||||
|
||||
// date as ddmmyy, time as hhmmsscc, and age in milliseconds
|
||||
void TinyGPS::get_datetime(unsigned long *date, unsigned long *time, unsigned long *age)
|
||||
{
|
||||
if (date) *date = _date;
|
||||
if (time) *time = _time;
|
||||
if (age) *age = _last_time_fix == GPS_INVALID_FIX_TIME ?
|
||||
GPS_INVALID_AGE : millis() - _last_time_fix;
|
||||
}
|
||||
|
||||
void TinyGPS::f_get_position(float *latitude, float *longitude, unsigned long *fix_age)
|
||||
{
|
||||
long lat, lon;
|
||||
get_position(&lat, &lon, fix_age);
|
||||
*latitude = lat == GPS_INVALID_ANGLE ? GPS_INVALID_F_ANGLE : (lat / 100000.0);
|
||||
*longitude = lat == GPS_INVALID_ANGLE ? GPS_INVALID_F_ANGLE : (lon / 100000.0);
|
||||
}
|
||||
|
||||
void TinyGPS::crack_datetime(int *year, byte *month, byte *day,
|
||||
byte *hour, byte *minute, byte *second, byte *hundredths, unsigned long *age)
|
||||
{
|
||||
unsigned long date, time;
|
||||
get_datetime(&date, &time, age);
|
||||
if (year)
|
||||
{
|
||||
*year = date % 100;
|
||||
*year += *year > 80 ? 1900 : 2000;
|
||||
}
|
||||
if (month) *month = (date / 100) % 100;
|
||||
if (day) *day = date / 10000;
|
||||
if (hour) *hour = time / 1000000;
|
||||
if (minute) *minute = (time / 10000) % 100;
|
||||
if (second) *second = (time / 100) % 100;
|
||||
if (hundredths) *hundredths = time % 100;
|
||||
}
|
||||
|
||||
float TinyGPS::f_altitude()
|
||||
{
|
||||
return _altitude == GPS_INVALID_ALTITUDE ? GPS_INVALID_F_ALTITUDE : _altitude / 100.0;
|
||||
}
|
||||
|
||||
float TinyGPS::f_course()
|
||||
{
|
||||
return _course == GPS_INVALID_ANGLE ? GPS_INVALID_F_ANGLE : _course / 100.0;
|
||||
}
|
||||
|
||||
float TinyGPS::f_speed_knots()
|
||||
{
|
||||
return _speed == GPS_INVALID_SPEED ? GPS_INVALID_F_SPEED : _speed / 100.0;
|
||||
}
|
||||
|
||||
float TinyGPS::f_speed_mph()
|
||||
{
|
||||
float sk = f_speed_knots();
|
||||
return sk == GPS_INVALID_F_SPEED ? GPS_INVALID_F_SPEED : _GPS_MPH_PER_KNOT * f_speed_knots();
|
||||
}
|
||||
|
||||
float TinyGPS::f_speed_mps()
|
||||
{
|
||||
float sk = f_speed_knots();
|
||||
return sk == GPS_INVALID_F_SPEED ? GPS_INVALID_F_SPEED : _GPS_MPS_PER_KNOT * f_speed_knots();
|
||||
}
|
||||
|
||||
float TinyGPS::f_speed_kmph()
|
||||
{
|
||||
float sk = f_speed_knots();
|
||||
return sk == GPS_INVALID_F_SPEED ? GPS_INVALID_F_SPEED : _GPS_KMPH_PER_KNOT * f_speed_knots();
|
||||
}
|
||||
|
||||
const float TinyGPS::GPS_INVALID_F_ANGLE = 1000.0;
|
||||
const float TinyGPS::GPS_INVALID_F_ALTITUDE = 1000000.0;
|
||||
const float TinyGPS::GPS_INVALID_F_SPEED = -1.0;
|
||||
@@ -0,0 +1,153 @@
|
||||
/*
|
||||
TinyGPS - a small GPS library for Arduino providing basic NMEA parsing
|
||||
Based on work by and "distance_to" and "course_to" courtesy of Maarten Lamers.
|
||||
Suggestion to add satellites(), course_to(), and cardinal(), by Matt Monson.
|
||||
Copyright (C) 2008-2012 Mikal Hart
|
||||
All rights reserved.
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
This library is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
Lesser General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU Lesser General Public
|
||||
License along with this library; if not, write to the Free Software
|
||||
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*/
|
||||
|
||||
#ifndef TinyGPS_h
|
||||
#define TinyGPS_h
|
||||
|
||||
#if defined(ARDUINO) && ARDUINO >= 100
|
||||
#include "Arduino.h"
|
||||
#else
|
||||
#include "WProgram.h"
|
||||
#endif
|
||||
|
||||
#define _GPS_VERSION 12 // software version of this library
|
||||
#define _GPS_MPH_PER_KNOT 1.15077945
|
||||
#define _GPS_MPS_PER_KNOT 0.51444444
|
||||
#define _GPS_KMPH_PER_KNOT 1.852
|
||||
#define _GPS_MILES_PER_METER 0.00062137112
|
||||
#define _GPS_KM_PER_METER 0.001
|
||||
// #define _GPS_NO_STATS
|
||||
|
||||
class TinyGPS
|
||||
{
|
||||
public:
|
||||
enum {
|
||||
GPS_INVALID_AGE = 0xFFFFFFFF, GPS_INVALID_ANGLE = 999999999,
|
||||
GPS_INVALID_ALTITUDE = 999999999, GPS_INVALID_DATE = 0,
|
||||
GPS_INVALID_TIME = 0xFFFFFFFF, GPS_INVALID_SPEED = 999999999,
|
||||
GPS_INVALID_FIX_TIME = 0xFFFFFFFF, GPS_INVALID_SATELLITES = 0xFF,
|
||||
GPS_INVALID_HDOP = 0xFFFFFFFF
|
||||
};
|
||||
|
||||
static const float GPS_INVALID_F_ANGLE, GPS_INVALID_F_ALTITUDE, GPS_INVALID_F_SPEED;
|
||||
|
||||
TinyGPS();
|
||||
bool encode(char c); // process one character received from GPS
|
||||
TinyGPS &operator << (char c) {encode(c); return *this;}
|
||||
|
||||
// lat/long in hundred thousandths of a degree and age of fix in milliseconds
|
||||
void get_position(long *latitude, long *longitude, unsigned long *fix_age = 0);
|
||||
|
||||
// date as ddmmyy, time as hhmmsscc, and age in milliseconds
|
||||
void get_datetime(unsigned long *date, unsigned long *time, unsigned long *age = 0);
|
||||
|
||||
// signed altitude in centimeters (from GPGGA sentence)
|
||||
inline long altitude() { return _altitude; }
|
||||
|
||||
// course in last full GPRMC sentence in 100th of a degree
|
||||
inline unsigned long course() { return _course; }
|
||||
|
||||
// speed in last full GPRMC sentence in 100ths of a knot
|
||||
inline unsigned long speed() { return _speed; }
|
||||
|
||||
// satellites used in last full GPGGA sentence
|
||||
inline unsigned short satellites() { return _numsats; }
|
||||
|
||||
// horizontal dilution of precision in 100ths
|
||||
inline unsigned long hdop() { return _hdop; }
|
||||
|
||||
void f_get_position(float *latitude, float *longitude, unsigned long *fix_age = 0);
|
||||
void crack_datetime(int *year, byte *month, byte *day,
|
||||
byte *hour, byte *minute, byte *second, byte *hundredths = 0, unsigned long *fix_age = 0);
|
||||
float f_altitude();
|
||||
float f_course();
|
||||
float f_speed_knots();
|
||||
float f_speed_mph();
|
||||
float f_speed_mps();
|
||||
float f_speed_kmph();
|
||||
|
||||
static int library_version() { return _GPS_VERSION; }
|
||||
|
||||
static float distance_between (float lat1, float long1, float lat2, float long2);
|
||||
static float course_to (float lat1, float long1, float lat2, float long2);
|
||||
static const char *cardinal(float course);
|
||||
|
||||
#ifndef _GPS_NO_STATS
|
||||
void stats(unsigned long *chars, unsigned short *good_sentences, unsigned short *failed_cs);
|
||||
#endif
|
||||
|
||||
private:
|
||||
enum {_GPS_SENTENCE_GPGGA, _GPS_SENTENCE_GPRMC, _GPS_SENTENCE_OTHER};
|
||||
|
||||
// properties
|
||||
unsigned long _time, _new_time;
|
||||
unsigned long _date, _new_date;
|
||||
long _latitude, _new_latitude;
|
||||
long _longitude, _new_longitude;
|
||||
long _altitude, _new_altitude;
|
||||
unsigned long _speed, _new_speed;
|
||||
unsigned long _course, _new_course;
|
||||
unsigned long _hdop, _new_hdop;
|
||||
unsigned short _numsats, _new_numsats;
|
||||
|
||||
unsigned long _last_time_fix, _new_time_fix;
|
||||
unsigned long _last_position_fix, _new_position_fix;
|
||||
|
||||
// parsing state variables
|
||||
byte _parity;
|
||||
bool _is_checksum_term;
|
||||
char _term[15];
|
||||
byte _sentence_type;
|
||||
byte _term_number;
|
||||
byte _term_offset;
|
||||
bool _gps_data_good;
|
||||
|
||||
#ifndef _GPS_NO_STATS
|
||||
// statistics
|
||||
unsigned long _encoded_characters;
|
||||
unsigned short _good_sentences;
|
||||
unsigned short _failed_checksum;
|
||||
unsigned short _passed_checksum;
|
||||
#endif
|
||||
|
||||
// internal utilities
|
||||
int from_hex(char a);
|
||||
unsigned long parse_decimal();
|
||||
unsigned long parse_degrees();
|
||||
bool term_complete();
|
||||
bool gpsisdigit(char c) { return c >= '0' && c <= '9'; }
|
||||
long gpsatol(const char *str);
|
||||
int gpsstrcmp(const char *str1, const char *str2);
|
||||
};
|
||||
|
||||
#if !defined(ARDUINO)
|
||||
// Arduino 0012 workaround
|
||||
#undef int
|
||||
#undef char
|
||||
#undef long
|
||||
#undef byte
|
||||
#undef float
|
||||
#undef abs
|
||||
#undef round
|
||||
#endif
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,190 @@
|
||||
#ifndef WiiChuck_h
|
||||
#define WiiChuck_h
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "Wire.h"
|
||||
#include "math.h"
|
||||
|
||||
|
||||
// these may need to be adjusted for each nunchuck for calibration
|
||||
#define ZEROX 510
|
||||
#define ZEROY 490
|
||||
#define ZEROZ 460
|
||||
#define RADIUS 210 // probably pretty universal
|
||||
|
||||
#define DEFAULT_ZERO_JOY_X 124
|
||||
#define DEFAULT_ZERO_JOY_Y 132
|
||||
|
||||
#define MAXANGLE 90
|
||||
#define MINANGLE -90
|
||||
|
||||
class WiiChuck {
|
||||
private:
|
||||
byte cnt;
|
||||
uint8_t status[6]; // array to store wiichuck output
|
||||
byte averageCounter;
|
||||
int i;
|
||||
int total;
|
||||
uint8_t zeroJoyX; // these are about where mine are
|
||||
uint8_t zeroJoyY; // use calibrateJoy when the stick is at zero to correct
|
||||
int lastJoyX;
|
||||
int lastJoyY;
|
||||
int angles[3];
|
||||
|
||||
boolean lastZ, lastC;
|
||||
|
||||
|
||||
public:
|
||||
|
||||
byte joyX;
|
||||
byte joyY;
|
||||
boolean buttonZ;
|
||||
boolean buttonC;
|
||||
boolean plugd;
|
||||
void begin()
|
||||
{
|
||||
Wire.begin();
|
||||
cnt = 0;
|
||||
averageCounter = 0;
|
||||
Wire.beginTransmission (0x52); // transmit to device 0x52
|
||||
Wire.write((byte)0x40); // sends memory address
|
||||
Wire.write((byte)0x00); // sends memory address
|
||||
Wire.endTransmission (); // stop transmitting
|
||||
update();
|
||||
for (i = 0; i<3;i++) {
|
||||
angles[i] = 0;
|
||||
}
|
||||
zeroJoyX = DEFAULT_ZERO_JOY_X;
|
||||
zeroJoyY = DEFAULT_ZERO_JOY_Y;
|
||||
}
|
||||
|
||||
|
||||
void calibrateJoy() {
|
||||
zeroJoyX = joyX;
|
||||
zeroJoyY = joyY;
|
||||
}
|
||||
|
||||
void CheckifPlugd()
|
||||
{
|
||||
if (this->plugd == false){
|
||||
this->begin();
|
||||
|
||||
#ifdef GLCD
|
||||
GLCD.CursorTo(1, 1);
|
||||
GLCD.print("No Chuck");
|
||||
delay(100);
|
||||
#endif
|
||||
|
||||
this->begin();
|
||||
if (this->plugd)
|
||||
this->calibrateJoy();
|
||||
}
|
||||
}
|
||||
|
||||
void update() {
|
||||
|
||||
Wire.requestFrom (0x52, 6); // request data from nunchuck
|
||||
while (Wire.available ()) {
|
||||
// receive byte as an integer
|
||||
status[cnt] = _nunchuk_decode_byte((byte)Wire.read()); //
|
||||
cnt++;
|
||||
}
|
||||
if (cnt > 5) {
|
||||
lastZ = buttonZ;
|
||||
lastC = buttonC;
|
||||
lastJoyX = readJoyX();
|
||||
lastJoyY = readJoyY();
|
||||
|
||||
cnt = 0;
|
||||
joyX = (status[0]);
|
||||
joyY = (status[1]);
|
||||
for (i = 0; i < 3; i++)
|
||||
angles[i] = (status[i+2] << 2) + ((status[5] & (B00000011 << ((i+1)*2) ) >> ((i+1)*2)));
|
||||
|
||||
buttonZ = !( status[5] & B00000001);
|
||||
buttonC = !((status[5] & B00000010) >> 1);
|
||||
_send_zero(); // send the request for next bytes
|
||||
plugd = true;
|
||||
}
|
||||
else if (cnt == 0)
|
||||
plugd = false;
|
||||
}
|
||||
|
||||
float readAccelX() {
|
||||
return (float)angles[0] - ZEROX;
|
||||
}
|
||||
float readAccelY() {
|
||||
return (float)angles[1] - ZEROY;
|
||||
}
|
||||
float readAccelZ() {
|
||||
return (float)angles[2] - ZEROZ;
|
||||
}
|
||||
|
||||
boolean zPressed() {
|
||||
return (buttonZ && ! lastZ);
|
||||
}
|
||||
boolean cPressed() {
|
||||
return (buttonC && ! lastC);
|
||||
}
|
||||
|
||||
// for using the joystick like a directional button
|
||||
boolean rightJoy(int thresh=60) {
|
||||
return (readJoyX() > thresh and lastJoyX <= thresh);
|
||||
}
|
||||
|
||||
// for using the joystick like a directional button
|
||||
boolean leftJoy(int thresh=60) {
|
||||
return (readJoyX() < -thresh and lastJoyX >= -thresh);
|
||||
}
|
||||
|
||||
|
||||
int readJoyX() {
|
||||
return (int) joyX - zeroJoyX;
|
||||
}
|
||||
|
||||
int readJoyY() {
|
||||
return (int)joyY - zeroJoyY;
|
||||
}
|
||||
|
||||
|
||||
// R, the radius, generally hovers around 210 (at least it does with mine)
|
||||
int R() {
|
||||
return sqrt(readAccelX() * readAccelX() +readAccelY() * readAccelY() + readAccelZ() * readAccelZ());
|
||||
}
|
||||
|
||||
|
||||
// returns roll degrees
|
||||
int readRoll() {
|
||||
return (int)(atan2(readAccelX(),readAccelZ())/ M_PI * 180.0);
|
||||
}
|
||||
|
||||
// returns pitch in degrees
|
||||
int readPitch() {
|
||||
return (int) (acos(readAccelY()/RADIUS)/ M_PI * 180.0); // optionally swap 'RADIUS' for 'R()'
|
||||
}
|
||||
|
||||
private:
|
||||
byte _nunchuk_decode_byte (byte x)
|
||||
{
|
||||
x = (x ^ 0x17) + 0x17;
|
||||
return x;
|
||||
}
|
||||
|
||||
void _send_zero()
|
||||
{
|
||||
Wire.beginTransmission (0x52); // transmit to device 0x52
|
||||
Wire.write ((byte)0x00); // sends one byte
|
||||
Wire.endTransmission (); // stop transmitting
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,62 @@
|
||||
class gedrag{
|
||||
|
||||
private:
|
||||
|
||||
data_line PositieInformatie;
|
||||
body_struct main_positie;
|
||||
int HuidigePositieinProgramma;
|
||||
int matrix[..] MatrixmetdeGedragPatroonen;
|
||||
|
||||
public:
|
||||
|
||||
run()
|
||||
{
|
||||
/*
|
||||
run normaal gedrag patroon.
|
||||
|
||||
[loop patroon dat niet word beinvloed door userinput]
|
||||
vraag aankomende gedrag op uit matrix,
|
||||
-bereken de nieuwe posities
|
||||
--sla deze op in cache
|
||||
---wacht tot de watchdog een timeout geeft
|
||||
----geef de cache door aan de servo controller.
|
||||
|
||||
[loop patroon dat WORD beinvloed door userinput]
|
||||
vraag aankomende gedrag op uit matrix met userinput,
|
||||
-bereken de nieuwe posities
|
||||
--sla deze op in cache
|
||||
---loop tot de watchdog een timeout geeft
|
||||
----geef de cache door aan de servo controller.
|
||||
*/
|
||||
};
|
||||
|
||||
quite()
|
||||
{
|
||||
while()
|
||||
{
|
||||
/*
|
||||
maak routine af om terug te gaan naar de main positie.
|
||||
daarna return 0;
|
||||
|
||||
verplaats de body in een neutrale positie
|
||||
+als niet mogeleijk verplaats de poten die problemen geven
|
||||
-mogelijk ga verder met de poten een voor een in de goode positie te zetten
|
||||
*/
|
||||
}
|
||||
};
|
||||
|
||||
exit()
|
||||
{
|
||||
/*
|
||||
NOOD, stop nu en return positie;
|
||||
*/
|
||||
};
|
||||
|
||||
userinput()
|
||||
{
|
||||
/*
|
||||
Mogelijkheid tot user input.
|
||||
*/
|
||||
};
|
||||
|
||||
};
|
||||
@@ -0,0 +1,105 @@
|
||||
// hexapod.cpp
|
||||
|
||||
#ifndef HEX
|
||||
#define HEX
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "hexapod.h"
|
||||
|
||||
void hexapod::FireFly()
|
||||
{
|
||||
Serial.println("");
|
||||
Serial.println(" ____ ____ ___ ");
|
||||
Serial.println("/\\ _`\\ __ /\\ _`\\ /\\_ \\ ");
|
||||
Serial.println("\\ \\ \\__ /\\_\\ _ __ __\\ \\ \\__ //\\ \\ __ __ ");
|
||||
Serial.println(" \\ \\ _\\ \\/\\ \\/\\`'__\\/'__`\\ \\ _\\ \\ \\ \\ /\\ \\/\\ \\ ");
|
||||
Serial.println(" \\ \\ \\/ \\ \\ \\ \\ \\//\\ __/\\ \\ \\/ \\_\\ \\_\\ \\ \\_\\ \\ ");
|
||||
Serial.println(" \\ \\_\\ \\ \\_\\ \\_\\\\ \\____\\\\ \\_\\ /\\____\\\\/`____ \\ ");
|
||||
Serial.println(" \\/_/ \\/_/\\/_/ \\/____/ \\/_/ \\/____/ `/___/> \\");
|
||||
Serial.println(" /\\___/");
|
||||
Serial.println(" \\/__/");
|
||||
Serial.println("");
|
||||
Serial.println("Version 3.1");
|
||||
delay(800);
|
||||
irrecv = new IRrecv((int)5);
|
||||
irrecv->enableIRIn();
|
||||
}
|
||||
|
||||
int hexapod::GetNewCmd()
|
||||
{
|
||||
if (irrecv->decode(&results)) {
|
||||
if (results.value != old.value){
|
||||
if (results.decode_type == RC6) {
|
||||
#ifdef DEBUG
|
||||
Serial.print("### GetNewCmd->Decoded RC6: ");
|
||||
Serial.println(results.value, HEX);
|
||||
#endif
|
||||
old.value = results.value;
|
||||
rr = false;
|
||||
s = true;
|
||||
}
|
||||
else if (rr != true)
|
||||
{
|
||||
#ifdef DEBUG
|
||||
Serial.println("### GetNewCmd->Not RC6");
|
||||
#endif
|
||||
rr = true;
|
||||
}
|
||||
}
|
||||
irrecv->resume(); // Receive the next value
|
||||
}
|
||||
|
||||
if (s)
|
||||
{
|
||||
s = false;
|
||||
return results.value;
|
||||
}
|
||||
else return 0;
|
||||
}
|
||||
|
||||
float microsecondsToCentimeters(float microseconds)
|
||||
{
|
||||
// The speed of sound is 340 m/s or 29 microseconds per centimeter.
|
||||
// The ping travels out and back, so to find the distance of the
|
||||
// object we take half of the distance travelled.
|
||||
return microseconds / 29 / 2;
|
||||
}
|
||||
|
||||
float hexapod::GetUltraSoneDistance()
|
||||
{
|
||||
float duration, cm;
|
||||
|
||||
pinMode(PingS, OUTPUT);
|
||||
digitalWrite(PingS, LOW);
|
||||
delayMicroseconds(2);
|
||||
digitalWrite(PingS, HIGH);
|
||||
delayMicroseconds(5);
|
||||
digitalWrite(PingS, LOW);
|
||||
|
||||
pinMode(PingR, INPUT);
|
||||
duration = pulseIn(PingR, HIGH);
|
||||
|
||||
// convert the time into a distance
|
||||
cm = microsecondsToCentimeters(duration);
|
||||
|
||||
return cm;
|
||||
}
|
||||
|
||||
void hexapod::CoreSend(DataObject *Input)
|
||||
{
|
||||
//Init
|
||||
Converter Con;
|
||||
InverseKinemetics IK;
|
||||
Servos SSC32;
|
||||
Data *DataLine = new Data;
|
||||
|
||||
//Main
|
||||
DataLine->input(Input);
|
||||
DataLine->WalkTime = 1000;
|
||||
|
||||
IK.IK(DataLine);
|
||||
Con.Convert(DataLine);
|
||||
SSC32.Send(DataLine);
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,65 @@
|
||||
// hexapod.h
|
||||
|
||||
#ifndef HEXAPOD_H
|
||||
#define HEXAPOD_H
|
||||
|
||||
#include "Arduino.h"
|
||||
|
||||
//Core
|
||||
#include "Converter.h"
|
||||
#include "InverseKinemetics.h"
|
||||
#include "Servos.h"
|
||||
#include "Data.h"
|
||||
#include "math.h"
|
||||
#include "SerD.h"
|
||||
//#include "Gedrags_Classen.h"
|
||||
|
||||
//Addons
|
||||
#include "Segment.h"
|
||||
#include "IRremote.h"
|
||||
#include "TinyGPS.h"
|
||||
//#include "WiiChuck.h"
|
||||
//#include "Wire.h"
|
||||
|
||||
#define PingS 3
|
||||
#define PingR 4
|
||||
#define Gps Serial2
|
||||
|
||||
#define power 1036
|
||||
#define up 1112
|
||||
#define down 1113
|
||||
#define left 1114
|
||||
#define right 1115
|
||||
#define ok 1116
|
||||
#define forward 1056
|
||||
#define back 1057
|
||||
#define IR_0 1024
|
||||
#define IR_1 1025
|
||||
#define IR_2 1026
|
||||
#define IR_3 1027
|
||||
#define IR_4 1028
|
||||
#define IR_5 1029
|
||||
#define IR_6 1030
|
||||
#define IR_7 1031
|
||||
#define IR_8 1032
|
||||
#define IR_9 1033
|
||||
|
||||
class hexapod
|
||||
{
|
||||
public:
|
||||
boolean rr, s;
|
||||
|
||||
IRrecv *irrecv;
|
||||
decode_results results, old;
|
||||
|
||||
void FireFly();
|
||||
|
||||
int GetNewCmd();
|
||||
|
||||
float GetUltraSoneDistance();
|
||||
|
||||
void CoreSend(DataObject *Input);
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,151 @@
|
||||
#######################################
|
||||
# Syntax Coloring Map Servo
|
||||
#######################################
|
||||
|
||||
#######################################
|
||||
# Datatypes (KEYWORD1)
|
||||
#######################################
|
||||
|
||||
WiiChuck KEYWORD1
|
||||
|
||||
#######################################
|
||||
# Methods and Functions (KEYWORD2)
|
||||
#######################################
|
||||
|
||||
update KEYWORD2
|
||||
calibrateJoy KEYWORD2
|
||||
zPressed KEYWORD2
|
||||
cPressed KEYWORD2
|
||||
readAccelY KEYWORD2
|
||||
readAccelX KEYWORD2
|
||||
readJoyY KEYWORD2
|
||||
readJoyX KEYWORD2
|
||||
|
||||
#######################################
|
||||
# Constants (LITERAL1)
|
||||
#######################################
|
||||
|
||||
PingS LITERAL1
|
||||
PingR LITERAL1
|
||||
Gps LITERAL1
|
||||
|
||||
#######################################
|
||||
# Syntax Coloring Map For IRremote
|
||||
#######################################
|
||||
|
||||
#######################################
|
||||
# Datatypes (KEYWORD1)
|
||||
#######################################
|
||||
|
||||
decode_results KEYWORD1
|
||||
IRrecv KEYWORD1
|
||||
IRsend KEYWORD1
|
||||
|
||||
#######################################
|
||||
# Methods and Functions (KEYWORD2)
|
||||
#######################################
|
||||
|
||||
blink13 KEYWORD2
|
||||
decode KEYWORD2
|
||||
enableIRIn KEYWORD2
|
||||
resume KEYWORD2
|
||||
enableIROut KEYWORD2
|
||||
sendNEC KEYWORD2
|
||||
sendSony KEYWORD2
|
||||
sendRaw KEYWORD2
|
||||
sendRC5 KEYWORD2
|
||||
sendRC6 KEYWORD2
|
||||
#
|
||||
#######################################
|
||||
# Constants (LITERAL1)
|
||||
#######################################
|
||||
|
||||
NEC LITERAL1
|
||||
SONY LITERAL1
|
||||
RC5 LITERAL1
|
||||
RC6 LITERAL1
|
||||
UNKNOWN LITERAL1
|
||||
REPEAT LITERAL1
|
||||
|
||||
#######################################
|
||||
# Syntax Coloring Map for TinyGPS
|
||||
#######################################
|
||||
|
||||
#######################################
|
||||
# Datatypes (KEYWORD1)
|
||||
#######################################
|
||||
|
||||
TinyGPS KEYWORD1
|
||||
|
||||
#######################################
|
||||
# Methods and Functions (KEYWORD2)
|
||||
#######################################
|
||||
|
||||
encode KEYWORD2
|
||||
get_position KEYWORD2
|
||||
get_datetime KEYWORD2
|
||||
altitude KEYWORD2
|
||||
speed KEYWORD2
|
||||
course KEYWORD2
|
||||
stats KEYWORD2
|
||||
f_get_position KEYWORD2
|
||||
crack_datetime KEYWORD2
|
||||
f_altitude KEYWORD2
|
||||
f_course KEYWORD2
|
||||
f_speed_knots KEYWORD2
|
||||
f_speed_mph KEYWORD2
|
||||
f_speed_mps KEYWORD2
|
||||
f_speed_kmph KEYWORD2
|
||||
library_version KEYWORD2
|
||||
distance_between KEYWORD2
|
||||
course_to KEYWORD2
|
||||
satellites KEYWORD2
|
||||
hdop KEYWORD2
|
||||
|
||||
#######################################
|
||||
# Constants (LITERAL1)
|
||||
#######################################
|
||||
|
||||
GPS_INVALID_AGE LITERAL1
|
||||
GPS_INVALID_ANGLE LITERAL1
|
||||
GPS_INVALID_ALTITUDE LITERAL1
|
||||
GPS_INVALID_DATE LITERAL1
|
||||
GPS_INVALID_TIME LITERAL1
|
||||
GPS_INVALID_HDOP LITERAL1
|
||||
GPS_INVALID_SATELLITES LITERAL1
|
||||
GPS_INVALID_F_ANGLE LITERAL1
|
||||
GPS_INVALID_F_ALTITUDE LITERAL1
|
||||
GPS_INVALID_F_SPEED LITERAL1
|
||||
|
||||
#######################################
|
||||
# Syntax Coloring Map For Wire
|
||||
#######################################
|
||||
|
||||
#######################################
|
||||
# Datatypes (KEYWORD1)
|
||||
#######################################
|
||||
|
||||
#######################################
|
||||
# Methods and Functions (KEYWORD2)
|
||||
#######################################
|
||||
|
||||
begin KEYWORD2
|
||||
beginTransmission KEYWORD2
|
||||
endTransmission KEYWORD2
|
||||
requestFrom KEYWORD2
|
||||
send KEYWORD2
|
||||
receive KEYWORD2
|
||||
onReceive KEYWORD2
|
||||
onRequest KEYWORD2
|
||||
|
||||
#######################################
|
||||
# Instances (KEYWORD2)
|
||||
#######################################
|
||||
|
||||
Wire KEYWORD2
|
||||
hexapod KEYWORD2
|
||||
|
||||
#######################################
|
||||
# Constants (LITERAL1)
|
||||
#######################################
|
||||
|
||||
Reference in new issue
Block a user