blynkIWalk05-sin.ino

#include <Wire.h>
#include <PCA9685.h>            //PCA9685用ヘッダーファイル（秋月電子通商作成）
#include <BlynkSimpleEsp32_BLE.h>
#include <BLEDevice.h>
#include <BLEServer.h>


#define BLYNK_USE_DIRECT_CONNECT
#define BLYNK_PRINT Serial

char auth[] = "Authコード";

PCA9685 pwm = PCA9685(0x40);    //PCA9685のアドレス指定（アドレスジャンパ未接続時）

#define SERVOMIN 130            //最小パルス幅 (標準的なサーボパルスに設定)
#define SERVOMAX 640            //最大パルス幅 (標準的なサーボパルスに設定)

int16_t offset[] = {0, 79, 117, 36, 86, 36, 97, 82, 119, 35, 100, 69, 125}; 

//ロボット形状
float L1=50,L2=56;//unit:mm

//逆運動学関連
float fRthd0 = 0;
float fRx = 0, fRz = 65;

float rRthd0 = 0;
float rRx = 0, rRz = 65;

float fLthd0 = 0;
float fLx = 0, fLz = 65;

float rLthd0 = 0;
float rLx = 0, rLz = 65;


int Step = 0, Jump = 0, walkAd = 0, walkBack = 0, walkL = 0, walkR = 0, zz = 0;
int walkHeight = 10, period = 80, stride = 10, tilt =10;


//Blynk ヴァーチャルピンデータ受信
BLYNK_WRITE(V13) {
  Step = param.asInt();
}
BLYNK_WRITE(V21) {
  Jump = param.asInt();
}


BLYNK_WRITE(V0) {
  fRthd0 = param.asInt();
  rRthd0 = -fRthd0;
  fLthd0 = fRthd0;
  rLthd0 = -fRthd0;
}
BLYNK_WRITE(V1) {
  zz = param.asInt();
}



BLYNK_WRITE(V20) {
  if(param[1].asInt() == 1){
    walkAd = 1;
    walkBack = 0;
    walkL = 0;
    walkR = 0;
  }else if(param[1].asInt() == -1){
    walkAd = 0;
    walkBack = 1;
    walkL = 0;
    walkR = 0;
  }else if(param[0].asInt() == -1){
    walkAd = 0;
    walkBack = 0;
    walkL = 1;
    walkR = 0;
  }else if(param[0].asInt() == 1){
    walkAd = 0;
    walkBack = 0;
    walkL = 0;
    walkR = 1;
  }else{
    walkAd = 0;
    walkBack = 0;
    walkL = 0;
    walkR = 0;
  }
}

BLYNK_WRITE(V12) {
  fRz = param.asInt();
  rRz = fRz;
  fLz = fRz;
  rLz = fRz;
}
BLYNK_WRITE(V14) {
  walkHeight = param.asInt();
}
BLYNK_WRITE(V16) {
  stride = param.asInt();
}
BLYNK_WRITE(V17) {
  tilt = param.asInt();
}
BLYNK_WRITE(V15) {
  period = param.asInt();
}


void setup() {
  Serial.begin(115200);

  Blynk.setDeviceName("miniPupper");
  Blynk.begin(auth);
  
  pwm.begin();                   //初期設定 (アドレス0x40用)
  pwm.setPWMFreq(60);            //PWM周期を60Hzに設定 (アドレス0x40用)
}


