版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

           Distributed under MIT license.See file LICENSE for detail or copy at

           https://opensource.org/licenses/MIT

           by 机器谱 2023-04-07 https://www.robotway.com/

  ------------------------------*/

#include <Servo.h>

#define servo_speed 120   //servo_speed

#define action_delay 1000 //action_delay

Servo myServo[5];

int servo_port[5]={4, 7, 3, 8, 12};   //servo_pin

float value_init[5]={115, 60, 156, 35, 75};   //servo_value_inital

int num = 10;   

int servo_num = sizeof(servo_port)/sizeof(servo_port[0]); //servo_pin length

boolean debug = false;   //if(true) debug

void setup() {

  Serial.begin(9600);  

  for(int i=0;i<servo_num;i++)

    ServoGo(i,value_init[i]);  

  delay(action_delay);

}

void loop() {

      servo_move(115, 105, 160, 110, 20);

      servo_move(140, 60, 160, 35, 75);

      get_milk();

      putMilk_near();

      servo_move(115, 105, 160, 110, 20);

      servo_move(140, 60, 160, 35, 75);

      get_milk();

      putMilk_far();

}

void ServoStart(int which)

{

  if(!myServo[which].attached())myServo[which].attach(servo_port[which]);

  pinMode(servo_port[which], OUTPUT);

}

void ServoStop(int which)

{

  myServo[which].detach();

  digitalWrite(servo_port[which],LOW);

}

void ServoGo(int which , int where)

{

  if(where!=200)

  {

    if(where==201) ServoStop(which);

    else

    {

      ServoStart(which);

      myServo[which].write(where);

    }

  }  

  ServoStart(which);

  myServo[which].write(where);

}

void get_milk()

{

  servo_move(115, 105, 160, 110, 20);   //down

  servo_move(160, 95, 160, 95, 40);    //get

  servo_move(160, 60, 160, 35, 75);    //up  

  delay(action_delay);  

  servo_move(160, 60, 25, 35, 75); //turn  

}

void putMilk_near()

{

  //Serial.println("near");

  servo_move(160, 60, 25, 60, 65); //down

  servo_move(140, 60, 25, 60, 65); //put

  servo_move(115, 60, 160, 35, 75);//reset

}

void putMilk_far()

{

  //Serial.println("far");

  servo_move(160, 100, 22, 56, 54); //down

  servo_move(140, 103, 22, 56, 54); //put

  servo_move(115, 60, 160, 35, 75); //reset

}

void servo_move(float value0, float value1, float value2, float value3, float value4)

{

  //Serial.println("begin");  

  float value_arguments[] = {value0, value1, value2, value3, value4};

  float value_delta[servo_num];  

  for(int i=0;i<servo_num;i++)

  {

    value_delta[i] = (value_arguments[i] - value_init[i]) / num;

  }  

  for(int i=0;i<num;i++)

  {

    for(int k=0;k<servo_num;k++)

    {

      value_init[k] = value_delta[k] == 0 ? value_arguments[k] : value_init[k] + value_delta[k];

    }

    for(int j=0;j<servo_num;j++)

    {

      ServoGo(j,value_init[j]);

    }

    delay(servo_speed);

  }

  delay(action_delay);  

}