Self levelling table project

Hi
I’m trying to sort out a sketch for my project, I’m a complete novice and I’m struggling. I will post some pictures of the project itself up so you can have an idea of what I’m working with.

I’m using a MPU6050 accelerometer/Gyro to obviously find the angle and I’m wanting to use the map function to relay that to the motor which is a 6 wire, 4 coil, stepper motor with a 1.8 degree step. The motor is controlled through a driver circuit which I have made. the motor works with a separate program I have but I’m completely lost when trying to combine the two parts of the project.

Any help would be really appreciated :~

here is my program so far, I know is wrong I’ve been trying to work through it step by step.

/*1*/
/*2*/#include "wire.h"
/*3*/
/*4*///I2Cdev and MPU6050 libraries required
/*5*/#include "I2Cdev.h"
/*6*/#include "MPU6050.h"
/*7*/
/*8*/MPU6050 accelgyro;
/*9*/
/*10*/int16_t ax, ay, az;
      int16_t gx, gy, gz;

      #define LED_PIN 13
      bool blinkstate = false;

      #define stepper()

      #define c1 4  //pin 4 controls coil 1
      #define c2 7  //pin 7 controls coil 2
/*20*/#define c3 8  //pin 8 controls coil 3
      #define c4 12 //pin 12 controls coils 4

      int i;
      int a = 0;
      int b = 0;

      void setup() 
      {
      //set coil control pins as outputs
 /*30*/pinMode(c1, OUTPUT);
       pinMode(c2, OUTPUT);
       pinMode(c3, OUTPUT);
       pinMode(c4, OUTPUT);
 
       //sensor setup
       Wire.begin();
       Serial.begin(38400);

       // intialize device
 /*40*/Serial.println("Intializing I2C devices...");
       Serial.println(accelgyro.testConnection() ? "MPU6050 connection sucessful" : "Mpu6050 connection failed");

       // Configure Arduino LED 
       pinMode(LED_PIN, OUTPUT);
      }

       void loop()
       {
       //Motor loop
/*50*/
//**************** DON'T THINK I NEED THIS AS MAP WILL DICTATE THE NO. OF STEPS  *********************
       //step_cw = step clockwise - (No. of steps, millieseconds between steps);
       //step_cw(25, 500);  //take 25 steps colckwise

       //delay(500);  //wait half a second

       //Step counter clockwise
       //step_ccw(-25, 500);

/*60*/ //off()  //turn off all coils 

       //delay(500);  //wait half a second
//************************************************************************************************************************

       //sensor loop
       //read raw accel/gyro measurements from device
       accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

       ax=ax*0.005;
/*70*/ ay=ay*0.005;
       az=az*0.005;

       ax=atan(ax/(sqrt(ay*ay+az*az)))*180/3.142;
       ay=atan(ay/(sqrt(ax*ax+az*az)))*180/3.142;

       double angle = map(ax,-45, 45, 25, -25);  //should map measurements for the motor will require adjustments to motor sketch
       stepper.write(angle); 
       //myservo.write(angle); // this is used for servo motors and not stepper
       //display tab-separated accel/gyro x/y/z values
 /*80*/Serial.print("a/g:\t");
       Serial.print(ax); Serial.print("\t");
       Serial.print(ay); Serial.print("\t");
       Serial.println(stepper);  //might be the motor control part might need to declare this at the setup

       //blink LED to indicate activity
       blinkstate = !blinkstate;
       digitalWrite(LED_PIN, blinkstate);
       delay(500);
       }
