arduino机械狗子

这几天写了写机械狗子的代码，使用十个舵机，分别当做狗子的头，脖子，四只脚和四条腿，来模拟狗子的各种动作，加了接触传感器，来做一些好玩的互动。我是看到淘宝https://item.taobao.com/item.htm?spm=a230r.1.14.1.3f2e7bdf1Bt4hj&id=585685365716&ns=1&abbucket=5#detail 四百多买一个成本不足一百的东西，有点亏
以下是代码：

#include 
#include "Adafruit_APDS9960.h"
#define ditou 30
#define pingshi 45
#define taitou  60
#define zuo 60
#define you 120
#define zhishi 90
uint8_t flagup = 30;
uint8_t flagri = 90;
Adafruit_APDS9960 apds;
Servo head, bozi, rfoot, rleg, lfoot, lleg, rfeet, rlegs, lfeet, llegs;

void setup()
{
  Serial.begin(115200);
  pinset();
  apds9960INIT();
  setzero();
}
void loop()
{
  int i = random(10);
  uint8_t data = handleGesture();
  Serial.println(data);
  if (data<10)
  {
    switch (data){
      case 1:
        flagup = taitou;
        break;
      case 2:
        flagup = ditou;
        break;
      case 3:
        flagri = zuo;
        break;
      case 4:
        flagri = you;
        break;
      default:
        break;
    }
  }
  else
  {
    if (data>200)
    {
      flagup = pingshi;
      flagri = zhishi;
    }
  }
  stand();
  switch (i) {
    case 0:
      huifu();
      break;
    case 1:
      lanyao();
      break;
    case 2:
      run();
      break;
    case 3:
      qiandun();
      break;
    case 4:
      houdun();
      break;
    case 5:
      woshou();
      break;
    case 6:
      xuxu();
      break;
    case 7:
      qiandun();
      break;
    case 8:
      qianpu();
      break;
    case 9:
      paotu();
      break;
    default:
      stand();
      break;
      // do something
      
  }
  delay(500);
}
int handleGesture() {
  uint8_t gesture = apds.readGesture();
  Serial.println(gesture);
  if(gesture == 0)
  {
    uint8_t dis = apds.readProximity();
    if(dis>200)
    {
        return dis;
    }
  }
  else
  {
    return gesture;
  }
}
void paotu()
{
  int j = 5;
  while (j--)
  {
    for (int i = 180; i >= 0; i -= 30)
    {
      head.write(flagup);
      bozi.write(flagri);
      rfoot.write(i);
      rleg.write(0);
      lfoot.write(180 - i);
      lleg.write(180);
      rfeet.write(140);
      rlegs.write(180);
      lfeet.write(140);
      llegs.write(0);
      delay(20);
    }
    for (int i = 0; i <= 180; i += 30)
    {
      head.write(flagup);
      bozi.write(flagri);
      rfoot.write(i);
      rleg.write(0);
      lfoot.write(180 - i);
      lleg.write(180);
      rfeet.write(140);
      rlegs.write(180);
      lfeet.write(140);
      llegs.write(0);
      delay(20);
    }
  }
}
void qianpu()
{
  setzero();
  head.write(pingshi);
  bozi.write(90);
  rfoot.write(0);
  rleg.write(0);
  lfoot.write(0);
  lleg.write(180);
  rfeet.write(20);
  rlegs.write(180);
  lfeet.write(40);
  llegs.write(0);
}
void run()
{
  int j = 5;
  while (j--)
  {
    for (int i = 0; i < 180; i += 10)
    {
      head.write(pingshi);
      bozi.write(90);
      rfoot.write(i);
      rleg.write(0);
      lfoot.write(i);
      lleg.write(180);
      rfeet.write(180);
      rlegs.write(180);
      lfeet.write(180);
      llegs.write(0);
      delay(20);
    }
    for (int i = 0; i <= 90; i += 10)
    {
      head.write(pingshi);
      bozi.write(90);
      rfoot.write(180);
      rleg.write(0);
      lfoot.write(180);
      lleg.write(180);
      rfeet.write(i);
      rlegs.write(180);
      lfeet.write(i);
      llegs.write(0);
      delay(20);
    }
  }
}
void lanyao()
{
  for (int i = 160; i > 0; i--)
  {
    head.write(flagup);
    bozi.write(flagri);
    rfoot.write(i);
    rleg.write(0);
    lfoot.write(i);
    lleg.write(180);
    rfeet.write(180 - i);
    rlegs.write(180);
    lfeet.write(180 - i);
    llegs.write(0);
    delay(20);
  }
  for (int i = 0; i < 180; i++)
  {
    head.write(pingshi);
    bozi.write(90);
    rfoot.write(i);
    rleg.write(0);
    lfoot.write(i);
    lleg.write(180);
    rfeet.write(180 - i);
    rlegs.write(180);
    lfeet.write(180 - i);
    llegs.write(0);
    delay(20);
  }
}
void xuxu()
{
  for (int i = 180; i > 0; i--)
  {
    head.write(taitou);
    bozi.write(90);
    rfoot.write(90);
    rleg.write(90);
    lfoot.write(90);
    lleg.write(90);
    rfeet.write(90);
    rlegs.write(90);
    lfeet.write(90);
    llegs.write(90);
    delay(20);
  }
  for (int i = 180; i > 0; i--)
  {
    head.write(taitou);
    bozi.write(40);
    rfoot.write(90);
    rleg.write(90);
    lfoot.write(90);
    lleg.write(90);
    rfeet.write(90);
    rlegs.write(90);
    lfeet.write(0);
    llegs.write(90);
    delay(20);
  }

}
void woshou()
{
  for (int i = 180; i > 0; i--)
  {
    head.write(flagup);
    bozi.write(flagri);
    rfoot.write(0);
    rleg.write(0);
    lfoot.write(90);
    lleg.write(90);
    rfeet.write(90);
    rlegs.write(90);
    lfeet.write(90);
    llegs.write(90);
    delay(20);
  }
  for (int i = 180; i > 0; i--)
  {
    head.write(flagup);
    bozi.write(flagri);
    rfoot.write(90);
    rleg.write(90);
    lfoot.write(90);
    lleg.write(90);
    rfeet.write(90);
    rlegs.write(90);
    lfeet.write(90);
    llegs.write(90);
    delay(20);
  }

}
void houdun()
{
  for (int i = 180; i > 0; i--)
  {
    head.write(taitou);
    bozi.write(90);
    rfoot.write(90);
    rleg.write(90);
    lfoot.write(90);
    lleg.write(90);
    rfeet.write(180);
    rlegs.write(180);
    lfeet.write(180);
    llegs.write(0);
    delay(20);
  }
}
void qiandun()
{
  for (int i = 180; i > 0; i--)
  {
    head.write(pingshi);
    bozi.write(40);
    rfoot.write(180);
    rleg.write(180);
    lfoot.write(180);
    lleg.write(0);
    rfeet.write(90);
    rlegs.write(90);
    lfeet.write(90);
    llegs.write(90);
    delay(20);
  }
}

