Hello, I am working on a tvc rocket mount prototype and have been stuck on this problem for some time. I am new to using arduino ide, as well as creating circuits in general. The problem that I am experiencing is mainly with how the servos interact. When one servo reaches near its maximum angle, it tends to stray off in the opposite direction (causes the other servo to move), or both of them begin to spasm. I am using a 6V battery via 4 AAs, which is connected to a pca9685 board that is used to control the two mg90 servos. Everything is wired together on a breadboard for prototyping purposes, connected to a raspberry pi pico 2. If there is an issue with my code, I have linked it down below.
#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <IMU_Fusion_SYC.h>
#include <iostream>
#include <vector>
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
IMU imu(Wire);
#define SERVOMIN 150
#define SERVOMAX 455
std::vector<int> servos = {0, 1};
void set_default() {
for (int i : servos) {
pwm.setPWM(i, 0, map(90, 0, 180, SERVOMIN, SERVOMAX));
}
}
void setup() {
// Serial initiation
Serial.begin(9600);
// Wire initiation
Wire.begin();
// MPU6050 initiation
imu.begin();
imu.MPU6050_CalcGyroOffsets();
// Analog initiation
// analogReadResolution(12);
// Servo initiation
pwm.begin();
pwm.setPWMFreq(60);
set_default();
delay(1000);
// Buzzer initiation
pinMode(0, OUTPUT);
tone(0, 622.25, 500); //e
delay(333);
tone(0, 932.33, 500); //b
// LED initiation
pinMode(3, OUTPUT);
// Switch initiation
pinMode(2, INPUT);
delay(1000);
}
// Global functions
void blink() {
digitalWrite(1, HIGH);
tone(0, 500, 500);
delay(1000);
digitalWrite(1, LOW);
delay(1000);
}
// Global variables
uint32_t switch_state;
uint32_t touch_state;
uint32_t previous_x;
uint32_t previous_y;
void loop() {
// LED Check
switch_state = digitalRead(2);
if (switch_state == HIGH) {
digitalWrite(3, LOW);
// Local Variables
uint32_t current_x = ceil(imu.getAngleX() + 90);
uint32_t current_y = ceil(imu.getAngleY() + 90);
// MPU6050 Check
imu.Calculate();
if (ceil(imu.getAngleX() + 90) >= 0 && ceil(imu.getAngleX() + 90) <= 180 && current_x != previous_x) {
Serial.print("X-Angle: ");
Serial.println(imu.getAngleX() + 90);
pwm.setPWM(0, 0, map(current_x, 0, 180, SERVOMIN, SERVOMAX));
previous_x = current_x;
}
if (ceil(imu.getAngleY() + 90) >= 0 && ceil(imu.getAngleY() + 90) <= 180 && current_y != previous_y) {
Serial.print("Y-Angle: ");
Serial.println(imu.getAngleY() + 90);
pwm.setPWM(1, 0, map(current_y, 0, 180, SERVOMIN, SERVOMAX));
previous_y = current_y;
}
delay(50);
} else {
digitalWrite(3, HIGH);
}
}
void setup1() {
//LED initiation
pinMode(1, OUTPUT);
delay(2000);
}
void loop1() {
if (switch_state == HIGH) {
blink();
} else {
digitalWrite(1, LOW);
}
}
