1

I am trying to control dc motor speed with PWM. When I test it with simple program like spin in one direction with set PWM value it works. If PWM>70 the motor will start spinning and max is 255.

But when I try to do the same in code below it only spins if PWM value is 255. Can someone help me?

test.ino

/*--------------------*/
// encoders
/*--------------------*/
//motor_encoder
int encoder0PinA = 3;
int encoder0PinB=2;
volatile int encoder0Count = 0;
volatile float angle0 = 0;
volatile float velocity0 = 0;
volatile float angle0_previous = 0;
volatile float angle0_post = 0;
//pendulum_encoder
int encoder1PinA = 20;
int encoder1PinB = 21;
volatile int encoder1Count = 0;
volatile float angle1 = 0;
volatile float velocity1 = 0;
volatile float angle1_previous = 0;
volatile float angle1_post = 0;
float error=0;
/*--------------------*/
// motor
/*--------------------*/
int enA = 10;
int in1 = 9;
int in2 = 8;
/*--------------------*/
// for Timer2
/*--------------------*/
volatile int tcnt2 = 131;
volatile int t = 0;

volatile float error_proportional = 0;
volatile float error_derivative = 0;
volatile float error_integral = 0;
float kp =50;
float kd = 0;
float ki = 0;
float u = 0;
float motor_input = 0;
int INPUT_MAX = 255;
int P=0;
int I=0;
int D=0;
int integral=0;
void setup()
{

  pinMode(enA, OUTPUT);
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);

  /*--------------------*/
  // for encoder
  /*--------------------*/  
  // for encoder 0
  pinMode(encoder0PinA, INPUT); 
  pinMode(encoder0PinB, INPUT); 
  attachInterrupt(0, doEncoder0A, CHANGE);  
  attachInterrupt(1, doEncoder0B, CHANGE);  
  // for encoder 1
  pinMode(encoder1PinA, INPUT); 
  pinMode(encoder1PinB, INPUT); 
  attachInterrupt(2, doEncoder1A, CHANGE);  
  attachInterrupt(3, doEncoder1B, CHANGE);    

  /*--------------------*/
  // for Timer2
  /*--------------------*/
  // interrupts every 1 ms
  TIMSK2 &= ~(1<<TOIE2);
  TCCR2A &= ~((1<<WGM21) | (1<<WGM20));
  TCCR2B &= ~(1<<WGM22);
  ASSR &= ~(1<<AS2);
  TIMSK2 &= ~(1<<OCIE2A);
  TCCR2B |= (1<<CS22)  | (1<<CS20);
  TCCR2B &= ~(1<<CS21);
  TCNT2 = tcnt2;
  TIMSK2 |= (1<<TOIE2);

  Serial.begin (9600); // for debugging
}

void loop()
{

 error = angle1;

 if (abs(error) < 2){ 
 integral = integral + error; 
 }
 else {
 integral=0; 
 }
 P = error*kp;
 I = integral*ki; 
 D = (angle1_previous-angle1)*kd; 
  u = P + I + D;
  motor_input = 255/24 * u; 

  Serial.print("u: ");
  Serial.print( u);
  Serial.print(" ");
  motor_input = (int)motor_input;

  if (motor_input >= INPUT_MAX){
    motor_input = INPUT_MAX;
  }
  else if (motor_input <= -INPUT_MAX){
    motor_input = -INPUT_MAX;
  }

  Serial.print("error_proportional: ");
  Serial.print(error_proportional);
  Serial.print(" ");
  Serial.print("error_derivative: ");
  Serial.print(error_derivative);
  Serial.print(" ");
  Serial.print("error_integral: ");
  Serial.print(error_integral);
  Serial.print(" ");
  Serial.print("motor_input: ");
  Serial.println(motor_input);
  setMotorSpeed(motor_input); 
}

measure_states.ino

