Post Reply 
 
Thread Rating:
  • 0 Votes - 0 Average
  • 1
  • 2
  • 3
  • 4
  • 5
Dificulty cleaning the code
01-10-2017, 12:55 PM
Post: #1
Dificulty cleaning the code
Hi,

I am trying to make a really simple self balancing robot using stepper motor with nothing more. There are tons of codes in the internet but they have tooo much "garbage", with things that I wont need. So I found this https://github.com/JJulio/b-robot/blob/m..._ROBOT.ino to be the best code.

I removed everything I thought is not gonna be used in my case (distance sensor, WITA..) and now I have this:

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,4
// DIR  Motor1: D8 -> PORTD,5
// STEP Motor2: D12-> PORTD,6
// DIR  Motor2: D13-> PORTD,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


#include <Wire.h>
#include <I2Cdev.h>
#include <JJ_MPU6050_DMP_6Axis.h>  // Modified version of the library to work with DMP (see comments inside)

#define DEBUG 0

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

#define ZERO_SPEED 65535
#define MAX_ACCEL 7

#define MAX_THROTTLE 530
#define MAX_STEERING 136
#define MAX_TARGET_ANGLE 12

// PRO MODE = MORE AGGRESSIVE
#define MAX_THROTTLE_PRO 650
#define MAX_STEERING_PRO 240
#define MAX_TARGET_ANGLE_PRO 18


//#define I2C_SPEED 100000L
#define I2C_SPEED 400000L
//#define I2C_SPEED 800000L

#define ACCEL_SCALE_G 8192             // (2G range) G = 8192
#define ACCEL_WEIGHT 0.01
#define GYRO_BIAS_WEIGHT 0.005

// MPU6000 sensibility   (0.0609 => 1/16.4LSB/deg/s at 2000deg/s, 0.03048 1/32.8LSB/deg/s at 1000deg/s)
#define Gyro_Gain 0.03048
#define Gyro_Scaled(x) x*Gyro_Gain //Return the scaled gyro raw data in degrees per second

#define RAD2GRAD 57.2957795
#define GRAD2RAD 0.01745329251994329576923690768489

// Default control terms  
#define KP 0.20 // 0.22        
#define KD 26   // 30 28        
#define KP_THROTTLE 0.065  //0.08
#define KI_THROTTLE 0.05



#define ITERM_MAX_ERROR 40   // Iterm windup constants
#define ITERM_MAX 5000



bool Robot_shutdown=false;   // Robot shutdown flag => Out of battery
uint8_t mode;   // 0: MANUAL MODE   1: autonomous MODE
uint8_t autonomous_mode_status;   // 1: NORMAL STATUS=Walking  2:
int16_t autonomous_mode_counter;  
int16_t autonomous_mode_distance;
int16_t pushUp_counter;  // for pushUp functionality (experimental)



// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (for us 18 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[18]; // FIFO storage buffer
Quaternion q;

uint8_t loop_counter;       // To generate a medium loop 40Hz
uint8_t slow_loop_counter;  // slow loop 2Hz
long timer_old;
long timer_value;
int debug_counter;
float dt;

// class default I2C address is 0x68
MPU6050 mpu;

float angle_adjusted;
float angle_adjusted_Old;

float Kp=KP;
float Kd=KD;
float Kp_thr=KP_THROTTLE;
float Ki_thr=KI_THROTTLE;
float Kp_user=KP;
float Kd_user=KD;
float Kp_thr_user=KP_THROTTLE;
float Ki_thr_user=KI_THROTTLE;
bool newControlParameters = false;
bool modifing_control_parameters=false;
float PID_errorSum;
float PID_errorOld = 0;
float PID_errorOld2 = 0;
float setPointOld = 0;
float target_angle;
float throttle;
float steering;
float max_throttle = MAX_THROTTLE;
float max_steering = MAX_STEERING;
float max_target_angle = MAX_TARGET_ANGLE;
float control_output;
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,4
// DIR  Motor1: D8 -> PORTD,5
// STEP Motor2: D12-> PORTD,6
// DIR  Motor2: D13-> PORTD,7


int freeRam () {
  extern int __heap_start, *__brkval;
  int v;
  return (int) &v - (__brkval == 0 ? (int) &__heap_start : (int) __brkval);
}

// DMP FUNCTIONS
// This function defines the weight of the accel on the sensor fusion
// default value is 0x80
// The official invensense name is inv_key_0_96 (??)
void dmpSetSensorFusionAccelGain(uint8_t gain)
{
  // INV_KEY_0_96
  mpu.setMemoryBank(0);
  mpu.setMemoryStartAddress(0x60);
  mpu.writeMemoryByte(0);
  mpu.writeMemoryByte(gain);
  mpu.writeMemoryByte(0);
  mpu.writeMemoryByte(0);
}

// Quick calculation to obtein Phi angle from quaternion solution
float dmpGetPhi() {
   mpu.getFIFOBytes(fifoBuffer, 16); // We only read the quaternion
   mpu.dmpGetQuaternion(&q, fifoBuffer);
   mpu.resetFIFO();  // We always reset FIFO
    
   return( asin(-2*(q.x * q.z - q.w * q.y)) * 180/M_PI); //roll
   //return (atan2(2*(q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z)* RAD2GRAD);
}

// PD implementation. DT is in miliseconds
float stabilityPDControl(float DT, float input, float setPoint,  float Kp, float Kd)
{
  float error;
  float output;

  error = setPoint-input;

  // Kd is implemented in two parts
  //    The biggest one using only the input (sensor) part not the SetPoint input-input(t-2)
  //    And the second using the setpoint to make it a bit more agressive   setPoint-setPoint(t-1)
  output = Kp*error + (Kd*(setPoint - setPointOld) - Kd*(input - PID_errorOld2))/DT;       // + error - PID_error_Old2
  //Serial.print(Kd*(error-PID_errorOld));Serial.print("\t");
  PID_errorOld2 = PID_errorOld;
  PID_errorOld = input;  // error for Kd is only the input component
  setPointOld = setPoint;
  return(output);
}

// P control implementation.
float speedPControl(float input, float setPoint,  float Kp)
{
  float error;

  error = setPoint-input;

  return(Kp*error);
}

// PI implementation. DT is in miliseconds
float speedPIControl(float DT, float input, float setPoint,  float Kp, float Ki)
{
  float error;
  float output;

  error = setPoint-input;
  PID_errorSum += constrain(error,-ITERM_MAX_ERROR,ITERM_MAX_ERROR);
  PID_errorSum = constrain(PID_errorSum,-ITERM_MAX,ITERM_MAX);

  output = Kp*error + Ki*PID_errorSum*DT*0.001;
  return(output);
}



// 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(PORTD,5);  // DIR Motor 1
    else
      CLR(PORTD,5);
    // 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,4); // STEP Motor 1
    delayMicroseconds(1);
    CLR(PORTD,4);
    }
  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(PORTD,7);   // DIR Motor 2
    else
      CLR(PORTD,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
  

}


void setup()
{
  // STEPPER PINS
  pinMode(4,OUTPUT);  // STEP MOTOR 1 PORTD,4
  pinMode(5,OUTPUT);  // DIR MOTOR 1
  pinMode(6,OUTPUT); // STEP MOTOR 2 PORTD,6
  pinMode(7,OUTPUT); // DIR MOTOR 2
  digitalWrite(4,HIGH);   // Disbale motors
  
  Serial.begin(115200);
  
  //servoAux.attach();
  
  
  // Join I2C bus
  Wire.begin();
  // 4000Khz fast mode
  TWSR = 0;
  TWBR = ((16000000L/I2C_SPEED)-16)/2;
  TWCR = 1<<TWEN;
  
  Serial.println("Initializing I2C devices...");
  //mpu.initialize();
  mpu.setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
  mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
  mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
  mpu.setDLPFMode(MPU6050_DLPF_BW_20);  //10,20,42,98,188
  mpu.setRate(4);   // 0=1khz 1=500hz, 2=333hz, 3=250hz 4=200hz
  mpu.setSleepEnabled(false);
  
  delay(2000);
  Serial.println(F("Initializing DMP..."));
  devStatus = mpu.dmpInitialize();
  if (devStatus == 0) {
        // turn on the DMP, now that it's ready
        Serial.println(F("Enabling DMP..."));
        mpu.setDMPEnabled(true);
        mpuIntStatus = mpu.getIntStatus();
        dmpReady = true;
        
    } else { // ERROR!
        // 1 = initial memory load failed
        // 2 = DMP configuration updates failed
        // (if it's going to break, usually the code will be 1)
        Serial.print(F("DMP Initialization failed (code "));
        Serial.print(devStatus);
        Serial.println(F(")"));
    }
  
  // Gyro calibration
  // The robot must be steady during initialization
  delay(15000);   // Time to settle things... the bias_from_no_motion algorithm needs some time to take effect and reset gyro bias.

  Serial.print("Free RAM: ");
  Serial.println(freeRam());
  Serial.print("Max_throttle: ");
  Serial.println(max_throttle);
  Serial.print("Max_steering: ");
  Serial.println(max_steering);
  Serial.print("Max_target_angle: ");
  Serial.println(max_target_angle);

  // verify connection
  Serial.println("Testing device connections...");
  Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
  timer_old = millis();

  
  //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);
  
  //Adjust sensor fusion gain
  Serial.println("Adjusting DMP sensor fusion gain...");
  dmpSetSensorFusionAccelGain(0x20);
  
  Serial.println("Initializing Stepper motors...");
  delay(1000);
  TIMSK1 |= (1<<OCIE1A);  // Enable Timer1 interrupt

  
  // Little motor vibration to indicate that robot is ready
  for (uint8_t k=0;k<3;k++)
  {
  setMotorSpeed(0,3);
  setMotorSpeed(1,-3);
  delay(150);
  setMotorSpeed(0,-3);
  setMotorSpeed(1,3);
  delay(150);
  }

  mpu.resetFIFO();
  timer_old = millis();
  Robot_shutdown = false;
}


