Code Mẫu, điều Khiển Motor Bằng PID - NgLeY

Đến nội dung /* Controlling Motor using PID */ //Define the DEBUG functions to quickly enable debuging mode. #define DEBUG #ifdef DEBUG #define DEBUG_PRINT(x) Serial.println (x) #define PRINT(s,v) { Serial.print(F(s));Serial.print(":"); Serial.print(v);Serial.print("\t");} #define PRINTLN(s,v) { Serial.print(F(s));Serial.print(":"); Serial.println(v);} #else #define DEBUG_PRINT(x) #define PRINT(s,v) #define PRINTLN(s,v) #endif // Include libraries #include <MegunoLink.h> #include <CommandHandler.h> //for using command to control the Arduino #include <ArduinoTimer.h> //an easy way to implement timer #include <EEPROMStore.h> //to save the config, variable to EEPROMS, which will not be deteled when power off #include <Filter.h> #include <FastPID.h> #include <Encoder.h> #include <Wire.h> #include <LiquidCrystal_I2C.h> //Define PIN #define encoderMotor1 3 // pin D3 #define encoderMotor2 2 // pin D2 #define IN1 6 #define IN2 5 //Define variables #define MAX_SPEED 255 //từ 0-255 #define MIN_SPEED 30 #define encoderPerRound 1300 bool output_signed = false, systemOn = false, TimerMode = false; int output_bits = 8, SetSpeed = 200, SetTime = 0, currentMotorSpeed, PIDOutput; long lastMotorPosition = 0, lastTimeCal = 0; float Kp = 0.1, Ki = 0.5, Kd = 0, Hz = 10; ArduinoTimer Timer1; //Timer for PID and Speed, increase or reduce this may cause error in speed calculation. ArduinoTimer Timer2; //Timer for user to set timer CommandHandler<> SerialCommandHandler; ExponentialFilter<long> SpeedFilter(10, 0); Encoder motorEncoder(encoderMotor1, encoderMotor2); FastPID myPID(Kp, Ki, Kd, Hz, output_bits, output_signed); void setup() { // put your setup code here, to run once: Serial.begin(9600); pinMode(encoderMotor1, INPUT_PULLUP); pinMode(encoderMotor2, INPUT_PULLUP); SerialCommandHandler.AddCommand(F("speed"), Cmd_SetSpeed); SerialCommandHandler.AddCommand(F("run"), Cmd_OnOff); SerialCommandHandler.AddCommand(F("timeron"), Cmd_TimerStart); } void loop() { // put your main code here, to run repeatedly: if (systemOn) { if (Timer1.TimePassed_Milliseconds(50)) { currentMotorSpeed = MotorSpeedCalculate(); PIDOutput = myPID.step(SetSpeed, currentMotorSpeed); Motor(PIDOutput); PRINT("Speed", currentMotorSpeed); PRINTLN("PIDout", PIDOutput); SerialCommandHandler.Process(); } if ((Timer2.EllapsedSeconds() > SetTime) && (TimerMode)) { Timer2.Reset(); systemOn = false; TimerMode = false; } } else { SerialCommandHandler.Process(); currentMotorSpeed = MotorSpeedCalculate(); Motor(); //Motor stop } SerialCommandHandler.Process(); } /* Define CMD functions */ void Cmd_SetSpeed(CommandParameter &Parameters) { SetSpeed = constrain(Parameters.NextParameterAsInteger(), 0, 300); } void Cmd_SetTime(CommandParameter &Parameters) { SetTime = Parameters.NextParameterAsInteger(); } void Cmd_OnOff(CommandParameter &Parameters) { systemOn = !systemOn; } void Cmd_TimerStart(CommandParameter &Parameters) { Timer2.Reset(); SetTime = Parameters.NextParameterAsInteger(); systemOn = true; TimerMode = true; } /* Define Motor control code */ float MotorSpeedCalculate() { long currentMotorPosition = motorEncoder.read(); long currentTimeCal = millis(); int Speed = ((currentMotorPosition - lastMotorPosition) * 60000 / encoderPerRound ) / (currentTimeCal - lastTimeCal) ; SpeedFilter.Filter(Speed); lastMotorPosition = currentMotorPosition; lastTimeCal = currentTimeCal; return SpeedFilter.Current(); } void Motor() { digitalWrite(IN1, LOW); digitalWrite(IN2, LOW); } void Motor(int speed) { // speed = constrain(speed, MIN_SPEED, MAX_SPEED);//đảm báo giá trị nằm trong một khoảng từ 0 - MAX_SPEED - http://arduino.vn/reference/constrain speed = MIN_SPEED + speed * (MAX_SPEED - MIN_SPEED) / 255; digitalWrite(IN1, LOW);// chân này không có PWM analogWrite(IN2, speed); }

Chia sẻ:

• Tweet
• Email
• In

Thích Đang tải...

Có liên quan

Đăng bởi ley995

A Vietnamese Biomedical Engineer - Like Green | Astronomy | Xem thêm bài viết

Điều hướng bài viết

Bài trước Bài cũ hơn: Sử dụng thư viện Encoder để đọc dữ liệu encoderBài sau Bài tiếp theo: Hướng dẫn setup Cura Phiên bản >4 cho máy in Anet A6

Bình luận về bài viết này Hủy trả lời

Δ

Trang web này sử dụng Akismet để lọc thư rác. Tìm hiểu cách xử lý bình luận của bạn.

Trang này sử dụng cookie. Tìm hiểu cách kiểm soát ở trong: Chính Sách Cookie

• Bình luận
• Đăng lại
• Theo dõi Đã theo dõi
  • NgLeY
  Theo dõi ngay
  • Đã có tài khoản WordPress.com? Đăng nhập.
• • NgLeY
  • Tùy biến
  • Theo dõi Đã theo dõi
  • Đăng ký
  • Đăng nhập
  • URL rút gọn
  • Báo cáo nội dung
  • Xem toàn bộ bài viết
  • Quản lý theo dõi
  • Ẩn menu

%d Tạo trang giống vầy với WordPress.comHãy bắt đầu