JJRobots COMMUNITY
Uneven motor movements - Printable Version

+- JJRobots COMMUNITY (http://forums.jjrobots.com)
+-- Forum: JJrobots (/forumdisplay.php?fid=1)
+--- Forum: B-Robot (/forumdisplay.php?fid=2)
+--- Thread: Uneven motor movements (/showthread.php?tid=35)



Uneven motor movements - Zondre - 05-14-2015 05:24 PM

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


RE: Uneven motor movements - KomX - 06-10-2015 08:18 PM

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 ...


RE: Uneven motor movements - gabe - 08-25-2015 11:54 AM

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/HV9HRL6Q/FY8U2FIHV9HRL6Q.LARGE.jpg