void loop() {
  Blynk.run();

  if(Step){ //足踏み
    float tim,tt;
    unsigned long time_mSt= millis();

    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(fRx, fRthd0, fRz - walkHeight * sin(tt));
      rLIK(rLx, rLthd0, rLz - walkHeight * sin(tt));
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(fRx, fRthd0, fRz - walkHeight * cos(tt));
      rLIK(rLx, rLthd0, rLz - walkHeight * cos(tt));
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      rRIK(rRx, rRthd0, rRz - walkHeight * sin(tt));
      fLIK(fLx, fLthd0, fLz - walkHeight * sin(tt));
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      rRIK(rRx, rRthd0, rRz - walkHeight * cos(tt));
      fLIK(fLx, fLthd0, fLz - walkHeight * cos(tt));
    }
  }else if(walkAd){ //前進
    float tim,tt;
    unsigned long time_mSt= millis();

    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(fRx - stride * cos(tt), fRthd0, fRz - walkHeight * sin(tt));
      rLIK(rLx - stride * cos(tt), rLthd0, rLz - walkHeight * sin(tt));
      rRIK(rRx + stride * sin(tt), rRthd0, rRz- walkHeight * cos(tt));
      fLIK(fLx + stride * sin(tt), fLthd0, fLz- walkHeight * cos(tt));
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      rRIK(rRx + stride - 2 * stride * sin(tt), rRthd0, rRz);
      fLIK(fLx + stride - 2 * stride * sin(tt), fLthd0, fLz);
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(fRx + stride * sin(tt), fRthd0, fRz - walkHeight * cos(tt));
      rLIK(rLx + stride * sin(tt), rLthd0, rLz - walkHeight * cos(tt));
      rRIK(rRx - stride * cos(tt), rRthd0, rRz - walkHeight * sin(tt));
      fLIK(fLx - stride * cos(tt), fLthd0, fLz - walkHeight * sin(tt));
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(fRx + stride - 2 * stride * sin(tt), fRthd0, fRz);
      rLIK(rLx + stride - 2 * stride * sin(tt), rLthd0, rLz);
    }
  }else if(walkBack){ //後進
    float tim,tt;
    unsigned long time_mSt= millis();

    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(fRx + stride * cos(tt), fRthd0, fRz - walkHeight * sin(tt));
      rLIK(rLx + stride * cos(tt), rLthd0, rLz - walkHeight * sin(tt));
      rRIK(rRx, rRthd0, rRz- walkHeight * cos(tt));
      fLIK(fLx, fLthd0, fLz- walkHeight * cos(tt));
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      rRIK(rRx + stride * sin(tt), rRthd0, rRz);
      fLIK(fLx + stride * sin(tt), fLthd0, fLz);
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(fRx, fRthd0, fRz - walkHeight * cos(tt));
      rLIK(rLx, rLthd0, rLz - walkHeight * cos(tt));
      rRIK(rRx + stride * cos(tt), rRthd0, rRz - walkHeight * sin(tt));
      fLIK(fLx + stride * cos(tt), fLthd0, fLz - walkHeight * sin(tt));
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(fRx + stride * sin(tt), fRthd0, fRz);
      rLIK(rLx + stride * sin(tt), rLthd0, rLz);
    }
  }else if(walkL){ //左旋回
    float tim,tt;
    unsigned long time_mSt= millis();

    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(fRx, fRthd0 -tilt + 2 * tilt * sin(tt), fRz - walkHeight * sin(tt));
      rLIK(rLx, rLthd0 -tilt + 2 * tilt * sin(tt), rLz - walkHeight * sin(tt));
      rRIK(rRx, rRthd0 + tilt * cos(tt), rRz- walkHeight * cos(tt));
      fLIK(fLx, fLthd0 + tilt * cos(tt), fLz- walkHeight * cos(tt));
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      rRIK(rRx, rRthd0 - tilt * sin(tt), rRz);
      fLIK(fLx, fLthd0 - tilt * sin(tt), fLz);
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(fRx, fRthd0 + tilt * cos(tt), fRz - walkHeight * cos(tt));
      rLIK(rLx, rLthd0 + tilt * cos(tt), rLz - walkHeight * cos(tt));
      rRIK(rRx, rRthd0 -tilt + 2 * tilt * sin(tt), rRz - walkHeight * sin(tt));
      fLIK(fLx, fLthd0 -tilt + 2 * tilt * sin(tt), fLz - walkHeight * sin(tt));
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(fRx, fRthd0 - tilt * sin(tt), fRz);
      rLIK(rLx, rLthd0 - tilt * sin(tt), rLz);
    }
  }else if(walkR){ //右旋回
    float tim,tt;
    unsigned long time_mSt= millis();

    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(fRx, fRthd0 + tilt - 2 * tilt * sin(tt), fRz - walkHeight * sin(tt));
      rLIK(rLx, rLthd0 + tilt - 2 * tilt * sin(tt), rLz - walkHeight * sin(tt));
      rRIK(rRx, rRthd0 - tilt * cos(tt), rRz- walkHeight * cos(tt));
      fLIK(fLx, fLthd0 - tilt * cos(tt), fLz- walkHeight * cos(tt));
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      rRIK(rRx, rRthd0 + tilt * sin(tt), rRz);
      fLIK(fLx, fLthd0 + tilt * sin(tt), fLz);
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(fRx, fRthd0 - tilt * cos(tt), fRz - walkHeight * cos(tt));
      rLIK(rLx, rLthd0 - tilt * cos(tt), rLz - walkHeight * cos(tt));
      rRIK(rRx, rRthd0 + tilt - 2 * tilt *sin(tt), rRz - walkHeight * sin(tt));
      fLIK(fLx, fLthd0 + tilt - 2 * tilt *sin(tt), fLz - walkHeight * sin(tt));
    }

    time_mSt= millis();
    tim=0;    
    while(tim<period){
      tim=millis()-time_mSt;
      tt=float(tim * PI / 2 / period);
      fRIK(fRx, fRthd0 + tilt * sin(tt), fRz);
      rLIK(rLx, rLthd0 + tilt * sin(tt), rLz);
    }
  }else if(Jump){ //ジャンプ
    for(int i= fRz; i >= 35; i-= 5){
      fRIK(fRx, fRthd0, i);
      rRIK(rRx, rRthd0, i);
      fLIK(fLx, fLthd0, i);
      rLIK(rLx, rLthd0, i);
      delay(50);
    }
    
    fRIK(fRx, fRthd0, 100);
    rRIK(rRx, rRthd0, 100);
    fLIK(fLx, fLthd0, 100);
    rLIK(rLx, rLthd0, 100);
    delay(100);
    fRIK(fRx, fRthd0, fRz);
    rRIK(rRx, rRthd0, fRz);
    fLIK(fLx, fLthd0, fRz);
    rLIK(rLx, rLthd0, fRz);

    Blynk.virtualWrite(V21, LOW);
    Jump = 0;
  }else{
    fRIK(fRx, fRthd0, fRz + zz);
    rRIK(rRx, rRthd0, rRz + zz);
    fLIK(fLx, fLthd0, fLz + zz);
    rLIK(rLx, rLthd0, rLz + zz);
  }
}

