#include <Servo.h>
Servo myservo1;
Servo myservo2;
Servo myservo3;
Servo myservo4;
int pos1 = 0;
int pos2 = 0;
int pos3 = 0;
int pos4 = 0;
#define sw1 10
#define sw2 11
#define r_led 8
#define g_led 9
#define sensor1 A0
#define sensor2 A1
#define sensor3 A2
#define sensor4 A3
int vsensor1 = 0;
int vsensor2 = 0;
int vsensor3 = 0;
int vsensor4 = 0;
int ovsensor1 = 0;
int ovsensor2 = 0;
int ovsensor3 = 0;
int ovsensor4 = 0;
unsigned char vlog1[250];
unsigned char vlog2[250];
unsigned char vlog3[250];
unsigned char vlog4[250];
bool osw1=0;
bool osw2=0;
unsigned long swdebto=100ul;
unsigned long sw1deb=millis()+swdebto;
unsigned long sw2deb=millis()+swdebto;
byte rec_size = 1000;
unsigned long rec_rate = 50;
unsigned long rec_rate_counter = 0;
bool rec_active = 0;
bool play_active = 0;
unsigned char rec_counter = 0;
bool blinker;
unsigned long rec_blink_rate = 300ul;
unsigned long rec_blink_ratecounter = millis() + rec_blink_rate;
#define com1 Serial
void setup() {
com1.begin(19200);
com1.println("Micro Arm Started");
myservo1.attach(3);
myservo2.attach(4);
myservo3.attach(5);
myservo4.attach(6);
myservo4.write(155);
pinMode(sw1, INPUT_PULLUP);
pinMode(sw2, INPUT_PULLUP);
pinMode(r_led, OUTPUT);
pinMode(g_led, OUTPUT);
digitalWrite(r_led, LOW);
digitalWrite(g_led, LOW);
digitalWrite(sw1, HIGH);
digitalWrite(sw2, HIGH);
delay(1000);
//tepukTangan(10);
//beriHormat();
}
void playing() {
digitalWrite(g_led, HIGH);
Serial.print("Start playing ... (");
Serial.print(rec_counter);
Serial.println(')');
for (int i=0; i<rec_counter-1; i++) {
Serial.print("Step: ");
Serial.println(i);
moveServos(vlog1[i], vlog2[i], vlog3[i], vlog4[i]);
//delay(rec_rate);
delay(500);
readSW();
if (play_active==0) break;
}
digitalWrite(g_led, LOW);
Serial.println("Stop playing ...");
}
void recording(byte s1, byte s2, byte s3, byte s4) {
if (rec_active==1) {
if (millis() > rec_blink_ratecounter) {
rec_blink_ratecounter = millis() + rec_blink_rate;
if (blinker)
digitalWrite(r_led, LOW); else digitalWrite(r_led, HIGH);
blinker=!blinker;
}
/*if (millis() > rec_rate_counter) {
vlog1[rec_counter] = s1;
vlog2[rec_counter] = s2;
vlog3[rec_counter] = s3;
vlog4[rec_counter] = s4;
Serial.print("Rec step ... ");
Serial.println(rec_counter);
rec_counter++;
if (rec_counter>rec_size-1) {
digitalWrite(r_led, LOW);
Serial.println("Stop recording ...");
rec_active = 0;
} else {
}
rec_rate_counter = millis() + rec_rate;
}*/
}
}
void sw1action() {
if (rec_active==0) {
play_active = 0;
digitalWrite(g_led, LOW);
rec_counter = 0;
Serial.println("Start recording ...");
rec_active = 1;
} else {
Serial.println("Stop recording ...");
digitalWrite(r_led, LOW);
rec_active = 0;
}
}
void sw2action() {
digitalWrite(g_led, HIGH);
if (rec_active==1) {
vlog1[rec_counter] = 180-vsensor1;
vlog2[rec_counter] = 180-vsensor2;
vlog3[rec_counter] = vsensor3;
vlog4[rec_counter] = vsensor4;
Serial.print("Rec step ... ");
Serial.println(rec_counter);
rec_counter++;
} else if (play_active==0) {
rec_active = 0;
play_active = 1;
} else {
play_active = 0;
}
}
void sw3action() {
tepukTangan(10);
}
void readSW() {
bool vsw1 = digitalRead(sw1);
bool vsw2 = digitalRead(sw2);
if (millis()>sw1deb) {
if (vsw1!=osw1) {
osw1 = vsw1;
if (vsw1==0) {
sw1action();
}
}
sw1deb=millis() + swdebto;
}
if (millis()>sw2deb) {
if (vsw2!=osw2) {
osw2 = vsw2;
if (vsw2==0) {
if (vsw1==LOW) {
sw3action();
} else sw2action();
}
}
sw2deb=millis() + swdebto;
}
//if (
}
void tepukTangan(int count) {
moveServos(90, 75, 162, 155); delay(1000);
for (int i = 0; i < count; i++) {
readSW();
myservo4.write(140);delay(120);
myservo4.write(175);delay(120);
}
}
void beriHormat() {
moveServos(90, 75, 162, 141); delay(1000);
for (int i=0; i<45; i++) {
moveServos(90-i, 75, 162, 141); delay(5);
}
for (int i=0; i<45; i++) {
moveServos(45, 75-i, 162, 141); delay(20);
}
for (int i=0; i<45; i++) {
moveServos(45, 30+i, 162, 141); delay(10);
}
for (int i=0; i<90; i++) {
moveServos(45+i, 75, 162, 141); delay(5);
}
for (int i=0; i<45; i++) {
moveServos(135, 75-i, 162, 141); delay(20);
}
for (int i=0; i<45; i++) {
moveServos(135, 30+i, 162, 141); delay(10);
}
for (int i=0; i<45; i++) {
moveServos(135-i, 75, 162, 141); delay(5);
}
for (int i=0; i<45; i++) {
moveServos(90, 75-i, 162, 141); delay(20);
}
for (int i=0; i<45; i++) {
moveServos(90, 30+i, 162, 141); delay(10);
}
delay(1500);
//x,30,162,x
for (int i=0; i<45; i++) {
moveServos(90, 75-i, 162, 141); delay(5);
}
}
void readSensor() {
vsensor1 = analogRead(sensor1);//delay(1);
vsensor2 = analogRead(sensor2);//delay(1);
vsensor3 = analogRead(sensor3);//delay(1);
vsensor4 = analogRead(sensor4);
// 100,964 160,1000 921,150 0, 660
vsensor1 = map(vsensor1, 50, 964, 0,180);
vsensor2 = map(vsensor2, 160, 1000, 0,180);
vsensor3 = map(vsensor3, 1000, 0, 0,180);
vsensor4 = map(vsensor4, 0, 1023, 127,176);
moveServos(180-vsensor1, 180-vsensor2, vsensor3, vsensor4);
recording(180-vsensor1, 180-vsensor2, vsensor3, vsensor4);
// Serial.print(vsensor1);Serial.print(',');
// Serial.print(vsensor2);Serial.print(',');
// Serial.print(vsensor3);Serial.print(',');
// Serial.println(vsensor4);
}
void moveServos(int p1, int p2, int p3, int p4) {
if (p1 >= 0 && p1 <= 180) {
myservo1.write(p1);
}
if (p2 >= 0 && p2 <= 180) myservo2.write(p2);
if (p3 > 0 && p3 <= 180) myservo3.write(p3);
if (p4 > 25) myservo4.write(p4);
}
int serialListener() {
if (com1.available()) {
char channel[10];
int i = 0;
while (com1.available()) {
char ch = com1.read(); delay(5);
if (ch != '\n' && i < 4) {
if (i < 4) {
channel[i] = ch;
i++;
channel[i] = '\0';
}
} else {
pos1 = ((byte) channel[0]) - 30;
pos2 = ((byte) channel[1]) - 30;
pos3 = ((byte) channel[2]) - 30;
pos4 = ((byte) channel[3]) - 30;
// com1.print(pos1);com1.print(',');
// com1.print(pos2);com1.print(',');
// com1.print(pos3);com1.print(',');
// com1.println(pos4);
if (pos1==200 && pos2==201) {
tepukTangan(pos3-200);
} else if (pos1==200 && pos2==202) {
beriHormat();
} else {
moveServos(pos1, pos2, pos3, pos4);
}
channel[0] = '\0';
}
}
return 1;
} else return 0;
}
void loop() {
readSW();
readSensor();
if (play_active==1) {
playing();
}
}