#include /* Just in case of using the vscode ide for arduino programming, not needed if using the commom arduino ide */ #include // library for encoder using #include // library for the IR receiver #define pieza 0 #if pieza==0 #define SYNC_IN A0 #define SYNC_OUT A1 #else #define SYNC_IN A1 #define SYNC_OUT A0 #endif // IR receiver parameters const int IR = 18; // pin connected IRrecv IrReceiv(IR); decode_results result; // IR codes const int codeON = 0xAA1; const int codeOFF = 0xAA5; // Arm & Forearm 1 EN - DIR - PUL(STP) #define ARM1_EN 22 #define ARM1_DIR 23 #define ARM1_PUL 24 #define FOREARM1_EN 25 #define FOREARM1_DIR 26 #define FOREARM1_PUL 27 // Arm & Forearm 2 EN - DIR - PUL(STP) #define ARM2_EN 28 #define ARM2_DIR 29 #define ARM2_PUL 30 #define FOREARM2_EN 31 #define FOREARM2_DIR 32 #define FOREARM2_PUL 33 // Arm & Forearm 3 EN - DIR - PUL(STP) #define ARM3_EN 34 #define ARM3_DIR 35 #define ARM3_PUL 36 #define FOREARM3_EN 37 #define FOREARM3_DIR 38 #define FOREARM3_PUL 39 // Arm & Forearm 4 EN - DIR - PUL(STP) #define ARM4_EN 40 #define ARM4_DIR 41 #define ARM4_PUL 42 #define FOREARM4_EN 43 #define FOREARM4_DIR 44 #define FOREARM4_PUL 45 // Delay between steps == velocity const int delayTime = 250; const int delayTimeF = 250; // Resolution relation const float RELATION_ARM = 21.8; const float RELATION_FOREARM = 20.625;//11.35; #define BACK HIGH #define FRONT LOW void setup() { // set pins as output Serial.begin(9600); pinMode(ARM1_DIR, OUTPUT); pinMode(ARM1_PUL, OUTPUT); pinMode(ARM2_DIR, OUTPUT); pinMode(ARM2_PUL, OUTPUT); pinMode(ARM3_DIR, OUTPUT); pinMode(ARM3_PUL, OUTPUT); pinMode(ARM4_DIR, OUTPUT); pinMode(ARM4_PUL, OUTPUT); pinMode(SYNC_IN, INPUT_PULLUP); pinMode(SYNC_OUT, OUTPUT); digitalWrite(SYNC_OUT, HIGH); pinMode(FOREARM1_PUL, OUTPUT); pinMode(FOREARM1_DIR, OUTPUT); pinMode(FOREARM2_PUL, OUTPUT); pinMode(FOREARM2_DIR, OUTPUT); pinMode(FOREARM3_PUL, OUTPUT); pinMode(FOREARM3_DIR, OUTPUT); pinMode(FOREARM4_PUL, OUTPUT); pinMode(FOREARM4_DIR, OUTPUT); pinMode(ARM1_EN, OUTPUT); pinMode(ARM2_EN, OUTPUT); pinMode(ARM3_EN, OUTPUT); pinMode(ARM4_EN, OUTPUT); pinMode(FOREARM1_EN, OUTPUT); pinMode(FOREARM2_EN, OUTPUT); pinMode(FOREARM3_EN, OUTPUT); pinMode(FOREARM4_EN, OUTPUT); // enable the motor driver digitalWrite(ARM1_EN, LOW); digitalWrite(ARM2_EN, LOW); digitalWrite(ARM3_EN, LOW); digitalWrite(ARM4_EN, LOW); digitalWrite(FOREARM1_EN, LOW); #if pieza==0 digitalWrite(FOREARM2_EN, LOW); #else digitalWrite(FOREARM2_EN, HIGH); #endif digitalWrite(FOREARM3_EN, LOW); digitalWrite(FOREARM4_EN, LOW); //Serial.begin(9600); // or higher baud rate IrReceiv.enableIRIn(); move_arm(BACK, 30, 0); #if pieza==0 delay(20000); #else delay(1000); #endif digitalWrite(SYNC_OUT, LOW); } void move_arm(int dir, int angle, int angleF) { digitalWrite(ARM1_DIR, dir);// digitalWrite(ARM2_DIR, dir);// digitalWrite(ARM3_DIR, dir);// digitalWrite(ARM4_DIR, dir);// digitalWrite(FOREARM1_DIR, dir);// #if pieza==0 digitalWrite(FOREARM2_DIR, dir);// #else digitalWrite(FOREARM2_DIR, !dir);// #endif digitalWrite(FOREARM3_DIR, dir);// digitalWrite(FOREARM4_DIR, dir);// float correction = angle*(800/360.); //Serial.println(correction); int i = 0; while(i < angle * RELATION_ARM) { i++; digitalWrite(ARM1_PUL, HIGH); digitalWrite(ARM2_PUL, HIGH); digitalWrite(ARM3_PUL, HIGH); digitalWrite(ARM4_PUL, HIGH); if (i<(correction + angleF*RELATION_FOREARM)) { digitalWrite(FOREARM1_PUL, HIGH); #if pieza==0 digitalWrite(FOREARM2_PUL, HIGH); #else digitalWrite(FOREARM2_PUL, LOW); #endif digitalWrite(FOREARM3_PUL, HIGH); digitalWrite(FOREARM4_PUL, HIGH); } delayMicroseconds(delayTime); digitalWrite(ARM1_PUL, LOW); digitalWrite(ARM2_PUL, LOW); digitalWrite(ARM3_PUL, LOW); digitalWrite(ARM4_PUL, LOW); if (i<(correction + angleF*RELATION_FOREARM)) { digitalWrite(FOREARM1_PUL, LOW); #if pieza==0 digitalWrite(FOREARM2_PUL, LOW); #else digitalWrite(FOREARM2_PUL, HIGH); #endif digitalWrite(FOREARM3_PUL, LOW); digitalWrite(FOREARM4_PUL, LOW); } delayMicroseconds(delayTime); } while(i<(correction + angleF*RELATION_FOREARM)) { i++; digitalWrite(FOREARM1_PUL, HIGH); #if pieza==0 digitalWrite(FOREARM2_PUL, HIGH); #else digitalWrite(FOREARM2_PUL, LOW); #endif digitalWrite(FOREARM3_PUL, HIGH); digitalWrite(FOREARM4_PUL, HIGH); delayMicroseconds(delayTime); digitalWrite(FOREARM1_PUL, LOW); #if pieza==0 digitalWrite(FOREARM2_PUL, LOW); #else digitalWrite(FOREARM2_PUL, HIGH); #endif digitalWrite(FOREARM3_PUL, LOW); digitalWrite(FOREARM4_PUL, LOW); delayMicroseconds(delayTime); } } void move_forearm(int dir, int angle) { digitalWrite(FOREARM1_DIR, dir);// #if pieza==0 digitalWrite(FOREARM2_DIR, dir);// #else digitalWrite(FOREARM2_DIR, !dir);// #endif digitalWrite(FOREARM3_DIR, dir);// digitalWrite(FOREARM4_DIR, dir);// int i = 0; while(i < angle * RELATION_FOREARM) { i++; digitalWrite(FOREARM1_PUL, HIGH); #if pieza==0 digitalWrite(FOREARM2_PUL, HIGH); #else digitalWrite(FOREARM2_PUL, LOW); #endif digitalWrite(FOREARM3_PUL, HIGH); digitalWrite(FOREARM4_PUL, HIGH); delayMicroseconds(delayTimeF); //getPosition_A1(); //getPosition_FA1(); //Serial.println(); // getPosition_A2(); // getPosition_FA2(); // //Serial.println(); // getPosition_A3(); // getPosition_FA3(); // //Serial.println(); // getPosition_A4(); // getPosition_FA4(); // //Serial.println(); digitalWrite(FOREARM1_PUL, LOW); #if pieza==0 digitalWrite(FOREARM2_PUL, LOW); #else digitalWrite(FOREARM2_PUL, HIGH); #endif digitalWrite(FOREARM3_PUL, LOW); digitalWrite(FOREARM4_PUL, LOW); delayMicroseconds(delayTimeF); } } int code = codeON; void loop() { /*if (IrReceiv.decode(&result)) // If a signal is detected { //Serial.print(result.bits); //Serial.print(": "); //Serial.println(result.value, HEX); // Prints the code received IrReceiv.resume(); // Clean the receiver for new signals code = result.value; if (code!=codeOFF) code = codeON; }*/ switch (code) { case codeON: if(!digitalRead(SYNC_IN)) { move_arm(FRONT, 360 + 30, 0); digitalWrite(SYNC_OUT, HIGH); delay(1000); move_forearm(FRONT, 360 + 90);//Compensacion delay(1000); move_arm(BACK, 30, 90); //move_forearm(BACK, 90);//Compensacion delay(1000); digitalWrite(SYNC_OUT, LOW); } // commands to get data from both arm and forearm, uncomment to separated adjustment of arm and forearm //getPosition_A1(); //getPosition_FA1(); //Serial.println(); // getPosition_A2(); // getPosition_FA2(); // //Serial.println(); // getPosition_A3(); // getPosition_FA3(); // //Serial.println(); // getPosition_A4(); // getPosition_FA4(); // //Serial.println(); /*digitalWrite(ARM1_DIR, HIGH);// digitalWrite(ARM2_DIR, HIGH);// digitalWrite(ARM3_DIR, HIGH);// digitalWrite(ARM4_DIR, HIGH);// for (int i = 0; i < 100 * RELATION_ARM; i++) // multiply the steps for the correct resolution { digitalWrite(ARM1_PUL, HIGH); digitalWrite(ARM2_PUL, HIGH); digitalWrite(ARM3_PUL, HIGH); digitalWrite(ARM4_PUL, HIGH); delayMicroseconds(delayTime); //getPosition_A1(); //getPosition_FA1(); //Serial.println(); // getPosition_A2(); // getPosition_FA2(); // //Serial.println(); // getPosition_A3(); // getPosition_FA3(); // //Serial.println(); // getPosition_A4(); // getPosition_FA4(); // //Serial.println(); digitalWrite(ARM1_PUL, LOW); digitalWrite(ARM2_PUL, LOW); digitalWrite(ARM3_PUL, LOW); digitalWrite(ARM4_PUL, LOW); delayMicroseconds(delayTime); } digitalWrite(ARM1_DIR, LOW);// digitalWrite(ARM2_DIR, LOW);// digitalWrite(ARM3_DIR, LOW);// digitalWrite(ARM4_DIR, LOW);// // ARM: turn 800 steps (360°) into direction 1 for (int i = 0; i < 400 * RELATION_ARM; i++) // multiply the steps for the correct resolution { digitalWrite(ARM1_PUL, HIGH); digitalWrite(ARM2_PUL, HIGH); digitalWrite(ARM3_PUL, HIGH); digitalWrite(ARM4_PUL, HIGH); delayMicroseconds(delayTime); //getPosition_A1(); //getPosition_FA1(); //Serial.println(); // getPosition_A2(); // getPosition_FA2(); // //Serial.println(); // getPosition_A3(); // getPosition_FA3(); // //Serial.println(); // getPosition_A4(); // getPosition_FA4(); // //Serial.println(); digitalWrite(ARM1_PUL, LOW); digitalWrite(ARM2_PUL, LOW); digitalWrite(ARM3_PUL, LOW); digitalWrite(ARM4_PUL, LOW); delayMicroseconds(delayTime); } delay(100); digitalWrite(FOREARM1_DIR, LOW);// #if pieza==0 digitalWrite(FOREARM2_DIR, LOW);// #else digitalWrite(FOREARM2_DIR, HIGH);// #endif digitalWrite(FOREARM3_DIR, LOW);// digitalWrite(FOREARM4_DIR, LOW);// // FOREARM: turn 900 steps (360°+45°) into direction 1 for (int i = 0; i < 900 * RELATION_FOREARM; i++) { digitalWrite(FOREARM1_PUL, HIGH); #if pieza==0 digitalWrite(FOREARM2_PUL, HIGH); #else digitalWrite(FOREARM2_PUL, LOW); #endif digitalWrite(FOREARM3_PUL, HIGH); digitalWrite(FOREARM4_PUL, HIGH); delayMicroseconds(delayTimeF); //getPosition_A1(); //getPosition_FA1(); //Serial.println(); // getPosition_A2(); // getPosition_FA2(); // //Serial.println(); // getPosition_A3(); // getPosition_FA3(); // //Serial.println(); // getPosition_A4(); // getPosition_FA4(); // //Serial.println(); digitalWrite(FOREARM1_PUL, LOW); #if pieza==0 digitalWrite(FOREARM2_PUL, LOW); #else digitalWrite(FOREARM2_PUL, HIGH); #endif digitalWrite(FOREARM3_PUL, LOW); digitalWrite(FOREARM4_PUL, LOW); delayMicroseconds(delayTimeF); } delay(250); digitalWrite(FOREARM1_DIR, HIGH);// #if pieza==0 digitalWrite(FOREARM2_DIR, HIGH);// #else digitalWrite(FOREARM2_DIR, LOW);// #endif digitalWrite(FOREARM3_DIR, HIGH);// digitalWrite(FOREARM4_DIR, HIGH);// // FOREARM: turn 100 steps (-45°) into direction 0 for (int i = 0; i < 100 * RELATION_FOREARM; i++) { digitalWrite(FOREARM1_PUL, HIGH); #if pieza==0 digitalWrite(FOREARM2_PUL, HIGH); #else digitalWrite(FOREARM2_PUL, LOW); #endif digitalWrite(FOREARM3_PUL, HIGH); digitalWrite(FOREARM4_PUL, HIGH); delayMicroseconds(delayTimeF); //getPosition_A1(); //getPosition_FA1(); //Serial.println(); // getPosition_A2(); // getPosition_FA2(); // //Serial.println(); // getPosition_A3(); // getPosition_FA3(); // //Serial.println(); // getPosition_A4(); // getPosition_FA4(); // //Serial.println(); digitalWrite(FOREARM1_PUL, LOW); #if pieza==0 digitalWrite(FOREARM2_PUL, LOW); #else digitalWrite(FOREARM2_PUL, HIGH); #endif digitalWrite(FOREARM3_PUL, LOW); digitalWrite(FOREARM4_PUL, LOW); delayMicroseconds(delayTimeF); } delay(100); digitalWrite(ARM1_DIR, HIGH);// digitalWrite(ARM2_DIR, HIGH);// digitalWrite(ARM3_PUL, HIGH);// digitalWrite(ARM4_PUL, HIGH);// // ARM: turn 100 steps (-45°) into direction 0 for (int i = 0; i < 100 * RELATION_ARM; i++) { digitalWrite(ARM1_PUL, HIGH); digitalWrite(ARM2_PUL, HIGH); digitalWrite(ARM3_PUL, HIGH); digitalWrite(ARM4_PUL, HIGH); delayMicroseconds(delayTime); //getPosition_A1(); //getPosition_FA1(); //Serial.println(); // getPosition_A2(); // getPosition_FA2(); // //Serial.println(); // getPosition_A3(); // getPosition_FA3(); // //Serial.println(); // getPosition_A4(); // getPosition_FA4(); // //Serial.println(); digitalWrite(ARM1_PUL, LOW); digitalWrite(ARM2_PUL, LOW); digitalWrite(ARM3_PUL, LOW); digitalWrite(ARM4_PUL, LOW); delayMicroseconds(delayTime); } delay(1000); digitalWrite(ARM1_DIR, LOW);// digitalWrite(ARM2_DIR, LOW);// digitalWrite(ARM3_DIR, LOW);// digitalWrite(ARM4_DIR, LOW);// // FOREARM: turn 100 steps (45°) into direction 1 for (int i = 0; i < 100 * RELATION_ARM; i++) { digitalWrite(ARM1_PUL, HIGH); digitalWrite(ARM2_PUL, HIGH); digitalWrite(ARM3_PUL, HIGH); digitalWrite(ARM4_PUL, HIGH); delayMicroseconds(delayTime); //getPosition_A1(); //getPosition_FA1(); //Serial.println(); // getPosition_A2(); // getPosition_FA2(); // //Serial.println(); // getPosition_A3(); // getPosition_FA3(); // //Serial.println(); // getPosition_A4(); // getPosition_FA4(); // //Serial.println(); digitalWrite(ARM1_PUL, LOW); digitalWrite(ARM2_PUL, LOW); digitalWrite(ARM3_PUL, LOW); digitalWrite(ARM4_PUL, LOW); delayMicroseconds(delayTime); }*/ break; case codeOFF: // nothing goes here break; default: //Serial.println("ERROR"); break; } }