Arduino ArduinoMotorCarrier - PID用法及代碼示例

pid1. setGains(int P, int I, int D)

#include <ArduinoMotorCarrier.h>
#define INTERRUPT_PIN 6

//Variable to change the motor speed and direction
static int duty = 0;

int target;
float P;
float I;
float D;

void setup()
{
  //Serial port initialization
  Serial.begin(115200);
  while (!Serial);

  //Establishing the communication with the motor shield
  if (controller.begin())
  {
    Serial.print("MKR Motor Shield connected, firmware version ");
    Serial.println(controller.getFWVersion());
  }
  else
  {
    Serial.println("Couldn't connect! Is the red led blinking? You may need to update the firmware with FWUpdater sketch");
    while (1);
  }

  // Reboot the motor controller; brings every value back to default
  Serial.println("reboot");
  controller.reboot();
  delay(500); 

  int dutyInit = 0; 
  M1.setDuty(dutyInit);
  M2.setDuty(dutyInit);
  M3.setDuty(dutyInit);
  M4.setDuty(dutyInit);
  Serial.print("Duty: ");
  Serial.println(dutyInit);

  P = 0.07f;//0.07 //0.2
  I = 0.0f;
  D = 0.007f;

  /************* PID 1 ***********************/

  pid1.setControlMode(CL_POSITION);

  //pid1.resetGains();  
  pid1.setGains(P, I, D); //Proportional(change) Integral(change) Derivative
  Serial.print("P Gain: ");
  Serial.println((float)pid1.getPgain());
  Serial.print("I Gain: ");
  Serial.println((float)pid1.getIgain());
  Serial.print("D Gain: ");
  Serial.println((float)pid1.getDgain(), 7);
  Serial.println("");

  encoder1.resetCounter(0);
  Serial.print("encoder1: ");
  Serial.println(encoder1.getRawCount());
  target = 5000;
  pid1.setSetpoint(TARGET_POSITION, target);
}

void loop() {

  Serial.print("encoder1: ");
  Serial.print(encoder1.getRawCount());
  Serial.print(" target: ");
  Serial.println(target);
  if (encoder1.getRawCount() == target) {
    target += 1000;
    Serial.print("Target reached: Setting new target..");
    pid1.setSetpoint(TARGET_POSITION, target);
    //delay(5000);
  }

  delay(50);
}