diff --git a/.gitignore b/.gitignore index e7388b9..1da8ce5 100644 --- a/.gitignore +++ b/.gitignore @@ -32,3 +32,4 @@ # Debug files *.dSYM/ +.DS_Store diff --git a/firmware/FlexiTimer2.cpp b/firmware/FlexiTimer2.cpp new file mode 100755 index 0000000..67cb733 --- /dev/null +++ b/firmware/FlexiTimer2.cpp @@ -0,0 +1,209 @@ +/* + FlexiTimer2.h - Using timer2 with a configurable resolution + Wim Leers + + Based on MsTimer2 + Javier Valencia + + History: + 16/Dec/2011 - Added Teensy/Teensy++ support (bperrybap) + note: teensy uses timer4 instead of timer2 + 25/April/10 - Based on MsTimer2 V0.5 (from 29/May/09) + + 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 "FlexiTimer2.h" + +unsigned long FlexiTimer2::time_units; +void (*FlexiTimer2::func)(); +volatile unsigned long FlexiTimer2::count; +volatile char FlexiTimer2::overflowing; +volatile unsigned int FlexiTimer2::tcnt2; + +void FlexiTimer2::set(unsigned long ms, void (*f)()) { + FlexiTimer2::set(ms, 0.001, f); +} + + +/** + * @param resolution + * 0.001 implies a 1 ms (1/1000s = 0.001s = 1ms) resolution. Therefore, + * 0.0005 implies a 0.5 ms (1/2000s) resolution. And so on. + */ +void FlexiTimer2::set(unsigned long units, double resolution, void (*f)()) { + float prescaler = 0.0; + + if (units == 0) + time_units = 1; + else + time_units = units; + + func = f; + +#if defined (__AVR_ATmega168__) || defined (__AVR_ATmega48__) || defined (__AVR_ATmega88__) || defined (__AVR_ATmega328P__) || defined (__AVR_ATmega1280__) || defined (__AVR_ATmega2560__) || defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__) + TIMSK2 &= ~(1<= 1000000UL) && (F_CPU <= 16000000UL)) { // prescaler set to 64 + TCCR2B |= (1< 16Mhz, prescaler set to 128 + TCCR2B |= ((1<= 1000000UL) && (F_CPU <= 16000000UL)) { // prescaler set to 64 + TCCR2 |= (1< 16Mhz, prescaler set to 128 + TCCR2 |= ((1<= 1000000UL) && (F_CPU <= 16000000UL)) { // prescaler set to 64 + TCCR2 |= ((1< 16Mhz, prescaler set to 256 + TCCR2 |= (1<= 16000000L) { + TCCR4B = (1<= 8000000L) { + TCCR4B = (1<= 4000000L) { + TCCR4B = (1<= 2000000L) { + TCCR4B = (1<= 1000000L) { + TCCR4B = (1<= 500000L) { + TCCR4B = (1<= time_units && !overflowing) { + overflowing = 1; + count = count - time_units; // subtract time_uints to catch missed overflows + // set to 0 if you don't want this. + (*func)(); + overflowing = 0; + } +} +#if defined (__AVR_ATmega32U4__) +ISR(TIMER4_OVF_vect) { +#else +ISR(TIMER2_OVF_vect) { +#endif +#if defined (__AVR_ATmega168__) || defined (__AVR_ATmega48__) || defined (__AVR_ATmega88__) || defined (__AVR_ATmega328P__) || defined (__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) || defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__) + TCNT2 = FlexiTimer2::tcnt2; +#elif defined (__AVR_ATmega128__) + TCNT2 = FlexiTimer2::tcnt2; +#elif defined (__AVR_ATmega8__) + TCNT2 = FlexiTimer2::tcnt2; +#elif defined (__AVR_ATmega32U4__) + // not necessary on 32u4's high speed timer4 +#endif + FlexiTimer2::_overflow(); +} + diff --git a/firmware/FlexiTimer2.h b/firmware/FlexiTimer2.h new file mode 100755 index 0000000..aadcb2e --- /dev/null +++ b/firmware/FlexiTimer2.h @@ -0,0 +1,25 @@ +#ifndef FlexiTimer2_h +#define FlexiTimer2_h + +#ifdef __AVR__ +#include +#else +#error FlexiTimer2 library only works on AVR architecture +#endif + + +namespace FlexiTimer2 { + extern unsigned long time_units; + extern void (*func)(); + extern volatile unsigned long count; + extern volatile char overflowing; + extern volatile unsigned int tcnt2; + + void set(unsigned long ms, void (*f)()); + void set(unsigned long units, double resolution, void (*f)()); + void start(); + void stop(); + void _overflow(); +} + +#endif diff --git a/firmware/GoBLE.cpp b/firmware/GoBLE.cpp new file mode 100755 index 0000000..d0f1d2e --- /dev/null +++ b/firmware/GoBLE.cpp @@ -0,0 +1,261 @@ +#include "GoBLE.h" + +_GoBLE Goble; + +/* + The following constants tell, for each accelerometer + axis, which values are returned when the axis measures + zero acceleration. +*/ + +_GoBLE::_GoBLE() { + initRecvDataPack(); + + _joystickX = 127; + _joystickY = 127; + for (int i = 0; i < MAXBUTTONID; i++) { + _button[i] = RELEASED; + } + + for (int i = 0; i < 20; i++) bleQueue.push(0x00); + for (int i = 0; i < 20; i++) bleQueue.pop(); + + +} + + +boolean _GoBLE::available() { + /* + function introduction: + push the new valid data to the data buffer package + throw away the invalid byte + parse the data package when the command length is matching the protocol + */ + + if (Serial.available()) bleDataReceiver(); +/* + if (DEBUGDATARAW) { + Serial.println("GoBLE availalbe -> new data package!"); + for (int i = 0; i < rDataPack.commandLength; i++) { + Serial.print(bleQueue.pop(), HEX); + } + Serial.println(); + } + + if (DEBUGPARSER) { + Serial.print("GoBLE availalbe -> bleQueue Counter: "); + Serial.print(bleQueue.count()); + Serial.println(); + } +*/ + if (rDataPack.commandFlag && bleQueue.count() == rDataPack.commandLength) { + + rDataPack.parseState = bleDataPackageParser(); + + if (rDataPack.parseState == PARSESUCCESS) { + updateJoystickVal(); + updateButtonState(); + return true; + } + } + + return false; +} + + +int _GoBLE::readJoystickX() { + return _joystickX; +} +int _GoBLE::readJoystickY() { + return _joystickY; +} + +boolean _GoBLE::readSwitchUp() { + return _button[SWITCH_UP]; +} + +boolean _GoBLE::readSwitchDown() { + return _button[SWITCH_DOWN]; +} + +boolean _GoBLE::readSwitchLeft() { + return _button[SWITCH_LEFT]; +} + +boolean _GoBLE::readSwitchRight() { + return _button[SWITCH_RIGHT]; +} + +boolean _GoBLE::readSwitchSelect() { + return _button[SWITCH_SELECT]; +} + +boolean _GoBLE::readSwitchStart() { + return _button[SWITCH_START]; +} + +// Private functions + +int _GoBLE::bleDataPackageParser() { + /* + 0x10 - Parse success + 0x11 - Wrong header charactors + 0x12 - Wrong button number + 0x13 - Check Sum Error + */ + byte calculateSum = 0; + + rDataPack.header1 = bleQueue.pop(), calculateSum += rDataPack.header1; + rDataPack.header2 = bleQueue.pop(), calculateSum += rDataPack.header2; + + if (rDataPack.header1 != DEFAULTHEADER1) return 0x11; + if (rDataPack.header2 != DEFAULTHEADER2) return 0x11; + + rDataPack.address = bleQueue.pop(), calculateSum += rDataPack.address; + + rDataPack.latestDigitalButtonNumber = rDataPack.digitalButtonNumber; + rDataPack.digitalButtonNumber = bleQueue.pop(), calculateSum += rDataPack.digitalButtonNumber; + + int digitalButtonLength = rDataPack.digitalButtonNumber; + + + if (DEBUGCHECKSUM) { + Serial.print("Parser -> digitalButtonLength: "); + Serial.println(digitalButtonLength); + } + if (digitalButtonLength > MAXBUTTONNUMBER) return 0x12; + + rDataPack.joystickPosition = bleQueue.pop(), calculateSum += rDataPack.joystickPosition; + + // read button data package - dynamic button payload length + for (int buttonPayloadPointer = 0; buttonPayloadPointer < digitalButtonLength; buttonPayloadPointer++) { + rDataPack.buttonPayload[buttonPayloadPointer] = bleQueue.pop(); + calculateSum += rDataPack.buttonPayload[buttonPayloadPointer]; + } + // read 4 byte joystick data package + for (int i = 0; i < 4; i++) rDataPack.joystickPayload[i] = bleQueue.pop(), calculateSum += rDataPack.joystickPayload[i]; + + rDataPack.checkSum = bleQueue.pop(); + + if (DEBUGCHECKSUM) { + Serial.print("Parser -> sum calculation: "); + Serial.println(calculateSum); + + Serial.print("Parser -> checkSum byte value: "); + Serial.println(rDataPack.checkSum); + } + + // check sum and update the parse state value + // if the checksum byte is not correct, return 0x12 + + rDataPack.commandFlag = false; + + if (rDataPack.checkSum == calculateSum) return PARSESUCCESS; + else return 0x13; +} + +void _GoBLE::bleDataReceiver() { + + byte inputByte = Serial.read(); + + if (DEBUGDATARECEIVER) { + Serial.print("bleDataReceiver -> new data:"); + Serial.println(inputByte, HEX); + } + + // throw the trash data and restore the useful data to the queue buffer + if (inputByte == DEFAULTHEADER1 || rDataPack.commandFlag == true) { + bleQueue.push(inputByte); + rDataPack.commandFlag = true; + + // auto adjust the command length based on the button command value + if (bleQueue.count() == PACKBUTTONSIGN) { + // max button input at one moment should less than 6 buttons + if (inputByte > 0 && inputByte < MAXBUTTONNUMBER) { + + // default command length + button number + rDataPack.commandLength = DEFAULTPACKLENGTH + inputByte; + if (DEBUGDATARECEIVER) Serial.print("bleDataReceiver -> Command Length:"), Serial.println(rDataPack.commandLength); + } + else rDataPack.commandLength = DEFAULTPACKLENGTH; + } + } + +} + +void _GoBLE::initRecvDataPack() { + rDataPack.commandFlag = false; + rDataPack.commandLength = DEFAULTPACKLENGTH; + rDataPack.parseState = PARSESUCCESS; + + rDataPack.digitalButtonNumber = 0; + rDataPack.latestDigitalButtonNumber = 0; +} + +void _GoBLE::updateJoystickVal() { + _joystickX = rDataPack.joystickPayload[0]; + _joystickY = rDataPack.joystickPayload[1]; +} + +void _GoBLE::updateButtonState() { + + if (rDataPack.digitalButtonNumber == 0 && rDataPack.latestDigitalButtonNumber != 0) { + for (int i = 0; i < MAXBUTTONID; i++) { + if (_button[i] == PRESSED) { + if (DEBUGUPDATEBUTTON) { + Serial.print("updateButtonState -> clear Pressed button number: "); + Serial.println(i); + } + _button[i] = RELEASED; + } + } + } + + for (int i = 0; i < rDataPack.digitalButtonNumber; i++) _button[rDataPack.buttonPayload[i]] = PRESSED; +} + + + +/* + unsigned int _GoBLE::readChannel(byte channel) { + + digitalWrite(MUX_ADDR_PINS[0], (channel & 1) ? HIGH : LOW); + digitalWrite(MUX_ADDR_PINS[1], (channel & 2) ? HIGH : LOW); + digitalWrite(MUX_ADDR_PINS[2], (channel & 4) ? HIGH : LOW); + digitalWrite(MUX_ADDR_PINS[3], (channel & 8) ? HIGH : LOW); + // workaround to cope with lack of pullup resistor on joystick switch + if (channel == CH_JOYSTICK_SW) { + pinMode(MUX_COM_PIN, INPUT_PULLUP); + unsigned int joystickSwitchState = (digitalRead(MUX_COM_PIN) == HIGH) ? 1023 : 0; + digitalWrite(MUX_COM_PIN, LOW); + return joystickSwitchState; + } + else + return analogRead(MUX_COM_PIN); + + } + + + boolean _GoBLE::readButton(byte ch) { + if (ch >= SWITCH_1 && ch <= SWITCH_5) { + ch; + } + + switch(ch) { + + } + + unsigned int val = readChannel(ch); + return (val > 512) ? HIGH : LOW; + } + + boolean _GoBLE::readJoystickButton() { + if (readChannel(CH_JOYSTICK_SW) == 1023) { + return HIGH; + } else if (readChannel(CH_JOYSTICK_SW) == 0) { + return LOW; + } + } + +*/ + diff --git a/firmware/GoBLE.h b/firmware/GoBLE.h new file mode 100755 index 0000000..7f547cb --- /dev/null +++ b/firmware/GoBLE.h @@ -0,0 +1,123 @@ +#ifndef GOBLE_H_ +#define GOBLE_H_ +#include "QueueArray.h" + +/**************** Debugger Configuration ******************/ + +#define DEBUGDATARECEIVER 0 +#define DEBUGDATARAW 0 +#define DEBUGPARSER 0 +#define DEBUGCHECKSUM 0 +#define DEBUGUPDATEBUTTON 0 + +const byte SWITCH_1 = 1; +const byte SWITCH_2 = 2; +const byte SWITCH_3 = 3; +const byte SWITCH_4 = 4; +const byte SWITCH_5 = 5; +const byte SWITCH_6 = 6; + +const byte SWITCH_UP = SWITCH_1; +const byte SWITCH_RIGHT = SWITCH_2; +const byte SWITCH_DOWN = SWITCH_3; +const byte SWITCH_LEFT = SWITCH_4; + +const byte SWITCH_SELECT = SWITCH_5; +const byte SWITCH_START = SWITCH_6; + +/* + These constants can be use for comparison with the value returned + by the readButton() method. +*/ +const boolean PRESSED = LOW; +const boolean RELEASED = HIGH; + + + +/* + Data structure for the command buffer + +*/ + +// Package protocol configuration +#define PACKHEADER 1 +#define PACKHEADER2 2 +#define PACKADDRESS 3 +#define PACKBUTTONSIGN 4 +#define PACKJOYSTICKSIGN 5 +#define PACKPAYLOAD 6 + + +#define DEFAULTHEADER1 0x55 +#define DEFAULTHEADER2 0xAA +#define DEFAULTADDRESS 0x11 +#define DEFAULTPACKLENGTH 10 + +#define MAXBUTTONNUMBER 6 +#define MAXBUTTONID 7 + +#define PARSESUCCESS 0x10 + +//DL package +#pragma pack(1) +typedef struct +{ + byte header1; // 0x55 + byte header2; // 0xAA + byte address; // 0x11 + + byte latestDigitalButtonNumber; + byte digitalButtonNumber; + + byte joystickPosition; + byte buttonPayload[MAXBUTTONNUMBER]; + byte joystickPayload[4]; + byte checkSum; + + byte commandLength; + byte parseState; + boolean commandFlag; +} sDataLink; +#pragma pack() + +class _GoBLE { + + public: + _GoBLE(); + + boolean available(); + + int readJoystickX(); + int readJoystickY(); + /* + Reads the current state of a button. It will return + LOW if the button is pressed, and HIGH otherwise. + */ + boolean readSwitchUp(); + boolean readSwitchDown(); + boolean readSwitchLeft(); + boolean readSwitchRight(); + boolean readSwitchSelect(); + boolean readSwitchStart(); + + private: + + sDataLink rDataPack; + // create a queue of characters. + QueueArray bleQueue; + + int _joystickX, _joystickY; + int _button[MAXBUTTONID]; + + void updateJoystickVal(); + void updateButtonState(); + + void initRecvDataPack(); + int bleDataPackageParser(); + void bleDataReceiver(); + +}; + +extern _GoBLE Goble; + +#endif // GOBLE_H_ diff --git a/firmware/QueueArray.h b/firmware/QueueArray.h new file mode 100755 index 0000000..2c18375 --- /dev/null +++ b/firmware/QueueArray.h @@ -0,0 +1,313 @@ +/* + * QueueArray.h + * + * Library implementing a generic, dynamic queue (array version). + * + * --- + * + * Copyright (C) 2010 Efstathios Chatzikyriakidis (contact@efxa.org) + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program 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 General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * --- + * + * Version 1.0 + * + * 2014-02-03 Brian Fletcher + * + * - added enqueue(), dequeue() and front(). + * + * 2010-09-29 Efstathios Chatzikyriakidis + * + * - added resize(): for growing, shrinking the array size. + * + * 2010-09-25 Efstathios Chatzikyriakidis + * + * - added exit(), blink(): error reporting and handling methods. + * + * 2010-09-24 Alexander Brevig + * + * - added setPrinter(): indirectly reference a Serial object. + * + * 2010-09-20 Efstathios Chatzikyriakidis + * + * - initial release of the library. + * + * --- + * + * For the latest version see: http://www.arduino.cc/ + */ + +// header defining the interface of the source. +#ifndef _QUEUEARRAY_H +#define _QUEUEARRAY_H +#include +// the definition of the queue class. +template +class QueueArray { + public: + // init the queue (constructor). + QueueArray (); + + // clear the queue (destructor). + ~QueueArray (); + + // add an item to the queue. + void enqueue (const T i); + + // remove an item from the queue. + T dequeue (); + + // push an item to the queue. + void push (const T i); + + // pop an item from the queue. + T pop (); + + // get the front of the queue. + T front () const; + + // get an item from the queue. + T peek () const; + + // check if the queue is empty. + bool isEmpty () const; + + // get the number of items in the queue. + int count () const; + + // check if the queue is full. + bool isFull () const; + + // set the printer of the queue. + void setPrinter (Print & p); + + private: + // resize the size of the queue. + void resize (const int s); + + // exit report method in case of error. + void exit (const char * m) const; + + // led blinking method in case of error. + void blink () const; + + // the initial size of the queue. + static const int initialSize = 2; + + // the pin number of the on-board led. + static const int ledPin = 13; + + Print * printer; // the printer of the queue. + T * contents; // the array of the queue. + + int size; // the size of the queue. + int items; // the number of items of the queue. + + int head; // the head of the queue. + int tail; // the tail of the queue. +}; + +// init the queue (constructor). +template +QueueArray::QueueArray () { + size = 0; // set the size of queue to zero. + items = 0; // set the number of items of queue to zero. + + head = 0; // set the head of the queue to zero. + tail = 0; // set the tail of the queue to zero. + + printer = NULL; // set the printer of queue to point nowhere. + + // allocate enough memory for the array. + contents = (T *) malloc (sizeof (T) * initialSize); + + // if there is a memory allocation error. + if (contents == NULL) + exit ("QUEUE: insufficient memory to initialize queue."); + + // set the initial size of the queue. + size = initialSize; +} + +// clear the queue (destructor). +template +QueueArray::~QueueArray () { + free (contents); // deallocate the array of the queue. + + contents = NULL; // set queue's array pointer to nowhere. + printer = NULL; // set the printer of queue to point nowhere. + + size = 0; // set the size of queue to zero. + items = 0; // set the number of items of queue to zero. + + head = 0; // set the head of the queue to zero. + tail = 0; // set the tail of the queue to zero. +} + +// resize the size of the queue. +template +void QueueArray::resize (const int s) { + // defensive issue. + if (s <= 0) + exit ("QUEUE: error due to undesirable size for queue size."); + + // allocate enough memory for the temporary array. + T * temp = (T *) malloc (sizeof (T) * s); + + // if there is a memory allocation error. + if (temp == NULL) + exit ("QUEUE: insufficient memory to initialize temporary queue."); + + // copy the items from the old queue to the new one. + for (int i = 0; i < items; i++) + temp[i] = contents[(head + i) % size]; + + // deallocate the old array of the queue. + free (contents); + + // copy the pointer of the new queue. + contents = temp; + + // set the head and tail of the new queue. + head = 0; tail = items; + + // set the new size of the queue. + size = s; +} + +// add an item to the queue. +template +void QueueArray::enqueue (const T i) { + // check if the queue is full. + if (isFull ()) + // double size of array. + resize (size * 2); + + // store the item to the array. + contents[tail++] = i; + + // wrap-around index. + if (tail == size) tail = 0; + + // increase the items. + items++; +} + +// push an item to the queue. +template +void QueueArray::push (const T i) { + enqueue(i); +} + +// remove an item from the queue. +template +T QueueArray::dequeue () { + // check if the queue is empty. + if (isEmpty ()) + exit ("QUEUE: can't pop item from queue: queue is empty."); + + // fetch the item from the array. + T item = contents[head++]; + + // decrease the items. + items--; + + // wrap-around index. + if (head == size) head = 0; + + // shrink size of array if necessary. + if (!isEmpty () && (items <= size / 4)) + resize (size / 2); + + // return the item from the array. + return item; +} + +// pop an item from the queue. +template +T QueueArray::pop () { + return dequeue(); +} + +// get the front of the queue. +template +T QueueArray::front () const { + // check if the queue is empty. + if (isEmpty ()) + exit ("QUEUE: can't get the front item of queue: queue is empty."); + + // get the item from the array. + return contents[head]; +} + +// get an item from the queue. +template +T QueueArray::peek () const { + return front(); +} + +// check if the queue is empty. +template +bool QueueArray::isEmpty () const { + return items == 0; +} + +// check if the queue is full. +template +bool QueueArray::isFull () const { + return items == size; +} + +// get the number of items in the queue. +template +int QueueArray::count () const { + return items; +} + +// set the printer of the queue. +template +void QueueArray::setPrinter (Print & p) { + printer = &p; +} + +// exit report method in case of error. +template +void QueueArray::exit (const char * m) const { + // print the message if there is a printer. + if (printer) + printer->println (m); + + // loop blinking until hardware reset. + blink (); +} + +// led blinking method in case of error. +template +void QueueArray::blink () const { + // set led pin as output. + pinMode (ledPin, OUTPUT); + + // continue looping until hardware reset. + while (true) { + digitalWrite (ledPin, HIGH); // sets the LED on. + delay (250); // pauses 1/4 of second. + digitalWrite (ledPin, LOW); // sets the LED off. + delay (250); // pauses 1/4 of second. + } + + // solution selected due to lack of exit() and assert(). +} + +#endif // _QUEUEARRAY_H diff --git a/firmware/firmware.ino b/firmware/firmware.ino new file mode 100644 index 0000000..a091704 --- /dev/null +++ b/firmware/firmware.ino @@ -0,0 +1,1667 @@ +/* ----------------------------------------------------------------------------- + - Project: Remote control Crawling robot + - Author: panerqiang@sunfounder.com + - Date: 2015/1/27 + ----------------------------------------------------------------------------- + - Overview + - This project was written for the Crawling robot desigened by Sunfounder. + https://github.com/sunfounder/SunFounder_Crawling_Quadruped_Robot_Kit_for_Arduino + + modified by Regis https://www.thingiverse.com/thing:2204279 + modified by John Crombie https://www.thingiverse.com/thing:2825097 + modified by manic-3d-print https://www.thingiverse.com/thing:2901132 + ---------------------------------------------------------------------------*/ + + + +/* Includes ------------------------------------------------------------------*/ +#include +#include +#include //to define and control servos +#include "FlexiTimer2.h"//to set a timer to manage all servos +#include "GoBLE.h" + +#define EEPROM_MAGIC 0xabcd +#define EEPROM_OFFSET 2 //eeprom starting offset to store servo offset of calibration +/* Servos --------------------------------------------------------------------*/ +//define 12 servos for 4 legs +Servo servo[4][3]; +//define servos' ports +const int servo_pin[4][3] = { {2, 3, 4}, {5, 6, 7}, {8, 9, 10}, {11, 12, 13} }; +int servo_error[4][3] = { {0, 0, 0}, {0, 0, 0}, {0, 0, 0}, {0, 0, 0} }; // angle trim offset for servo derivation in polar_to_servo +/* Size of the robot ---------------------------------------------------------*/ +const int femur_servo_index = 0; +const int tibia_servo_index = 1; +const int coxa_servo_index = 2; +const float femur = 55; +const float tibia = 81.5; +const float coxa = 33; +const float length_side = 65.5; +const float z_absolute = -20; +/* Constants for movement ----------------------------------------------------*/ +const float z_default = -50, z_up = -20, z_boot = z_absolute, z_bow_relative = 25, z_tall = -100; +const float x_default = 62, x_offset = 0; +const float y_start = 0, y_step = 40; +const float sprawl_leg = 95; // used for both X and Y on sprawl movement +const int WALK = 0; +const int RELAX = 1; +const int SPRAWL = 2; +const bool FALSE = 0; +const bool TRUE = 1; +/* variables for movement ----------------------------------------------------*/ +volatile float site_now[4][3]; //real-time coordinates of the end of each leg +volatile float site_expect[4][3]; //expected coordinates of the end of each leg +volatile bool capture = FALSE; +float temp_speed[4][3]; //each axis' speed, needs to be recalculated before each movement +float move_speed; //movement speed +float speed_multiple = 1; //movement speed multiple +int leg_position; // one of WALK, RELAX, SPRAWL +const float spot_turn_speed = 4; +const float leg_move_speed = 8; +const float body_move_speed = 3; +const float stand_seat_speed = 1; +const float gyrate_speed = 0.8; +const float bow_speed = 1; +volatile int rest_counter; //+1/0.02s, for automatic rest +//functions' parameter +const float KEEP = 255; +//define PI for calculation +const float pi = 3.1415926; +volatile float site_cg[4][3]; // record current position to determine how to move cg +/* Constants for turn --------------------------------------------------------*/ +//temp length +const float temp_a = sqrt(pow(2 * x_default + length_side, 2) + pow(y_step, 2)); +const float temp_b = 2 * (y_start + y_step) + length_side; +const float temp_c = sqrt(pow(2 * x_default + length_side, 2) + pow(2 * y_start + y_step + length_side, 2)); +const float temp_alpha = acos((pow(temp_a, 2) + pow(temp_b, 2) - pow(temp_c, 2)) / 2 / temp_a / temp_b); +//site for turn +const float turn_x1 = (temp_a - length_side) / 2; +const float turn_y1 = y_start + y_step / 2; +const float turn_x0 = turn_x1 - temp_b * cos(temp_alpha); +const float turn_y0 = temp_b * sin(temp_alpha) - turn_y1 - length_side; +// +const float half_step = 10; // forward and backward +const float cg_shift = 14; // distance to move CG to retain balance with leg raised +/* goBLE----------------------------------------------------------------------*/ +#define TIME_INTERVAL 5000 +#define SERIAL_DATA_PERIOD 200 +#define FORWARD 'f' +#define LEFT 'l' +#define STAND 's' +#define RIGHT 'r' +#define BACKWARD 'b' +#define BODY_FR_BR 'z' +#define BODY_SI_SI 'x' +#define BODY_SI_SI_DOWN 'y' +#define GYRATE 'i' +#define TALL 't' +#define BOW 'w' +#define STAGGER 'u' +#define HAND_WAV 'h' +#define HAND_SHAKE 'k' +#define SIT 'o' +#define SPRAWL 'p' +#define RELAX_LEG '?' + +volatile char cmd = STAND; +volatile bool stop_serial = false; +//bool auto_mode = true; +bool auto_mode = false; +bool random_walk = false; +unsigned long cur_time, prev_serial_data_time; +jmp_buf jump_env; + +/* + - setup function + ---------------------------------------------------------------------------*/ +void setup() +{ + //start serial for debug + Serial.begin(115200); // baud rate chosen for bluetooth compatability + // + int val = EEPROMReadWord(0); + if (val != EEPROM_MAGIC) { + EEPROMWriteWord(0, EEPROM_MAGIC); + storeTrim(); + } else { + loadTrim(); + } + + Serial.println("Robot starts initialization"); + //initialize default parameter + set_site(0, x_default - x_offset, y_start + y_step, z_boot); + set_site(1, x_default - x_offset, y_start + y_step, z_boot); + set_site(2, x_default + x_offset, y_start, z_boot); + set_site(3, x_default + x_offset, y_start, z_boot); + + leg_position = WALK; + + for (int i = 0; i < 4; i++) + { + for (int j = 0; j < 3; j++) + { + site_now[i][j] = site_expect[i][j]; + servo[i][j].attach(servo_pin[i][j]); + } + } + calib_servo(); + prev_serial_data_time = millis(); + + FlexiTimer2::set(20, servo_service); + FlexiTimer2::start(); + Serial.println("Robot initialization Complete"); + stand(); + +} + +/* + - loop function + ---------------------------------------------------------------------------*/ +void loop() +{ + int rc = setjmp(jump_env); + // if (rc != 0) Serial.println("jumped from delay()"); + gaits(); +} + +#define CAL_TRIGGER_PIN A5 +void calib_servo(void) +{ + pinMode(CAL_TRIGGER_PIN, OUTPUT); + digitalWrite(CAL_TRIGGER_PIN, 0); + pinMode(CAL_TRIGGER_PIN, INPUT); + if (digitalRead(CAL_TRIGGER_PIN)) { + for (int i = 0; i < 4; i++) { + servo[i][femur_servo_index].write(90 + servo_error[i][femur_servo_index]); + servo[i][tibia_servo_index].write(90 + servo_error[i][tibia_servo_index]); + servo[i][coxa_servo_index].write(90 + servo_error[i][coxa_servo_index]); + } + while (digitalRead(CAL_TRIGGER_PIN)) delay(1000); + } +} + +boolean check_goble() { + bool rc = false; + if (Goble.available()) { + Serial.end(); + rc = stop_serial = true; + int joystickX, joystickY; + joystickY = Goble.readJoystickY(); + joystickX = Goble.readJoystickX(); + if (!auto_mode) { + if (joystickX > 190) cmd = FORWARD; + else if (joystickX < 80) cmd = BACKWARD; + else if (joystickY > 190) cmd = RIGHT; + else if (joystickY < 80) cmd = LEFT; + else if (Goble.readSwitchLeft() == PRESSED && Goble.readSwitchRight() == PRESSED) + cmd = BOW; + else if (Goble.readSwitchLeft() == PRESSED && Goble.readSwitchDown() == PRESSED) + cmd = HAND_SHAKE; + else if (Goble.readSwitchRight() == PRESSED && Goble.readSwitchDown() == PRESSED) + cmd = HAND_WAV; + else if (Goble.readSwitchUp() == PRESSED) + cmd = BODY_FR_BR; + else if (Goble.readSwitchDown() == PRESSED) + cmd = TALL; + else if (Goble.readSwitchLeft() == PRESSED) + cmd = BODY_SI_SI_DOWN; + else if (Goble.readSwitchRight() == PRESSED) + cmd = GYRATE; + else { + cmd = STAND; + } + } + + if (Goble.readSwitchStart() == PRESSED) { + auto_mode = true; + goto __auto; + } else if (Goble.readSwitchSelect() == PRESSED) { + Serial.println("ENTERING BLE CONTROL MODE"); + auto_mode = false; + cmd = STAND; + } + + } // if Goble.available + + if (auto_mode) { + static char movements[] = { + FORWARD, LEFT, + GYRATE, BODY_FR_BR, BODY_SI_SI, BODY_SI_SI_DOWN, + TALL, BOW, STAGGER, HAND_WAV, HAND_SHAKE, SPRAWL, + RIGHT, BACKWARD, SIT, STAND + }; + static unsigned long old_time = cur_time; + static int c = 0; + if (cur_time - old_time >= TIME_INTERVAL ) { + old_time = cur_time; +__auto: + if (!random_walk) { + c = c % (sizeof(movements) / sizeof(char)); + cmd = movements[c++]; + } else { + c = (int)random(0, sizeof(movements) / sizeof(char)); + cmd = movements[c]; + } + } + } + return rc; +} + +boolean gaits() { + bool taken = true; + + //Serial.println("cmd " + String(cmd)); + + switch (cmd) { + case FORWARD: + step_forward(1); + break; + case BACKWARD: + step_back(1); + break; + case RIGHT: + turn_right(1); + break; + case LEFT: + turn_left(1); + break; + case STAND: + stand(); + break; + case BODY_FR_BR: + body_forward_backward(1); + break; + case BODY_SI_SI: + body_side_to_side(1); + break; + case BODY_SI_SI_DOWN: + body_side_up_side_down(1); + break; + case GYRATE: + gyrate(1); + break; + case BOW: + bow(1); + break; + case SPRAWL: + sprawl(); + delay(1000); + case RELAX_LEG: + relax_legs(); + delay(2000); + break; + case TALL: + tall(2000); + break; + case STAGGER: + stagger(1); + break; + case HAND_WAV: + hand_wave(1); + break; + case HAND_SHAKE: + hand_shake(1); + break; + case SIT: + sit(); + break; + default: + taken = false; + } + return taken; +} + + +void loop2() +{ + //leg position test + /* relax_legs(); + set_site(0, KEEP, KEEP, z_default - z_bow_relative); + set_site(1, KEEP, KEEP, z_default - z_bow_relative); + set_site(2, KEEP, KEEP, z_default - z_bow_relative); + set_site(3, KEEP, KEEP, z_default - z_bow_relative); + */ + + Serial.println("Stand"); + stand(); + delay(2000); + // JC added moves + Serial.println("Relaxed Pose"); + relax_legs(); + delay(2000); + Serial.println("Body move Forward and Backwards"); + body_forward_backward(4); + delay(2000); + Serial.println("Body move side to side"); + body_side_to_side(4); + delay(2000); + Serial.println("Sprawl"); + sprawl(); + delay(2000); + Serial.println("Gyrate"); + gyrate(3); + delay(2000); + Serial.println("Front bow"); + bow(1); + delay(2000); + //Serial.println("Back bow"); + //back_bow(5); + // delay(2000); + Serial.println("Tall"); + tall(2000); // raise up. delay and return to normal height + relax_legs(); + delay(2000); + Serial.println("Body Twist"); + body_twist(3); + delay(2000); + Serial.println("Move Body side up to side down"); + body_side_up_side_down(3); + delay(2000); + Serial.println("stagger"); + stagger(3); + delay(2000); + // end of JC added moves + Serial.println("Step forward"); + step_forward(5); + delay(2000); + Serial.println("Step back"); + step_back(5); + delay(2000); + Serial.println("Turn left"); + turn_left(5); + delay(2000); + Serial.println("Turn right"); + turn_right(5); + delay(2000); + Serial.println("Hand wave"); + hand_wave(3); + delay(2000); + Serial.println("Hand wave"); + hand_shake(3); + delay(2000); + Serial.println("Sit"); + sit(); + delay(5000); + +} + +// Start of JC added move routines +/* + - JC + - body gyration movement + - blocking function + ---------------------------------------------------------------------------*/ +void gyrate(unsigned int number_of_times) +{ + // set leg starting position + relax_legs(); + move_speed = gyrate_speed; + //capture = TRUE; + set_site(0, KEEP, KEEP, z_default - z_bow_relative); + set_site(3, KEEP, KEEP, z_default + z_bow_relative); + wait_all_reach(); + + while (number_of_times-- > 0) + { + set_site(2, KEEP, KEEP, z_default - z_bow_relative); + set_site(1, KEEP, KEEP, z_default + z_bow_relative); + set_site(0, KEEP, KEEP, z_default); + set_site(3, KEEP, KEEP, z_default); + wait_all_reach(); + + set_site(3, KEEP, KEEP, z_default - z_bow_relative); + set_site(0, KEEP, KEEP, z_default + z_bow_relative); + set_site(2, KEEP, KEEP, z_default); + set_site(1, KEEP, KEEP, z_default); + wait_all_reach(); + + set_site(1, KEEP, KEEP, z_default - z_bow_relative); + set_site(2, KEEP, KEEP, z_default + z_bow_relative); + set_site(3, KEEP, KEEP, z_default); + set_site(0, KEEP, KEEP, z_default); + wait_all_reach(); + + set_site(0, KEEP, KEEP, z_default - z_bow_relative); + set_site(3, KEEP, KEEP, z_default + z_bow_relative); + set_site(1, KEEP, KEEP, z_default); + set_site(2, KEEP, KEEP, z_default); + wait_all_reach(); + } + + // return legs to starting position + set_site(1, KEEP, KEEP, z_default); + set_site(3, KEEP, KEEP, z_default); + capture = FALSE; + wait_all_reach(); +} +/* + - JC + - body gyration movement + - blocking function + ---------------------------------------------------------------------------*/ +void body_side_up_side_down(unsigned int number_of_times) +{ + // set leg starting position + relax_legs(); + move_speed = gyrate_speed; + //capture = TRUE; + set_site(0, KEEP, KEEP, z_default - z_bow_relative); + set_site(1, KEEP, KEEP, z_default - z_bow_relative); + set_site(2, KEEP, KEEP, z_default + z_bow_relative); + set_site(3, KEEP, KEEP, z_default + z_bow_relative); + wait_all_reach(); + + while (number_of_times-- > 0) + { + set_site(2, KEEP, KEEP, z_default - z_bow_relative); + set_site(3, KEEP, KEEP, z_default - z_bow_relative); + set_site(0, KEEP, KEEP, z_default + z_bow_relative); + set_site(1, KEEP, KEEP, z_default + z_bow_relative); + wait_all_reach(); + + set_site(0, KEEP, KEEP, z_default - z_bow_relative); + set_site(1, KEEP, KEEP, z_default - z_bow_relative); + set_site(2, KEEP, KEEP, z_default + z_bow_relative); + set_site(3, KEEP, KEEP, z_default + z_bow_relative); + wait_all_reach(); + } + + // return legs to starting position + set_site(0, KEEP, KEEP, z_default); + set_site(1, KEEP, KEEP, z_default); + set_site(2, KEEP, KEEP, z_default); + set_site(3, KEEP, KEEP, z_default); + capture = FALSE; + wait_all_reach(); +} + +/* + - JC + - bow front legs + - blocking function +*/ +void bow( unsigned int times) +{ + relax_legs(); + move_speed = bow_speed; + while (times-- > 0) + { + set_site(0, KEEP, KEEP, z_default + z_bow_relative); + set_site(2, KEEP, KEEP, z_default + z_bow_relative); + wait_all_reach(); + delay(300); + set_site(0, KEEP, KEEP, z_default); + set_site(2, KEEP, KEEP, z_default); + wait_all_reach(); + delay(300); + } +} +/* + - JC + - twist body clockwise and counter clockwise + - blocking function +*/ +void body_twist( unsigned int times) +{ + relax_legs(); + + move_cg(0); + set_site(0, x_default - x_offset, y_start + y_step, z_up); + wait_all_reach(); + set_site(0, turn_x1, turn_y1, z_up); + wait_all_reach(); + return_cg(0); + set_site(0, turn_x1, turn_y1, z_default); + wait_all_reach(); + + move_cg(1); + set_site(1, x_default - x_offset, y_start + y_step, z_up); + wait_all_reach(); + set_site(1, turn_x0, turn_y0, z_up); + wait_all_reach(); + return_cg(1); + set_site(1, turn_x0, turn_y0, z_default); + wait_all_reach(); + + move_cg(2); + set_site(2, x_default - x_offset, y_start + y_step, z_up); + wait_all_reach(); + set_site(2, turn_x0, turn_y0, z_up); + wait_all_reach(); + return_cg(2); + set_site(2, turn_x0, turn_y0, z_default); + wait_all_reach(); + + move_cg(3); + set_site(3, x_default - x_offset, y_start + y_step, z_up); + wait_all_reach(); + set_site(3, turn_x1, turn_y1, z_up); + wait_all_reach(); + return_cg(3); + set_site(3, turn_x1, turn_y1, z_default); + wait_all_reach(); + + move_speed = bow_speed; + while (times-- > 0) + { + set_site(0, turn_x0, turn_y0, KEEP); + set_site(1, turn_x1, turn_y1, KEEP); + set_site(2, turn_x1, turn_y1, KEEP); + set_site(3, turn_x0, turn_y0, KEEP); + wait_all_reach(); + delay(300); + set_site(0, turn_x1, turn_y1, KEEP); + set_site(1, turn_x0, turn_y0, KEEP); + set_site(2, turn_x0, turn_y0, KEEP); + set_site(3, turn_x1, turn_y1, KEEP); + wait_all_reach(); + delay(300); + } + move_speed = leg_move_speed; + + move_cg(0); + set_site(0, KEEP, KEEP, z_up); + wait_all_reach(); + set_site(0, x_default - x_offset, y_start + y_step, z_up); + wait_all_reach(); + return_cg(0); + set_site(0, x_default - x_offset, y_start + y_step, z_default); + wait_all_reach(); + + move_cg(1); + set_site(1, KEEP, KEEP, z_up); + wait_all_reach(); + set_site(1, x_default - x_offset, y_start + y_step, z_up); + wait_all_reach(); + return_cg(1); + set_site(1, x_default - x_offset, y_start + y_step, z_default); + wait_all_reach(); + + move_cg(2); + set_site(2, KEEP, KEEP, z_up); + wait_all_reach(); + set_site(2, x_default - x_offset, y_start + y_step, z_up); + wait_all_reach(); + return_cg(2); + set_site(2, x_default - x_offset, y_start + y_step, z_default); + wait_all_reach(); + + move_cg(3); + set_site(3, KEEP, KEEP, z_up); + wait_all_reach(); + set_site(3, x_default - x_offset, y_start + y_step, z_up); + wait_all_reach(); + return_cg(3); + set_site(3, x_default - x_offset, y_start + y_step, z_default); + wait_all_reach(); + +} +/* + - JC + - forward and backward body movement based on twist + - blocking function +*/ +void body_forward_backward( unsigned int times) +{ + relax_legs(); + move_speed = bow_speed; + set_site(0, x_default - x_offset, y_start + y_step - half_step, z_default); + set_site(1, x_default - x_offset, y_start + y_step + half_step, z_default); + set_site(2, x_default - x_offset, y_start + y_step - half_step, z_default); + set_site(3, x_default - x_offset, y_start + y_step + half_step, z_default); + wait_all_reach(); + + + while (times-- > 0) + { + set_site(0, x_default - x_offset, y_start + y_step + 2 * half_step, z_default); + set_site(1, x_default - x_offset, y_start + y_step - 2 * half_step, z_default); + set_site(2, x_default - x_offset, y_start + y_step + 2 * half_step, z_default); + set_site(3, x_default - x_offset, y_start + y_step - 2 * half_step, z_default); + wait_all_reach(); + delay(300); + set_site(0, x_default - x_offset, y_start + y_step - 2 * half_step, z_default); + set_site(1, x_default - x_offset, y_start + y_step + 2 * half_step, z_default); + set_site(2, x_default - x_offset, y_start + y_step - 2 * half_step, z_default); + set_site(3, x_default - x_offset, y_start + y_step + 2 * half_step, z_default); + wait_all_reach(); + delay(300); + } + set_site(0, x_default - x_offset, y_start + y_step, z_default); + set_site(1, x_default - x_offset, y_start + y_step, z_default); + set_site(2, x_default - x_offset, y_start + y_step, z_default); + set_site(3, x_default - x_offset, y_start + y_step, z_default); + wait_all_reach(); + move_speed = leg_move_speed; + +} +/* + - JC + - forward and backward body movement based on twist + - blocking function +*/ +void body_side_to_side( unsigned int times) +{ + relax_legs(); + move_speed = bow_speed; + set_site(0, x_default - x_offset - half_step, y_start + y_step, z_default); + set_site(1, x_default - x_offset - half_step, y_start + y_step, z_default); + set_site(2, x_default - x_offset + half_step, y_start + y_step, z_default); + set_site(3, x_default - x_offset + half_step, y_start + y_step, z_default); + wait_all_reach(); + + + while (times-- > 0) + { + set_site(0, x_default - x_offset + 2 * half_step, y_start + y_step, z_default); + set_site(1, x_default - x_offset + 2 * half_step, y_start + y_step, z_default); + set_site(2, x_default - x_offset - 2 * half_step, y_start + y_step, z_default); + set_site(3, x_default - x_offset - 2 * half_step, y_start + y_step, z_default); + wait_all_reach(); + delay(300); + set_site(0, x_default - x_offset - 2 * half_step, y_start + y_step, z_default); + set_site(1, x_default - x_offset - 2 * half_step, y_start + y_step, z_default); + set_site(2, x_default - x_offset + 2 * half_step, y_start + y_step, z_default); + set_site(3, x_default - x_offset + 2 * half_step, y_start + y_step, z_default); + wait_all_reach(); + delay(300); + } + set_site(0, x_default - x_offset, y_start + y_step, z_default); + set_site(1, x_default - x_offset, y_start + y_step, z_default); + set_site(2, x_default - x_offset, y_start + y_step, z_default); + set_site(3, x_default - x_offset, y_start + y_step, z_default); + wait_all_reach(); + move_speed = leg_move_speed; + +} +/* + - JC + - lift_up front legs + - blocking function +*/ +void tall( int time) +{ + relax_legs(); + move_speed = bow_speed; + set_site(0, KEEP, KEEP, z_tall); + set_site(2, KEEP, KEEP, z_tall); + set_site(1, KEEP, KEEP, z_tall); + set_site(3, KEEP, KEEP, z_tall); + wait_all_reach(); + delay(time); + set_site(0, KEEP, KEEP, z_default); + set_site(2, KEEP, KEEP, z_default); + set_site(1, KEEP, KEEP, z_default); + set_site(3, KEEP, KEEP, z_default); + wait_all_reach(); +} +/* + - JC + - bow back legs - looks a bit rude. + - blocking function +*/ +void back_bow( unsigned int times) +{ + relax_legs(); + move_speed = bow_speed; + while (times-- > 0) + { + set_site(1, KEEP, KEEP, z_default - z_bow_relative); + set_site(3, KEEP, KEEP, z_default - z_bow_relative); + wait_all_reach(); + set_site(1, KEEP, KEEP, z_default); + set_site(3, KEEP, KEEP, z_default); + wait_all_reach(); + } +} +/* + - JC + - dance + - blocking function + ---------------------------------------------------------------------------*/ +void stagger(unsigned int times) +{ + move_speed = leg_move_speed; + while (times-- > 0) + { + walk_legs(); + relax_legs(); + walk_legs_other(); + relax_legs(); + } + capture = FALSE; +} + +/* + - JC + - Change_legs to for a X for a relaxed pose + - blocking function + ---------------------------------------------------------------------------*/ +void relax_legs(void) +{ + move_speed = leg_move_speed; + switch (leg_position) + { + case WALK: + if (site_now[3][1] == y_start) + { + move_cg(2); + set_site(2, KEEP, KEEP, z_up); + wait_all_reach(); + set_site(2, x_default - x_offset, y_start + y_step, z_up); + wait_all_reach(); + return_cg(2); + set_site(2, x_default - x_offset, y_start + y_step, z_default); + wait_all_reach(); + move_cg(3); + set_site(3, KEEP, KEEP , z_up); + wait_all_reach(); + set_site(3, x_default - x_offset, y_start + y_step, z_up); + wait_all_reach(); + return_cg(3); + set_site(3, x_default - x_offset, y_start + y_step, z_default); + wait_all_reach(); + } + else + { + move_cg(0); + set_site(0, KEEP, KEEP, z_up); + wait_all_reach(); + set_site(0, x_default - x_offset, y_start + y_step, z_up); + wait_all_reach(); + return_cg(0); + set_site(0, x_default - x_offset, y_start + y_step, z_default); + wait_all_reach(); + move_cg(1); + set_site(1, KEEP, KEEP , z_up); + wait_all_reach(); + set_site(1, x_default - x_offset, y_start + y_step, z_up); + wait_all_reach(); + return_cg(1); + set_site(1, x_default - x_offset, y_start + y_step, z_default); + wait_all_reach(); + } + break; + + case SPRAWL: + set_site(1, KEEP, KEEP, z_up); + wait_all_reach(); + set_site(1, x_default - x_offset, y_start + y_step, KEEP); + wait_all_reach(); + + set_site(3, KEEP, KEEP, z_up); + wait_all_reach(); + set_site(3, x_default - x_offset, y_start + y_step, KEEP); + wait_all_reach(); + + set_site(0, KEEP, KEEP, z_up); + wait_all_reach(); + set_site(0, x_default - x_offset, y_start + y_step, KEEP); + wait_all_reach(); + + set_site(2, KEEP, KEEP, z_up); + wait_all_reach(); + set_site(2, x_default - x_offset, y_start + y_step, KEEP); + wait_all_reach (); + + for (int i = 0; i < 4; i++) + { + set_site(i, x_default - x_offset, y_start + y_step, z_default); + } + wait_all_reach(); + break; + } + leg_position = RELAX; +} +/* + - JC + - Change_legs to for a start of walk pose V on one side = on other + - blocking function + ---------------------------------------------------------------------------*/ +void walk_legs(void) +{ + move_speed = leg_move_speed; + + switch (leg_position) + { + case RELAX: + move_cg(2); + set_site(2, KEEP, KEEP, z_up); + wait_all_reach(); + set_site(2, x_default - x_offset, y_start, z_up); + wait_all_reach(); + return_cg(2); + set_site(2, x_default - x_offset, y_start, z_default); + wait_all_reach(); + move_cg(3); + set_site(3, KEEP, KEEP, z_up); + wait_all_reach(); + set_site(3, x_default - x_offset, y_start, z_up); + wait_all_reach(); + return_cg(3); + set_site(3, x_default - x_offset, y_start, z_default); + wait_all_reach(); + break; + + case SPRAWL: + relax_legs(); + walk_legs(); + break; + } + leg_position = WALK; +} +/* + - JC + - Change_legs to for a start of walk pose V on one side = on other mirror image of walk_legs + - blocking function + ---------------------------------------------------------------------------*/ +void walk_legs_other(void) +{ + move_speed = leg_move_speed; + + switch (leg_position) + { + case RELAX: + move_cg(0); + set_site(0, KEEP, KEEP, z_up); + wait_all_reach(); + set_site(0, x_default - x_offset, y_start, z_up); + wait_all_reach(); + return_cg(0); + set_site(0, x_default - x_offset, y_start, z_default); + wait_all_reach(); + move_cg(1); + set_site(1, KEEP, KEEP, z_up); + wait_all_reach(); + set_site(1, x_default - x_offset, y_start, z_up); + wait_all_reach(); + return_cg(1); + set_site(1, x_default - x_offset, y_start, z_default); + wait_all_reach(); + break; + + case SPRAWL: + relax_legs(); + walk_legs_other(); + break; + } + leg_position = WALK; +} + +/* + - JC + - Change_legs to for a X for a sprawl pose + - blocking function + ---------------------------------------------------------------------------*/ +void sprawl(void) +{ + move_speed = leg_move_speed; + + relax_legs(); + for (int i = 0; i < 4; i++) + { + set_site(i, sprawl_leg, sprawl_leg, KEEP); + } + wait_all_reach(); + leg_position = SPRAWL; +} +/* + - JC + - Change_leg_z + - leg to change + - blocking function + ---------------------------------------------------------------------------*/ +void change_leg_z(int leg, float z) +{ + set_site(leg, KEEP, KEEP, z); + wait_all_reach(); +} +/* + - JC + - move_cg + - leg to change + - blocking function + - move the body of the robot away from the leg that is going to be lifted + ---------------------------------------------------------------------------*/ +void move_cg(int leg) +{ + // reducing right leg and increasing left leg X moves right + // reducing front leg and increasing back leg Y moves forward + float temp_move_speed = move_speed; + move_speed = body_move_speed; + + //site_now contains current leg position - remember this + for (int i = 0; i < 4; i++) + { + for (int j = 0; j < 3; j++) + { + site_cg[i][j] = site_now[i][j]; + } + if (capture) + { + Serial.println("CG, " + String(i) + ", " + String(site_cg[i][0]) + ", " + String(site_cg[i][1]) + ", " + String(site_cg[i][2])); + } + } + + switch (leg) + { + case 0: // front right leg - move body weight back and left + set_site(0, site_cg[0][0] + cg_shift, site_cg[0][1] + cg_shift, site_cg[0][2]); + set_site(1, site_cg[1][0] + cg_shift, site_cg[1][1] - cg_shift, site_cg[1][2]); + set_site(2, site_cg[2][0] - cg_shift, site_cg[2][1] + cg_shift, site_cg[2][2]); + set_site(3, site_cg[3][0] - cg_shift, site_cg[3][1] - cg_shift, site_cg[3][2]); + break; + + case 1: // back right leg - move body weight forward and left + set_site(0, site_cg[0][0] + cg_shift, site_cg[0][1] - cg_shift, site_cg[0][2]); + set_site(1, site_cg[1][0] + cg_shift, site_cg[1][1] + cg_shift, site_cg[1][2]); + set_site(2, site_cg[2][0] - cg_shift, site_cg[2][1] - cg_shift, site_cg[2][2]); + set_site(3, site_cg[3][0] - cg_shift, site_cg[3][1] + cg_shift, site_cg[3][2]); + break; + + case 2: // front left leg - move body weight back and right + set_site(0, site_cg[0][0] - cg_shift, site_cg[0][1] + cg_shift, site_cg[0][2]); + set_site(1, site_cg[1][0] - cg_shift, site_cg[1][1] - cg_shift, site_cg[1][2]); + set_site(2, site_cg[2][0] + cg_shift, site_cg[2][1] + cg_shift, site_cg[2][2]); + set_site(3, site_cg[3][0] + cg_shift, site_cg[3][1] - cg_shift, site_cg[3][2]); + break; + + case 3: // back left leg - move body weight forward and right + set_site(0, site_cg[0][0] - cg_shift, site_cg[0][1] - cg_shift, site_cg[0][2]); + set_site(1, site_cg[1][0] - cg_shift, site_cg[1][1] + cg_shift, site_cg[1][2]); + set_site(2, site_cg[2][0] + cg_shift, site_cg[2][1] - cg_shift, site_cg[2][2]); + set_site(3, site_cg[3][0] + cg_shift, site_cg[3][1] + cg_shift, site_cg[3][2]); + break; + } + wait_all_reach(); + move_speed = temp_move_speed; + +} +/* + - JC + - return_cg + - leg to change + - blocking function + - move the body of the robot back towards lifted leg + ---------------------------------------------------------------------------*/ +void return_cg(int leg) +{ + float temp_move_speed = move_speed; + move_speed = body_move_speed; + for (int i = 0; i < 4; i++) + { + if (i != leg) + { + set_site(i, site_cg[i][0], site_cg[i][1], site_cg[i][2]); + } + } + + move_speed = temp_move_speed; + +} +// -- end of JC added move routines -- + +/* + - sit + - blocking function + ---------------------------------------------------------------------------*/ +void sit(void) +{ + move_speed = stand_seat_speed; + for (int leg = 0; leg < 4; leg++) + { + set_site(leg, KEEP, KEEP, z_boot); + } + wait_all_reach(); +} + +/* + - stand + - blocking function + ---------------------------------------------------------------------------*/ +void stand(void) +{ + move_speed = stand_seat_speed; + for (int leg = 0; leg < 4; leg++) + { + set_site(leg, KEEP, KEEP, z_default); + } + wait_all_reach(); +} + + +/* + - spot turn to left + - blocking function + - parameter step steps wanted to turn + ---------------------------------------------------------------------------*/ +void turn_left(unsigned int step) +{ + walk_legs(); // JC - get legs in correct starting position + move_speed = spot_turn_speed; + while (step-- > 0) + { + if (site_now[3][1] == y_start) + { + //leg 3&1 move + set_site(3, x_default + x_offset, y_start, z_up); + wait_all_reach(); + + set_site(0, turn_x1 - x_offset, turn_y1, z_default); + set_site(1, turn_x0 - x_offset, turn_y0, z_default); + set_site(2, turn_x1 + x_offset, turn_y1, z_default); + set_site(3, turn_x0 + x_offset, turn_y0, z_up); + wait_all_reach(); + + set_site(3, turn_x0 + x_offset, turn_y0, z_default); + wait_all_reach(); + + set_site(0, turn_x1 + x_offset, turn_y1, z_default); + set_site(1, turn_x0 + x_offset, turn_y0, z_default); + set_site(2, turn_x1 - x_offset, turn_y1, z_default); + set_site(3, turn_x0 - x_offset, turn_y0, z_default); + wait_all_reach(); + + set_site(1, turn_x0 + x_offset, turn_y0, z_up); + wait_all_reach(); + + set_site(0, x_default + x_offset, y_start, z_default); + set_site(1, x_default + x_offset, y_start, z_up); + set_site(2, x_default - x_offset, y_start + y_step, z_default); + set_site(3, x_default - x_offset, y_start + y_step, z_default); + wait_all_reach(); + + set_site(1, x_default + x_offset, y_start, z_default); + wait_all_reach(); + } + else + { + //leg 0&2 move + set_site(0, x_default + x_offset, y_start, z_up); + wait_all_reach(); + + set_site(0, turn_x0 + x_offset, turn_y0, z_up); + set_site(1, turn_x1 + x_offset, turn_y1, z_default); + set_site(2, turn_x0 - x_offset, turn_y0, z_default); + set_site(3, turn_x1 - x_offset, turn_y1, z_default); + wait_all_reach(); + + set_site(0, turn_x0 + x_offset, turn_y0, z_default); + wait_all_reach(); + + set_site(0, turn_x0 - x_offset, turn_y0, z_default); + set_site(1, turn_x1 - x_offset, turn_y1, z_default); + set_site(2, turn_x0 + x_offset, turn_y0, z_default); + set_site(3, turn_x1 + x_offset, turn_y1, z_default); + wait_all_reach(); + + set_site(2, turn_x0 + x_offset, turn_y0, z_up); + wait_all_reach(); + + set_site(0, x_default - x_offset, y_start + y_step, z_default); + set_site(1, x_default - x_offset, y_start + y_step, z_default); + set_site(2, x_default + x_offset, y_start, z_up); + set_site(3, x_default + x_offset, y_start, z_default); + wait_all_reach(); + + set_site(2, x_default + x_offset, y_start, z_default); + wait_all_reach(); + } + } +} + +/* + - spot turn to right + - blocking function + - parameter step steps wanted to turn + ---------------------------------------------------------------------------*/ +void turn_right(unsigned int step) +{ + walk_legs(); // JC - get legs in correct starting position + move_speed = spot_turn_speed; + while (step-- > 0) + { + if (site_now[2][1] == y_start) + { + //leg 2&0 move + set_site(2, x_default + x_offset, y_start, z_up); + wait_all_reach(); + + set_site(0, turn_x0 - x_offset, turn_y0, z_default); + set_site(1, turn_x1 - x_offset, turn_y1, z_default); + set_site(2, turn_x0 + x_offset, turn_y0, z_up); + set_site(3, turn_x1 + x_offset, turn_y1, z_default); + wait_all_reach(); + + set_site(2, turn_x0 + x_offset, turn_y0, z_default); + wait_all_reach(); + + set_site(0, turn_x0 + x_offset, turn_y0, z_default); + set_site(1, turn_x1 + x_offset, turn_y1, z_default); + set_site(2, turn_x0 - x_offset, turn_y0, z_default); + set_site(3, turn_x1 - x_offset, turn_y1, z_default); + wait_all_reach(); + + set_site(0, turn_x0 + x_offset, turn_y0, z_up); + wait_all_reach(); + + set_site(0, x_default + x_offset, y_start, z_up); + set_site(1, x_default + x_offset, y_start, z_default); + set_site(2, x_default - x_offset, y_start + y_step, z_default); + set_site(3, x_default - x_offset, y_start + y_step, z_default); + wait_all_reach(); + + set_site(0, x_default + x_offset, y_start, z_default); + wait_all_reach(); + } + else + { + //leg 1&3 move + set_site(1, x_default + x_offset, y_start, z_up); + wait_all_reach(); + + set_site(0, turn_x1 + x_offset, turn_y1, z_default); + set_site(1, turn_x0 + x_offset, turn_y0, z_up); + set_site(2, turn_x1 - x_offset, turn_y1, z_default); + set_site(3, turn_x0 - x_offset, turn_y0, z_default); + wait_all_reach(); + + set_site(1, turn_x0 + x_offset, turn_y0, z_default); + wait_all_reach(); + + set_site(0, turn_x1 - x_offset, turn_y1, z_default); + set_site(1, turn_x0 - x_offset, turn_y0, z_default); + set_site(2, turn_x1 + x_offset, turn_y1, z_default); + set_site(3, turn_x0 + x_offset, turn_y0, z_default); + wait_all_reach(); + + set_site(3, turn_x0 + x_offset, turn_y0, z_up); + wait_all_reach(); + + set_site(0, x_default - x_offset, y_start + y_step, z_default); + set_site(1, x_default - x_offset, y_start + y_step, z_default); + set_site(2, x_default + x_offset, y_start, z_default); + set_site(3, x_default + x_offset, y_start, z_up); + wait_all_reach(); + + set_site(3, x_default + x_offset, y_start, z_default); + wait_all_reach(); + } + } +} + +/* + - go forward + - blocking function + - parameter step steps wanted to go + ---------------------------------------------------------------------------*/ +void step_forward(unsigned int step) +{ + walk_legs(); // JC - get legs in correct starting position + move_speed = leg_move_speed; + while (step-- > 0) + { + if (site_now[2][1] == y_start) + { + //leg 2&1 move + set_site(2, x_default + x_offset, y_start, z_up); + wait_all_reach(); + set_site(2, x_default + x_offset, y_start + 2 * y_step, z_up); + wait_all_reach(); + set_site(2, x_default + x_offset, y_start + 2 * y_step, z_default); + wait_all_reach(); + + move_speed = body_move_speed; + + set_site(0, x_default + x_offset, y_start, z_default); + set_site(1, x_default + x_offset, y_start + 2 * y_step, z_default); + set_site(2, x_default - x_offset, y_start + y_step, z_default); + set_site(3, x_default - x_offset, y_start + y_step, z_default); + wait_all_reach(); + + move_speed = leg_move_speed; + + set_site(1, x_default + x_offset, y_start + 2 * y_step, z_up); + wait_all_reach(); + set_site(1, x_default + x_offset, y_start, z_up); + wait_all_reach(); + set_site(1, x_default + x_offset, y_start, z_default); + wait_all_reach(); + } + else + { + //leg 0&3 move + set_site(0, x_default + x_offset, y_start, z_up); + wait_all_reach(); + set_site(0, x_default + x_offset, y_start + 2 * y_step, z_up); + wait_all_reach(); + set_site(0, x_default + x_offset, y_start + 2 * y_step, z_default); + wait_all_reach(); + + move_speed = body_move_speed; + + set_site(0, x_default - x_offset, y_start + y_step, z_default); + set_site(1, x_default - x_offset, y_start + y_step, z_default); + set_site(2, x_default + x_offset, y_start, z_default); + set_site(3, x_default + x_offset, y_start + 2 * y_step, z_default); + wait_all_reach(); + + move_speed = leg_move_speed; + + set_site(3, x_default + x_offset, y_start + 2 * y_step, z_up); + wait_all_reach(); + set_site(3, x_default + x_offset, y_start, z_up); + wait_all_reach(); + set_site(3, x_default + x_offset, y_start, z_default); + wait_all_reach(); + } + } +} + +/* + - go back + - blocking function + - parameter step steps wanted to go + ---------------------------------------------------------------------------*/ +void step_back(unsigned int step) +{ + walk_legs(); // JC - get legs in correct starting position + move_speed = leg_move_speed; + while (step-- > 0) + { + if (site_now[3][1] == y_start) + { + //leg 3&0 move + set_site(3, x_default + x_offset, y_start, z_up); + wait_all_reach(); + set_site(3, x_default + x_offset, y_start + 2 * y_step, z_up); + wait_all_reach(); + set_site(3, x_default + x_offset, y_start + 2 * y_step, z_default); + wait_all_reach(); + + move_speed = body_move_speed; + + set_site(0, x_default + x_offset, y_start + 2 * y_step, z_default); + set_site(1, x_default + x_offset, y_start, z_default); + set_site(2, x_default - x_offset, y_start + y_step, z_default); + set_site(3, x_default - x_offset, y_start + y_step, z_default); + wait_all_reach(); + + move_speed = leg_move_speed; + + set_site(0, x_default + x_offset, y_start + 2 * y_step, z_up); + wait_all_reach(); + set_site(0, x_default + x_offset, y_start, z_up); + wait_all_reach(); + set_site(0, x_default + x_offset, y_start, z_default); + wait_all_reach(); + } + else + { + //leg 1&2 move + set_site(1, x_default + x_offset, y_start, z_up); + wait_all_reach(); + set_site(1, x_default + x_offset, y_start + 2 * y_step, z_up); + wait_all_reach(); + set_site(1, x_default + x_offset, y_start + 2 * y_step, z_default); + wait_all_reach(); + + move_speed = body_move_speed; + + set_site(0, x_default - x_offset, y_start + y_step, z_default); + set_site(1, x_default - x_offset, y_start + y_step, z_default); + set_site(2, x_default + x_offset, y_start + 2 * y_step, z_default); + set_site(3, x_default + x_offset, y_start, z_default); + wait_all_reach(); + + move_speed = leg_move_speed; + + set_site(2, x_default + x_offset, y_start + 2 * y_step, z_up); + wait_all_reach(); + set_site(2, x_default + x_offset, y_start, z_up); + wait_all_reach(); + set_site(2, x_default + x_offset, y_start, z_default); + wait_all_reach(); + } + } +} + +// add by RegisHsu + +void body_left(int i) +{ + set_site(0, site_now[0][0] + i, KEEP, KEEP); + set_site(1, site_now[1][0] + i, KEEP, KEEP); + set_site(2, site_now[2][0] - i, KEEP, KEEP); + set_site(3, site_now[3][0] - i, KEEP, KEEP); + wait_all_reach(); +} + +void body_right(int i) +{ + set_site(0, site_now[0][0] - i, KEEP, KEEP); + set_site(1, site_now[1][0] - i, KEEP, KEEP); + set_site(2, site_now[2][0] + i, KEEP, KEEP); + set_site(3, site_now[3][0] + i, KEEP, KEEP); + wait_all_reach(); +} + +void hand_wave(int i) +{ + float x_tmp; + float y_tmp; + float z_tmp; + walk_legs(); // JC - get legs in correct starting position + move_speed = 1; + if (site_now[3][1] == y_start) + { + body_right(15); + x_tmp = site_now[2][0]; + y_tmp = site_now[2][1]; + z_tmp = site_now[2][2]; + move_speed = body_move_speed; + for (int j = 0; j < i; j++) + { + set_site(2, turn_x1, turn_y1, 50); + wait_all_reach(); + set_site(2, turn_x0, turn_y0, 50); + wait_all_reach(); + } + set_site(2, x_tmp, y_tmp, z_tmp); + wait_all_reach(); + move_speed = 1; + body_left(15); + } + else + { + body_left(15); + x_tmp = site_now[0][0]; + y_tmp = site_now[0][1]; + z_tmp = site_now[0][2]; + move_speed = body_move_speed; + for (int j = 0; j < i; j++) + { + set_site(0, turn_x1, turn_y1, 50); + wait_all_reach(); + set_site(0, turn_x0, turn_y0, 50); + wait_all_reach(); + } + set_site(0, x_tmp, y_tmp, z_tmp); + wait_all_reach(); + move_speed = 1; + body_right(15); + } +} + +void hand_shake(int i) +{ + float x_tmp; + float y_tmp; + float z_tmp; + walk_legs(); // JC - get legs in correct starting position + move_speed = 1; + if (site_now[3][1] == y_start) + { + body_right(15); + x_tmp = site_now[2][0]; + y_tmp = site_now[2][1]; + z_tmp = site_now[2][2]; + move_speed = body_move_speed; + for (int j = 0; j < i; j++) + { + set_site(2, x_default - 30, y_start + 2 * y_step, 55); + wait_all_reach(); + set_site(2, x_default - 30, y_start + 2 * y_step, 10); + wait_all_reach(); + } + set_site(2, x_tmp, y_tmp, z_tmp); + wait_all_reach(); + move_speed = 1; + body_left(15); + } + else + { + body_left(15); + x_tmp = site_now[0][0]; + y_tmp = site_now[0][1]; + z_tmp = site_now[0][2]; + move_speed = body_move_speed; + for (int j = 0; j < i; j++) + { + set_site(0, x_default - 30, y_start + 2 * y_step, 55); + wait_all_reach(); + set_site(0, x_default - 30, y_start + 2 * y_step, 10); + wait_all_reach(); + } + set_site(0, x_tmp, y_tmp, z_tmp); + wait_all_reach(); + move_speed = 1; + body_right(15); + } +} + + + +/* + - microservos service /timer interrupt function/50Hz + - when set site expected,this function move the end point to it in a straight line + - temp_speed[4][3] should be set before set expect site,it make sure the end point + move in a straight line,and decide move speed. + ---------------------------------------------------------------------------*/ +void servo_service(void) +{ + sei(); + static float alpha, beta, gamma; + + for (int i = 0; i < 4; i++) + { + for (int j = 0; j < 3; j++) + { + if (abs(site_now[i][j] - site_expect[i][j]) >= abs(temp_speed[i][j])) + site_now[i][j] += temp_speed[i][j]; + else + site_now[i][j] = site_expect[i][j]; + } + + cartesian_to_polar(alpha, beta, gamma, site_now[i][0], site_now[i][1], site_now[i][2]); + polar_to_servo(i, alpha, beta, gamma); + } + + cur_time = millis(); + if (cur_time - prev_serial_data_time >= 100) { + prev_serial_data_time = cur_time; + if (stop_serial) { + Serial.begin(115200); + stop_serial = false; + } + } + +} + +/* + - set one of end points' expect site + - this founction will set temp_speed[4][3] at same time + - non - blocking function + ---------------------------------------------------------------------------*/ +void set_site(int leg, float x, float y, float z) +{ + float length_x = 0, length_y = 0, length_z = 0; + + if (capture) + { + Serial.println("P, " + String(leg) + ", " + String(x) + ", " + String(y) + ", " + String(z)); // log the set leg position + } + + if (x != KEEP) + length_x = x - site_now[leg][0]; + if (y != KEEP) + length_y = y - site_now[leg][1]; + if (z != KEEP) + length_z = z - site_now[leg][2]; + + float length = sqrt(pow(length_x, 2) + pow(length_y, 2) + pow(length_z, 2)); + + temp_speed[leg][0] = length_x / length * move_speed * speed_multiple; + temp_speed[leg][1] = length_y / length * move_speed * speed_multiple; + temp_speed[leg][2] = length_z / length * move_speed * speed_multiple; + + if (x != KEEP) + site_expect[leg][0] = x; + if (y != KEEP) + site_expect[leg][1] = y; + if (z != KEEP) + site_expect[leg][2] = z; +} + +/* + - wait one of end points move to expect site + - blocking function + ---------------------------------------------------------------------------*/ +void wait_reach(int leg) +{ + boolean rc = false; + while (1) { + rc = check_goble(); + if (site_now[leg][0] == site_expect[leg][0]) + if (site_now[leg][1] == site_expect[leg][1]) + if (site_now[leg][2] == site_expect[leg][2]) + break; + } + if (rc)longjmp(jump_env, 2); +} + +/* + - wait all of end points move to expect site + - blocking function + ---------------------------------------------------------------------------*/ +void wait_all_reach(void) +{ + for (int i = 0; i < 4; i++) + wait_reach(i); +} + +/* + - trans site from cartesian to polar + - mathematical model 2/2 + ---------------------------------------------------------------------------*/ +void cartesian_to_polar(volatile float &alpha, volatile float &beta, volatile float &gamma, volatile float x, volatile float y, volatile float z) +{ + //calculate w-z degree + float v, w; + w = (x >= 0 ? 1 : -1) * (sqrt(pow(x, 2) + pow(y, 2))); + v = w - coxa; + alpha = atan2(z, v) + acos((pow(femur, 2) - pow(tibia, 2) + pow(v, 2) + pow(z, 2)) / 2 / femur / sqrt(pow(v, 2) + pow(z, 2))); + beta = acos((pow(femur, 2) + pow(tibia, 2) - pow(v, 2) - pow(z, 2)) / 2 / femur / tibia); + //calculate x-y-z degree + gamma = (w >= 0) ? atan2(y, x) : atan2(-y, -x); + //trans degree pi->180 + alpha = alpha / pi * 180; + beta = beta / pi * 180; + gamma = gamma / pi * 180; +} + +/* + - trans site from polar to microservos + - mathematical model map to fact + - the errors saved in eeprom will be add + ---------------------------------------------------------------------------*/ +void polar_to_servo(int leg, float alpha, float beta, float gamma) +{ + alpha += servo_error[leg][0]; + beta += servo_error[leg][1]; + gamma += servo_error[leg][2]; + + if (leg == 0) + { + alpha = 90 - alpha; + beta = beta; + gamma += 90; + } + else if (leg == 1) + { + alpha += 90; + beta = 180 - beta; + gamma = 90 - gamma; + } + else if (leg == 2) + { + alpha += 90; + beta = 180 - beta; + gamma = 90 - gamma; + } + else if (leg == 3) + { + alpha = 90 - alpha; + beta = beta; + gamma += 90; + } + + if (capture) // debug data + { + // Serial.println("A, "+String(leg)+","+String(alpha)+","+String(beta)+","+String(gamma)); + } + + servo[leg][femur_servo_index].write(alpha + servo_error[leg][femur_servo_index]); + servo[leg][tibia_servo_index].write(beta + servo_error[leg][tibia_servo_index]); + servo[leg][coxa_servo_index].write(gamma + servo_error[leg][coxa_servo_index]); + +} + +void delay(int period) { + long timeout = millis() + period; + while (millis() <= timeout) { + if (check_goble()) + longjmp(jump_env, 1); + } +} + +void storeTrim() { + int addr = EEPROM_OFFSET; + String s(""); + + for (int i = 0; i < 4; i++) { + s = s + "{"; + for (int j = 0; j < 3; j++) { + EEPROMWriteWord(addr, servo_error[i][j]); + s = s + String(servo_error[i][j]) + " "; + delay(100); + addr += 2; + } + s = s + "}"; + } + Serial.println("Saved: " + s); +} + +void loadTrim() { + int addr = EEPROM_OFFSET; + String s(""); + + for (int i = 0; i < 4; i++) { + s = s + "{"; + for (int j = 0; j < 3; j++) { + servo_error[i][j] = EEPROMReadWord(addr); + s = s + String(servo_error[i][j]) + " "; + //Serial.println(servo_error[i][j]); + addr += 2; + } + s = s + "}"; + } + Serial.println("Stored: " + s); +} + +int EEPROMReadWord(int p_address) +{ + byte lowByte = EEPROM.read(p_address); + byte highByte = EEPROM.read(p_address + 1); + + return ((lowByte << 0) & 0xFF) + ((highByte << 8) & 0xFF00); +} + +void EEPROMWriteWord(int p_address, int p_value) +{ + byte lowByte = ((p_value >> 0) & 0xFF); + byte highByte = ((p_value >> 8) & 0xFF); + + EEPROM.write(p_address, lowByte); + EEPROM.write(p_address + 1, highByte); +}