Compare commits
1
Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 mangavalk
|
81dbbdc834 |
No files matched your search
@@ -0,0 +1,33 @@
|
||||
// Version 1
|
||||
|
||||
#ifndef CustomTimer_H
|
||||
#define CustomTimer_H
|
||||
|
||||
class CustomTimer
|
||||
{
|
||||
int time;
|
||||
boolean start;
|
||||
public: CustomTimer()
|
||||
{
|
||||
start=false;
|
||||
}
|
||||
public: boolean Delay(int delay)
|
||||
{
|
||||
if (!start)
|
||||
{
|
||||
time = millis();
|
||||
start = true;
|
||||
}
|
||||
if (millis() - time >= delay)
|
||||
{
|
||||
start = false;
|
||||
return true;
|
||||
}
|
||||
else
|
||||
return false;
|
||||
}
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
@@ -0,0 +1,259 @@
|
||||
// Version 1
|
||||
|
||||
#ifndef Foot_H
|
||||
#define Foot_H
|
||||
|
||||
#include "Vector.h";
|
||||
#include "Global.h"
|
||||
|
||||
const double pi = 3.14159265359;
|
||||
|
||||
class Foot
|
||||
{
|
||||
public:
|
||||
float TranslateX, TranslateY, TranslateZ;
|
||||
boolean Invert;
|
||||
|
||||
void SetFootCoordinates(Vector P)
|
||||
{
|
||||
Xi = P.x;
|
||||
Yi = P.y;
|
||||
Zi = P.z;
|
||||
}
|
||||
|
||||
// ?used to put in position from body?
|
||||
void Translate(int x, int y, int z)
|
||||
{
|
||||
TranslateX = x;
|
||||
TranslateY = y;
|
||||
TranslateZ = z;
|
||||
}
|
||||
|
||||
void Translate(Vector i)
|
||||
{
|
||||
TranslateX = i.x;
|
||||
TranslateY = i.y;
|
||||
TranslateZ = i.z;
|
||||
}
|
||||
|
||||
// Inverse Kinemetics Region
|
||||
double l3, s, O1, O2, O3, X2, Y2, Z2;
|
||||
double l1, l2;
|
||||
double Xi, Yi, Zi;
|
||||
double X, Y, Z;
|
||||
boolean error;
|
||||
|
||||
void RotateX()
|
||||
{
|
||||
Y = Y * cos(radians(rotationX)) - Z * sin(radians(rotationX));
|
||||
Z = Y * sin(radians(rotationX)) + Z * cos(radians(rotationX));
|
||||
}
|
||||
|
||||
void RotateY()
|
||||
{
|
||||
Z = Z * cos(radians(rotationY)) - X * sin(radians(rotationY));
|
||||
X = Z * sin(radians(rotationY)) + X * cos(radians(rotationY));
|
||||
}
|
||||
|
||||
void RotateZ()
|
||||
{
|
||||
X = X * cos(radians(rotationZ)) - Y * sin(radians(rotationZ));
|
||||
Y = X * sin(radians(rotationZ)) + Y * cos(radians(rotationZ));
|
||||
}
|
||||
|
||||
void RL3 ()
|
||||
{
|
||||
double x = X;
|
||||
double z = Z;
|
||||
|
||||
if ((x == 0) && (z == 0))
|
||||
l3 = 0;
|
||||
else l3 = sqrt((float)(x * x + z * z));
|
||||
|
||||
if (x < 0)
|
||||
l3 = l3 * -1;
|
||||
}
|
||||
|
||||
float FO1 ()
|
||||
{
|
||||
RL3();
|
||||
|
||||
double x = X;
|
||||
double z = Z;
|
||||
double r;
|
||||
|
||||
if ((x != 0) & (z != 0)) {
|
||||
double a = (z / x);
|
||||
return atan((float)(a));
|
||||
}
|
||||
else
|
||||
return atan(0.0); // Math Error Protection.
|
||||
}
|
||||
|
||||
float T1 ()
|
||||
{
|
||||
float x = X;
|
||||
float y = Y;
|
||||
float z = Z;
|
||||
float x2 = x * x;
|
||||
float y2 = y * y;
|
||||
float z2 = z * z;
|
||||
float l32 = l3 * l3;
|
||||
|
||||
s = sqrt((float)(x2 + y2 + z2));
|
||||
|
||||
float t1 = 0;
|
||||
|
||||
float s2 = s * s;
|
||||
|
||||
if ((y == 0) && (l3 == 0))
|
||||
t1 = 0; // Math Error Protection.
|
||||
else
|
||||
t1 = (atan2((float)(l3), (float)(y)));
|
||||
|
||||
float a = (l1 * l1) - (l2 * l2) + s2;
|
||||
float b = 2 * l1 * s;
|
||||
float t2 = acos(a / b);
|
||||
|
||||
float ret = 0;
|
||||
|
||||
ret = t1 + abs(t2);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
float T3 ()
|
||||
{
|
||||
return (acos((float)(((l1 * l1) + (l2 * l2) - (s * s)) / (2 * l1 * l2))));
|
||||
}
|
||||
|
||||
void setleg()
|
||||
{
|
||||
setleg(Invert);
|
||||
}
|
||||
|
||||
void setleg(boolean Side)
|
||||
{
|
||||
l1 = 10;
|
||||
l2 = 15;
|
||||
|
||||
X = Xi;
|
||||
Y = Yi;
|
||||
Z = Zi;
|
||||
|
||||
// Rotation from body point
|
||||
if (Invert)
|
||||
{ // Right
|
||||
X = (-1 * X) + TranslateX;
|
||||
Z = (-1 * Z) + TranslateZ;
|
||||
}
|
||||
else
|
||||
{
|
||||
X = Xi + TranslateX;
|
||||
Z = (-1 * Z) + TranslateZ;
|
||||
}
|
||||
|
||||
RotateY();
|
||||
RotateX();
|
||||
RotateZ();
|
||||
|
||||
// Remove gaits
|
||||
if (Invert)
|
||||
{ // Right
|
||||
X = -1 * (X - TranslateX);
|
||||
Z = -1 * (Z - TranslateZ);
|
||||
}
|
||||
else
|
||||
{
|
||||
X -= TranslateX;
|
||||
Z = -1 * (Z - TranslateZ);
|
||||
}
|
||||
|
||||
if (Invert)
|
||||
{ // right
|
||||
double a = (90 - degrees(FO1()));
|
||||
double b = (180 - degrees(T1()));
|
||||
double c = degrees(T3());
|
||||
|
||||
/*if ((a >= 15) & (a <= 165)
|
||||
& (b >= 15) & (b <= 165)
|
||||
& (c >= 15) & (c <= 165))
|
||||
{*/
|
||||
error = false;
|
||||
|
||||
O1 = a;
|
||||
O2 = b;
|
||||
O3 = c;
|
||||
|
||||
double d = sin((float)(b * (pi / 180)));
|
||||
if ( d != 0 )
|
||||
X2 = d * l1;
|
||||
else
|
||||
X2 = 0;
|
||||
|
||||
d = cos((float)(b * (pi / 180)));
|
||||
if ( d != 0 )
|
||||
Y2 = d * l1;
|
||||
else
|
||||
Y2 = 0;
|
||||
|
||||
d = sin((float)((a - 90) * (pi / 180)));
|
||||
if ( d != 0 )
|
||||
Z2 = d * abs((float)(X2));
|
||||
else
|
||||
Z2 = 0;
|
||||
/*}
|
||||
else
|
||||
{
|
||||
error = true;
|
||||
}*/
|
||||
}
|
||||
else
|
||||
{
|
||||
double a = (90 - degrees((float)(FO1())));
|
||||
double b = degrees((float)(T1()));
|
||||
double c = 180 - degrees((float)(T3()));
|
||||
|
||||
/*if ((a >= 15) & (a <= 165)
|
||||
& (b >= 15) & (b <= 165)
|
||||
& (c >= 15) & (c <= 165))
|
||||
{*/
|
||||
error = false;
|
||||
|
||||
O1 = a;
|
||||
O2 = b;
|
||||
O3 = c;
|
||||
|
||||
a = (90 - degrees((float)(FO1())));
|
||||
b = (180 - degrees((float)(T1())));
|
||||
c = degrees((float)(T3()));
|
||||
|
||||
double d = sin((float)(b * (pi / 180)));
|
||||
if ( d != 0 )
|
||||
X2 = d * l1;
|
||||
else
|
||||
X2 = 0;
|
||||
|
||||
d = sin((float)(b * (pi / 180)));
|
||||
if ( d != 0 )
|
||||
Y2 = d * l1;
|
||||
else
|
||||
Y2 = 0;
|
||||
|
||||
d = sin((float)((a - 90) * (pi / 180)));
|
||||
if ( d != 0 )
|
||||
Z2 = d * abs((float)(X2));
|
||||
else
|
||||
Z2 = 0;
|
||||
/*}
|
||||
else
|
||||
{
|
||||
error = true;
|
||||
Serial.println("ERROR");
|
||||
}*/
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,30 @@
|
||||
// Version 1
|
||||
|
||||
#ifndef Global_H
|
||||
#define Global_H
|
||||
|
||||
float rotationX = 0;
|
||||
float rotationY = 0;
|
||||
float rotationZ = 0;
|
||||
|
||||
CustomTimer timerAnimation = CustomTimer();
|
||||
CustomTimer TimerFPS_SERVO = CustomTimer();
|
||||
|
||||
const int FPS = 30;
|
||||
int FPS_SERVO = 30;
|
||||
|
||||
float counterRotation = 0;
|
||||
int counterXRotation = 0;
|
||||
int counterYRotation = 0;
|
||||
int counterZRotation = 0;
|
||||
|
||||
Vector *walk1 = new Vector[8];
|
||||
|
||||
float robotHeigth = 12;
|
||||
|
||||
int rotationRange = 8;
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
@@ -0,0 +1,376 @@
|
||||
// Version 1
|
||||
|
||||
#ifndef Hexapod_H
|
||||
#define Hexapod_H
|
||||
|
||||
#include "FootClass.h";
|
||||
#include "Vector.h";
|
||||
#include "CustomTimer.h";
|
||||
#include "Global.h"
|
||||
#include <Streaming.h>
|
||||
|
||||
Foot foot1 = Foot();
|
||||
Foot foot2 = Foot();
|
||||
Foot foot3 = Foot();
|
||||
Foot foot4 = Foot();
|
||||
Foot foot5 = Foot();
|
||||
Foot foot6 = Foot();
|
||||
|
||||
// Spaces from body to feet
|
||||
// 9.5
|
||||
// 14
|
||||
// 9.5
|
||||
|
||||
// 10
|
||||
// 10
|
||||
|
||||
// LAYOUT HARDWARE
|
||||
// _U_
|
||||
// 1==/ \==4
|
||||
// >***<
|
||||
// 2===|---|===5
|
||||
// >***<
|
||||
// 3==\___/==6
|
||||
|
||||
// LAYOUT SOFTWARE
|
||||
// _U_
|
||||
// 1==/ \==6
|
||||
// >***<
|
||||
// 2===|---|===5
|
||||
// >***<
|
||||
// 3==\___/==4
|
||||
|
||||
Vector point1 = Vector(4.75, 0, 10);
|
||||
Vector point2 = Vector(7, 0, 0);
|
||||
Vector point3 = Vector(4.75, 0, -10);
|
||||
Vector point4 = Vector(-4.75, 0, -10);
|
||||
Vector point5 = Vector(-7, 0, 0);
|
||||
Vector point6 = Vector(-4.75, 0, 10);
|
||||
|
||||
Vector fpoint1 = Vector(10, 5, 0);
|
||||
Vector fpoint2 = Vector(10, 5, 0); // y negative is down from body.
|
||||
Vector fpoint3 = Vector(10, 5, 0);
|
||||
Vector fpoint4 = Vector(10, 5, 0);
|
||||
Vector fpoint5 = Vector(10, 5, 0);
|
||||
Vector fpoint6 = Vector(10, 5, 0);
|
||||
|
||||
Vector* standup = new Vector[2];
|
||||
|
||||
// Animation new
|
||||
int animationSpeed = 20;
|
||||
|
||||
int ConverttoMS(double Position, int low, int high, int _min = 15, int _max = 165, int correction = 0)
|
||||
{
|
||||
//Position += 90;
|
||||
|
||||
if (Position < _min | Position > _max)
|
||||
{
|
||||
Position = 90;
|
||||
Serial.println("ERROR");
|
||||
}
|
||||
|
||||
int tmp = ( Position * ( ( high - low ) / 180 ) );
|
||||
|
||||
//Serial1.println(Position);
|
||||
|
||||
return low + tmp + correction;
|
||||
}
|
||||
|
||||
void mSend(Foot f1, Foot f2, Foot f3, Foot f4, Foot f5, Foot f6)
|
||||
{
|
||||
int smin0 = 600;
|
||||
int smin1 = 960;
|
||||
int smin2 = 600;
|
||||
int smin3 = 600;
|
||||
int smax0 = 2400;
|
||||
int smax1 = 2100;
|
||||
int smax2 = 2400;
|
||||
int smax3 = 2400;
|
||||
|
||||
Serial1.print(" #");
|
||||
Serial1.print(0);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f4.O1, smin0, smax0));
|
||||
Serial1.print(" #");
|
||||
Serial1.print(1);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f4.O2, smin1, smax1));
|
||||
Serial1.print(" #");
|
||||
Serial1.print(2);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f4.O3, smin2, smax2));
|
||||
Serial1.print(" #");
|
||||
Serial1.print(3);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f5.O1, smin0, smax0));
|
||||
Serial1.print(" #");
|
||||
Serial1.print(4);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f5.O2, smin1, smax1));
|
||||
Serial1.print(" #");
|
||||
Serial1.print(5);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f5.O3, smin2, smax2));
|
||||
Serial1.print(" #");
|
||||
Serial1.print(6);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f6.O1, smin0, smax0));
|
||||
Serial1.print(" #");
|
||||
Serial1.print(7);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f6.O2, smin1, smax1));
|
||||
Serial1.print(" #");
|
||||
Serial1.print(8);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f6.O3, smin2, smax2));
|
||||
|
||||
Serial1.print(" #");
|
||||
Serial1.print(9);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(90, smin3, smax3));
|
||||
|
||||
Serial1.print(" #");
|
||||
Serial1.print(16);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f1.O1, smin0, smax0));
|
||||
Serial1.print(" #");
|
||||
Serial1.print(17);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f1.O2, smin1, smax1));
|
||||
Serial1.print(" #");
|
||||
Serial1.print(18);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f1.O3, smin2, smax2));
|
||||
Serial1.print(" #");
|
||||
Serial1.print(19);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f2.O1, smin0, smax0));
|
||||
Serial1.print(" #");
|
||||
Serial1.print(20);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f2.O2, smin1, smax1));
|
||||
Serial1.print(" #");
|
||||
Serial1.print(21);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f2.O3, smin2, smax2));
|
||||
Serial1.print(" #");
|
||||
Serial1.print(22);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f3.O1, smin0, smax0));
|
||||
Serial1.print(" #");
|
||||
Serial1.print(23);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f3.O2, smin1, smax1));
|
||||
Serial1.print(" #");
|
||||
Serial1.print(24);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f3.O3, smin2, smax2));
|
||||
|
||||
Serial1.print(" T");
|
||||
Serial1.println(0);
|
||||
|
||||
|
||||
|
||||
/*Serial << " #";
|
||||
Serial << 0;
|
||||
Serial << " P";
|
||||
Serial << f4.O1;
|
||||
Serial << " #";
|
||||
Serial << 1;
|
||||
Serial << " P";
|
||||
Serial << f4.O2;
|
||||
Serial << " #";
|
||||
Serial << 2;
|
||||
Serial << " P";
|
||||
Serial << f4.O3;
|
||||
Serial << " #";
|
||||
Serial << 3;
|
||||
Serial << " P";
|
||||
Serial << f5.O1;
|
||||
Serial << " #";
|
||||
Serial << 4;
|
||||
Serial << " P";
|
||||
Serial << f5.O2;
|
||||
Serial << " #";
|
||||
Serial << 5;
|
||||
Serial << " P";
|
||||
Serial << f5.O3;
|
||||
Serial << " #";
|
||||
Serial << 6;
|
||||
Serial << " P";
|
||||
Serial << f6.O1;
|
||||
Serial << " #";
|
||||
Serial << 7;
|
||||
Serial << " P";
|
||||
Serial << f6.O2;
|
||||
Serial << " #";
|
||||
Serial << 8;
|
||||
Serial << " P";
|
||||
Serial << f6.O3;
|
||||
|
||||
Serial << " #";
|
||||
Serial << 16;
|
||||
Serial << " P";
|
||||
Serial << f1.O1;
|
||||
Serial << " #";
|
||||
Serial << 17;
|
||||
Serial << " P";
|
||||
Serial << f1.O2;
|
||||
Serial << " #";
|
||||
Serial << 18;
|
||||
Serial << " P";
|
||||
Serial << f1.O3;
|
||||
Serial << " #";
|
||||
Serial << 19;
|
||||
Serial << " P";
|
||||
Serial << f2.O1;
|
||||
Serial << " #";
|
||||
Serial << 20;
|
||||
Serial << " P";
|
||||
Serial << f2.O2;
|
||||
Serial << " #";
|
||||
Serial << 21;
|
||||
Serial << " P";
|
||||
Serial << f2.O3;
|
||||
Serial << " #";
|
||||
Serial << 22;
|
||||
Serial << " P";
|
||||
Serial << f3.O1;
|
||||
Serial << " #";
|
||||
Serial << 23;
|
||||
Serial << " P";
|
||||
Serial << f3.O2;
|
||||
Serial << " #";
|
||||
Serial << 24;
|
||||
Serial << " P";
|
||||
Serial << f3.O3;
|
||||
|
||||
Serial << " T";
|
||||
Serial << 0 << "\n\r";*/
|
||||
}
|
||||
//////////////////////////////////////////////////
|
||||
|
||||
void DrawFeet()
|
||||
{
|
||||
if (TimerFPS_SERVO.Delay(1000 / FPS_SERVO))
|
||||
{
|
||||
foot1.Translate(point1);
|
||||
foot1.SetFootCoordinates(fpoint1);
|
||||
foot1.Invert = true;
|
||||
foot1.setleg();
|
||||
|
||||
foot1.Translate(point2);
|
||||
foot2.SetFootCoordinates(fpoint2);
|
||||
foot2.Invert = true;
|
||||
foot2.setleg();
|
||||
|
||||
foot1.Translate(point3);
|
||||
foot3.SetFootCoordinates(fpoint3);
|
||||
foot3.Invert = true;
|
||||
foot3.setleg();
|
||||
|
||||
foot1.Translate(point4);
|
||||
foot4.SetFootCoordinates(fpoint4);
|
||||
foot4.Invert = false;
|
||||
foot4.setleg();
|
||||
|
||||
foot1.Translate(point5);
|
||||
foot5.SetFootCoordinates(fpoint5);
|
||||
foot5.Invert = false;
|
||||
foot5.setleg();
|
||||
|
||||
foot1.Translate(point6);
|
||||
foot6.SetFootCoordinates(fpoint6);
|
||||
foot6.Invert = false;
|
||||
foot6.setleg();
|
||||
|
||||
mSend(foot1, foot2, foot3, foot4, foot5, foot6);
|
||||
}
|
||||
}
|
||||
|
||||
/*void RobotWalk()
|
||||
{
|
||||
if (robotStanding)
|
||||
{
|
||||
//animationRepeat = true;
|
||||
animationSpeed = 20;
|
||||
|
||||
if (!animationRepeatComplete)
|
||||
if (timerAnimation.Delay(animationSpeed))
|
||||
animationSubStep++;
|
||||
|
||||
Serial1.println("robot walk animation");
|
||||
|
||||
foot1.SetFootCoordinates(Animation(walk1, 8, 0)); // BULLSHIT BUG
|
||||
foot1.SetFootCoordinates(Animation(walk1, 8, 0));
|
||||
foot2.SetFootCoordinates(Animation(walk1, 8, 4));
|
||||
foot3.SetFootCoordinates(Animation(walk1, 8, 0));//
|
||||
foot4.SetFootCoordinates(Animation(walk1, 8, 4));//
|
||||
foot5.SetFootCoordinates(Animation(walk1, 8, 0));
|
||||
foot6.SetFootCoordinates(Animation(walk1, 8, 4));
|
||||
}
|
||||
}*/
|
||||
|
||||
float _animationProgress = 0;
|
||||
bool rotation = false;
|
||||
|
||||
void RobotStandup()
|
||||
{
|
||||
standup[0] = Vector(10, 5, 0);
|
||||
standup[1] = Vector(10, 15, 0);
|
||||
|
||||
if (timerAnimation.Delay(1000 / FPS))
|
||||
{
|
||||
Serial.println("robot standup animation");
|
||||
|
||||
fpoint1.Lerp(standup[0], standup[1], _animationProgress);
|
||||
fpoint2.Lerp(standup[0], standup[1], _animationProgress);
|
||||
fpoint3.Lerp(standup[0], standup[1], _animationProgress);
|
||||
fpoint4.Lerp(standup[0], standup[1], _animationProgress);
|
||||
fpoint5.Lerp(standup[0], standup[1], _animationProgress);
|
||||
fpoint6.Lerp(standup[0], standup[1], _animationProgress);
|
||||
|
||||
Serial.println(point1.Print());
|
||||
|
||||
int _speed = 2;
|
||||
|
||||
_animationProgress = _animationProgress < 1.0f ? (_animationProgress + (1.0f / (FPS * _speed))) : 1.0f;
|
||||
|
||||
/*if (_animationProgress == 1)
|
||||
{
|
||||
int uhh = (millis() / 100) % 100;
|
||||
if (rotation == true)
|
||||
{
|
||||
Serial.println("rotate up");
|
||||
rotationZ = map((millis() / 100) % 100, 0, 99, -25, 25);
|
||||
if (uhh >= 99)
|
||||
{
|
||||
rotation = true;
|
||||
uhh++;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
Serial.println("rotate upside down");
|
||||
rotationZ = map((millis() / 100) % 100, 0, 99, 25, -25);
|
||||
if (uhh >= 99)
|
||||
{
|
||||
rotation = false;
|
||||
uhh++;
|
||||
}
|
||||
}
|
||||
}*/
|
||||
}
|
||||
}
|
||||
|
||||
void DrawBody()
|
||||
{
|
||||
RobotStandup();
|
||||
|
||||
//RobotWalk();
|
||||
|
||||
DrawFeet();
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,53 @@
|
||||
// Version 1
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "CustomTimer.h"
|
||||
#include "HexapodClass.h"
|
||||
#include "Vector.h"
|
||||
#include "Global.h"
|
||||
|
||||
//#include <avr/wdt.h>
|
||||
|
||||
void setup()
|
||||
{
|
||||
//wdt_enable(WDTO_1S);
|
||||
|
||||
walk1[0] = Vector(12, robotHeigth, 2);
|
||||
walk1[1] = Vector(12, robotHeigth, 0);
|
||||
walk1[2] = Vector(12, robotHeigth, -2);
|
||||
walk1[3] = Vector(12, robotHeigth, -2);
|
||||
walk1[4] = Vector(12, robotHeigth - 10, -2);
|
||||
walk1[5] = Vector(12, robotHeigth - 10, 0);
|
||||
walk1[6] = Vector(12, robotHeigth - 10, 2);
|
||||
walk1[7] = Vector(12, robotHeigth, 2);
|
||||
|
||||
Serial.begin(115200);
|
||||
Serial1.begin(115200);
|
||||
|
||||
Serial.println("");
|
||||
Serial.println(" ____ ____ ___ ");
|
||||
Serial.println("/\\ _`\\ __ /\\ _`\\ /\\_ \\ ");
|
||||
Serial.println("\\ \\ \\L\\_\\/\\_\\ _ __ __\\ \\ \\L\\_\\//\\ \\ __ __ ");
|
||||
Serial.println(" \\ \\ _\\/\\/\\ \\/\\`'__\\/'__`\\ \\ _\\/ \\ \\ \\ /\\ \\/\\ \\ ");
|
||||
Serial.println(" \\ \\ \\/ \\ \\ \\ \\ \\//\\ __/\\ \\ \\/ \\_\\ \\_\\ \\ \\_\\ \\ ");
|
||||
Serial.println(" \\ \\_\\ \\ \\_\\ \\_\\\\ \\____\\\\ \\_\\ /\\____\\\\/`____ \\ ");
|
||||
Serial.println(" \\/_/ \\/_/\\/_/ \\/____/ \\/_/ \\/____/ `/___/> \\");
|
||||
Serial.println(" /\\___/");
|
||||
Serial.println(" \\/__/ ");
|
||||
Serial.println("");
|
||||
Serial.println("Version 3.0");
|
||||
delay(800);
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
// the program is alive...for now.
|
||||
//wdt_reset();
|
||||
|
||||
// Rotation calculation, please use transformation matrixes instead.
|
||||
//rotationX = ( counterXRotation % (rotationRange * 4) < (rotationRange * 2) ? -rotationRange + counterXRotation % (rotationRange * 2) : rotationRange + (-1 * (counterXRotation % (rotationRange * 2))) );
|
||||
//rotationY = ( counterYRotation % (rotationRange * 4) < (rotationRange * 2) ? -rotationRange + counterYRotation % (rotationRange * 2) : rotationRange + (-1 * (counterYRotation % (rotationRange * 2))) );
|
||||
//rotationZ = ( counterZRotation % (rotationRange * 4) < (rotationRange * 2) ? -rotationRange + counterZRotation % (rotationRange * 2) : rotationRange + (-1 * (counterZRotation % (rotationRange * 2))) );
|
||||
|
||||
DrawBody();
|
||||
}
|
||||
@@ -0,0 +1,104 @@
|
||||
// Version 1
|
||||
|
||||
//#ifndef Servo_H
|
||||
//#define Servo_H
|
||||
|
||||
/*#include "FootClass.h";
|
||||
|
||||
int ConverttoMS(int Position, int low, int high)
|
||||
{
|
||||
int tmp = ( Position * ( ( high - low ) / 180 ) );
|
||||
return low + tmp;
|
||||
}
|
||||
|
||||
void mSend(Foot f1, Foot f2, Foot f3, Foot f4, Foot f5, Foot f6)
|
||||
{
|
||||
int smin0 = 600;
|
||||
int smin1 = 960;
|
||||
int smin2 = 600;
|
||||
int smin3 = 600;
|
||||
int smax0 = 2400;
|
||||
int smax1 = 2100;
|
||||
int smax2 = 2400;
|
||||
int smax3 = 2400;
|
||||
|
||||
Serial1.print(" #");
|
||||
Serial1.print(0);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f1.O1, smin0, smax0));
|
||||
Serial1.print(" #");
|
||||
Serial1.print(1);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f1.O2, smin1, smax1));
|
||||
Serial1.print(" #");
|
||||
Serial1.print(2);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f1.O3, smin2, smax2));
|
||||
Serial1.print(" #");
|
||||
Serial1.print(4);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f1.O1, smin0, smax0));
|
||||
Serial1.print(" #");
|
||||
Serial1.print(5);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f1.O2, smin1, smax1));
|
||||
Serial1.print(" #");
|
||||
Serial1.print(6);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f1.O3, smin2, smax2));
|
||||
Serial1.print(" #");
|
||||
Serial1.print(7);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f1.O1, smin0, smax0));
|
||||
Serial1.print(" #");
|
||||
Serial1.print(8);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f1.O2, smin1, smax1));
|
||||
Serial1.print(" #");
|
||||
Serial1.print(9);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f1.O3, smin2, smax2));
|
||||
|
||||
Serial1.print(" #");
|
||||
Serial1.print(16);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f1.O1, smin0, smax0));
|
||||
Serial1.print(" #");
|
||||
Serial1.print(17);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f1.O2, smin1, smax1));
|
||||
Serial1.print(" #");
|
||||
Serial1.print(18);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f1.O3, smin2, smax2));
|
||||
Serial1.print(" #");
|
||||
Serial1.print(19);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f1.O1, smin0, smax0));
|
||||
Serial1.print(" #");
|
||||
Serial1.print(20);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f1.O2, smin1, smax1));
|
||||
Serial1.print(" #");
|
||||
Serial1.print(21);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f1.O3, smin2, smax2));
|
||||
Serial1.print(" #");
|
||||
Serial1.print(22);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f1.O1, smin0, smax0));
|
||||
Serial1.print(" #");
|
||||
Serial1.print(23);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f1.O2, smin1, smax1));
|
||||
Serial1.print(" #");
|
||||
Serial1.print(24);
|
||||
Serial1.print(" P");
|
||||
Serial1.print(ConverttoMS(f1.O3, smin2, smax2));
|
||||
|
||||
Serial1.print(" T");
|
||||
Serial1.println(0);
|
||||
}*/
|
||||
|
||||
//#endif
|
||||
|
||||
@@ -0,0 +1,66 @@
|
||||
// Version 1
|
||||
|
||||
#ifndef Servo_H
|
||||
#define Servo_H
|
||||
|
||||
class Vector
|
||||
{
|
||||
public:
|
||||
float x, y, z;
|
||||
Vector(float X, float Y, float Z)
|
||||
{
|
||||
x = X;
|
||||
y = Y;
|
||||
z = Z;
|
||||
}
|
||||
void Init(float X, float Y, float Z)
|
||||
{
|
||||
x = X;
|
||||
y = Y;
|
||||
z = Z;
|
||||
}
|
||||
Vector()
|
||||
{
|
||||
|
||||
}
|
||||
Vector Lerp(Vector start, Vector end, float percent)
|
||||
{
|
||||
Vector _new = (start + ((end - start) * percent));
|
||||
x = _new.x;
|
||||
y = _new.y;
|
||||
z = _new.z;
|
||||
return _new;
|
||||
}
|
||||
Vector operator + (const Vector other)
|
||||
{
|
||||
Vector _new = Vector();
|
||||
_new.x = x + other.x;
|
||||
_new.y = y + other.y;
|
||||
_new.z = z + other.z;
|
||||
|
||||
return _new;
|
||||
}
|
||||
Vector operator - (const Vector other)
|
||||
{
|
||||
Vector _new = Vector();
|
||||
_new.x = x - other.x;
|
||||
_new.y = y - other.y;
|
||||
_new.z = z - other.z;
|
||||
|
||||
return _new;
|
||||
}
|
||||
Vector operator * (const float other)
|
||||
{
|
||||
Vector _new = Vector(x * other, y * other, z * other);
|
||||
|
||||
return _new;
|
||||
}
|
||||
String Print()
|
||||
{
|
||||
return String(x) + " " + String(y) + " " + String(z);
|
||||
}
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
};
|
||||
Reference in new issue
Block a user