/*
Custom-built Servo Motor - Arduino Code
by Dejan, www.HowToMechatronics.com
Libraries:
AS5600 encoder: https://github.com/RobTillaart/AS5600
PID conroller: https://github.com/br3ttb/Arduino-PID-Library/blob/master/PID_v1.h
L298N
https://wokwi.com/projects/386822856593519617
*/
#include <Adafruit_AS5600.h>
//#include "AS5600.h"
#include "Wire.h"
#include <PID_v1.h>
const int stepsPerRev = 800; // 800 steps for a full rotation (using 1/4 micromotor_IN1g)
Adafruit_AS5600 as5600;
//AS5600 as5600; // use default Wire
double Pk1 = 2; //speed it gets there
double Ik1 = 0;
double Dk1 = 0.025;
//Define Variables we'll be connecting to
double Setpoint, Input, Output;
PID myPID(&Input, &Output, &Setpoint, Pk1, Ik1, Dk1, DIRECT);
#define motor_IN1 5
#define motor_IN2 6
#define ch1 2
#define centerSet 7
#define inputSwitch 3
#define modeSwitch 4
int ch1Value;
int encoderValue, inputValue, pwmValue;
String inString = ""; // string to hold input
int centerAngle = 2047; // 180 degrees
int angleDifference = 0;
int angleValue = 0;
int leftLimit = 30;
int rightLimit = 4067;
int rangeAdjustment = 0;
float sensitivityAdjustment = 0;
float angle = 0;
int quadrantNumber = 2;
int previousQuadrantNumber = 3;
int numberOfTurns = 0;
float totalAngle = 0;
int error = 0;
char incomingByte = 0;
int intInput = 0;
//=====================================================================
//=====================================================================
//=====================================================================
void setup() {
Serial.begin(115200);
//Serial.print("File:\t");
//Serial.println(__FILE__);
//Serial.print("AS5600_LIB_VERSION: ");
//Serial.println(AS5600_LIB_VERSION);
Wire.begin();
Serial.println("AS5600 Setup");
delay(1000);
as5600_setup();
pinMode(motor_IN1, OUTPUT);
pinMode(motor_IN2, OUTPUT);
// Activate the Arduino internal pull-up resistors
pinMode(centerSet, INPUT_PULLUP);
pinMode(inputSwitch, INPUT_PULLUP);
pinMode(4, INPUT_PULLUP);
myPID.SetMode(AUTOMATIC); // PID Setup
myPID.SetOutputLimits(-255, 255);
myPID.SetSampleTime(20);
Serial.println("Setup Complete");
}
//=====================================================================
//=====================================================================
//=====================================================================
void loop() {
//uint16_t rawAngle = as5600.getRawAngle();
uint16_t angle = as5600.getAngle();
uint16_t encoder_angle = map(angle, 0, 4095, 0, 36000);
encoderValue = as5600.getRawAngle();
// Read encoder value - current position
//encoderValue = as5600.readAngle();
Serial.print("Encoder: "); Serial.print(encoderValue); Serial.print("/"); Serial.print(encoder_angle);
// Continuous rotation mode
if (digitalRead(modeSwitch) == 0) {
// Enter desired angle for the servo to go to through the serial monitor
while (Serial.available() > 0) {
int inChar = Serial.read();
if (isDigit(inChar)) {
// convert the incoming byte to a char and add it to the string:
inString += (char)inChar;
}
// if you get a newline, print the string, then the string's value:
if (inChar == '\n') {
Setpoint = inString.toInt(); // Setpoint - desired angle
// clear the string for new input:
inString = "";
}
}
if (digitalRead(inputSwitch) == 0) { // Potentiometer as input
inputValue = analogRead(A0);
if (inputValue < 400) {
Setpoint = Setpoint - 0.3;
}
if (inputValue < 300) {
Setpoint = Setpoint - 0.3;
}
if (inputValue < 200) {
Setpoint = Setpoint - 0.3;
}
if (inputValue > 600) {
Setpoint = Setpoint + 0.3;
}
if (inputValue > 700) {
Setpoint = Setpoint + 0.3;
}
if (inputValue > 800) {
Setpoint = Setpoint + 0.3;
}
}
else if (digitalRead(inputSwitch) == 1) {
inputValue = pulseIn(ch1, HIGH, 30000); // RC receiver as input
if (inputValue < 1450) {
Setpoint--;
}
if (inputValue < 1350) {
Setpoint--;
}
if (inputValue < 1250) {
Setpoint--;
}
if (inputValue < 1150) {
Setpoint--;
}
if (inputValue > 1550) {
Setpoint++;
}
if (inputValue > 1650) {
Setpoint++;
}
if (inputValue > 1750) {
Setpoint++;
}
if (inputValue > 1850) {
Setpoint++;
}
}
// Convert encoder RAW values into angle value
angle = encoderValue * 0.087890625;
//0.087890625 = 360/4096
Serial.print("/"); Serial.println(angle);
// Quadrant 1
if (angle >= 0 && angle <= 90) {
quadrantNumber = 1;
}
// Quadrant 2
if (angle >= 90 && angle <= 180) {
quadrantNumber = 2;
}
// Quadrant 3
if (angle >= 180 && angle <= 270) {
quadrantNumber = 3;
}
// Quadrant 4
if (angle >= 270 && angle <= 360) {
quadrantNumber = 4;
}
if (quadrantNumber != previousQuadrantNumber) {
// Transition from 4th to 1st quadrant
if (quadrantNumber == 1 && previousQuadrantNumber == 4) {
numberOfTurns++;
}
// Transition from 1st to 4th quadrant
if (quadrantNumber == 4 && previousQuadrantNumber == 1) {
numberOfTurns--;
}
previousQuadrantNumber = quadrantNumber;
}
if (totalAngle >= 0) {
totalAngle = (numberOfTurns * 360) + angle;
}
else {
totalAngle = (numberOfTurns * 360) + angle;
}
// Establish Input value for PID
Input = totalAngle;
}
// Limited Rotation Mode
else if (digitalRead(modeSwitch) == 1) {
rangeAdjustment = analogRead(A1);
leftLimit = 0 + 30 + rangeAdjustment;
rightLimit = 4097 - 30 - rangeAdjustment;
if (digitalRead(inputSwitch) == 0) { // Analog input - Potentiometer
// Get value from potentiometer
inputValue = analogRead(A0);
if (inputValue < 15) {
inputValue = 15;
}
if (inputValue > 1008) {
inputValue = 1008;
}
Setpoint = map(inputValue, 15, 1008, -255, 255);
}
else if (digitalRead(inputSwitch) == 1) { // Digital input - RC transmitter
inputValue = pulseIn(ch1, HIGH, 30000); // Read RC receiver as input
Setpoint = map(inputValue, 1000, 2000, -255, 255);
}
// Set center angle
if (digitalRead(centerSet) == LOW) {
centerAngle = encoderValue;
angleDifference = 2047 - encoderValue;
delay(1000);
}
// Adjust angle value according to the center point (angleDifference)
if (centerAngle < 2047) {
angleValue = encoderValue + angleDifference;
if (encoderValue < 4097 && encoderValue > (4096 - angleDifference)) {
angleValue = encoderValue - 4097 + angleDifference;
}
}
if (centerAngle > 2047) {
angleValue = encoderValue + angleDifference;
if (encoderValue >= 0 && encoderValue < abs(angleDifference)) {
angleValue = encoderValue + 4097 + angleDifference;
}
}
else if (centerAngle == 2047) {
angleValue = encoderValue;
}
// Establish Input value for PID
Input = map(angleValue, leftLimit, rightLimit, -255, 255);
}
// Adjusting sensitivity
Pk1 = analogRead(A2) * 0.002;
myPID.SetTunings(Pk1, Ik1, Dk1);
// Run PID process to get Output value
myPID.Compute();
// Move right
if (Output > 1 ) {
pwmValue = Output;
if (pwmValue < 30 && pwmValue > 5) {
pwmValue = pwmValue + 30;
}
if (pwmValue <= 5) {
pwmValue = 0;
}
//digitalWrite(motor_IN1, LOW);
//analogWrite(motor_IN2, pwmValue);
analogWrite(motor_IN1, pwmValue);
digitalWrite(motor_IN2, LOW);
}
// Move left
else if (Output < 1 ) {
pwmValue = abs(Output);
if (pwmValue < 30 && pwmValue > 5) {
pwmValue = pwmValue + 30;
}
if (pwmValue <= 5) {
pwmValue = 0;
}
//analogWrite(motor_IN1, pwmValue);
//digitalWrite(motor_IN2, LOW);
analogWrite(motor_IN1, pwmValue);
digitalWrite(motor_IN2, HIGH);
}
// Do not move
else if (Output > -1 && Output < 1) {
pwmValue = 0;
digitalWrite(motor_IN1, LOW);
digitalWrite(motor_IN2, LOW);
}
//Serial.print(Setpoint);
//Serial.print("\t");
//Serial.println(totalAngle);
}
//=====================================================================
//=====================================================================
//=====================================================================
void as5600_setup() {
if (!as5600.begin()) {
Serial.println("Could not find AS5600 sensor, check wiring!");
while (1)
delay(10);
}
Serial.println("AS5600 found!");
// Test getZMCount function
uint8_t zmCount = as5600.getZMCount();
Serial.print("ZM Count (burn count): ");
Serial.println(zmCount);
// Test getZPosition function
uint16_t zPos = as5600.getZPosition();
Serial.print("Z Position: ");
Serial.println(zPos);
// Test setZPosition function (XOR current value with 0xADA to change it)
uint16_t testPos = zPos ^ 0xADA; // XOR with 0xADA to get different value
testPos &= 0x0FFF; // Keep within 12-bit range
Serial.print("Setting Z Position to ");
Serial.print(testPos);
Serial.print(" (0x");
Serial.print(testPos, HEX);
Serial.print(")... ");
if (as5600.setZPosition(testPos)) {
Serial.println("Success");
uint16_t newZPos = as5600.getZPosition();
Serial.print("New Z Position: ");
Serial.print(newZPos);
Serial.print(" (0x");
Serial.print(newZPos, HEX);
Serial.println(")");
} else {
Serial.println("Failed");
}
// Test getMPosition function
uint16_t mPos = as5600.getMPosition();
Serial.print("M Position: ");
Serial.println(mPos);
// Test setMPosition function (XOR current value with 0xBEE)
uint16_t testMPos = mPos ^ 0xBEE;
testMPos &= 0x0FFF;
Serial.print("Setting M Position to ");
Serial.print(testMPos);
Serial.print(" (0x");
Serial.print(testMPos, HEX);
Serial.print(")... ");
if (as5600.setMPosition(testMPos)) {
Serial.println("Success");
uint16_t newMPos = as5600.getMPosition();
Serial.print("New M Position: ");
Serial.print(newMPos);
Serial.print(" (0x");
Serial.print(newMPos, HEX);
Serial.println(")");
} else {
Serial.println("Failed");
}
// Test getMaxAngle function
uint16_t maxAngle = as5600.getMaxAngle();
Serial.print("Max Angle: ");
Serial.println(maxAngle);
// Test setMaxAngle function (XOR current value with 0xCAB)
uint16_t testMaxAngle = maxAngle ^ 0xCAB;
testMaxAngle &= 0x0FFF;
Serial.print("Setting Max Angle to ");
Serial.print(testMaxAngle);
Serial.print(" (0x");
Serial.print(testMaxAngle, HEX);
Serial.print(")... ");
if (as5600.setMaxAngle(testMaxAngle)) {
Serial.println("Success");
uint16_t newMaxAngle = as5600.getMaxAngle();
Serial.print("New Max Angle: ");
Serial.print(newMaxAngle);
Serial.print(" (0x");
Serial.print(newMaxAngle, HEX);
Serial.println(")");
} else {
Serial.println("Failed");
}
// Test watchdog functions
Serial.print("Enabling watchdog... ");
if (as5600.enableWatchdog(true)) {
Serial.println("Success");
Serial.print("Watchdog status: ");
Serial.println(as5600.getWatchdog() ? "ENABLED" : "DISABLED");
} else {
Serial.println("Failed");
}
Serial.print("Disabling watchdog... ");
if (as5600.enableWatchdog(false)) {
Serial.println("Success");
Serial.print("Watchdog status: ");
Serial.println(as5600.getWatchdog() ? "ENABLED" : "DISABLED");
} else {
Serial.println("Failed");
}
// Test power mode functions
Serial.print(
"Setting power mode to Normal (options: NOM=0, LPM1=1, LPM2=2, "
"LPM3=3)... ");
if (as5600.setPowerMode(AS5600_POWER_MODE_NOM)) {
Serial.println("Success");
as5600_power_mode_t mode = as5600.getPowerMode();
Serial.print("Power mode: ");
switch (mode) {
case AS5600_POWER_MODE_NOM:
Serial.println("Normal");
break;
case AS5600_POWER_MODE_LPM1:
Serial.println("Low Power Mode 1");
break;
case AS5600_POWER_MODE_LPM2:
Serial.println("Low Power Mode 2");
break;
case AS5600_POWER_MODE_LPM3:
Serial.println("Low Power Mode 3");
break;
}
} else {
Serial.println("Failed");
}
// Test hysteresis functions
Serial.print(
"Setting hysteresis to OFF (options: OFF=0, 1LSB=1, 2LSB=2, 3LSB=3)... ");
if (as5600.setHysteresis(AS5600_HYSTERESIS_OFF)) {
Serial.println("Success");
as5600_hysteresis_t hysteresis = as5600.getHysteresis();
Serial.print("Hysteresis: ");
switch (hysteresis) {
case AS5600_HYSTERESIS_OFF:
Serial.println("OFF");
break;
case AS5600_HYSTERESIS_1LSB:
Serial.println("1 LSB");
break;
case AS5600_HYSTERESIS_2LSB:
Serial.println("2 LSB");
break;
case AS5600_HYSTERESIS_3LSB:
Serial.println("3 LSB");
break;
}
} else {
Serial.println("Failed");
}
// Test output stage functions
Serial.print(
"Setting output stage to Analog Full (options: ANALOG_FULL=0, "
"ANALOG_REDUCED=1, DIGITAL_PWM=2, RESERVED=3)... ");
if (as5600.setOutputStage(AS5600_OUTPUT_STAGE_ANALOG_FULL)) {
Serial.println("Success");
as5600_output_stage_t outputStage = as5600.getOutputStage();
Serial.print("Output stage: ");
switch (outputStage) {
case AS5600_OUTPUT_STAGE_ANALOG_FULL:
Serial.println("Analog Full (0% to 100%)");
break;
case AS5600_OUTPUT_STAGE_ANALOG_REDUCED:
Serial.println("Analog Reduced (10% to 90%)");
break;
case AS5600_OUTPUT_STAGE_DIGITAL_PWM:
Serial.println("Digital PWM");
break;
case AS5600_OUTPUT_STAGE_RESERVED:
Serial.println("Reserved");
break;
}
} else {
Serial.println("Failed");
}
// Test PWM frequency functions
Serial.print(
"Setting PWM frequency to 115Hz (options: 115HZ=0, 230HZ=1, 460HZ=2, "
"920HZ=3)... ");
if (as5600.setPWMFreq(AS5600_PWM_FREQ_115HZ)) {
Serial.println("Success");
as5600_pwm_freq_t pwmFreq = as5600.getPWMFreq();
Serial.print("PWM frequency: ");
switch (pwmFreq) {
case AS5600_PWM_FREQ_115HZ:
Serial.println("115 Hz");
break;
case AS5600_PWM_FREQ_230HZ:
Serial.println("230 Hz");
break;
case AS5600_PWM_FREQ_460HZ:
Serial.println("460 Hz");
break;
case AS5600_PWM_FREQ_920HZ:
Serial.println("920 Hz");
break;
}
} else {
Serial.println("Failed");
}
// Test slow filter functions
Serial.print(
"Setting slow filter to 16x (options: 16X=0, 8X=1, 4X=2, 2X=3)... ");
if (as5600.setSlowFilter(AS5600_SLOW_FILTER_16X)) {
Serial.println("Success");
as5600_slow_filter_t slowFilter = as5600.getSlowFilter();
Serial.print("Slow filter: ");
switch (slowFilter) {
case AS5600_SLOW_FILTER_16X:
Serial.println("16x");
break;
case AS5600_SLOW_FILTER_8X:
Serial.println("8x");
break;
case AS5600_SLOW_FILTER_4X:
Serial.println("4x");
break;
case AS5600_SLOW_FILTER_2X:
Serial.println("2x");
break;
}
} else {
Serial.println("Failed");
}
// Test fast filter threshold functions
Serial.print(
"Setting fast filter threshold to Slow Only (options: SLOW_ONLY=0, "
"6LSB=1, 7LSB=2, 9LSB=3, 18LSB=4, 21LSB=5, 24LSB=6, 10LSB=7)... ");
if (as5600.setFastFilterThresh(AS5600_FAST_FILTER_THRESH_SLOW_ONLY)) {
Serial.println("Success");
as5600_fast_filter_thresh_t fastThresh = as5600.getFastFilterThresh();
Serial.print("Fast filter threshold: ");
switch (fastThresh) {
case AS5600_FAST_FILTER_THRESH_SLOW_ONLY:
Serial.println("Slow filter only");
break;
case AS5600_FAST_FILTER_THRESH_6LSB:
Serial.println("6 LSB");
break;
case AS5600_FAST_FILTER_THRESH_7LSB:
Serial.println("7 LSB");
break;
case AS5600_FAST_FILTER_THRESH_9LSB:
Serial.println("9 LSB");
break;
case AS5600_FAST_FILTER_THRESH_18LSB:
Serial.println("18 LSB");
break;
case AS5600_FAST_FILTER_THRESH_21LSB:
Serial.println("21 LSB");
break;
case AS5600_FAST_FILTER_THRESH_24LSB:
Serial.println("24 LSB");
break;
case AS5600_FAST_FILTER_THRESH_10LSB:
Serial.println("10 LSB");
break;
}
} else {
Serial.println("Failed");
}
// Reset position settings to defaults
as5600.setZPosition(0);
as5600.setMPosition(4095);
as5600.setMaxAngle(4095);
delay(2000);
}Range
Sensitivity
Mode
0=serial
1=Range/Sens
Center
Input
0=pot
1=PWM