/*90*/ void stepper(int angle, int off_time) // could this read the sensor values mapped to it?
//**************************  Turn Motor Clockwise  ****************************************
// MAY NOT NEED ANY OF THIS CODE WITH MAP FUNCTION IN PLACE
       void step_cw(int steps, int off_time)
       {
       for(i = 0; i < steps; i++) //counts steps
       {
       a = a++;  // adds 1 to the total number of steps
        //Steps are counted as +1 for clockwise
   
/*100*/ //Decides which coil to turn on
       b = 1 + (abs(a) % 4);  // b is always 1 through 4
  
       if(a < 0) // check to see if a is a negative number
       {
       // If a is negative make coils continue to go from 1 to 4
       b = 5 - b;
       }
  
       if(b == 1)  //rotor now alined with coil 1
/*110*/{ 
       // turning needed coils on to aline to that coil
       digitalWrite(c1, HIGH);  //coil 1 on
      digitalWrite(c2, LOW);
       digitalWrite(c3, LOW);
       digitalWrite(c4, LOW);
       
       //rotor move a step clockwise
}
       if(b == 2)  //rotor now alined with coil 2
/*120*/{
       digitalWrite(c1, LOW);
       digitalWrite(c2, HIGH);  //coil 2 on
      digitalWrite(c3, LOW);
      digitalWrite(c4, LOW);
}
  
       if(b == 3)
       {
      digitalWrite(c1, LOW);
/*130*/digitalWrite(c2, LOW);
       digitalWrite(c3, HIGH);  //coil 3 on
       digitalWrite(c4, LOW);
       }
  
       if(b == 4)
       {
       digitalWrite(c1, LOW);
       digitalWrite(c2, LOW);
       digitalWrite(c3, LOW);
/*140*/digitalWrite(c4, HIGH);  //coil4 on
       }
       delay(off_time);  //wait this long between steps
       }
       }

//****************  Turn Motor Counter-Clockwise  *********************************************

      void step_ccw(int steps, int off_time)
       {   
/*150*/for(i = 0; i < steps; i++)  //counts steps
       {
       a = a--;  //subtracts 1 from the total number of steps
       //steps are counted as -1 for counter-clockwise
  
       //decides which coil to turn on
       b = 1 + (abs(a) % 4);  //b is always 1 through 4
  
       if(a < 0)  //check to see if a is a negative number
      {
/*160*///if a is negative make coils continue to go from 1 to 4
      b = 5 - b;
       }
    
      if(b == 4)  
      {
      digitalWrite(c1, LOW);
       digitalWrite(c2, LOW);
       digitalWrite(c3, LOW);
       digitalWrite(c4, HIGH);  //coil 4 on
/*170*/}
    
       if(b == 3) 
       {
       digitalWrite(c1, LOW);
       digitalWrite(c2, LOW);
       digitalWrite(c3, HIGH);  //coil 3 on
       digitalWrite(c4, LOW);
      }
    
/*180*/if(b == 2)  
      {
      digitalWrite(c1, LOW);
      digitalWrite(c2, HIGH);  //coil 2 on
      digitalWrite(c3, LOW);
      digitalWrite(c4, LOW);
      
      }
   
       if(b == 1) 
      {
/*190*/digitalWrite(c1, HIGH);  //coil 1 on
       digitalWrite(c2, LOW);
       digitalWrite(c3, LOW);
       digitalWrite(c4, LOW);
       
      }
    
       delay(off_time);  //wait this long between steps
       }
       }
//*********************  Turn All Coils Off  ***********************************************************
/*200*/
       void off(void)
        {
       digitalWrite(c1, LOW);
       digitalWrite(c2, LOW);
       digitalWrite(c3, LOW);
       digitalWrite(c4, LOW);
        }
// ********************  End Of Sketch  *********************

Heres more pictures

I just have to ask, what’s with the line numbers?


Rob

lol It helped me with the fault finding as the faults are given with the line in the code so just helped me find the fault faster

(thinking out loud)
If I would do a table project I would start with retracting all legs to the max.

repeat
determine lowest leg from 2 levels (2 directions)
give that lowest leg a twist (leave the others as is)
until no lowest leg can be found.

feezer89: lol It helped me with the fault finding as the faults are given with the line in the code so just helped me find the fault faster

If you're using the Arduino IDE then the IDE shows the line number of the line that the cursor is at in the lower left corner of the window.

I guess that you're trying to level the table on one axis by using the stepper to tilt the table.

