Форум РадиоКот

Код: Выделить всё

//magnetic levitation program: PID controller

float Kp=0.5;
float Kd=8.5;
int Ki=0;
int midValue=400; // Hall sensor value for equilibrium state

int in1=11;
int in2=10;
int ena=9; // PWM pins for y coils

int in3=8;
int in4=7;
int enb=6; // PWM pins for x coils

void setup()
{
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(ena, OUTPUT);
 
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);
  pinMode(enb, OUTPUT);
 
  Serial.begin(9600);
}

  int errorX; // midValue - current X hall sensor reading
  int prevErrX=0; // previous error for calculating dirivative
  int dErrorX; //derivative of error in X
 
  int errorY; // midValue - current Y hall sensor reading
  int prevErrY=0; // previous error for calculating dirivative
  int dErrorY; // derivative of error in Y
 
  int powerX; //PWM number of driving X coil
  int powerY;  //PWM number of driving Y coil
 
  int HallX;
  int HallY; //Hall sensor readings
 
  int j=0; //control how many data to send to computer
 
void loop()
{
  j++;
 
  int AveHallX=0;
  int AveHallY=0;// average hall sensor readings
 
  for(int i=0;i<4;i++)
  {
    HallX=analogRead(A1);
    HallY=analogRead(A0);
    AveHallX=AveHallX+HallX;
    AveHallY=AveHallY+HallY;
  }
  AveHallX=AveHallX/4;
  AveHallY=AveHallY/4;
 
  errorX=midValue-AveHallX;
  errorY=midValue-AveHallY;
 
  dErrorX=errorX-prevErrX;
  dErrorY=errorY-prevErrY;
   
  powerX=errorX*Kp+dErrorX*Kd;
  powerY=errorY*Kp+dErrorY*Kd;
 
  if(powerX>0) // need to push in -x direction
  {
    digitalWrite(in3, HIGH);
    digitalWrite(in4, LOW);
    if(powerX>255)
    {
      powerX=255;
    }
    analogWrite(enb, powerX);
  }
  else //need to push in +x direction
  {
    powerX=-powerX;
    digitalWrite(in3, LOW);
    digitalWrite(in4, HIGH);
    if(powerX>255)
    {
      powerX=255;
    }
    analogWrite(enb, powerX);
  }
 
  if(powerY>0) //need to push in -y direction
  {
    digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW);
    if(powerY>255)
    {
      powerY=255;
    }
    analogWrite(ena, powerY);
  }
  else // need to push in +y direction
  {
    powerY=-powerY;
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);
    if(powerY>255)
    {
      powerY=255;
    }
    analogWrite(ena, powerY);
  }
 
  prevErrX=errorX;
  prevErrY=errorY;
 

}