カテゴリー: バランスロボット

  // calc motor power
  float Duty, P, D, now, dt, Time;
  now = TARGET - roll_data;    // calc delta
  if (-25 < now && now < 25) { if (abs(now) >= 0){
      Time = micros();
      dt = (Time - preTime)/1000000;
      preTime = Time;
      P = now/90; // (-90~90) → (-1.0~1.0)
      I += P*dt;
      D = (P - preP)/dt;
      preP = P;
      power = P_val * P + I_val * I + D_val * D;
      //Serial.println(power);
      if (power < -1) power = -1;    // limit the max value
      if (1 < power)  power = 1; 
      if (power < 0){
        dir = 1;
      }else{
        dir = -1;
        }               // set forward/reverse
    }