[4 DOF Robot Arm Keyestudio][Lab 03][AX-TinySAM][PCA9685] Servo 2 and 3 Test

#include <Arduino.h>
#include <PCA9685.h>

#include "Constants.h"


PCA9685 pca9685;
int pos1 = 60;
int pos2 = 100;
//uint16_t servo_pulse_duration;

void setup()
{
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(3, OUTPUT);
  pinMode(10, OUTPUT);

  digitalWrite(LED_BUILTIN, LOW);
  sw1_press();
  servoPCA9685Init(0x40);
  servoPCA9685Degree(1, pos1);
  servoPCA9685Degree(2, pos2);
  delay(1000);

}

void loop()
{
  for(pos2; pos2 > 30; pos2--)
  {
    servoPCA9685Degree(2, pos2);
    delay(7);
  }
  delay(1000);
    for(pos2 = 30; pos2 < 100; pos2++)
  {
    servoPCA9685Degree(2, pos2);
    delay(7);
  }
  delay(1000);
}
void sw1_press()
{
  while(analogRead(2)>10){}
  while(analogRead(2)<=10){}

  tone(10, 1000, 100);
  delay(100);

  noTone(10);
  digitalWrite(LED_BUILTIN, HIGH);
  for(int i = 0; i < 3; i++)
  {
    digitalWrite(3, HIGH);
    delay(500);
    digitalWrite(3, LOW);
    delay(500);
  }
  digitalWrite(3, HIGH);
}
void servoPCA9685Init(const uint8_t _address)
{
  pca9685.setupSingleDevice(Wire,_address);
  pca9685.setToServoFrequency();
}
void servoPCA9685Degree(uint8_t servo,signed int angle)
{
  if(servo == 0)
  {
    if(angle == -1)
    {
      pca9685.setChannelServoPulseDuration(0,0);
    }
    else
    {
      pca9685.setChannelServoPulseDuration(0,map(angle, 0, 180, constants::servo_pulse_duration_min, constants::servo_pulse_duration_max));
    }
  }
    if(servo == 1)
  {
    if(angle == -1)
    {
      pca9685.setChannelServoPulseDuration(1,0);
    }
    else
    {
      pca9685.setChannelServoPulseDuration(1,map(angle, 0, 180, constants::servo_pulse_duration_min, constants::servo_pulse_duration_max));
    }
  }
    if(servo == 2)
  {
    if(angle == -1)
    {
      pca9685.setChannelServoPulseDuration(2,0);
    }
    else
    {
      pca9685.setChannelServoPulseDuration(2,map(angle, 0, 180, constants::servo_pulse_duration_min, constants::servo_pulse_duration_max));
    }
  }
    if(servo == 3)
  {
    if(angle == -1)
    {
      pca9685.setChannelServoPulseDuration(3,0);
    }
    else
    {
      pca9685.setChannelServoPulseDuration(3,map(angle, 0, 180, constants::servo_pulse_duration_min, constants::servo_pulse_duration_max));
    }
  }
    if(servo == 4)
  {
    if(angle == -1)
    {
      pca9685.setChannelServoPulseDuration(4,0);
    }
    else
    {
      pca9685.setChannelServoPulseDuration(4,map(angle, 0, 180, constants::servo_pulse_duration_min, constants::servo_pulse_duration_max));
    }
  }
      if(servo == 5)
  {
    if(angle == -1)
    {
      pca9685.setChannelServoPulseDuration(5,0);
    }
    else
    {
      pca9685.setChannelServoPulseDuration(5,map(angle, 0, 180, constants::servo_pulse_duration_min, constants::servo_pulse_duration_max));
    }
  }
}