Advertisement
KRITSADA

PID Control Simple P and I use DC Motor and Volume

Aug 10th, 2016
219
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 1.60 KB | None | 0 0
  1. #include <ModbusRtu.h>
  2.  
  3. // data array for modbus network sharing
  4. uint16_t au16data[16];
  5.  
  6. #define E2 3
  7. #define I3 12
  8. #define I4 13
  9. int Target_VR = A4;
  10. int Position_VR = A5;
  11. int Target = 0;
  12. int feedback = 0;
  13. double Error = 0;
  14. double Output=0;
  15.  
  16. double Out_p = 0;
  17. double Out_i = 0;
  18. double Temp_i = 0;
  19. double Kp = 1.0;
  20. double Ki = 0.025;
  21.  
  22.  
  23. Modbus slave(1,1,0); // this is slave @1 and RS-232 or USB-FTDI
  24.  
  25. void setup() {
  26.  
  27. slave.begin( 19200 ); // baud-rate at 19200
  28.  
  29. pinMode(E2,OUTPUT);
  30. pinMode(I3,OUTPUT);
  31. pinMode(I4,OUTPUT);
  32. Serial.begin(9600);
  33.  
  34.  
  35.  
  36. }
  37.  
  38. void loop() {
  39.  
  40.   Target = analogRead(Target_VR);
  41.   feedback = analogRead(Position_VR);
  42.  
  43.                Error = Target - feedback;
  44.                Out_p = Error * Kp;
  45.                Temp_i = Temp_i +Error;
  46.                Out_i = Temp_i *Ki;
  47.                if(Out_i>255)Out_i = 255;
  48.                if(Out_i<-255)Out_i = -255;
  49.                Output = Out_p + Out_i;  
  50.  
  51.  
  52.  if(Output>0)
  53.  {
  54.   if(Output > 255) Output=255;
  55.   digitalWrite(I3,HIGH);
  56.   digitalWrite(I4,LOW);
  57.  }
  58.  if(Output<0)
  59.  {
  60.   if(Output < -255) Output = -255;
  61.  }
  62.   analogWrite(E2,Output);
  63.  
  64.  
  65.  
  66.  
  67. Serial.print("\nTarget  Feedback  Output2 OUT       P     I");
  68.  Serial.print("\n");
  69.  Serial.print(Target);
  70.  Serial.print("\t");
  71.  Serial.print(feedback);
  72.  Serial.print("\t");
  73.  Serial.print( 700);
  74.  Serial.print("\t");
  75.  Serial.print(300);
  76.  //Serial.print("\t");
  77.  //Serial.print(Out_p);
  78.  //Serial.print("\t");
  79.  //Serial.print(Out_i);
  80.  
  81.  au16data[1]= feedback;
  82.  au16data[2]= Error;
  83.  Target = au16data[3];
  84.  
  85.  delay(30);
  86.  slave.poll( au16data, 16 );
  87. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement