Post Reply 
 
Thread Rating:
  • 0 Votes - 0 Average
  • 1
  • 2
  • 3
  • 4
  • 5
Uneven motor movements
05-14-2015, 05:24 PM
Post: #1
Uneven motor movements
Hello guys,

I´m currently trying to build my own B-robot, but I´ve met some problems. The motors will not run smooth and the robot is not able to keep balance. I´ve made my own circuit and I´m pretty sure that it is correct, cause the motors run smoothly with the following program
Code:
#define pinEnable 4 // Enable driver
#define pinStep 12
#define pinDir 13
#define pinStep2 6
#define pinDir2 8


void setup ()
{
  pinMode (pinEnable, OUTPUT);
  pinMode (pinDir, OUTPUT);
  pinMode (pinStep, OUTPUT);
  pinMode (pinDir2, OUTPUT);
  pinMode (pinStep2, OUTPUT);
}

void loop ()
{
  digitalWrite (pinDir, HIGH);  // Forward direction
  digitalWrite (pinStep, LOW);  // Initialize the step pin
  digitalWrite (pinDir2, HIGH);  // Forward direction
  digitalWrite (pinStep2, LOW);  // Initialize the step pin
  digitalWrite (pinEnable, LOW);
  
  // Forward
  for (int i = 0; i <1600; i ++)
  {
    Serial.println (i);
    digitalWrite (pinStep, HIGH);
    digitalWrite (pinStep2, HIGH);
    delayMicroseconds(1000);
    digitalWrite (pinStep, LOW);
    digitalWrite (pinStep2, LOW);
    delayMicroseconds(1000);
  }
  
  // Disable motor while waiting
  digitalWrite (pinEnable, HIGH);
  delay(2000);
  digitalWrite (pinEnable, LOW);
  
  // Changing direction
  digitalWrite (pinDir, LOW);
  digitalWrite (pinDir2, LOW);
  
  for (int i = 0; i <1600; i ++)
  {
    Serial.println (i);
    digitalWrite (pinStep, HIGH);
    digitalWrite (pinStep2, HIGH);
    delayMicroseconds(1000);
    digitalWrite (pinStep, LOW);
    digitalWrite (pinStep2, LOW);
    delayMicroseconds(1000);
  }
  
  // Disable motor while waiting
  digitalWrite (pinEnable, HIGH);
  delay(5000);
}

I also tried to modify the original program and remove everything except the motor stuff, but they still doesn´t run smooth. This is the modified program
Code:
// B-ROBOT  SELF BALANCE ROBOT WITH STEPPER MOTORS
// WITA Board (Arduino Leonardo + Wifi module board)
// MPU6050 using DMP processor
// Author: Jose Julio
// Date: 04/10/2013
// Updated: 18/10/2013
// License: GPL v2

// Angle calculations and control part is running at 200Hz from DMP solution
// DMP is using the gyro_bias_no_motion correction method.
// The board needs at least 10-15 seconds to give good values...

// STEPPER MOTOR PINS
// ENABLE PIN: D4
// STEP Motor1: D6 -> PORTD,7
// DIR  Motor1: D8 -> PORTB,4
// STEP Motor2: D12-> PORTD,6
// DIR  Motor2: D13-> PORTC,7
// To control the stepper motors we use Timer1 interrupt running at 25Khz. We control the speed of the motors
// Note: We could not use WITA led because pin 13 is already used for stepper motor control

// Robot servo arm connected to D5
// Distance sensor (sonar) connected to A0 (analog 0)
// Battery monitor (voltage divider) connected to A5

// We use a standard PID control for robot stability
// We have a P control for speed control and a PD control for stability (robot angle)
//         The output of the control (motor speed) is integrated so it´s really an acceleration

// We control the robot from a WIFI module using OSC standard UDP messages (OSCmini library)
//    fadder1: Throttle
//    fadder2: Steering
//    push1: Move arm (and robot raiseup)
//    push2: push up
//    toggle1: autonomous mode
//    PAGE2: PID adjustements

// Robot autonomous mode
//   the robot start walking until reach an obstacle. When find an obstacle, start steering until it find a free way and continue walking

