[4 DOF Robot Arm Keyestudio][Lab 04][AX-TinySAM][PCA9685] 4 Servo Demo Test

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

#include "Constants.h"


#define TIME 7
PCA9685 pca9685;
int pos0 = 80;
int pos1 = 60;
int pos2 = 100;
int pos3 = 73;
void setup()
{
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(3, OUTPUT);
  pinMode(10, OUTPUT);

  digitalWrite(LED_BUILTIN, LOW);
  servoPCA9685Init(0x40);
  servoPCA9685Degree(0, -1);
  servoPCA9685Degree(1, -1);
  servoPCA9685Degree(2, -1);
  servoPCA9685Degree(3, -1);
  sw1_press();
  servoPCA9685Degree(0, pos0);
  servoPCA9685Degree(1, pos1);
  servoPCA9685Degree(2, pos2);
  servoPCA9685Degree(3, pos3);
  delay(500);

}

void loop()
{
  sw1_press();
  delay(400);
  Arm1TurnRight();
  delay(400);
  Arm4Open();
  delay(400);
  Arm3StretchedOut();
  delay(400);
  Arm4Close();
  delay(400);
  Arm3DrawBack();
  delay(400);
  Arm2Up();
  delay(400);
  Arm1TurnLeft();
  delay(400);
  Arm2Down();
  delay(400);
  Arm3StretchedOut();
  delay(400);
  Arm4Open();
  delay(400);
  Arm3DrawBack();
  delay(400);
  Arm4Open();
  delay(400);
  Arm3StretchedOut();
  delay(400);
  Arm4Close();
  delay(400);
  Arm3DrawBack();
    delay(400);
  Arm2Up();
  delay(400);
  Arm1TurnRight();
  delay(400);
  Arm2Down();
  delay(400);
  Arm3StretchedOut();
  delay(400);
  Arm4Open();
  delay(400);
  Arm3DrawBack();
  delay(400);
}
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));
    }
  }
}
void Arm1TurnLeft()
{
  for(pos0;pos0<150;pos0++)
  {
    servoPCA9685Degree(0, pos0);
    delay(TIME);
  }
}
void Arm1TurnRight()
{
  for(pos0;pos0>10;pos0--)
  {
    servoPCA9685Degree(0, pos0);
    delay(TIME);
  }
}
void Arm2Up()
{
  for(pos1;pos1>30;pos1--)
  {
    servoPCA9685Degree(1, pos1);
    delay(TIME);
  }
}
void Arm2Down()
{
  for(pos1;pos1<60;pos1++)
  {
    servoPCA9685Degree(1, pos1);
    delay(TIME);
  }
}
void Arm3StretchedOut()
{
  for(pos2;pos2>80;pos2--)
  {
    servoPCA9685Degree(2, pos2);
    delay(TIME);
  }
}
void Arm3DrawBack()
{
  for(pos2;pos2<100;pos2++)
  {
    servoPCA9685Degree(2, pos2);
    delay(TIME);
  }
}
void Arm4Close()
{
  for(pos3;pos3>73;pos3--)
  {
    servoPCA9685Degree(3, pos3);
  }
}
void Arm4Open()
{
  for(pos3;pos3<165;pos3++)
  {
    servoPCA9685Degree(3, pos3);
  }
}