/*--------------------*/
// enkoder od motora
/*--------------------*/
void doEncoder0A(){  
  if (digitalRead(encoder0PinA) == HIGH) { 
    if (digitalRead(encoder0PinB) == LOW) {
      encoder0Count = encoder0Count + 1;
    } 
    else {
      encoder0Count = encoder0Count - 1;
    }
  }
  else {                                     
    if (digitalRead(encoder0PinB) == HIGH) { 
      encoder0Count = encoder0Count + 1;
    } 
    else {
      encoder0Count = encoder0Count - 1;
    }
  }
  angle0 = 0.000785*encoder0Count;  // radian  (2*pi/8000)
}

void doEncoder0B(){  // interrupt 1 function
  if (digitalRead(encoder0PinB) == HIGH) {   
    if (digitalRead(encoder0PinA) == HIGH) { 
      encoder0Count = encoder0Count + 1;
    } 
    else {
      encoder0Count = encoder0Count - 1;
    }
  }
  else {                                   
    if (digitalRead(encoder0PinA) == LOW) { 
      encoder0Count = encoder0Count + 1;
    } 
    else {
      encoder0Count = encoder0Count - 1;
    }
  }
  angle0 = 0.000785*encoder0Count;  
}

/*--------------------*/
// for encoder 1
/*--------------------*/
void doEncoder1A(){  
  if (digitalRead(encoder1PinA) == HIGH) {  
    if (digitalRead(encoder1PinB) == LOW) { 
      encoder1Count = encoder1Count + 1;
    } 
    else {
      encoder1Count = encoder1Count - 1;
    }
  }
  else {                                     
    if (digitalRead(encoder1PinB) == HIGH) { 
      encoder1Count = encoder1Count + 1;
    } 
    else {
      encoder1Count = encoder1Count - 1;
    }
  }
  angle1 = 0.000785*encoder1Count;  
}

void doEncoder1B(){  
  if (digitalRead(encoder1PinB) == HIGH) {  
    if (digitalRead(encoder1PinA) == HIGH) { 
      encoder1Count = encoder1Count + 1;
    } 
    else {
      encoder1Count = encoder1Count - 1;
    }
  }
  else {                                   
    if (digitalRead(encoder1PinA) == LOW) { 
      encoder1Count = encoder1Count + 1;
    } 
    else {
      encoder1Count = encoder1Count - 1;
    }
  }
  angle1 = 0.000785*encoder1Count;  
}

ISR(TIMER2_OVF_vect) {  
  TCNT2 = tcnt2;  
  t++;

  // estimate velocity
  if (t == 1){
    angle0_previous = angle0;
    angle1_previous = angle1;
  }
  else if (t == 20){
    angle0_post = angle0;  
    angle1_post = angle1;
  }
  else if (t == 21){
    velocity0 = (angle0_post - angle0_previous)*50;  // rad/s
    velocity1 = (angle1_post - angle1_previous)*50;

    error_proportional = 3.1416 - angle1;
    error_derivative = 3.1416 - velocity1;
    error_integral = error_integral + error_proportional;

    t = 0;
  }
}

motor.ino

void setMotorSpeed(int speed)
{
  if (speed < 0)
  {
    digitalWrite(in1, LOW);
    digitalWrite(in2, HIGH);

  }
  else
  {
    digitalWrite(in1, HIGH);
    digitalWrite(in2, LOW);

  }

  analogWrite(enA,abs(speed));

  angle1_previous=angle1;
}

UPDATE 1:

I found that the code below for setting timer messes PWM but Im not sure why. Anyone have any idea?

TIMSK2 &= ~(1<<TOIE2);
  TCCR2A &= ~((1<<WGM21) | (1<<WGM20));
  TCCR2B &= ~(1<<WGM22);
  ASSR &= ~(1<<AS2);
  TIMSK2 &= ~(1<<OCIE2A);
  TCCR2B |= (1<<CS22)  | (1<<CS20);
  TCCR2B &= ~(1<<CS21);
  TCNT2 = tcnt2;
  TIMSK2 |= (1<<TOIE2);
  • Arduino core uses built in timers to perform PWM - setting them manually using the AVR registers messes it up. This may be helpful - see the part where it says which timer corresponds to which pins. – Sebi Jan 28 '16 at 16:12

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.