#define CLR(x,y) (x&=(~(1<<y)))
#define SET(x,y) (x|=(1<<y))

#define ZERO_SPEED 65535
#define MAX_ACCEL 7

int16_t motor1;
int16_t motor2;

int16_t speed_m[2];           // Actual speed of motors
uint8_t dir_m[2];             // Actual direction of steppers motors
int16_t actual_robot_speed;          // overall robot speed (measured from steppers speed)
int16_t actual_robot_speed_Old;          // overall robot speed (measured from steppers speed)
float estimated_speed_filtered;

uint16_t counter_m[2];        // counters for periods
uint16_t period_m[2][8];      // Eight subperiods
uint8_t period_m_index[2];    // index for subperiods


// STEPPER MOTOR PINS
// ENABLE PIN: D4
// STEP Motor1: D6 -> PORTD,7
// DIR  Motor1: D8 -> PORTB,4
// STEP Motor2: D12-> PORTD,6
// DIR  Motor2: D13-> PORTC,7

// 200ns => 4 instructions at 16Mhz
void delay_200ns()  
{
  __asm__ __volatile__ (
        "nop" "\n\t"
                "nop" "\n\t"
                "nop" "\n\t"
        "nop");
}

ISR(TIMER1_COMPA_vect)
{
  counter_m[0]++;
  counter_m[1]++;
  if (counter_m[0] >= period_m[0][period_m_index[0]])
    {
    counter_m[0] = 0;
    if (period_m[0][0]==ZERO_SPEED)
      return;
    if (dir_m[0])
      SET(PORTB,4);  // DIR Motor 1
    else
      CLR(PORTB,4);
    // We need to wait at lest 200ns to generate the Step pulse...
    period_m_index[0] = (period_m_index[0]+1)&0x07; // period_m_index from 0 to 7
    //delay_200ns();
    SET(PORTD,7); // STEP Motor 1
    delayMicroseconds(1);
    CLR(PORTD,7);
    }
  if (counter_m[1] >= period_m[1][period_m_index[1]])
    {
    counter_m[1] = 0;
    if (period_m[1][0]==ZERO_SPEED)
      return;
    if (dir_m[1])
      SET(PORTC,7);   // DIR Motor 2
    else
      CLR(PORTC,7);
    period_m_index[1] = (period_m_index[1]+1)&0x07;
    //delay_200ns();
    SET(PORTD,6); // STEP Motor 1
    delayMicroseconds(1);
    CLR(PORTD,6);
    }
}


// Dividimos en 8 subperiodos para aumentar la resolucion a velocidades altas (periodos pequeños)
// subperiod = ((1000 % vel)*8)/vel;
// Examples 4 subperiods:
// 1000/260 = 3.84  subperiod = 3
// 1000/240 = 4.16  subperiod = 0
// 1000/220 = 4.54  subperiod = 2
// 1000/300 = 3.33  subperiod = 1
void calculateSubperiods(uint8_t motor)
{
  int subperiod;
  int absSpeed;
  uint8_t j;
  
  if (speed_m[motor] == 0)
    {
    for (j=0;j<8;j++)
      period_m[motor][j] = ZERO_SPEED;
    return;
    }
  if (speed_m[motor] > 0 )   // Positive speed
    {
    dir_m[motor] = 1;
    absSpeed = speed_m[motor];
    }
  else                       // Negative speed
    {
    dir_m[motor] = 0;
    absSpeed = -speed_m[motor];
    }
    
  for (j=0;j<8;j++)
    period_m[motor][j] = 1000/absSpeed;
  // Calculate the subperiod. if module <0.25 => subperiod=0, if module < 0.5 => subperiod=1. if module < 0.75 subperiod=2 else subperiod=3
  subperiod = ((1000 % absSpeed)*8)/absSpeed;   // Optimized code to calculate subperiod (integer math)
  if (subperiod>0)
   period_m[motor][1]++;
  if (subperiod>1)
   period_m[motor][5]++;
  if (subperiod>2)
   period_m[motor][3]++;
  if (subperiod>3)
   period_m[motor][7]++;
  if (subperiod>4)
   period_m[motor][0]++;
  if (subperiod>5)
   period_m[motor][4]++;
  if (subperiod>6)
   period_m[motor][2]++;
  
  // DEBUG
  /*
  if ((motor==0)&&((debug_counter%10)==0)){
    Serial.print(1000.0/absSpeed);Serial.print("\t");Serial.print(absSpeed);Serial.print("\t");
    Serial.print(period_m[motor][0]);Serial.print("-");
    Serial.print(period_m[motor][1]);Serial.print("-");
    Serial.print(period_m[motor][2]);Serial.print("-");
    Serial.println(period_m[motor][3]);
    }
  */  
}