Can you reliably read the current tilt of the axis you're trying to level? I assume that the sensor is attached to the part that you're trying to level, so it should return a value representing 'level' when the table is level. It would probably be a good idea to process your tilt sensor input so give you zero when the table is level, a positive number when it is tilted one way and a negative value when it is tilted the other. Have you achieved that yet?

Once you have that, you could just use a simple proportional feedback system to step the motor in the direction which brings the table towards level, with the speed proportional to how far it is currently away from being level. A call to map() would be all you need for that. The output of that algorithm would be a demanded speed and direction for the motor.

You need to recalculate the tilt and update the motor speed at regular intervals. I'd suggest using the non-blocking techniques demonstrated in the 'blink without delay' example to schedule this regular activity.

[/quote]

If you're using the Arduino IDE then the IDE shows the line number of the line that the cursor is at in the lower left corner of the window.

I guess that you're trying to level the table on one axis by using the stepper to tilt the table.

Can you reliably read the current tilt of the axis you're trying to level? I assume that the sensor is attached to the part that you're trying to level, so it should return a value representing 'level' when the table is level. It would probably be a good idea to process your tilt sensor input so give you zero when the table is level, a positive number when it is tilted one way and a negative value when it is tilted the other. Have you achieved that yet?

Once you have that, you could just use a simple proportional feedback system to step the motor in the direction which brings the table towards level, with the speed proportional to how far it is currently away from being level. A call to map() would be all you need for that. The output of that algorithm would be a demanded speed and direction for the motor.

You need to recalculate the tilt and update the motor speed at regular intervals. I'd suggest using the non-blocking techniques demonstrated in the 'blink without delay' example to schedule this regular activity. [/quote]

Thanks for the tip i never even noticed it lol

The project is designed to work on board a boat and counter act the motion of the sea if you look at the pictures the sensor is mounted on the blue part of the meccano which represents the deck of the boat. i am trying to make the motor move in the opposite direction of the sensors angle and stay level.

feezer89:
Hi
I’m trying to sort out a sketch for my project, I’m a complete novice and I’m struggling. I will post some pictures of the project itself up so you can have an idea of what I’m working with.

Any help would be really appreciated :~

here is my program so far, I know is wrong I’ve been trying to work through it step by step.

....

