0
\$\begingroup\$

I am trying to control just one motor using a H-bridge as shown in the picture (one motor only). I am attempting to use PD control, but for some reason I only obtain a sin wave when I multiply sin(x) by a large number such as 250, i wanted to be bounded between -1 and 1. Is there a problem with my sampling rate? I am reading the encoder in degrees and obtaining the error between the setpoint (the sin wave) and the encoder reading (in degrees). See the output sin curve attached.Harmonic response

Schematics

#include <Encoder.h>

//Gearbox values
//-------------------------
int gearRatio=298; // 298:1
int ppr=3;//(pulses per revolution of encoder)
//--------------------------

//-------------------
volatile long int Position=0;// counts the number of tick from encoder , volatile is to make sure the value is updated is the main loop

float degPosition=0.0; // holds the rotating angle in degrees, accepts decimal points 
float radPosition=0.0; // holds the rotating anngle in radians
//---------------------

//----- peripheral pins-------------------
const int motorPin1  = 6;  // Pin 2 of L293
const int motorPin2  =7;  // Pin 7 of L293
const int enablePin= 10;
const byte interruptPin = 2;
//-----------------------

//  Initialize timer

const int freq = 1000;
//  Calculate sampling time in us
const int dt = round((1.0/(float)freq)*1000000.0); 

float PID_error = 0;
float previous_error = 0;
long unsigned change_in_time, current_time, prev_time;
float PID_value = 0;

// 
float setpointPosition = 0; 
float currentPostion = 0;

float kp = 12;    float ki = 0;   float kd = 2.5;
float P_term = 0;    float I_term = 0;    float D_term = 0;


void setup() {
  Serial.begin(9600);
   //Set pins as outputs
   pinMode(motorPin1, OUTPUT);
   pinMode(motorPin2, OUTPUT);
   pinMode(enablePin,OUTPUT); 
   digitalWrite(enablePin, HIGH); //ebales the chip to work

   pinMode(interruptPin, INPUT_PULLUP);
   pinMode(12,INPUT);
   attachInterrupt(digitalPinToInterrupt(interruptPin), encoder, RISING);
}

void loop() {

    //elapsed time
    current_time = micros();
    change_in_time = current_time-prev_time;
    
    // reset Degrees when they go above 360 deg. 
    if (degPosition>=360.0){Position=0;}
    if (degPosition<=-360.0){Position=0;}

    // transform Position to degrees
    degPosition = (Position*360.0/(ppr*gearRatio)); 
    radPosition=degPosition*0.0174532925;

    
    //target points and reference sin curve
    setpointPosition = 250*sin(float(prev_time/1e6));
    //Serial.print("Target position: ");
    //Serial.println(setpointPosition);
    //Serial.print(",");  
    //Serial.print("Degree position: ");
    Serial.println(degPosition,10);// 
    
    if (change_in_time > 7000){ 
         
    PID_error = (setpointPosition-degPosition);
    //Serial.print("Error is: ");
    //Serial.println(PID_error,3);
    P_term = kp*PID_error;
    I_term = I_term + ki*PID_error*change_in_time; 
    D_term = kd*(PID_error - previous_error)/change_in_time;
    
    PID_value = P_term + I_term + D_term;

    if (PID_value < -255){PID_value = -255;}
    if ((PID_value > -80) && (PID_value < 0)){PID_value = -80;}
    if (PID_value > 255){PID_value = 255;}
    if ((PID_value < 80) && (PID_value > 0)){PID_value = 80;}
    //Serial.print("PID value: ");
    //Serial.println(PID_value,3);
    
    if (PID_value >= 0){ 
      analogWrite(motorPin1,0);
      analogWrite(motorPin2,PID_value);
    } 
    else{
      analogWrite(motorPin2,0);
      analogWrite(motorPin1,-PID_value);
    }
      prev_time = current_time;
    }
    
}


void encoder(){ //
  
  if(digitalRead(12)==HIGH){
    Position++;
  }else {
    Position--;
  }
}
\$\endgroup\$
6
  • \$\begingroup\$ Have you checked the system for a step signal as the setpoint? Why does the encoder appear to depend only on a single pin 12? The setpoint 250*sin(t) is a fast changing setpoint. Perhaps try out a lower frequency. \$\endgroup\$ – AJN Jun 4 at 1:05
  • \$\begingroup\$ Please clarify this sentence: "for some reason I only obtain a sin wave when I multiply sin(x) by a large number such as 250, i wanted to be bounded between -1 and 1.". If you multiply sin with 250, the result will lie between +-250. \$\endgroup\$ – AJN Jun 4 at 1:07
  • \$\begingroup\$ The circuit diagram is too small to read the labels on the devices. Please replace it with a higher resolution diagram. \$\endgroup\$ – AJN Jun 4 at 5:21
  • \$\begingroup\$ Hi @AJN thank you for looking over my question. When I try a step response with a setpoint of 90 degrees, it works smoothly. But again this is a large setpoint, if I were to try a setpoint of 1 it would not work. This is what I mean (to answer your second question) I want to attempt an unscaled (i.e. not by 250 or 90) step/harmonic response, rather than having the need of scaling by a large number. Will upload a better quality image now. The encoder uses an interrupt pin (2) for phase A and pin (12) for Phase b. \$\endgroup\$ – Brandon Jun 4 at 10:47
  • \$\begingroup\$ If i am not mistaken, the analogwrite needs an input between 0-255. For small angle changes, the friction probably prevents the motor from moving. For small step input, does the motor move when the error persists for a long duration and the integrator charges and crosses the friction threshold? \$\endgroup\$ – AJN Jun 4 at 14:09

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.