113 lines
2.3 KiB
C++
113 lines
2.3 KiB
C++
///// ultim vers yo 14/05/23//////
|
|
|
|
|
|
|
|
#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>
|
|
#define PPR 29
|
|
#define RPWM 5
|
|
#define LPWM 6
|
|
|
|
/*
|
|
Pins connected to aduino( in english )
|
|
|
|
IBT-2 pin 1 (RPWM) to Arduino pin 5(PWM)
|
|
IBT-2 pin 2 (LPWM) to Arduino pin 6(PWM)
|
|
IBT-2 pins 3 (R_EN), 4 (L_EN), 7 (VCC) to Arduino 5V
|
|
IBT-2 pin 8 (GND) to Arduino's GND
|
|
IBT-2 pins 5 (R_IS) and 6 (L_IS) not connected
|
|
*/
|
|
|
|
// rotary encoder pins
|
|
#define PIN_IN1 2
|
|
#define PIN_IN2 3
|
|
|
|
// bts7960 pins
|
|
const int L_EN = 7;
|
|
const int R_EN = 8;
|
|
const int L_PWM = 6;
|
|
const int R_PWM = 5;
|
|
|
|
// potentiometer pin
|
|
const int pot = A0;
|
|
float vel = 150;
|
|
|
|
// declaration of encoder object
|
|
//RotaryEncoder encoder(DT, CLK, RotaryEncoder::LatchMode::TWO03);
|
|
RotaryEncoder *encoder = nullptr;
|
|
|
|
float tgtangle = 800;
|
|
float angle = 0;
|
|
|
|
void moveTo(float now, float target)
|
|
{
|
|
if (target > 0)
|
|
{
|
|
if (now <= target)
|
|
{
|
|
analogWrite(LPWM, 0);
|
|
analogWrite(RPWM, vel);
|
|
}
|
|
else
|
|
{
|
|
analogWrite(RPWM, 0);
|
|
analogWrite(LPWM, 0);
|
|
delay(200);
|
|
tgtangle = -1600;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (now >= target)
|
|
{
|
|
analogWrite(RPWM, 0);
|
|
analogWrite(LPWM, vel);
|
|
}
|
|
else
|
|
{
|
|
analogWrite(RPWM, 0);
|
|
analogWrite(LPWM, 0);
|
|
delay(200);
|
|
tgtangle = 800;
|
|
}
|
|
}
|
|
}
|
|
|
|
void getAngle()
|
|
{
|
|
static int pos = 0;
|
|
encoder->tick();
|
|
int newPos = encoder->getPosition();
|
|
if (pos != newPos)
|
|
{
|
|
angle = (float)newPos / PPR * 360;
|
|
Serial.print("Desired angle: ");
|
|
Serial.print(tgtangle);
|
|
Serial.print(" || Actual angle: ");
|
|
Serial.print(angle);
|
|
Serial.print(" || Direction: ");
|
|
Serial.print((int)(encoder->getDirection()));
|
|
Serial.print(" || RPM: ");
|
|
Serial.println((int)(encoder->getRPM()));
|
|
pos = newPos;
|
|
}
|
|
}
|
|
|
|
void checkPosition()
|
|
{
|
|
encoder->tick(); // just call tick() to check the state.
|
|
}
|
|
|
|
void setup()
|
|
{
|
|
Serial.begin(9600);
|
|
encoder = new RotaryEncoder(PIN_IN1, PIN_IN2, RotaryEncoder::LatchMode::TWO03);
|
|
attachInterrupt(digitalPinToInterrupt(PIN_IN1), checkPosition, CHANGE);
|
|
attachInterrupt(digitalPinToInterrupt(PIN_IN2), checkPosition, CHANGE);
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
getAngle();
|
|
moveTo(angle, tgtangle);
|
|
} |