estha/FinalCodeBase.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
// FINAL CODEBASE: OPTIMISED SPECIFICALLY FOR ARDUINO IDE
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <ESP8266WiFi.h>

// Initialize the PWM servo driver
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();

// Define minimum and maximum pulse lengths for the servos
#define SERVOMIN 150
#define SERVOMAX 600

// Define the servo channels for each leg joint
#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 convert an angle in degrees to a pulse length
int angleToPulse(int angle) {
  return map(angle, 0, 180, SERVOMIN, SERVOMAX);
}

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

// Helper function to move all servos to specified angles
void moveAllServos(int hipAngle, int kneeAngle, int ankleAngle) {
  moveServo(LEG1_HIP, hipAngle);
  moveServo(LEG1_KNEE, kneeAngle);
  moveServo(LEG1_ANKLE, ankleAngle);

  moveServo(LEG2_HIP, hipAngle);
  moveServo(LEG2_KNEE, kneeAngle);
  moveServo(LEG2_ANKLE, ankleAngle);

  moveServo(LEG3_HIP, hipAngle);
  moveServo(LEG3_KNEE, kneeAngle);
  moveServo(LEG3_ANKLE, ankleAngle);

  moveServo(LEG4_HIP, hipAngle);
  moveServo(LEG4_KNEE, kneeAngle);
  moveServo(LEG4_ANKLE, ankleAngle);
}

// Function to set the robot to a sitting position
void sit() {
  moveAllServos(90, 90, 90);
}

// Function to set the robot to a standby position
void standBy() {
  moveAllServos(90, 90, 90);
}

// Function to make the robot walk forward
void forwardWalk() {
  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();  // Return to standby position after walking
}

// Function to make the robot perform a "bye-bye" wave
void byeBye() {
  moveServo(LEG1_HIP, 60);
  moveServo(LEG1_KNEE, 120);
  moveServo(LEG1_ANKLE, 90);

  delay(500);

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

  delay(500);

  moveAllServos(90, 90, 90); // Equivalent to standBy
}

// Function to make the robot perform a "hello" wave
void hello() {
  moveServo(LEG1_HIP, 120);
  moveServo(LEG1_KNEE, 90);
  moveServo(LEG1_ANKLE, 90);

  delay(500);

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

  delay(500);

  moveAllServos(90, 90, 90); // Equivalent to standBy
}

// Setup function to initialize the servo driver and set the robot to standby position
void setup() {
  Serial.begin(115200);           // Initialize serial communication at 115200 baud
  pwm.begin();                    // Initialize the PWM servo driver
  pwm.setPWMFreq(60);             // Set the PWM frequency to 60 Hz
  standBy();                      // Set the robot to standby position
}

// Main loop function to listen for serial commands and execute corresponding actions
void loop() {
  if (Serial.available()) {
    String command = Serial.readStringUntil('\n');  // Read the command from serial input
    command.trim();                                 // Trim any leading/trailing whitespace

    // Execute actions based on the received command
    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");  // Print error message for invalid commands
    }
  }
}