Hexapod/hexapod-robot
1
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update

Browse Source
This commit is contained in:
mangavalk authored and MangaValk committed 2026-10-25 00:43:37 +02:00
1 parent bc13cdbbbe
commit 3e28a0aec6
13 files changed
+1473
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

2019-teensy-nrf24/robot/Hexapod_Teensy.ino
+93
View File
@@ -0,0 +1,93 @@
#include "Nrf.h"
#include <Wire.h>
#include <MPU6050_tockn.h>
#include "PWMServo.h"
#include "config.h"
PWMServo servos[18];
MPU6050 mpu (Wire);
dataStruct2 data;
void setup()
{
Serial.begin(115200);
Serial1.begin(115200);
// while (!Serial) ; // wait for arduino serial monitor
for (int i = 0; i < 18; i++)
servos[i].attach(pins[i], servoMin[i] + servoCallibration[i],
servoMax[i] + servoCallibration[i]); // 900-2100 for high torque servos
// while (true)
// {
for (int i = 0; i < 18; i++)
servos[i].write(90 + servoOffset[i]);
// }
Serial.println("Starting mpu6050");
// // Initialization
// Wire.begin();
// mpu.begin();
//
// mpu.calcGyroOffsets(true, 300, 1000);
//
// // Calibration
// Serial.println("=====================================");
// Serial.println("Starting calibration...");
// mpu.calcGyroOffsets();
// Serial.println("Calibration complete!");
Serial.println("Starting nrf");
NrfInit();
}
void STOP()
{
for (int i = 0; i < 18; i++)
pinMode(pins[i], INPUT);
while (true)
{}
}
int i = 0;
void loop()
{
network.update();
if (false)
{
mpu.update();
Serial.print("AngX = ");
Serial.print(mpu.getAngleX());
Serial.print(" / AngY = ");
Serial.print(mpu.getAngleY());
Serial.print(" / AngZ = ");
Serial.println(mpu.getAngleZ());
}
// Serial.print(" / AngX = ");
// Serial.print(mpu.getGyroAngleX());
// Serial.print(" / AngY = ");
// Serial.print(mpu.getGyroAngleY());
// Serial.print(" / AngZ = ");
// Serial.println(mpu.getGyroAngleZ());
data = NrfGetData(data, true);
// if (data.STOP)
// STOP();
// test
// data.servoPositions[0] = data.servoPositions[0] + 0.1f;
// data = NrfSendData(data, false);
if (data.remotePasstroughControl == true)
for (int i = 0; i < 18; i++)
servos[i].write(data.servoPositions[i] + servoOffset[i]);
delay(20);
}
2019-teensy-nrf24/robot/Nrf.h
+66
View File
@@ -0,0 +1,66 @@
#include <SPI.h>
#include "RF24.h"
#include <RF24Network.h>
#include "TransferDataStructure.h"
// Nrf24 settings
RF24 radio(39, 15); // CE, CSN
RF24Network network(radio);
const uint16_t this_node = 01; // Address of our node in Octal format
const uint16_t other_node = 00; // Address of the other node in Octal format
void NrfInit()
{
SPI.setSCK(14);
SPI.setMOSI(11);
SPI.setMISO(12);
SPI.begin();
radio.begin();
network.begin(/*channel*/ 90, /*node address*/ this_node);
// Set the PA Level low to prevent power supply related issues since this is a
// getting_started sketch, and the likelihood of close proximity of the devices. RF24_PA_MAX is default.
radio.setPALevel(RF24_PA_LOW);
}
dataStruct2 NrfGetData(dataStruct2 data, bool debug = false)
{
// network.update();
int timeout = millis() + 100;
while ( network.available() && timeout > millis() )
{
RF24NetworkHeader header;
network.read(header, &data, sizeof(data));
if (debug)
{
Serial.print("Received packet ");
Serial.println(data.servoPositions[0]);
}
}
return data;
}
dataStruct2 NrfSendData(dataStruct2 data, bool debug = false)
{
// network.update();
if (debug)
Serial.print("Sending...");
RF24NetworkHeader header(/*to node*/ other_node);
bool ok = network.write(header, &data, sizeof(data));
if (debug)
{
if (ok)
Serial.println("ok.");
else
Serial.println("failed.");
}
return data;
}
2019-teensy-nrf24/robot/PWMServo.cpp
+226
View File
@@ -0,0 +1,226 @@
#include "Arduino.h"
#include "PWMServo.h"
/*
PWMServo.cpp - Hardware Servo Timer Library
http://arduiniana.org/libraries/pwmservo/
Author: Jim Studt, jim@federated.com
Copyright (c) 2007 David A. Mellis. All right reserved.
renamed to PWMServo by Mikal Hart
ported to other chips by Paul Stoffregen
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
*/
#define NO_ANGLE (0xff)
#if defined(__AVR__)
#include <avr/interrupt.h>
uint8_t PWMServo::attachedA = 0;
uint8_t PWMServo::attachedB = 0;
#ifdef SERVO_PIN_C
uint8_t PWMServo::attachedC = 0;
#endif
void PWMServo::seizeTimer1()
{
uint8_t oldSREG = SREG;
cli();
TCCR1A = _BV(WGM11); /* Fast PWM, ICR1 is top */
TCCR1B = _BV(WGM13) | _BV(WGM12) /* Fast PWM, ICR1 is top */
| _BV(CS11) /* div 8 clock prescaler */
;
OCR1A = 3000;
OCR1B = 3000;
ICR1 = clockCyclesPerMicrosecond() * (20000L / 8); // 20000 uS is a bit fast for the refresh, 20ms, but
// it keeps us from overflowing ICR1 at 20MHz clocks
// That "/8" at the end is the prescaler.
#if defined(__AVR_ATmega8__)
TIMSK &= ~(_BV(TICIE1) | _BV(OCIE1A) | _BV(OCIE1B) | _BV(TOIE1) );
#else
TIMSK1 &= ~(_BV(OCIE1A) | _BV(OCIE1B) | _BV(TOIE1) );
#endif
SREG = oldSREG; // undo cli()
}
void PWMServo::releaseTimer1() {}
PWMServo::PWMServo() : pin(0), angle(NO_ANGLE) {}
uint8_t PWMServo::attach(int pinArg, int min, int max)
{
#ifdef SERVO_PIN_C
if (pinArg != SERVO_PIN_A && pinArg != SERVO_PIN_B && pinArg != SERVO_PIN_C) return 0;
#else
if (pinArg != SERVO_PIN_A && pinArg != SERVO_PIN_B) return 0;
#endif
min16 = min / 16;
max16 = max / 16;
pin = pinArg;
angle = NO_ANGLE;
digitalWrite(pin, LOW);
pinMode(pin, OUTPUT);
#ifdef SERVO_PIN_C
if (!attachedA && !attachedB && !attachedC) seizeTimer1();
#else
if (!attachedA && !attachedB) seizeTimer1();
#endif
if (pin == SERVO_PIN_A) {
attachedA = 1;
TCCR1A = (TCCR1A & ~_BV(COM1A0)) | _BV(COM1A1);
}
if (pin == SERVO_PIN_B) {
attachedB = 1;
TCCR1A = (TCCR1A & ~_BV(COM1B0)) | _BV(COM1B1);
}
#ifdef SERVO_PIN_C
if (pin == SERVO_PIN_C) {
attachedC = 1;
TCCR1A = (TCCR1A & ~_BV(COM1C0)) | _BV(COM1C1);
}
#endif
return 1;
}
void PWMServo::detach()
{
// muck with timer flags
if (pin == SERVO_PIN_A) {
attachedA = 0;
TCCR1A = TCCR1A & ~_BV(COM1A0) & ~_BV(COM1A1);
pinMode(pin, INPUT);
}
if (pin == SERVO_PIN_B) {
attachedB = 0;
TCCR1A = TCCR1A & ~_BV(COM1B0) & ~_BV(COM1B1);
pinMode(pin, INPUT);
}
#ifdef SERVO_PIN_C
if (pin == SERVO_PIN_C) {
attachedC = 0;
TCCR1A = TCCR1A & ~_BV(COM1C0) & ~_BV(COM1C1);
pinMode(pin, INPUT);
}
#endif
#ifdef SERVO_PIN_C
if (!attachedA && !attachedB && !attachedC) releaseTimer1();
#else
if (!attachedA && !attachedB) releaseTimer1();
#endif
}
void PWMServo::write(int angleArg)
{
uint16_t p;
if (angleArg < 0) angleArg = 0;
if (angleArg > 180) angleArg = 180;
angle = angleArg;
// bleh, have to use longs to prevent overflow, could be tricky if always a 16MHz clock, but not true
// That 8L on the end is the TCNT1 prescaler, it will need to change if the clock's prescaler changes,
// but then there will likely be an overflow problem, so it will have to be handled by a human.
p = (min16 * 16L * clockCyclesPerMicrosecond() + (max16 - min16) * (16L * clockCyclesPerMicrosecond()) * angle / 180L) / 8L;
if (pin == SERVO_PIN_A) OCR1A = p;
if (pin == SERVO_PIN_B) OCR1B = p;
#ifdef SERVO_PIN_C
if (pin == SERVO_PIN_C) OCR1C = p;
#endif
}
uint8_t PWMServo::attached()
{
if (pin == SERVO_PIN_A && attachedA) return 1;
if (pin == SERVO_PIN_B && attachedB) return 1;
#ifdef SERVO_PIN_C
if (pin == SERVO_PIN_C && attachedC) return 1;
#endif
return 0;
}
#elif defined(__arm__) && defined(TEENSYDUINO)
uint32_t PWMServo::attachedpins[(NUM_DIGITAL_PINS + 31) / 32]; // 1 bit per digital pin
PWMServo::PWMServo() : pin(255), angle(NO_ANGLE) {}
uint8_t PWMServo::attach(int pinArg, int min, int max)
{
// Serial.printf("attach, pin=%d, min=%d, max=%d\n", pinArg, min, max);
if (pinArg < 0 || pinArg >= NUM_DIGITAL_PINS) return 0;
if (!digitalPinHasPWM(pinArg)) return 0;
pin = pinArg;
analogWriteFrequency(pin, 50);
min16 = min >> 4;
max16 = max >> 4;
angle = NO_ANGLE;
digitalWrite(pin, LOW);
pinMode(pin, OUTPUT);
attachedpins[pin >> 5] |= (1 << (pin & 31));
// Serial.println("Succes!");
return 1;
}
void PWMServo::write(float angleArg)
{
//Serial.printf("write, pin=%d, angle=%d\n", pin, angleArg);
if (pin >= NUM_DIGITAL_PINS) return;
if (angleArg < 0) angleArg = 0;
if (angleArg > 180) angleArg = 180;
angle = angleArg;
uint32_t us = (((max16 - min16) * 46603 * (uint8_t)angle) >> 11) + (min16 << 12); // us*256
//uint32_t duty = (us * 3355) >> 22;
float usec = (float)((max16 - min16) << 4) * ((float)angle / 180.0f) + (float)(min16 << 4);
uint32_t duty = (int)(usec / 20000.0f * 4096.0f);
if (false)
Serial.printf("pin=%d, angle=%.2f, usec=%.2f, us=%.2f, duty=%d, min=%d, max=%d\n",
pin, (float)angle, usec, (float)us / 256.0f, duty, min16 << 4, max16 << 4);
#if TEENSYDUINO >= 137
noInterrupts();
uint32_t oldres = analogWriteResolution(12);
analogWrite(pin, duty);
analogWriteResolution(oldres);
interrupts();
#else
analogWriteResolution(12);
analogWrite(pin, duty);
#endif
}
uint8_t PWMServo::attached()
{
if (pin >= NUM_DIGITAL_PINS) return 0;
return (attachedpins[pin >> 5] & (1 << (pin & 31))) ? 1 : 0;
}
#endif
2019-teensy-nrf24/robot/PWMServo.h
+94
View File
@@ -0,0 +1,94 @@
#ifndef PWMServo_h
#define PWMServo_h
/*
PWMServo.h - Hardware Servo Timer Library
http://arduiniana.org/libraries/pwmservo/
Author: Jim Studt, jim@federated.com
Copyright (c) 2007 David A. Mellis. All right reserved.
renamed to PWMServo by Mikal Hart
ported to other chips by Paul Stoffregen
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 <inttypes.h>
#if defined(__AVR_ATmega168__) || defined(__AVR_ATmega328P__) // Arduino
#define SERVO_PIN_A 9
#define SERVO_PIN_B 10
#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__) // Arduino Mega
#define SERVO_PIN_A 11
#define SERVO_PIN_B 12
#define SERVO_PIN_C 13
#elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__) // Sanguino
#define SERVO_PIN_A 13
#define SERVO_PIN_B 12
#elif defined(__AVR_AT90USB162__) // Teensy 1.0
#define SERVO_PIN_A 17
#define SERVO_PIN_B 18
#define SERVO_PIN_C 15
#elif defined(__AVR_ATmega32U4__) // Teensy 2.0
#define SERVO_PIN_A 14
#define SERVO_PIN_B 15
#define SERVO_PIN_C 4
#elif defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__) // Teensy++
#define SERVO_PIN_A 25
#define SERVO_PIN_B 26
#define SERVO_PIN_C 27
#elif defined(__arm__) && defined(TEENSYDUINO)
#define SERVO_PIN_A 9
#define SERVO_PIN_B 10
// all PWM pins supported, but names not defined here...
// just use the actual PWM pin number with attach()
#else
#define SERVO_PIN_A 9
#define SERVO_PIN_B 10
#endif
class PWMServo
{
private:
uint8_t pin;
float angle; // in degrees
uint8_t min16; // minimum pulse, 16uS units (default is 34)
uint8_t max16; // maximum pulse, 16uS units, 0-4ms range (default is 150)
#if defined(__AVR__)
static void seizeTimer1();
static void releaseTimer1();
static uint8_t attachedA;
static uint8_t attachedB;
#ifdef SERVO_PIN_C
static uint8_t attachedC;
#endif
#elif defined(__arm__) && defined(TEENSYDUINO)
static uint32_t attachedpins[]; // 1 bit per digital pin
#endif
public:
PWMServo();
uint8_t attach(int pinArg) { return attach(pinArg, 544, 2400); }
// pulse length for 0 degrees in microseconds, 544uS default
// pulse length for 180 degrees in microseconds, 2400uS default
uint8_t attach(int pinArg, int min, int max);
// attach to a pin, sets pinMode, returns 0 on failure, won't
// position the servo until a subsequent write() happens
// Only works for 9 and 10.
void detach();
void write(float angleArg); // specify the angle in degrees, 0 to 180
uint8_t read() { return angle; }
uint8_t attached();
};
#endif
2019-teensy-nrf24/robot/TransferDataStructure.h
+29
View File
@@ -0,0 +1,29 @@
#ifndef dataStruct
#define dataStruct
struct __attribute__((__packed__)) dataStruct2
{
bool STOP = false;
float walkSpeed = 10; //step size
float walkSpeed2 = 0; //gate speed
float walkRotation = 0 ;
float walkDirection = 0;
float tranform[3] = {0, 0, 0};
float tranform2[3] = {0, 0, 0};
float rotation[3] = {0, 0, 0};
bool remotePasstroughControl = true;
float servoPositions[18] = {
90, 90, 90,
90, 90, 90,
90, 90, 90,
90, 90, 90,
90, 90, 90,
90, 90, 90
};
} dataStruct;
#endif
2019-teensy-nrf24/robot/config.h
+44
View File
@@ -0,0 +1,44 @@
byte pins[18] = {4, 3, 2,
7, 6, 5,
10, 9, 8,
23, 22, 21,
20, 36, 37,
17, 16, 38
};
int servoMin[18] = {553, 840, 553,
553, 840, 553,
553, 840, 553,
553, 840, 553,
553, 840, 553,
553, 840, 553
};
int servoMax[18] = {2425, 2050, 2425,
2425, 2050, 2425,
2425, 2050, 2425,
2425, 2050, 2425,
2425, 2050, 2425,
2425, 2050, 2425
};
int servoCallibration[18] = {0, 100, 0,
0, 100, -30,
0, 80, -50,
-120, 0, 0,
-10, 20, 0,
0, 20, -80
};
float servoOffset[18] = {0, 0, 0,
0, 0, 0,
0, 0, 0,
90, 0, 0,
0, 0, 0,
-90, 0, 0
};