void servo_write(int ch, int ang){ //動かすサーボチャンネルと角度を指定
  ang = map(ang, 0, 180, SERVOMIN, SERVOMAX); //角度（0～180）をPWMのパルス幅（130～640）に変換
  pwm.setPWM(ch, 0, ang);
}

//逆運動学の計算
void fRIK(float x,float th0,float z){
  float phi,ld, th1, th2;
  float zd = z / cos(th0/180.0 * PI);
  
  ld=sqrt(x*x + zd*zd);
  phi=atan2(x, zd);
  
  th1 = phi - acos((L1*L1 + ld*ld - L2*L2)/(2*L1*ld));
  th2 = acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2)) + th1;

  servo_write(1, th0 + offset[1]);
  servo_write(2, th1 * 180.0 / PI + offset[2]);
  servo_write(3, th2 * 180.0 / PI + offset[3]);
}

void rRIK(float x,float th0,float z){
  float phi,ld, th1, th2;
  float zd = z / cos(th0/180.0 * PI);
  
  ld=sqrt(x*x + zd*zd);
  phi=atan2(x, zd);

  th1 = phi - acos((L1*L1 + ld*ld - L2*L2)/(2*L1*ld));
  th2 = acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2)) + th1;

  servo_write(7, th0 + offset[7]);
  servo_write(8, th1 * 180.0 / PI + offset[8]);
  servo_write(9, th2 * 180.0 / PI + offset[9]);
}

void fLIK(float x,float th0,float z){
  float phi,ld, th1, th2;
  float zd = z / cos(th0/180.0 * PI);
  
  ld=sqrt(x*x + zd*zd);
  phi=atan2(x, zd);

  th1 = phi - acos((L1*L1 + ld*ld - L2*L2)/(2*L1*ld));
  th2 = acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2)) + th1;

  servo_write(4, th0 + offset[4]);
  servo_write(5, -(th1 * 180.0 / PI) + offset[5]);
  servo_write(6, -(th2 * 180.0 / PI) + offset[6]);
}

void rLIK(float x,float th0,float z){
  float phi,ld, th1, th2;
  float zd = z / cos(th0/180.0 * PI);
  
  ld=sqrt(x*x + zd*zd);
  phi=atan2(x, zd);
  
  th1 = phi - acos((L1*L1 + ld*ld - L2*L2)/(2*L1*ld));
  th2 = acos((ld*ld - L1*L1 - L2*L2)/(2*L1*L2)) + th1;

  servo_write(10, th0 + offset[10]);
  servo_write(11, -(th1 * 180.0 / PI) + offset[11]);
  servo_write(12, -(th2 * 180.0 / PI) + offset[12]);
}