void setMotorSpeed(uint8_t motor, int16_t tspeed)
{
  // WE LIMIT MAX ACCELERATION
  if ((speed_m[motor] - tspeed)>MAX_ACCEL)
    speed_m[motor] -= MAX_ACCEL;
  else if ((speed_m[motor] - tspeed)<-MAX_ACCEL)
    speed_m[motor] += MAX_ACCEL;
  else
    speed_m[motor] = tspeed;
  
  calculateSubperiods(motor);  // We use four subperiods to increase resolution
  
  // To save energy when its not running...
  if ((speed_m[0]==0)&&(speed_m[1]==0))
    digitalWrite(4,HIGH);   // Disable motors
  else
    digitalWrite(4,LOW);   // Enable motors
}

void setup()
{
  // STEPPER PINS
  pinMode(4,OUTPUT);  // ENABLE MOTORS
  pinMode(6,OUTPUT);  // STEP MOTOR 1 PORTD,7
  pinMode(8,OUTPUT);  // DIR MOTOR 1
  pinMode(12,OUTPUT); // STEP MOTOR 2 PORTD,6
  pinMode(13,OUTPUT); // DIR MOTOR 2
  digitalWrite(4,HIGH);   // Disbale motors
  
  Serial.begin(115200);

  //We are going to overwrite the Timer1 to use the stepper motors
  
  // STEPPER MOTORS INITIALIZATION
  // TIMER1 CTC MODE
  TCCR1B &= ~(1<<WGM13);
  TCCR1B |=  (1<<WGM12);
  TCCR1A &= ~(1<<WGM11);
  TCCR1A &= ~(1<<WGM10);

  // output mode = 00 (disconnected)
  TCCR1A &= ~(3<<COM1A0);
  TCCR1A &= ~(3<<COM1B0);
  
  // Set the timer pre-scaler
  // Generally we use a divider of 8, resulting in a 2MHz timer on 16MHz CPU
  TCCR1B = (TCCR1B & ~(0x07<<CS10)) | (2<<CS10);

  //OCR1A = 125;  // 16Khz
  //OCR1A = 100;  // 20Khz
  OCR1A = 80;   // 25Khz
  TCNT1 = 0;

  delay(2000);
  
  Serial.println("Initializing Stepper motors...");
  delay(1000);
  TIMSK1 |= (1<<OCIE1A);  // Enable Timer1 interrupt
  digitalWrite(4,LOW);    // Enable stepper drivers
}


// Main loop
void loop()
{
  setMotorSpeed(0,100);
  setMotorSpeed(1,100);
}

Does anyone experience the same thing or have any tips?

Best Regards
Sondre
Find all posts by this user
Quote this message in a reply
06-10-2015, 08:18 PM
Post: #2
RE: Uneven motor movements
1. It is exhibited microstep stepping motor driver (A4988)?
2. Try the new version of the sketch. In her service routine engine change.
If this does not work, we think ...
Find all posts by this user
Quote this message in a reply
08-25-2015, 11:54 AM (This post was last modified: 08-26-2015 03:36 PM by gabe.)
Post: #3
RE: Uneven motor movements
Change D13 to other digital pins, it works for me. D13 has a pullup for LED, one of your motor will only run in one direction. that causes robot to spin and fall.

I found CNC shield is ideal for BRobot uses
http://cdn.instructables.com/FY8/U2FI/HV....LARGE.jpg
Find all posts by this user
Quote this message in a reply
Post Reply 


Forum Jump:


User(s) browsing this thread: 1 Guest(s)