Arduino controlled Segway

Hello, I am new to arduino. I have just built a segway from John Warren’s book and have uploaded the code but what is happening is the accellerometer is not being read fast enough. The code reads the accellerometer and then the gyroscope and comes up with a filtered angle but the gyroscope takes a fraction of a second too long to register so the segway does not quite balance. If the gyro kept up to the accel then it would work. Could someone look at the first 20 lines of code and let me know how to fix this problem please? Specifically maybe the accel scale and gyro scale as well as the time (60 ms?)
thanks so much!!
Here is the code.

// DIY Seg-bot - rev 1.2
// JD Warren 2011 
// Arduino Duemilanove (tested)
// Sparkfun Razor 6 DOF IMU - only using X axis from accelerometer and gyroscope 
// Steering potentiometer used to steer bot
// Gain potentiometer used to set max speed (sensitivity) 
// Engage switch (button) used to enable motors
// 
// In no way do I take responisibility for what you may do with this code! Use at your own risk.
// Test thoroughly with wheels off the ground before attempting to ride - Wear a helmet!


// use SoftwareSerial library to communicate with the Sabertooth motor-controller
#include <SoftwareSerial.h> 
// define pins used for SoftwareSerial communication
#define rxPin 2
#define txPin 3
// set up a new SoftwareSerial port, named "mySerial" or whatever you want to call it.
SoftwareSerial mySerial =  SoftwareSerial(rxPin, txPin);


// Name Analog input pins
int gyro_pin = 1;
int accel_pin = 5;
int steeringPot = 3;
int gainPot = 2;

// Name Digital I/O pins
int engage_switch = 4;
int ledPin = 13;

// value to hold the final angle 
float angle = 0.00;
// the following 2 values should add together to equal 1.0
float gyro_weight = 0.96;
float accel_weight = 0.04;

// accelerometer values
int accel_reading;
int accel_raw;
int accel_offset = 511;
float accel_angle;
float accel_scale = 0.01;

// gyroscope values
int gyro_offset = 391;
int gyro_raw;
int gyro_reading;
float gyro_rate;
float gyro_scale = 0.04; // 0.01 by default
float gyro_angle;
int loop_time = 60; // this is the loop time in milliseconds
float loop_cycle = loop_time / 100; // this is the loop time / 100 to get the time in seconds

// engage button variables
int engage = false;
int engage_state = 1;

// timer variables
int last_update;
int cycle_time;
long last_cycle = 0;

// motor speed variables
int motor_out = 0;
int motor_1_out = 0;
int motor_2_out = 0;
int m1_speed = 0;
int m2_speed = 0;
int output;

// potentiometer variables
int steer_val;
int steer_range = 7;
int steer_reading;
int gain_reading;
int gain_val;

// end of Variables


void setup(){

  // Start the Serial monitor at 9600bps
  Serial.begin(9600);
  // define pinModes for tx and rx:
  pinMode(rxPin, INPUT);
  pinMode(txPin, OUTPUT);
  // set the data rate for the SoftwareSerial port
  mySerial.begin(9600);

  // set the engage_switch pin as an Input
  pinMode(engage_switch, INPUT);

  // enable the Arduino internal pull-up resistor on the engage_switch pin.
  digitalWrite(engage_switch, HIGH);
  // Tell Arduino to use the Aref pin for the Analog voltage, don't forget to connect 3.3v to Aref!
  analogReference(EXTERNAL);

}

void loop(){
  // Start the loop by getting a reading from the Accelerometer and coverting it to an angle
  sample_accel();
  // now read the gyroscope to estimate the angle change
  sample_gyro();
  // combine the accel and gyro readings to come up with a "filtered" angle reading
  calculate_angle();
  // read the values of each potentiomoeter
  read_pots();
  // make sure bot is level before activating the motors
  auto_level();
  // update the motors with the new values
  update_motor_speed();
  // check the loop cycle time and add a delay as necessary
  time_stamp();
  // Debug with the Serial monitor
  serial_print_stuff();

}




void sample_accel(){
  // Read and convert accelerometer value

  accel_reading = analogRead(accel_pin);
  accel_raw = accel_reading - accel_offset;
  accel_raw = constrain(accel_raw, -90, 90);
  accel_angle = (float)(accel_raw * accel_scale);
}

void sample_gyro(){
  // Read and convert gyro value

  gyro_reading = analogRead(gyro_pin);
  gyro_raw = gyro_reading - gyro_offset;
  gyro_raw = constrain(gyro_raw, -391, 391);
  // if when you tilt your Arduino/IMU, the gyro and accel readings go in opposite directions... change the loop_cycle variable to be negative or positive (opposite its current state)
  gyro_rate = (float)(gyro_raw * gyro_scale) * -loop_cycle;
  gyro_angle = angle + gyro_rate;
}

void calculate_angle(){
  angle = (float)(gyro_weight * gyro_angle) + (accel_weight * accel_angle);
}

