estha/OriginalCodebase.cpp at main · surgeon112005/estha · GitHub

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
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
// ANKIT'S ORIGINAL CODEBASE
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <ESP8266WiFi.h>

// Initialize PCA9685
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

// Servo configurations
#define SERVOMIN 150 // Minimum pulse length for servo
#define SERVOMAX 600 // Maximum pulse length for servo

// Define servo channels for legs (each leg has three joints: hip, knee, and ankle)
#define LEG1_HIP 0
#define LEG1_KNEE 1
#define LEG1_ANKLE 2

#define LEG2_HIP 3
#define LEG2_KNEE 4
#define LEG2_ANKLE 5

#define LEG3_HIP 6
#define LEG3_KNEE 7
#define LEG3_ANKLE 8

#define LEG4_HIP 9
#define LEG4_KNEE 10
#define LEG4_ANKLE 11

// Function to map servo angles to pulse width
int angleToPulse(int angle) {
  return map(angle, 0, 180, SERVOMIN, SERVOMAX);
}

// Function to move a servo
void moveServo(int channel, int angle) {
  pwm.setPWM(channel, 0, angleToPulse(angle));
}

// Function to perform "Sit" posture
void sit() {
  moveServo(LEG1_HIP, 90);
  moveServo(LEG1_KNEE, 90);
  moveServo(LEG1_ANKLE, 90);

  moveServo(LEG2_HIP, 90);
  moveServo(LEG2_KNEE, 90);
  moveServo(LEG2_ANKLE, 90);

  moveServo(LEG3_HIP, 90);
  moveServo(LEG3_KNEE, 90);
  moveServo(LEG3_ANKLE, 90);

  moveServo(LEG4_HIP, 90);
  moveServo(LEG4_KNEE, 90);
  moveServo(LEG4_ANKLE, 90);
}

// Function to perform "Stand-By" posture (neutral position)
void standBy() {
  moveServo(LEG1_HIP, 90);
  moveServo(LEG1_KNEE, 90);
  moveServo(LEG1_ANKLE, 90);

  moveServo(LEG2_HIP, 90);
  moveServo(LEG2_KNEE, 90);
  moveServo(LEG2_ANKLE, 90);

  moveServo(LEG3_HIP, 90);
  moveServo(LEG3_KNEE, 90);
  moveServo(LEG3_ANKLE, 90);

  moveServo(LEG4_HIP, 90);
  moveServo(LEG4_KNEE, 90);
  moveServo(LEG4_ANKLE, 90);
}

// Function to perform "Forward Walk"
void forwardWalk() {
  // Sample walk sequence (just an example)
  moveServo(LEG1_HIP, 90);
  moveServo(LEG1_KNEE, 70);
  moveServo(LEG1_ANKLE, 90);

  moveServo(LEG2_HIP, 90);
  moveServo(LEG2_KNEE, 110);
  moveServo(LEG2_ANKLE, 90);

  moveServo(LEG3_HIP, 90);
  moveServo(LEG3_KNEE, 70);
  moveServo(LEG3_ANKLE, 90);

  moveServo(LEG4_HIP, 90);
  moveServo(LEG4_KNEE, 110);
  moveServo(LEG4_ANKLE, 90);
  standBy();
}

// Function to perform "Bye-Bye" (e.g., waving one leg)
void byeBye() {
  moveServo(LEG1_HIP, 60);
  moveServo(LEG1_KNEE, 120);
  moveServo(LEG1_ANKLE, 90);

  delay(500); // Hold position for a brief moment

  moveServo(LEG1_HIP, 120);
  moveServo(LEG1_KNEE, 60);
  moveServo(LEG1_ANKLE, 90);

  delay(500); // Hold position for a brief moment

  moveServo(LEG1_HIP, 90); // Return to neutral position
  moveServo(LEG1_KNEE, 90);
  moveServo(LEG1_ANKLE, 90);
  standBy();
}

// Function to perform "Hello" (e.g., raising one leg)
void hello() {
  moveServo(LEG1_HIP, 120);
  moveServo(LEG1_KNEE, 90);
  moveServo(LEG1_ANKLE, 90);
  moveServo (LEG1_HIP, 120);
  moveServo(LEG1_KNEE, 90);
  moveServo(LEG1_ANKLE, 90);
  standBy();
}

void setup() {
  Serial.begin(115200);
  pwm.begin();
  pwm.setPWMFreq(60); // Set frequency to 60 Hz

  // Initialize servos to neutral position (90°)
  standBy();
}

void loop() {
  if (Serial.available()) {
    String command = Serial.readStringUntil('\n');
    command.trim();

    if (command == "F") {
      forwardWalk();
    } else if (command == "Y") {
      byeBye();
    } else if (command == "H") {
      hello();
    } else if (command == "S") {
      sit();
    } else if (command == "s") {
      standBy();
    } else {
      Serial.println("Invalid command");
    }
  }
}