537 lines
15 KiB
Arduino
537 lines
15 KiB
Arduino
|
#include <Arduino.h> /* Just in case of using the vscode ide for arduino programming, not needed if using the commom arduino ide */
|
||
|
|
#include <RotaryEncoder.h> // library for encoder using
|
||
|
|
#include <IRremote.hpp> // 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;
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|