139 lines
2.7 KiB
Arduino
139 lines
2.7 KiB
Arduino
#include <SoftwareSerial.h>
|
|
#include <Servo.h>
|
|
|
|
SoftwareSerial BT(3, 2); // RX, TX
|
|
|
|
Servo servo1, servo2, servo3, servo4;
|
|
|
|
int cur1, cur2, cur3, cur4;
|
|
|
|
void forceHomePose() {
|
|
servo1.write(50);
|
|
servo2.write(0);
|
|
servo3.write(140);
|
|
servo4.write(10);
|
|
|
|
delay(1200);
|
|
|
|
cur1 = 50; cur2 = 0; cur3 = 140; cur4 = 10;
|
|
}
|
|
|
|
void moveServoSmooth(Servo &s, int &cur, int target, int step, int dly) {
|
|
if (cur == target) return;
|
|
|
|
int dir = (target > cur) ? 1 : -1;
|
|
step = abs(step);
|
|
|
|
while (cur != target) {
|
|
cur += dir * step;
|
|
if ((dir > 0 && cur > target) || (dir < 0 && cur < target)) cur = target;
|
|
s.write(cur);
|
|
delay(dly);
|
|
}
|
|
}
|
|
|
|
void setHomePoseSmooth() {
|
|
moveServoSmooth(servo1, cur1, 50, 4, 20);
|
|
moveServoSmooth(servo2, cur2, 0, 4, 20);
|
|
moveServoSmooth(servo3, cur3, 140,4, 20);
|
|
moveServoSmooth(servo4, cur4, 10, 4, 20);
|
|
}
|
|
|
|
void action1() {
|
|
for (int i = 0; i <= 100; i += 4) {
|
|
int a3 = map(i, 0, 100, 140, 0);
|
|
servo3.write(a3);
|
|
cur3 = a3;
|
|
delay(10);
|
|
}
|
|
for (int i = 0; i <= 100; i += 4) {
|
|
int a2 = map(i, 0, 100, 0, 120);
|
|
int a3 = map(i, 0, 100, 0, 60);
|
|
servo2.write(a2);
|
|
servo3.write(a3);
|
|
cur2 = a2;
|
|
cur3 = a3;
|
|
delay(10);
|
|
}
|
|
|
|
delay(2000);
|
|
setHomePoseSmooth();
|
|
}
|
|
|
|
void action2() {
|
|
int start2 = cur2;
|
|
int start3 = cur3;
|
|
int start4 = cur4;
|
|
|
|
for (int t = 0; t <= 100; t += 4) {
|
|
int a2 = map(t, 0, 100, start2, 80);
|
|
int a3 = map(t, 0, 100, start3, 60);
|
|
int a4 = map(t, 0, 100, start4, 60);
|
|
servo2.write(a2);
|
|
servo3.write(a3);
|
|
servo4.write(a4);
|
|
cur2 = a2;
|
|
cur3 = a3;
|
|
cur4 = a4;
|
|
delay(10);
|
|
}
|
|
|
|
moveServoSmooth(servo1, cur1, 180, 4, 30);
|
|
moveServoSmooth(servo1, cur1, 0, 4, 30);
|
|
moveServoSmooth(servo1, cur1, 180, 4, 30);
|
|
moveServoSmooth(servo1, cur1, 0, 4, 30);
|
|
moveServoSmooth(servo1, cur1, 180, 4, 30);
|
|
moveServoSmooth(servo1, cur1, 50, 4, 30);
|
|
|
|
delay(2000);
|
|
setHomePoseSmooth();
|
|
}
|
|
|
|
void action3() {
|
|
for (int i = 0; i <= 100; i += 4) {
|
|
int a3 = map(i, 0, 100, 140, 0);
|
|
servo3.write(a3);
|
|
cur3 = a3;
|
|
delay(10);
|
|
}
|
|
delay(500);
|
|
for (int i = 0; i <= 100; i += 4) {
|
|
int a2 = map(i, 0, 100, 0, 120);
|
|
servo2.write(a2);
|
|
cur2 = a2;
|
|
delay(5);
|
|
}
|
|
|
|
delay(2000);
|
|
setHomePoseSmooth();
|
|
}
|
|
|
|
void setup() {
|
|
Serial.begin(9600);
|
|
BT.begin(9600);
|
|
|
|
servo1.attach(4);
|
|
servo2.attach(5);
|
|
servo3.attach(6);
|
|
servo4.attach(7);
|
|
|
|
// Home after start
|
|
forceHomePose();
|
|
|
|
Serial.println("Ready");
|
|
}
|
|
|
|
void loop() {
|
|
if (BT.available()) {
|
|
char c = BT.read();
|
|
if (c == '1') action1();
|
|
else if (c == '2') action2();
|
|
else if (c == '3') action3();
|
|
else if (c == 'H' || c == 'h') setHomePoseSmooth();
|
|
}
|
|
|
|
if (Serial.available()) {
|
|
BT.write(Serial.read());
|
|
}
|
|
}
|