2026年2月8日 星期日

[靈動狗] 持續循環：坐下 3 秒 → 站立 3 秒

 






程式：

  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
#include <Arduino.h>
#include <ESP32Servo.h>
#include <Preferences.h>

/* ====== 腳位（沿用你原本）====== */
#define LEFTHAND_CTRL_PIN  0
#define RIGHTHAND_CTRL_PIN 1
#define LEFTLEG_CTRL_PIN   10
#define RIGHTLEG_CTRL_PIN  7

Servo leftHandServo, rightHandServo, leftLegServo, rightLegServo;

/* ====== 舵機角度（沿用你原本的變數）====== */
int leftHandAngle  = 90;
int rightHandAngle = 90;
int leftLegAngle   = 90;
int rightLegAngle  = 90;

/* ====== 校正偏移（可選：沿用你原本的 Preferences）====== */
int leftHandOffset  = 0;
int rightHandOffset = 0;
int leftLegOffset   = 0;
int rightLegOffset  = 0;
Preferences preferences;

/* ====== 函式宣告 ====== */
void updateServo(uint8_t ucLeftHand, uint8_t ucRightHand, uint8_t ucLeftLeg, uint8_t ucRightLeg);
void standUp();
void sitDown();

void setup() {
  Serial.begin(115200);

  // 舵機 50Hz
  leftHandServo.setPeriodHertz(50);
  rightHandServo.setPeriodHertz(50);
  leftLegServo.setPeriodHertz(50);
  rightLegServo.setPeriodHertz(50);

  // attach(腳位, 最小脈寬, 最大脈寬)
  leftHandServo.attach(LEFTHAND_CTRL_PIN, 500, 2500);
  rightHandServo.attach(RIGHTHAND_CTRL_PIN, 500, 2500);
  leftLegServo.attach(LEFTLEG_CTRL_PIN, 500, 2500);
  rightLegServo.attach(RIGHTLEG_CTRL_PIN, 500, 2500);

  // 讀取偏移（沒有存過就會是0）
  preferences.begin("servo", true);
  leftHandOffset  = preferences.getInt("leftHandOffset", 0);
  rightHandOffset = preferences.getInt("rightHandOffset", 0);
  leftLegOffset   = preferences.getInt("leftLegOffset", 0);
  rightLegOffset  = preferences.getInt("rightLegOffset", 0);
  preferences.end();

  // 先站好
  standUp();
  delay(500);
}

void loop() {
  // 坐下 3 秒
  Serial.println("Sit down");
  sitDown();
  delay(3000);

  // 站立 3 秒
  Serial.println("Stand up");
  standUp();
  delay(3000);
}

/* ====== 舵機更新（沿用你原本邏輯：加偏移 + 右邊反轉）====== */
void updateServo(uint8_t ucLeftHand, uint8_t ucRightHand, uint8_t ucLeftLeg, uint8_t ucRightLeg) {
  int leftHand  = (int)ucLeftHand  + leftHandOffset;
  int rightHand = (int)ucRightHand + rightHandOffset;
  int leftLeg   = (int)ucLeftLeg   + leftLegOffset;
  int rightLeg  = (int)ucRightLeg  + rightLegOffset;

  leftHand  = constrain(leftHand,  0, 180);
  rightHand = constrain(rightHand, 0, 180);
  leftLeg   = constrain(leftLeg,   0, 180);
  rightLeg  = constrain(rightLeg,  0, 180);

  // 右側舵機角度反轉（沿用你原本）
  rightHand = 180 - rightHand;
  rightLeg  = 180 - rightLeg;

  leftHandServo.write(leftHand);
  rightHandServo.write(rightHand);
  leftLegServo.write(leftLeg);
  rightLegServo.write(rightLeg);
}

/* ====== 站立（沿用你原本的坐標與步進）====== */
void standUp() {
  // 腿先回到 90
  while ((abs(leftLegAngle - 90) > 5) || (abs(rightLegAngle - 90) > 5)) {
    if (leftLegAngle > 90) leftLegAngle -= 3;
    else if (leftLegAngle < 90) leftLegAngle += 3;

    if (rightLegAngle > 90) rightLegAngle -= 3;
    else if (rightLegAngle < 90) rightLegAngle += 3;

    delay(20);
    updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);
  }
  leftLegAngle = 90;
  rightLegAngle = 90;
  updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);

  // 手回到 90
  while ((abs(leftHandAngle - 90) > 5) || (abs(rightHandAngle - 90) > 5)) {
    if (leftHandAngle > 90) leftHandAngle -= 5;
    else if (leftHandAngle < 90) leftHandAngle += 5;

    if (rightHandAngle > 90) rightHandAngle -= 5;
    else if (rightHandAngle < 90) rightHandAngle += 5;

    delay(20);
    updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);
  }
  leftHandAngle = 90;
  rightHandAngle = 90;
  updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);
}

/* ====== 坐下（沿用你原本的坐標與步進）====== */
void sitDown() {
  // 腿到 40
  while ((abs(leftLegAngle - 40) > 5) || (abs(rightLegAngle - 40) > 5)) {
    if (leftLegAngle > 40) leftLegAngle -= 3;
    else if (leftLegAngle < 40) leftLegAngle += 3;

    if (rightLegAngle > 40) rightLegAngle -= 3;
    else if (rightLegAngle < 40) rightLegAngle += 3;

    delay(20);
    updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);
  }
  leftLegAngle = 40;
  rightLegAngle = 40;
  updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);

  // 手到 100
  while ((abs(leftHandAngle - 100) > 5) || (abs(rightHandAngle - 100) > 5)) {
    if (leftHandAngle > 100) leftHandAngle -= 5;
    else if (leftHandAngle < 100) leftHandAngle += 5;

    if (rightHandAngle > 100) rightHandAngle -= 5;
    else if (rightHandAngle < 100) rightHandAngle += 5;

    delay(20);
    updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);
  }
  leftHandAngle = 100;
  rightHandAngle = 100;
  updateServo(leftHandAngle, rightHandAngle, leftLegAngle, rightLegAngle);
}


張貼者： 於
標籤： , ,

沒有留言:

張貼留言

訂閱： 張貼留言 (Atom)