void read_pots(){
  // Read and convert potentiometer values
  // Steering potentiometer
  steer_reading = analogRead(steeringPot); // We want to coerce this into a range between -1 and 1, and set that to steer_val
  steer_val = map(steer_reading, 0, 1023, steer_range, -steer_range);
  if (angle == 0.00){
    gain_reading = 0;
  }
  // Gain potentiometer
  gain_reading = analogRead(gainPot);
  gain_val = map(gain_reading, 0, 1023, 32, 64);
}

void auto_level(){
  // enable auto-level turn On
  //engage_state = digitalRead(engage_switch);

  if (digitalRead(engage_switch) == 1){
    delay(10);
    if (digitalRead(engage_switch) == 1){
      engage = false;
    }
  }
  else {
    if (engage == false){
      if (angle < 0.02 && angle > -0.02)
        engage = true;
      else {
        engage = false;
      }
    }
    else {
      engage = true;
    }
  }

}

void update_motor_speed(){
  // Update the motors

  if (engage == true){
    if (angle < -0.4 || angle > 0.4){
      motor_out = 0;
    }
    else {
      output = (angle * -1000); // convert float angle back to integer format
      motor_out = map(output, -250, 250, -gain_val, gain_val); // map the angle
    }

    // assign steering bias
    motor_1_out = motor_out + (steer_val);
    motor_2_out = motor_out - (steer_val);

    // test for and correct invalid values
    if(motor_1_out > 64){
      motor_1_out = 64;
    }
    if(motor_1_out < -64){
      motor_1_out = -64;
    }
    if(motor_2_out > 64){
      motor_2_out = 64;
    }
    if(motor_2_out < -64){
      motor_2_out = -64;
    }

    // assign final motor output values
    m1_speed = 64 + motor_1_out;
    m2_speed = 192 + motor_2_out;
  }

  else{
    m1_speed = 0;
    m2_speed = 0;
  }

  // write the final output values to the Sabertooth via SoftwareSerial
  mySerial.print(m1_speed, BYTE);
  mySerial.print(m2_speed, BYTE);
}

void time_stamp(){
  // check to make sure it has been exactly 50 milliseconds since the last recorded time-stamp 
  while((millis() - last_cycle) < loop_time){
    delay(1);
  }
  // once the loop cycle reaches 50 mS, reset timer value and proceed
  cycle_time = millis() - last_cycle;
  last_cycle = millis();

}


void serial_print_stuff(){
  // Debug with the Serial monitor

  Serial.print("Accel: ");
  Serial.print(accel_angle);  // print the accelerometer angle
  Serial.print("  ");

  Serial.print("Gyro: ");
  Serial.print(gyro_angle);  // print the gyro angle
  Serial.print("  ");

  Serial.print("Filtered: ");
  Serial.print(angle);   // print the filtered angle
  Serial.print("  ");

  Serial.print(" time: ");
  Serial.print(cycle_time); // print the loop cycle time
  Serial.println("    ");

  /*
  
   Serial.print("o/m: ");
   Serial.print(output);
   Serial.print("/");
   Serial.print(motor_out);
   Serial.println("  "); 
   
   Serial.print("steer_val: ");
   Serial.print(steer_val);
   Serial.print("  "); 
   
   Serial.print("steer_reading: ");
   Serial.print(steer_reading);
   Serial.print("  "); 
   
   Serial.print("m1/m2: ");
   Serial.print(m1_speed);
   Serial.print("/");
   Serial.println(m2_speed);
   */
}

(tags added by moderator)

The delay is a bit redundant here:

 // check to make sure it has been exactly 50 milliseconds since the last recorded time-stamp
  while((millis() - last_cycle) < loop_time){
    delay(1);
  }

And why do the delays in any case? If your problem is slow response time, adding a 50 mS delay isn’t going to help is it?

Your loop calls out some functions which aren’t long. Ditch the functions and just put their code right into loop.
You have a bunch of prints to the serial monitor - crank up the speed there.

What happens if you take out the 50mS delay? Why not let it run faster?

I have just built a segway from John Warren's book and have uploaded the code but what is happening is the accellerometer is not being read fast enough.

Does the book have a name? How big is your segway? Approximate cost?

dmassey:
what is happening is the accellerometer is not being read fast enough. The code reads the accellerometer and then the gyroscope and comes up with a filtered angle but the gyroscope takes a fraction of a second too long to register so the segway does not quite balance. If the gyro kept up to the accel then it would work.

Why do you assume that the problem is the time taken to read the gyroscope? It seems more likely to me that your control algorithm isn’t right.

Along with Warren's book, you might find this Legway (http://www.teamhassenplug.org/robots/legway/) article, which includes C code, of interest.

Hello dmassey I am in the process of making a segway and then I found your code to but I can not be allowed to lie it on my arduino hope you can help me with it

mette: Hello dmassey I am in the process of making a segway and then I found your code to but I can not be allowed to lie it on my arduino hope you can help me with it

It seems to be 2 years since @dmassey was here, so don't hold your breath waiting for a reply.

I don't understand the words in bold, above? Who won't allow you? And what do you mean by "lie it on my Arduino"?

...R