Płynny ruch serwomechanizmu

[Klimwojt]

#include <Servo.h>
Servo servo_leg; 
Servo servo_knee;
const int toglle = 6; 
const int leg_base = 90; 
const int knee_base = 0; 
const int leg_move = 180; 
const int knee_move_1 = 15; 
const int knee_move_2 = 45; 
int X = 0; 
int Y = 0; 
int Z = 0; 
int V = 0; 
//
void IK(double x, double y, double z, double v){

  double a = atan2(y, x) * (180 / 3.145); 
  double a_2 = atan2(v, z) * (180 / 3.145);

  double l = sqrt(x * x + y * y); 
  double l_2 = sqrt(z * z + v * v); 

  double b = atan(x / y) * (180 / 3.145); 
  double b_2 = atan(z / v) * (180 / 3.145); 

  double a1 = b; 
  double b1 = b_2;

  servo_leg.write(a1); 
  servo_knee.write(b1);
}
//
void START_P(){
  IK(90, 90, 0, 0);
}
//
void INTERVAL(){
  int stepsPerMove = 5; 
  //
  double xDelta = (leg_move - X) / stepsPerMove; 
  double yDelta = (leg_move - Y) / stepsPerMove; 
  double zDelta = (knee_move_1 - Z) / stepsPerMove; 
  double vDelta = (knee_move_2 - V) / stepsPerMove; 
  //
  for(int i = 1; i <= stepsPerMove; i++){
    X += xDelta; 
    Y += yDelta; 
    Z += zDelta; 
    V += vDelta; 
  }
  //
  IK(leg_base, leg_base, knee_base, knee_base);
  delay(550); 
  IK(leg_base, leg_base, Z, V);
  delay(550); 
  IK(X, leg_base, Z, V);
  delay(550); 
  IK(X, leg_base, knee_base, knee_base);
  delay(550); 
}
//
void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  servo_leg.attach(5); 
  servo_knee.attach(3); 
  pinMode(toglle, INPUT_PULLUP);
  pinMode(2, OUTPUT);
  pinMode(4, OUTPUT);
}

void loop() {
  // put your main code here, to run repeatedly:
  if(digitalRead(toglle) == 1){
    START_P(); 
    digitalWrite(2, HIGH);
    digitalWrite(4, LOW);
    delay(15);
  }else{
    digitalWrite(2, LOW);
    digitalWrite(4, HIGH);
    INTERVAL(); 
  }
}

Czesc wszystkim, sprobowalem zrobic interwal do serwa aby uzyskac plynny ruch lecz malo zadzialal. Symulacje przeprowadzam w programie "wokwi". Link do kodu: https://wokwi.com/projects/362914202810529793 Czy dalby ktos rade na bazie mojego programu poprawic dzialanie tego kodu? 

Z gory dziekuje!

Wojtek

[farmaceuta]

Jaki zakres mają liczby a1/B1?