How to Build 6 DOF Robot Arm from DFRobot Kit

1//  Lee Curiosity 
2// Innstall the Adafruit_PWMServoDriver for PCA9685 for control the servo motors
3#include <Wire.h>
4#include <Adafruit_PWMServoDriver.h>
5
6Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
7
8// Analog pins & PWM channels for servos
9const int sensorPins[6] = {A0, A1, A2, A3, A6, A7};
10const int pwmChannels[6] = {0, 1, 2, 3, 4, 5};
11
12// Standard servo pulse range
13const int minPulse = 500;
14const int maxPulse = 2500;
15const int minPWM = 150;
16const int maxPWM = 600;
17
18// 360° base servo — stop around 375
19const int baseMin = 300;
20const int baseMax = 450;
21
22// Button config
23const int buttonPin = 6;
24bool isPlaying = false;
25
26int steps[20][6];  // Store up to 20 steps
27int stepCount = 0;
28
29unsigned long buttonPressTime = 0;
30bool buttonPressed = false;
31
32// Store previous analog values to avoid jitter
33int lastAnalogVals[6] = {0};
34
35void setup() {
36  Serial.begin(115200);
37  pwm.begin();
38  pwm.setPWMFreq(50);
39
40  pinMode(buttonPin, INPUT_PULLUP);
41  Serial.println("🤖 Ready to SAVE & LOOP PLAYBACK");
42}
43
44void loop() {
45  if (!isPlaying) {
46    // Manual control mode
47    for (int i = 0; i < 6; i++) {
48      int val = analogRead(sensorPins[i]);
49      if (abs(val - lastAnalogVals[i]) > 5) {  // Update only if value changes significantly
50        lastAnalogVals[i] = val;
51        int pwmVal;
52
53        if (i == 0) {
54          pwmVal = map(val, 0, 1023, baseMin, baseMax);
55        } else {
56          int pulse = map(val, 0, 1023, minPulse, maxPulse);
57          pwmVal = map(pulse, minPulse, maxPulse, minPWM, maxPWM);
58        }
59
60        pwm.setPWM(pwmChannels[i], 0, pwmVal);
61      }
62    }
63
64    // Button handling
65    if (digitalRead(buttonPin) == LOW) {
66      if (!buttonPressed) {
67        buttonPressed = true;
68        buttonPressTime = millis();
69      } else {
70        if (millis() - buttonPressTime > 2000) {
71          Serial.println("▶️ STARTING LOOP PLAYBACK...");
72          isPlaying = true;
73          buttonPressed = false;
74          delay(500);
75        }
76      }
77    } else {
78      if (buttonPressed && millis() - buttonPressTime < 500) {
79        if (stepCount < 20) {
80          for (int i = 0; i < 6; i++) {
81            steps[stepCount][i] = analogRead(sensorPins[i]);
82          }
83          Serial.print("✅ Saved step "); Serial.println(stepCount);
84          stepCount++;
85          delay(300);
86        } else {
87          Serial.println("⚠️ Memory full (20 steps max)");
88        }
89      }
90      buttonPressed = false;
91    }
92
93    delay(30);
94  } else {
95    // === LOOP PLAYBACK: Pick - Drop - Return to base ===
96    while (true) {
97      for (int s = 1; s < stepCount; s++) {
98        moveToStep(s);
99        delay(1000);
100      }
101      moveToStep(0);
102      delay(1000);
103    }
104  }
105}
106
107void moveToStep(int s) {
108  for (int i = 0; i < 6; i++) {
109    int val = steps[s][i];
110    int pwmVal;
111
112    if (i == 0) {
113      pwmVal = map(val, 0, 1023, baseMin, baseMax);
114    } else {
115      int pulse = map(val, 0, 1023, minPulse, maxPulse);
116      pwmVal = map(pulse, minPulse, maxPulse, minPWM, maxPWM);
117    }
118
119    pwm.setPWM(pwmChannels[i], 0, pwmVal);
120  }
121}