[4 DOF Robot Arm Keyestudio][Lab 02][AX-TinySAM][PCA9685] Servo 1 Test

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

#include "Constants.h"


PCA9685 pca9685;
int pos0 = 80;
//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);
  pos0 = 80;
  servoPCA9685Degree(0, pos0);
  delay(1000);
  pos0 = 0;
  servoPCA9685Degree(0, pos0);
  delay(1000);

//  servo_pulse_duration = constants::servo_pulse_duration_min;
}

void loop()
{
//  if (servo_pulse_duration > constants::servo_pulse_duration_max)
//  {
//    servo_pulse_duration = constants::servo_pulse_duration_min;
//  }
//  pca9685.setChannelServoPulseDuration(constants::channel,servo_pulse_duration);
//  servo_pulse_duration += constants::servo_pulse_duration_increment;
  for(pos0; pos0 < 180; pos0++)
  {
    servoPCA9685Degree(0, pos0);
    delay(7);
  }
  delay(1000);
    for(pos0 = 180; pos0 > 0; pos0--)
  {
    servoPCA9685Degree(0, pos0);
    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));
    }
  }
}