377 lines
8.2 KiB
C
377 lines
8.2 KiB
C
// 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
|
|
|