void step_cw(int steps, int off_time)
       {
       for(i = 0; i < steps; i++) //counts steps
       {
       a = a++;  // adds 1 to the total number of steps
        //Steps are counted as +1 for clockwise
// a is now 1
   
/100/ //Decides which coil to turn on
       b = 1 + (abs(a) % 4);  // b is always 1 through 4

//b is now 2…not what you intended.




Try instead:


for(i = 0; i < steps; i++) //counts steps
      {
            b = 1 + (abs(i) % 4);  // b is always 1 through 4




`   if(a < 0) // check to see if a is a negative number`

abs() always returns a positive number. abs(-1) = 1. so there's no need for this if statement.


if(a < 0) // check to see if a is a negative number
    {
       // If a is negative make coils continue to go from 1 to 4
       b = 5 - b;
       }




Now set all your coils off (LOW)


digitalWrite(c1, LOW);
      digitalWrite(c2, LOW);
      digitalWrite(c3, LOW);
      digitalWrite(c4, LOW);

if(b == 1)  //rotor now alined with coil 1
      {
              digitalWrite(c1, HIGH);  //coil 1 on
             }
       if(b == 2)  //rotor now alined with coil 2
      {
       digitalWrite(c2, HIGH);  //coil 2 on
      }
 
       if(b == 3)
       {
       digitalWrite(c3, HIGH);  //coil 3 on
            }
 
       if(b == 4)
       {
       digitalWrite(c4, HIGH);  //coil4 on
       }
      }
       delay(off_time);  //wait this long between steps
       }




//**************** Turn Motor Counter-Clockwise 
As above but change this line 
`for(i = 0; i < steps; i++)  //counts steps`

to


for(i = steps; i > 0;  i–)  //counts steps backwards

feezer89: The project is designed to work on board a boat and counter act the motion of the sea if you look at the pictures the sensor is mounted on the blue part of the meccano which represents the deck of the boat. i am trying to make the motor move in the opposite direction of the sensors angle and stay level.

That sounds tricky. Also, I get the impression you need to do this in two dimensions at the same time. I think you'll need to use the sort of analysis techniques the drone autopilots use to merge gyro/accel data to determine orientation. The theory behind this is quite complex and I'd go for an existing working solution if at all possible. I remember the Arduplane software has a 'gimbal' feature which aims to hold a camera steady as the drone moves around, and that sounds very similar to what you're trying to achieve. Unless you can complete this part and get an accurate and reliable measurement of the orientation of the assembly, it seems premature trying to use motors to control it.

feezer89: The project is designed to work on board a boat and counter act the motion of the sea if you look at the pictures the sensor is mounted on the blue part of the meccano which represents the deck of the boat. i am trying to make the motor move in the opposite direction of the sensors angle and stay level.

A very fast motor will be needed then, because the movement of the sea is not something that is slow, so be aware of that. and also, please put some isolator between your arduino and the base plate. This plate conducts electricity, so it may generate shorts or faulty readings or whatever, in any case it can't be good for you board.

I appreciate the help but spoke to one of my lecturers and he advised me to modify the given examples for stepper motor so I’m trying to do that now and I have one fault at the moment "invalid use of member (did you forget the ‘&’ ?) I have no idea where the & should go if it is even needed??? the fault on line 47 here’s my code:

#include "Wire.h"
#include <Stepper.h> 
// I2Cdev and MPU6050 libraries required
#include "I2Cdev.h"
#include "MPU6050.h"

MPU6050 accelgyro;

int16_t ax, ay, az;
int16_t gx, gy, gz;

#define LED_PIN 13
bool blinkState = false;

const int stepsPerRevolution = 200;
Stepper myStepper(stepsPerRevolution, 4, 7, 8, 12);
int stepCount = 0;

void setup() {
    Wire.begin();

    Serial.begin(38400);

    // initialize device
    Serial.println("Initializing I2C devices...");
    accelgyro.initialize();

    // verify connection
    Serial.println("Testing device connections...");
    Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

    // configure Arduino LED for
    pinMode(LED_PIN, OUTPUT);
}

void loop() {
    // read raw accel/gyro measurements from device
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
 
    ax=ax*0.005;
    ay=ay*0.005;
    az=az*0.005;
 
    ax=atan(ax/(sqrt(ay*ay+az*az)))*180/3.142;
    ay=atan(ay/(sqrt(ax*ax+az*az)))*180/3.142;
    myStepper.step = map(ax, -45, 45, 25, -25);
    //int sensorReading = analogRead(A2);
    //int motorSpeed = map(sensorReading, 0, 1023, 0, 100);
  
     
    //double angle = map(ax, -45, 45, 25, -25);
    //stepper.write(angle);
    
    // display tab-separated accel/gyro x/y/z values
    Serial.print("a/g:\t");
    Serial.print(ax); Serial.print("\t");
    Serial.println(ay);
    

    // blink LED to indicate activity
    blinkState = !blinkState;
    digitalWrite(LED_PIN, blinkState);
    delay(100);
}

Tell me what you think.

Line 47 is "myStepper.step = map(ax, -45, 45, 25, -25);" just so you dont have to count through them.

A very fast motor will be needed then, because the movement of the sea is not something that is slow, so be aware of that. and also, please put some isolator between your arduino and the base plate. This plate conducts electricity, so it may generate shorts or faulty readings or whatever, in any case it can't be good for you board.

The model is just measuring one axis (X) from the base of the Meccano and then driving the motor the opposite direction the model doesn't have to be perfect and the components im using for the model wouldn't be the same as the product design the models is merely showing the concept for a presentation. And don't worry the board is just resting there for the photo to you the board lol.

myStepper.step = map(ax, -45, 45, 25, -25);

I suspect you may have meant to call the method step(), with the value returned by map() as an argument.