Good afternoon, I am building a 6 axis robotic arm, using 6 DM996 servos, 6 push buttons rotary encoders, PCA9685 PWM servo controller and Arduino Mega 256.
I am facing a very strange problem:
I can perfectly read the different values from the rotary encoders, but when i pass the requested value to the selected servo the system freezes. no way to go further.
Any clue?
Thank you in advance.
Here is wiring and code for 2 encoders and servos only.
/******************************************
*Claudio Ungaro 23/09/2023
*
*rotary and servo motor
*using microseconds
*using AdaFruit PWM servo driver
******************************************/
//#include <Servo.h>
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
// called this way, it uses the default address 0x40
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
// for sensitivity reason all angles are multiplied by 2
// Min angle 45 deg, Max angle 135 deg
#define MAXANG 270
#define MINANG 90
#define HOME 180
#define MAXMICRO 2400
#define MINMICRO 600
#define HOMEMICRO 1500
#define SERVOMIN 150 // This is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX 600 // This is the 'maximum' pulse length count (out of 4096)
#define USMIN 600 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150
#define USMAX 2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600
#define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates
//Define the pin connection
int CLK_0 = 2; //CLK->D2
int DT_0 = 4; //DT->D3
int SW_0 = 5; //SW->D4
int CLK_1 = 3; //CLK->D2
int DT_1 = 6; //DT->D3
int SW_1 = 7; //SW->D4
int dummy = 1500;
// Interrupt 0 on pin 2
const int interrupt0 = 0;
const int interrupt1 = 1;
//Define the count
int count_0 = HOME;
int count_1 = HOME;
//CLK initial value
int lastCLK_0 = HOME;
int lastCLK_1 = HOME;
void setup() {
// put your setup code here, to run once:
Serial.begin(115200);
// setup the encoders
Serial.println("Setting the encoders");
pinMode(SW_0, INPUT);
digitalWrite(SW_0, HIGH);
pinMode(CLK_0, INPUT);
pinMode(DT_0, INPUT);
//Set the interrupt 0 handler, trigger level change
attachInterrupt(interrupt0, ClockChanged_0, CHANGE);
//
pinMode(SW_1, INPUT);
digitalWrite(SW_1, HIGH);
pinMode(CLK_1, INPUT);
pinMode(DT_1, INPUT);
//Set the interrupt 0 handler, trigger level change
attachInterrupt(interrupt1, ClockChanged_1, CHANGE);
// setting the PWN module
pwm.begin();
delay(10);
pwm.setOscillatorFrequency(27000000);
pwm.setPWMFreq(SERVO_FREQ); // Analog servos run at ~50 Hz updates
// setting servos to HOME position
Serial.println("Setting HOME position of each Servo");
pwm.writeMicroseconds(0, MINMICRO);
delay(1500);
pwm.writeMicroseconds(0, MAXMICRO);
delay(1500);
pwm.writeMicroseconds(0, HOMEMICRO);
delay(1500);
pwm.writeMicroseconds(1, MINMICRO);
delay(1500);
pwm.writeMicroseconds(1, MAXMICRO);
delay(1500);
pwm.writeMicroseconds(1, HOMEMICRO);
delay(1500);
Serial.println("End SETUP");
}
void loop() {
// put your main code here, to run repeatedly:
// if button is pushed set to zero encoder and Servo
if (!digitalRead(SW_0) && count_0 != HOME) //Read the button press and the count value to 0 when the counter reset
{
count_0 = HOME;
Serial.print("Encoder 0 Real Angle :");
int angleVal = count_0 / 2;
Serial.print(angleVal);
Serial.println(" Micro ");
Serial.println(HOMEMICRO);
pwm.writeMicroseconds(0, HOMEMICRO);
lastCLK_0 = HOME;
}
if (!digitalRead(SW_1) && count_1 != HOME) //Read the button press and the count value to 0 when the counter reset
{
count_1 = HOME;
Serial.print("Encoder 1 Real Angle:");
int angleVal = count_1 / 2;
Serial.println(angleVal);
Serial.println(" Micro ");
Serial.println(HOMEMICRO);
pwm.writeMicroseconds(1, HOMEMICRO);
lastCLK_1 = HOME;
}
// Put in a slight delay to help debounce the reading
delay(1);
}
void ClockChanged_0() {
int clkValue = digitalRead(CLK_0); //Read the CLK pin level
int dtValue = digitalRead(DT_0); //Read the DT pin level
if (lastCLK_0 != clkValue) {
lastCLK_0 = clkValue;
count_0 += (clkValue != dtValue ? 1 : -1); //CLK and inconsistent DT + 1, otherwise - 1
if (count_0 < MINANG) count_0 = MINANG;
if (count_0 >= MAXANG) count_0 = MAXANG;
Serial.print("Encoder 0 Real Angle :");
int angleVal = count_0 / 2;
Serial.print(angleVal);
Serial.print(" Microseconds ");
int microAngle = count_0 * 5 + 600;
Serial.println(microAngle);
pwm.writeMicroseconds(0, microAngle);
}
}
//The interrupt handlers
void ClockChanged_1() {
int clkValue = digitalRead(CLK_1); //Read the CLK pin level
int dtValue = digitalRead(DT_1); //Read the DT pin level
if (lastCLK_1 != clkValue) {
lastCLK_1 = clkValue;
count_1 += (clkValue != dtValue ? 1 : -1); //CLK and inconsistent DT + 1, otherwise - 1
if (count_1 < MINANG) count_1 = MINANG;
if (count_1 >= MAXANG) count_1 = MAXANG;
Serial.print("Encoder 1 Real Angle :");
int angleVal = count_1 / 2;
Serial.print(angleVal);
Serial.print(" Microseconds ");
int microAngle = count_1 * 5 + 600;
Serial.println(microAngle);
pwm.writeMicroseconds(1, microAngle);
}
}
