From 1f1715c8414c41b936f5b7ced25c4ae324715f5e Mon Sep 17 00:00:00 2001 From: Administrator Date: Mon, 17 Feb 2014 11:50:52 +0100 Subject: [PATCH] First commit --- .gitattributes | 37 +++ 2014-ik-core-task-queue/Hexapod.h | 317 +++++++++++++++++++++++ 2014-ik-core-task-queue/HexapodRobot.ino | 17 ++ 2014-ik-core-task-queue/SerD.h | 47 ++++ 4 files changed, 418 insertions(+) create mode 100644 .gitattributes create mode 100644 2014-ik-core-task-queue/Hexapod.h create mode 100644 2014-ik-core-task-queue/HexapodRobot.ino create mode 100644 2014-ik-core-task-queue/SerD.h diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000..533fe76 --- /dev/null +++ b/.gitattributes @@ -0,0 +1,37 @@ +*.wmv filter=lfs diff=lfs merge=lfs -text +*.WMV filter=lfs diff=lfs merge=lfs -text +*.mp4 filter=lfs diff=lfs merge=lfs -text +*.MP4 filter=lfs diff=lfs merge=lfs -text +*.MPG filter=lfs diff=lfs merge=lfs -text +*.mpg filter=lfs diff=lfs merge=lfs -text +*.jpg filter=lfs diff=lfs merge=lfs -text +*.JPG filter=lfs diff=lfs merge=lfs -text +*.jpeg filter=lfs diff=lfs merge=lfs -text +*.png filter=lfs diff=lfs merge=lfs -text +*.PNG filter=lfs diff=lfs merge=lfs -text +*.bmp filter=lfs diff=lfs merge=lfs -text +*.exr filter=lfs diff=lfs merge=lfs -text +*.pdf filter=lfs diff=lfs merge=lfs -text +*.docx filter=lfs diff=lfs merge=lfs -text +*.vsdx filter=lfs diff=lfs merge=lfs -text +*.dwg filter=lfs diff=lfs merge=lfs -text +*.f3d filter=lfs diff=lfs merge=lfs -text +*.zip filter=lfs diff=lfs merge=lfs -text +*.zi filter=lfs diff=lfs merge=lfs -text +*.stl filter=lfs diff=lfs merge=lfs -text +*.STL filter=lfs diff=lfs merge=lfs -text +*.HMI filter=lfs diff=lfs merge=lfs -text +*.exe filter=lfs diff=lfs merge=lfs -text +*.dll filter=lfs diff=lfs merge=lfs -text +*.pdb filter=lfs diff=lfs merge=lfs -text +*.idb filter=lfs diff=lfs merge=lfs -text +*.obj filter=lfs diff=lfs merge=lfs -text +*.ipch filter=lfs diff=lfs merge=lfs -text +*.sdf filter=lfs diff=lfs merge=lfs -text +*.suo filter=lfs diff=lfs merge=lfs -text +*.db filter=lfs diff=lfs merge=lfs -text +*.lnk filter=lfs diff=lfs merge=lfs -text +*.ide filter=lfs diff=lfs merge=lfs -text +*.ide-wal filter=lfs diff=lfs merge=lfs -text +*.ide-shm filter=lfs diff=lfs merge=lfs -text +2018-unity-sim/**/*.unity filter=lfs diff=lfs merge=lfs -text diff --git a/2014-ik-core-task-queue/Hexapod.h b/2014-ik-core-task-queue/Hexapod.h new file mode 100644 index 0000000..0b32d07 --- /dev/null +++ b/2014-ik-core-task-queue/Hexapod.h @@ -0,0 +1,317 @@ +// Version 1 + +#ifndef HEX +#define HEX + +#include "Arduino.h" +#include "SerD.h" +#include + +// Angle to Pulse Length converter. +#define smin0 600 +#define smin1 960 +#define smin2 600 +#define smin3 600 +#define smax0 2400 +#define smax1 2100 +#define smax2 2400 +#define smax3 2400 + +// Task Pipeline +#define H_TASK 001 +#define H_IK 002 +#define H_IKER 003 +#define H_ATPC 004 + +// ProgramControl +#define PC_EMPTY 101 +#define PC_NEEDDATA 102 + +// LAYOUT +// _U_ +// 0==/ \==3 +// >***< +// 1===|---|===4 +// >***< +// 2==\___/==5 + +class Hexapod +{ +private: + // Task Pipeline. + QueueList TaskQueue; + QueueList Parameter; + + // Program Control + int PcCommand; + + // Timing. + int OldTime; + int TimeBetweenNewPosition; + + /* Servo Data + # Including coordinate, Angle and Pulse Length. + */ + boolean DataReady; + SerD ServoData[6]; + + // Inverse Kinemetics. + float s; + float l3; +#define l2 15 +#define l1 10 + + void RL3 (int i) + { + float x = ServoData[i].x; + float z = ServoData[i].z; + + if ((x == 0) && (z == 0)) + l3 = 0; + else l3 = hypot(z, x); + + if (x < 0) + l3 *= -1; + } + + float O1 (int i) + { + RL3(i); + + float x = ServoData[i].x; + float z = ServoData[i].z; + float r; + + if ((x != 0) & (z != 0)){ + double a = (z / x); + return atan(a); + } + else + return atan(0.0); // Math Error Protection. + } + + float T1 (int i) + { + float x = ServoData[i].x; + float y = ServoData[i].y; + float z = ServoData[i].z; + float x2 = x * x; + float y2 = y * y; + float z2 = z * z; + float l32 = l3 * l3; + + s = sqrt(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(((l1*l1) + (l2*l2) - (s*s)) / (2 * l1 * l2))); + } + + void setleg (int i) + { + if (i < 3) + { + float a = ((float)90 - degrees(O1(i))); + float b = ((float)180 - degrees(T1(i))); + float c = degrees(T3()); + + if ((a >= 0) & (a <= 180) + & (b >= 0) & (b <= 180) + & (c >= 0) & (c <= 180)) + { + ServoData[i].o1 = a; + ServoData[i].o2 = b; + ServoData[i].o3 = c; + } + else + { + TaskQueue.push(H_IKER); + Parameter.push(i); + } + } + else + { + float a = ((float)90 + degrees(O1(i))); + float b = degrees(T1(i)); + float c = ((float)180 - degrees(T3())); + + if ((a >= 0) & (a <= 180) + & (b >= 0) & (b <= 180) + & (c >= 0) & (c <= 180)) + { + ServoData[i].o1 = a; + ServoData[i].o2 = b; + ServoData[i].o3 = c; + } + else + { + TaskQueue.push(H_IKER); + Parameter.push(i); + } + } + } + + void IK() + { + for (int i=0; i<6; i++) + setleg(i); + } + + // Angle to Pulse Length converter. + + int ConverttoMS(int Position, int low, int high) + { + int tmp = ( Position * ( ( high - low ) / 180 ) ); + return low + tmp; + } + + void ConvertAngleToPulseLength() + { + for (int i=0; i<6; i++) + { + ServoData[i].t1 = ConverttoMS(ServoData[i].o1, smin0, smax0); + ServoData[i].t2 = ConverttoMS(ServoData[i].o2, smin1, smax1); + ServoData[i].t3 = ConverttoMS(ServoData[i].o3, smin2, smax2); + } + } + + void ProgramControl( int Command ) + { + // Drop task from queue + TaskQueue.pop(); + + // Check Command. + switch (Command) + { + case PC_EMPTY: + { + + break; + } + + case PC_NEEDDATA: + { + /* + Queue = { + Get Move + Do IK + Convert To MS + Send } + */ + break; + } + + default: + break; + } + } + +public: + // Task Pipeline. + void loop() + { + PcCommand = 0; + do{ + /* + Runmonitoing methods and + Timingdependent methods. + */ + if (millis() >= (OldTime + TimeBetweenNewPosition)) + { + OldTime = millis(); + } + + //Add Task toqueue + TaskQueue.push(H_TASK); + + //task queue. + + switch ( TaskQueue.peek() ) + { + + case H_TASK: + { + + break; + } + + case H_IK: // Do InverseKinemetics. + { + IK(); + break; + } + + case H_IKER: // Inverse Kinemetics Error. + { + while ( !Parameter.isEmpty() ) + { + // Go troughtparameters. + /* + # W + # I + # P + */ + } + + break; + } + + case H_ATPC: // Angel to pulse length converter. + { + ConvertAngleToPulseLength(); + DataReady = true; + break; + } + + case H_SEND: + { + if (DataReady) + { + // Send + DataReady = false; + // Get New Move + } + else + { + PcCommand = PC_NEEDDATA; + } + break; + } + + default: // TaskList was empty. + { + // Do something when there is no task. + PcCommand = PC_EMPTY; + break; + } + } + + + //Program Control. + ProgramControl( PcCommand ); + } while (1); + } +}; + +#endif + diff --git a/2014-ik-core-task-queue/HexapodRobot.ino b/2014-ik-core-task-queue/HexapodRobot.ino new file mode 100644 index 0000000..0713ffc --- /dev/null +++ b/2014-ik-core-task-queue/HexapodRobot.ino @@ -0,0 +1,17 @@ +// Version 1 +#include "Hexapod.h" +#include + +void setup() +{ + +} + +Hexapod hexapod; + +void loop() +{ + hexapod.loop(); +} + + diff --git a/2014-ik-core-task-queue/SerD.h b/2014-ik-core-task-queue/SerD.h new file mode 100644 index 0000000..b8c088a --- /dev/null +++ b/2014-ik-core-task-queue/SerD.h @@ -0,0 +1,47 @@ +// Version 1 + +#ifndef SerD_H +#define SerD_H + +class SerD +{ +public: + + // Coordinate + int x; + int y; + int z; + + // Angles + int o1; + int o2; + int o3; + + // Pulse Length. + int t1; + int t2; + int t3; + + //Input + SerD& operator() (int X, int Y, int Z) + { + x = X; + y = Y; + z = Z; + + return *this; + } + + SerD& operator= (SerD& R) + { + x = R.x; + y = R.y; + z = R.z; + + return *this; + } + +}; + +#endif +