-
Notifications
You must be signed in to change notification settings - Fork 0
/
Kinematics.h
129 lines (105 loc) · 4.34 KB
/
Kinematics.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
#include <math.h>
#include "Servos.h"
#define PI 3.1415926535897932384626433832795
// Gait settings
#define GAIT_STEP_DURATION 500 // milliseconds
#define GAIT_STEP_HEIGHT 20 // millimeters
#define GAIT_STEP_DISTANCE 30 // millimeters
// Robot dimensions
const float UPPER_SEGMENT_LENGTH = 52.5;
const float MIDDLE_SEGMENT_LENGTH = 63.5;
const float LOWER_SEGMENT_LENGTH = 54.0;
// Robot states
enum RobotState { STANDING, WALKING };
RobotState currentState = STANDING;
// Forward declarations
void setLegPosition(LegChannels leg, float x, float y, float z);
void setLegAngles(LegChannels leg, int shoulderAngle, int thighAngle, int kneeAngle);
void moveLeg(LegChannels leg, float xOffset, float yOffset, float zOffset, int duration);
void applyGaitCycle(LegChannels leg, int duration);
void walkForward(int steps, int duration);
void maintainPosture();
void changeState(RobotState newState);
void setupKinematics() {
setupServos();
maintainPosture();
}
void setLegPosition(LegChannels leg, float x, float y, float z) {
float L1 = UPPER_SEGMENT_LENGTH;
float L2 = MIDDLE_SEGMENT_LENGTH;
float L3 = LOWER_SEGMENT_LENGTH;
float alpha, beta, gamma;
// Inverse kinematics equations
alpha = atan2(y, x);
float A = -z;
float B = L1 - sqrt(x * x + y * y);
float C = (A * A + B * B - L2 * L2 - L3 * L3) / (2 * L2 * L3);
gamma = atan2(sqrt(1 - C * C), C);
float D = atan2(L3 * sin(gamma), L2 + L3 * cos(gamma));
beta = atan2(B - A * tan(D), A + B * tan(D)) - D;
// Convert radians to degrees
int shoulderAngle = int(alpha * 180 / PI);
int thighAngle = int(beta * 180 / PI);
int kneeAngle = int(gamma * 180 / PI);
Serial.println("DEBUG: Inverse kinematics");
Serial.println("- Equations: ");
Serial.print(" alpha: ");
Serial.print(alpha);
Serial.print(" | beta: ");
Serial.print(beta);
Serial.print(" | gama: ");
Serial.println(gamma);
Serial.println("- Convert radians to degrees: ");
Serial.print(" shoulder: ");
Serial.print(shoulderAngle);
Serial.print(" | thigh: ");
Serial.print(thighAngle);
Serial.print(" | knee: ");
Serial.println(kneeAngle);
Serial.println("-------------------------------------------");
setLegAngles(leg, shoulderAngle, thighAngle, kneeAngle);
}
void setLegAngles(LegChannels leg, int shoulderAngle, int thighAngle, int kneeAngle) {
setLegSectionAngle(leg.upper, shoulderAngle);
setLegSectionAngle(leg.middle, thighAngle);
setLegSectionAngle(leg.lower, kneeAngle);
}
void moveLeg(LegChannels leg, float xOffset, float yOffset, float zOffset, int duration) {
int shoulderAngle = map(pwm.getPWM(leg.upper.channel), SERVO_MIN_PULSE, SERVO_MAX_PULSE, SERVO_MIN_ANGLE, SERVO_MAX_ANGLE);
int thighAngle = map(pwm.getPWM(leg.middle.channel), SERVO_MIN_PULSE, SERVO_MAX_PULSE, SERVO_MIN_ANGLE, SERVO_MAX_ANGLE);
int kneeAngle = map(pwm.getPWM(leg.lower.channel), SERVO_MIN_PULSE, SERVO_MAX_PULSE, SERVO_MIN_ANGLE, SERVO_MAX_ANGLE);
float x = UPPER_SEGMENT_LENGTH * cos(shoulderAngle * PI / 180) + MIDDLE_SEGMENT_LENGTH * cos((shoulderAngle + thighAngle) * PI / 180) + LOWER_SEGMENT_LENGTH * cos((shoulderAngle + thighAngle + kneeAngle) * PI / 180);
float y = UPPER_SEGMENT_LENGTH * sin(shoulderAngle * PI / 180);
float z = MIDDLE_SEGMENT_LENGTH * sin((shoulderAngle + thighAngle) * PI / 180) + LOWER_SEGMENT_LENGTH * sin((shoulderAngle + thighAngle + kneeAngle) * PI / 180);
float targetX = x + xOffset;
float targetY = y + yOffset;
float targetZ = z + zOffset;
setLegPosition(leg, targetX, targetY, targetZ);
}
void applyGaitCycle(LegChannels leg, int duration) {
moveLeg(leg, GAIT_STEP_DISTANCE / 2, 0, GAIT_STEP_HEIGHT, duration / 2);
moveLeg(leg, GAIT_STEP_DISTANCE / 2, 0, -GAIT_STEP_HEIGHT, duration / 2);
}
void walkForward(int steps, int duration) {
for (int step = 0; step < steps; step++) {
applyGaitCycle(frontLeftLeg, duration);
applyGaitCycle(frontRightLeg, duration);
}
}
void maintainPosture() {
setLegAngles(frontLeftLeg, frontLeftLeg.upper.midAngle, frontLeftLeg.middle.midAngle, frontLeftLeg.lower.midAngle);
setLegAngles(frontRightLeg, frontRightLeg.upper.midAngle, frontRightLeg.middle.midAngle, frontRightLeg.lower.midAngle);
}
void changeState(RobotState newState) {
if (currentState == newState) {
return;
}
currentState = newState;
switch (currentState) {
case STANDING:
maintainPosture();
break;
case WALKING:
break;
}
}