Scribd Logo
Upload

Welcome to Scribd!

  • 10 views

    SpeedControl Ino

    Uploaded by

    Ouail Makhebi
    This document contains code for controlling a motor using PID control based on encoder feedback. It defines pin connections, initializes global variables for position and velocity, and contains functions for setup, control loop, setting the motor direction and speed, and reading the encoder. The control loop computes two velocity measurements, filters the results, sets a target velocity, calculates the control signal, and sets the motor PWM and direction pins accordingly.

    Copyright:

    © All Rights Reserved
    Download as TXT, PDF, TXT or read online from Scribd

    SpeedControl Ino

    Uploaded by

    Ouail Makhebi
    10 views3 pages
    This document contains code for controlling a motor using PID control based on encoder feedback. It defines pin connections, initializes global variables for position and velocity, and contains functions for setup, control loop, setting the motor direction and speed, and reading the encoder. The control loop computes two velocity measurements, filters the results, sets a target velocity, calculates the control signal, and sets the motor PWM and direction pins accordingly.

    Original Title

    SpeedControl.ino

    Copyright

    © © All Rights Reserved

    Available Formats

    TXT, PDF, TXT or read online from Scribd

    Did you find this document useful?

    Is this content inappropriate?

    This document contains code for controlling a motor using PID control based on encoder feedback. It defines pin connections, initializes global variables for position and velocity, and contains functions for setup, control loop, setting the motor direction and speed, and reading the encoder. The control loop computes two velocity measurements, filters the results, sets a target velocity, calculates the control signal, and sets the motor PWM and direction pins accordingly.

    Copyright:

    © All Rights Reserved
    Download as TXT, PDF, TXT or read online from Scribd
    Download as txt, pdf, or txt
    10 views3 pages

    SpeedControl Ino

    Uploaded by

    Ouail Makhebi
    This document contains code for controlling a motor using PID control based on encoder feedback. It defines pin connections, initializes global variables for position and velocity, and contains functions for setup, control loop, setting the motor direction and speed, and reading the encoder. The control loop computes two velocity measurements, filters the results, sets a target velocity, calculates the control signal, and sets the motor PWM and direction pins accordingly.

    Copyright:

    © All Rights Reserved
    Download as TXT, PDF, TXT or read online from Scribd
    Download as txt, pdf, or txt
    of 3

    #include <util/atomic.

    h>

    // Pins
    #define ENCA 2
    #define ENCB 3
    #define PWM 5
    #define IN1 6
    #define IN2 7

    // globals
    long prevT = 0;
    int posPrev = 0;
    // Use the "volatile" directive for variables
    // used in an interrupt
    volatile int pos_i = 0;
    volatile float velocity_i = 0;
    volatile long prevT_i = 0;

    float v1Filt = 0;
    float v1Prev = 0;
    float v2Filt = 0;
    float v2Prev = 0;

    float eintegral = 0;

    void setup() {
    Serial.begin(115200);

    pinMode(ENCA,INPUT);
    pinMode(ENCB,INPUT);
    pinMode(PWM,OUTPUT);
    pinMode(IN1,OUTPUT);
    pinMode(IN2,OUTPUT);

    attachInterrupt(digitalPinToInterrupt(ENCA),
    readEncoder,RISING);
    }

    void loop() {

    // read the position in an atomic block


    // to avoid potential misreads
    int pos = 0;
    float velocity2 = 0;
    ATOMIC_BLOCK(ATOMIC_RESTORESTATE){
    pos = pos_i;
    velocity2 = velocity_i;
    }

    // Compute velocity with method 1


    long currT = micros();
    float deltaT = ((float) (currT-prevT))/1.0e6;
    float velocity1 = (pos - posPrev)/deltaT;
    posPrev = pos;
    prevT = currT;

    // Convert count/s to RPM


    float v1 = velocity1/600.0*60.0;
    float v2 = velocity2/600.0*60.0;
    // Low-pass filter (25 Hz cutoff)
    v1Filt = 0.854*v1Filt + 0.0728*v1 + 0.0728*v1Prev;
    v1Prev = v1;
    v2Filt = 0.854*v2Filt + 0.0728*v2 + 0.0728*v2Prev;
    v2Prev = v2;

    // Set a target
    float vt = 100*(sin(currT/1e6)>0);

    // Compute the control signal u


    float kp = 5;
    float ki = 10;
    float e = vt-v1Filt;
    eintegral = eintegral + e*deltaT;

    float u = kp*e + ki*eintegral;

    // Set the motor speed and direction


    int dir = 1;
    if (u<0){
    dir = -1;
    }
    int pwr = (int) fabs(u);
    if(pwr > 255){
    pwr = 255;
    }
    setMotor(dir,pwr,PWM,IN1,IN2);

    Serial.print(vt);
    Serial.print(" ");
    Serial.print(v1Filt);
    Serial.println();
    delay(1);
    }

    void setMotor(int dir, int pwmVal, int pwm, int in1, int in2){
    analogWrite(pwm,pwmVal); // Motor speed
    if(dir == 1){
    // Turn one way
    digitalWrite(in1,HIGH);
    digitalWrite(in2,LOW);
    }
    else if(dir == -1){
    // Turn the other way
    digitalWrite(in1,LOW);
    digitalWrite(in2,HIGH);
    }
    else{
    // Or dont turn
    digitalWrite(in1,LOW);
    digitalWrite(in2,LOW);
    }
    }

    void readEncoder(){
    // Read encoder B when ENCA rises
    int b = digitalRead(ENCB);
    int increment = 0;
    if(b>0){
    // If B is high, increment forward
    increment = 1;
    }
    else{
    // Otherwise, increment backward
    increment = -1;
    }
    pos_i = pos_i + increment;

    // Compute velocity with method 2


    long currT = micros();
    float deltaT = ((float) (currT - prevT_i))/1.0e6;
    velocity_i = increment/deltaT;
    prevT_i = currT;
    }

    You might also like