60 lines
1.1 KiB
Arduino
60 lines
1.1 KiB
Arduino
|
#include <Arduino.h>
|
||
|
|
#include <RotaryEncoder.h>
|
||
|
|
|
||
|
|
/* Code for discover the relation between Arm & Forearm pulley */
|
||
|
|
|
||
|
|
// 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
|
||
|
|
|
||
|
|
// 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);
|
||
|
|
|
||
|
|
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 setup()
|
||
|
|
{
|
||
|
|
|
||
|
|
}
|
||
|
|
void loop()
|
||
|
|
{
|
||
|
|
getPosition_A1();
|
||
|
|
getPosition_FA1();
|
||
|
|
Serial.println();
|
||
|
|
}
|