void huifu()
{
  int j = 3;
  while (j--)
  {
    for (int i = 20; i < 180; i++)
    {
      if (i < pingshi)
        head.write(pingshi);
      if (i < 60)
        bozi.write(60);
      rfoot.write(i);
      rleg.write(i);
      lfoot.write(i);
      lleg.write(180 - i);
      rfeet.write(i);
      rlegs.write(i);
      lfeet.write(i);
      llegs.write(180 - i);
      delay(15);
    }
    for (int i = 180; i > 20; i--)
    {
      if (i < 60)
        head.write(taitou);
      if (i < 120)
        bozi.write(120);
      rfoot.write(i);
      rleg.write(i);
      lfoot.write(i);
      lleg.write(180 - i);
      rfeet.write(i);
      rlegs.write(i);
      lfeet.write(i);
      llegs.write(180 - i);
      delay(15);
    }
  }
}
void stand()
{
  for (int i = 180; i > 0; i--)
  {
    head.write(pingshi);
    bozi.write(90);
    rfoot.write(90);
    rleg.write(90);
    lfoot.write(90);
    lleg.write(90);
    rfeet.write(90);
    rlegs.write(90);
    lfeet.write(90);
    llegs.write(90);
    delay(20);
  }
}
void setzero()
{
  for (int i = 180; i > 0; i--)
  {
    head.write(pingshi);
    bozi.write(90);
    rfoot.write(180);
    rleg.write(180);
    lfoot.write(180);
    lleg.write(0);
    rfeet.write(180);
    rlegs.write(180);
    lfeet.write(180);
    llegs.write(0);
    delay(20);
  }
}
void pinset()
{
  head.attach(2);
  bozi.attach(3);
  rfoot.attach(4);
  rleg.attach(5);
  lfoot.attach(6);
  lleg.attach(7);
  rfeet.attach(8);
  rlegs.attach(9);
  lfeet.attach(10);
  llegs.attach(11);

  delay(1000);
}
void apds9960INIT()
{
   if (!apds.begin()) 
   {
    Serial.println("failed to initialize device! Please check your wiring.");
   }
   else Serial.println("Device initialized!");
   apds.enableProximity(true);
   apds.enableGesture(true);
}