360 lines
8.5 KiB
C++
360 lines
8.5 KiB
C++
#include <Arduino.h>
|
|
#include <RotaryEncoder.h>
|
|
#include <IRremote.hpp>
|
|
|
|
/* CODE FOR 1/4 STEPS, 800 STEPS PER REVOLUTION, 1:1 RESOLUTION */
|
|
|
|
// Arm & Forearm 1 EN - DIR - PUL(STP)
|
|
#define ARM1_EN 22
|
|
#define ARM1_DIR 23
|
|
#define ARM1_STP 24
|
|
#define FOREARM1_EN 25
|
|
#define FOREARM1_DIR 26
|
|
#define FOREARM1_STP 27
|
|
|
|
// Arm & Forearm 2 EN - DIR - PUL(STP)
|
|
#define ARM2_EN 28
|
|
#define ARM2_DIR 29
|
|
#define ARM2_STP 30
|
|
#define FOREARM2_EN 31
|
|
#define FOREARM2_DIR 32
|
|
#define FOREARM2_STP 33
|
|
|
|
// Arm & Forearm 3 EN - DIR - PUL(STP)
|
|
#define ARM3_EN 34
|
|
#define ARM3_DIR 35
|
|
#define ARM3_STP 36
|
|
#define FOREARM3_EN 37
|
|
#define FOREARM3_DIR 38
|
|
#define FOREARM3_STP 39
|
|
|
|
// Arm & Forearm 4 EN - DIR - PUL(STP)
|
|
#define ARM4_EN 40
|
|
#define ARM4_DIR 41
|
|
#define ARM4_STP 42
|
|
#define FOREARM4_EN 43
|
|
#define FOREARM4_DIR 44
|
|
#define FOREARM4_STP 45
|
|
|
|
// Delay between steps == velocity
|
|
const int delayTime = 200;
|
|
|
|
// Resolution relation
|
|
const float RELATION_ARM = 19.1;
|
|
const float RELATION_FOREARM = 11.35;
|
|
|
|
// Rotary encoder pins
|
|
#define DT_A1 2
|
|
#define CLK_A1 3
|
|
#define DT_FA1 4
|
|
#define CLK_FA1 5
|
|
RotaryEncoder encoder_A1(DT_A1, CLK_A1, RotaryEncoder::LatchMode::TWO03);
|
|
RotaryEncoder encoder_FA1(DT_FA1, CLK_FA1, RotaryEncoder::LatchMode::TWO03);
|
|
|
|
#define DT_A2 6
|
|
#define CLK_A2 7
|
|
#define DT_FA2 8
|
|
#define CLK_FA2 9
|
|
RotaryEncoder encoder_A2(DT_A2, CLK_A2, RotaryEncoder::LatchMode::TWO03);
|
|
RotaryEncoder encoder_FA2(DT_FA2, CLK_FA2, RotaryEncoder::LatchMode::TWO03);
|
|
|
|
#define DT_A3 10
|
|
#define CLK_A3 11
|
|
#define DT_FA3 12
|
|
#define CLK_FA3 13
|
|
RotaryEncoder encoder_A3(DT_A3, CLK_A3, RotaryEncoder::LatchMode::TWO03);
|
|
RotaryEncoder encoder_FA3(DT_FA3, CLK_FA3, RotaryEncoder::LatchMode::TWO03);
|
|
|
|
#define DT_A4 14
|
|
#define CLK_A4 15
|
|
#define DT_FA4 16
|
|
#define CLK_FA4 17
|
|
RotaryEncoder encoder_A4(DT_A4, CLK_A4, RotaryEncoder::LatchMode::TWO03);
|
|
RotaryEncoder encoder_FA4(DT_A4, CLK_FA4, RotaryEncoder::LatchMode::TWO03);
|
|
|
|
void getPosition_A1()
|
|
{
|
|
static int pos = 0;
|
|
encoder_A1.tick();
|
|
int newPos = encoder_A1.getPosition();
|
|
if (pos != newPos)
|
|
{
|
|
pos = newPos;
|
|
}
|
|
Serial.print(" | ARM1 position = ");
|
|
Serial.print(pos);
|
|
Serial.print(" |");
|
|
}
|
|
void getPosition_FA1()
|
|
{
|
|
static int pos = 0;
|
|
encoder_FA1.tick();
|
|
int newPos = encoder_FA1.getPosition();
|
|
if (pos != newPos)
|
|
{
|
|
pos = newPos;
|
|
}
|
|
Serial.print(" | FOREARM1 position = ");
|
|
Serial.print(pos);
|
|
Serial.print(" |");
|
|
}
|
|
|
|
void getPosition_A2()
|
|
{
|
|
static int pos = 0;
|
|
encoder_A2.tick();
|
|
int newPos = encoder_A2.getPosition();
|
|
if (pos != newPos)
|
|
{
|
|
pos = newPos;
|
|
}
|
|
Serial.print(" | ARM2 position = ");
|
|
Serial.print(pos);
|
|
Serial.print(" |");
|
|
}
|
|
void getPosition_FA2()
|
|
{
|
|
static int pos = 0;
|
|
encoder_FA2.tick();
|
|
int newPos = encoder_FA2.getPosition();
|
|
if (pos != newPos)
|
|
{
|
|
pos = newPos;
|
|
}
|
|
Serial.print(" | FOREARM2 position = ");
|
|
Serial.print(pos);
|
|
Serial.print(" |");
|
|
}
|
|
|
|
void getPosition_A3()
|
|
{
|
|
static int pos = 0;
|
|
encoder_A3.tick();
|
|
int newPos = encoder_A3.getPosition();
|
|
if (pos != newPos)
|
|
{
|
|
pos = newPos;
|
|
}
|
|
Serial.print(" | ARM3 position = ");
|
|
Serial.print(pos);
|
|
Serial.print(" |");
|
|
}
|
|
void getPosition_FA3()
|
|
{
|
|
static int pos = 0;
|
|
encoder_FA3.tick();
|
|
int newPos = encoder_FA3.getPosition();
|
|
if (pos != newPos)
|
|
{
|
|
pos = newPos;
|
|
}
|
|
Serial.print(" | FOREARM3 position = ");
|
|
Serial.print(pos);
|
|
Serial.print(" |");
|
|
}
|
|
|
|
void getPosition_A4()
|
|
{
|
|
static int pos = 0;
|
|
encoder_A4.tick();
|
|
int newPos = encoder_A4.getPosition();
|
|
if (pos != newPos)
|
|
{
|
|
pos = newPos;
|
|
}
|
|
Serial.print(" | ARM4 position = ");
|
|
Serial.print(pos);
|
|
Serial.print(" |");
|
|
}
|
|
void getPosition_FA4()
|
|
{
|
|
static int pos = 0;
|
|
encoder_FA4.tick();
|
|
int newPos = encoder_FA4.getPosition();
|
|
if (pos != newPos)
|
|
{
|
|
pos = newPos;
|
|
}
|
|
Serial.print(" | FOREARM4 position = ");
|
|
Serial.print(pos);
|
|
Serial.print(" |");
|
|
}
|
|
|
|
void setup()
|
|
{
|
|
Serial.begin (230400);
|
|
pinMode(ARM1_DIR, OUTPUT);
|
|
pinMode(ARM1_STP, OUTPUT);
|
|
pinMode(ARM2_DIR, OUTPUT);
|
|
pinMode(ARM2_STP, OUTPUT);
|
|
pinMode(ARM3_DIR, OUTPUT);
|
|
pinMode(ARM3_STP, OUTPUT);
|
|
pinMode(ARM4_DIR, OUTPUT);
|
|
pinMode(ARM4_STP, OUTPUT);
|
|
pinMode(FOREARM1_STP, OUTPUT);
|
|
pinMode(FOREARM1_DIR, OUTPUT);
|
|
pinMode(FOREARM2_STP, OUTPUT);
|
|
pinMode(FOREARM2_DIR, OUTPUT);
|
|
pinMode(FOREARM3_STP, OUTPUT);
|
|
pinMode(FOREARM3_DIR, OUTPUT);
|
|
pinMode(FOREARM4_STP, 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);
|
|
digitalWrite(ARM1_EN, LOW);
|
|
digitalWrite(ARM2_EN, LOW);
|
|
digitalWrite(ARM3_EN, LOW);
|
|
digitalWrite(ARM4_EN, LOW);
|
|
digitalWrite(FOREARM1_EN, LOW);
|
|
digitalWrite(FOREARM2_EN, LOW);
|
|
digitalWrite(FOREARM3_EN, LOW);
|
|
digitalWrite(FOREARM4_EN, LOW);
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
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, 0);
|
|
digitalWrite(ARM2_DIR, 0);
|
|
digitalWrite(ARM3_DIR, 0);
|
|
digitalWrite(ARM4_DIR, 0);
|
|
for (int i = 0; i < 100 * RELATION_ARM; i++)
|
|
{
|
|
digitalWrite(ARM1_STP, HIGH);
|
|
digitalWrite(ARM2_STP, HIGH);
|
|
digitalWrite(ARM3_STP, HIGH);
|
|
digitalWrite(ARM4_STP, 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_STP, LOW);
|
|
digitalWrite(ARM2_STP, LOW);
|
|
digitalWrite(ARM3_STP, LOW);
|
|
digitalWrite(ARM4_STP, LOW);
|
|
delayMicroseconds(delayTime);
|
|
}
|
|
delay(100);
|
|
|
|
digitalWrite(ARM1_DIR, 1);
|
|
digitalWrite(ARM2_DIR, 1);
|
|
digitalWrite(ARM3_STP, 1);
|
|
digitalWrite(ARM4_STP, 1);
|
|
for (int i = 0; i < 900 * RELATION_ARM; i++)
|
|
{
|
|
digitalWrite(ARM1_STP, HIGH);
|
|
digitalWrite(ARM2_STP, HIGH);
|
|
digitalWrite(ARM3_STP, HIGH);
|
|
digitalWrite(ARM4_STP, 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_STP, LOW);
|
|
digitalWrite(ARM2_STP, LOW);
|
|
digitalWrite(ARM3_STP, LOW);
|
|
digitalWrite(ARM4_STP, LOW);
|
|
delayMicroseconds(delayTime);
|
|
}
|
|
delay(1000);
|
|
|
|
digitalWrite(FOREARM1_DIR, 1);
|
|
digitalWrite(FOREARM2_DIR, 1);
|
|
digitalWrite(FOREARM3_STP, 1);
|
|
digitalWrite(FOREARM4_STP, 1);
|
|
for (int i = 0; i < 900 * RELATION_FOREARM; i++)
|
|
{
|
|
digitalWrite(FOREARM1_STP, HIGH);
|
|
digitalWrite(FOREARM2_STP, HIGH);
|
|
digitalWrite(FOREARM3_STP, HIGH);
|
|
digitalWrite(FOREARM4_STP, 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(FOREARM1_STP, LOW);
|
|
digitalWrite(FOREARM2_STP, LOW);
|
|
digitalWrite(FOREARM3_STP, LOW);
|
|
digitalWrite(FOREARM4_STP, LOW);
|
|
delayMicroseconds(delayTime);
|
|
}
|
|
delay(250);
|
|
|
|
digitalWrite(FOREARM1_DIR, 0);
|
|
digitalWrite(FOREARM2_DIR, 0);
|
|
digitalWrite(FOREARM3_STP, 0);
|
|
digitalWrite(FOREARM4_STP, 0);
|
|
for (int i = 0; i < 100 * RELATION_FOREARM; i++)
|
|
{
|
|
digitalWrite(FOREARM1_STP, HIGH);
|
|
digitalWrite(FOREARM2_STP, HIGH);
|
|
digitalWrite(FOREARM3_STP, HIGH);
|
|
digitalWrite(FOREARM4_STP, 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(FOREARM1_STP, LOW);
|
|
digitalWrite(FOREARM2_STP, LOW);
|
|
digitalWrite(FOREARM3_STP, LOW);
|
|
digitalWrite(FOREARM4_STP, LOW);
|
|
delayMicroseconds(delayTime);
|
|
}
|
|
delay(100);
|
|
|
|
}
|