5. 功能实现

上位机：Controller 1.0

下位机编程环境：Arduino 1.8.19

① 将参考例程（get_milk_new2.ino）下载到主控板：

/*------------------------------------------------------------------------------------版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.Distributed under MIT license.See file LICENSE for detail or copy athttps://opensource.org/licenses/MITby 机器谱 2023-04-07 https://www.robotway.com/------------------------------*/#include <Servo.h>#define servo_speed 120   //servo_speed#define action_delay 1000 //action_delayServo myServo[5];int servo_port[5]={4, 7, 3, 8, 12};   //servo_pinfloat value_init[5]={115, 60, 156, 35, 75};   //servo_value_initalint num = 10;   int servo_num = sizeof(servo_port)/sizeof(servo_port[0]); //servo_pin lengthboolean debug = false;   //if(true) debugvoid 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);   //downservo_move(160, 95, 160, 95, 40);    //getservo_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); //downservo_move(140, 60, 25, 60, 65); //putservo_move(115, 60, 160, 35, 75);//reset}void putMilk_far(){//Serial.println("far");servo_move(160, 100, 22, 56, 54); //downservo_move(140, 103, 22, 56, 54); //putservo_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);  }