// Main loop
void loop()
{

  debug_counter++;
  
  
  timer_value = millis();
  // New DMP Orientation solution?
  fifoCount = mpu.getFIFOCount();
  if (fifoCount>=18)
      {
      if (fifoCount>18)  // If we have more than one packet we take the easy path: discard the buffer
        {
        Serial.println("FIFO RESET!!");
        mpu.resetFIFO();
        return;
        }
  loop_counter++;
        slow_loop_counter++;
    dt = (timer_value-timer_old);
    timer_old = timer_value;
    
    angle_adjusted_Old = angle_adjusted;
    angle_adjusted = dmpGetPhi();
  
    #if DEBUG==8
      Serial.print(throttle);
      Serial.print(" ");
      Serial.print(steering);
      Serial.print(" ");
      Serial.println(mode);
    #endif

    //angle_adjusted_radians = angle_adjusted*GRAD2RAD;
    #if DEBUG==1
      Serial.println(angle_adjusted);
    #endif
    //Serial.print("\t");
    mpu.resetFIFO();  // We always reset FIFO

    if (mode==1)
    

    // We calculate the estimated robot speed
    // Speed = angular_velocity_of_stepper_motors - angular_velocity_of_robot(angle measured by IMU)
    actual_robot_speed_Old = actual_robot_speed;
    actual_robot_speed = (speed_m[1] - speed_m[0])/2;  // Positive: forward

    int16_t angular_velocity = (angle_adjusted-angle_adjusted_Old)*90.0;     // 90 is an empirical extracted factor to adjust for real units
    int16_t estimated_speed = actual_robot_speed_Old - angular_velocity;     // We use robot_speed(t-1) or (t-2) to compensate the delay
    estimated_speed_filtered = estimated_speed_filtered*0.95 + (float)estimated_speed*0.05;
    #if DEBUG==2
      Serial.print(" ");
      Serial.println(estimated_speed_filtered);
    #endif
    
    //target_angle = (target_angle + speedPControl(estimated_speed_filtered,throttle,Kp_thr))/2.0;   // Some filtering : Average with previous output
    //target_angle = target_angle*0.3 + speedPIControl(dt,estimated_speed_filtered,throttle,Kp_thr,Ki_thr)*0.7;   // Some filtering
    target_angle = speedPIControl(dt,estimated_speed_filtered,throttle,Kp_thr,Ki_thr);
    target_angle = constrain(target_angle,-max_target_angle,max_target_angle);   // limited output
    
    #if DEBUG==3
      Serial.print(" ");Serial.println(estimated_speed_filtered);
      Serial.print(" ");Serial.println(target_angle);
    #endif
    
  
  if (pushUp_counter>0)  // pushUp mode?
      target_angle = 10;
    
    // We integrate the output (acceleration)
    control_output += stabilityPDControl(dt,angle_adjusted,target_angle,Kp,Kd);
    control_output = constrain(control_output,-500,500);   // Limit max output from control
    
        
    // The steering part of the control is injected directly on the output
    motor1 = control_output + steering;
    motor2 = -control_output + steering;   // Motor 2 is inverted
    
  // Limit max speed
    motor1 = constrain(motor1,-500,500);  
    motor2 = constrain(motor2,-500,500);
    
  // Is robot ready (upright?)
    if ((angle_adjusted<74)&&(angle_adjusted>-74))
      {
    
        // NORMAL MODE
        setMotorSpeed(0,motor1);
        setMotorSpeed(1,motor2);
        pushUp_counter=0;
    
    
      if ((angle_adjusted<40)&&(angle_adjusted>-40))
        {
        Kp = Kp_user;  // Default or user control gains
        Kd = Kd_user;
        Kp_thr = Kp_thr_user;
        Ki_thr = Ki_thr_user;
        }    
      else
        {
      
        }  
      }
  
    // New IMU data
  
  // Medium loop 40Hz
  if (loop_counter >= 5)
    {
    loop_counter = 0;
    // if we are in autonomous mode we read the distance sensor
  
    } // Medium loop
      
  if (slow_loop_counter>=99)  // 2Hz
    {
    slow_loop_counter = 0;
  
    }  // Slow loop
}

}

The problem is that when I run that code, it looks like it's gonna work but suddenly the motors start doing very strange noise and dont rotate anymore. See the video below:

https://goo.gl/photos/b7c9Enci9b7Wpg8o6

I would like to not use any microstep, should I change anything in the code above cause I only want 200 steps per revolutino with no microstepping.

Thank you guys, this project is awesome!
Find all posts by this user
Quote this message in a reply
01-11-2017, 11:56 AM
Post: #2
RE: Dificulty cleaning the code
Your pulsing the motors to fast. They can not spin faster than 1 pulse per 86 microseconds.

In the interrupt ISR(), there is a command "delayMicroseconds" that will not work in an interrupt function. You need to find the delay_1us() function and use that instead.

Mike
Find all posts by this user
Quote this message in a reply
01-13-2017, 01:04 PM
Post: #3
RE: Dificulty cleaning the code
delayMicroseconds() work fine in an interrupt function. delay() not.
Visit this user's website Find all posts by this user
Quote this message in a reply
Post Reply 


Forum Jump:


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