Pendulum Balance Robot

Hey guys, Its my first post, so be nice :slight_smile:

For a project I am supposed to create an Inverted pendulum balancing robot, controlled by PID via arduino.

I am using a Mega2560 with the motor shield, and an optical encoder, however I am struggling a little with the code, I dont think I'm too far away, but unfortunately when uploading, nothing happens. I was wondering if anyone could point me where the problem may be? Most of it is assembly of Library codes (Note: I have the encocer, stepper and PID libraries installed, so its not that)

#include <PID_v1.h>
#include <Encoder.h>
#include <Stepper.h>
const int pwmA = 3;
const int pwmB = 11;
const int brakeA = 9;
const int brakeB = 8;
const int dirA = 12;
const int dirB = 13;
const int STEPS = 200;
const int m = 0;
const int newPosition = 0;
Stepper myStepper(STEPS, dirA, dirB);
Encoder myEnc(20, 21);
PID myPID(newPosition, m, 0, 1, 20, 100, 1);
void setup() {
  myStepper.setSpeed(90);
  Serial.begin(9600);
  Serial.println("Basic Encoder Test:");
}

long oldPosition  = -999;

void loop() {
  myEnc.write(-180);
  long newPosition = myEnc.read();
  if (newPosition != oldPosition) {
    oldPosition = newPosition;
    Serial.println(newPosition);

  }
  myPID.Compute();
  myStepper.step(m);
  Serial.println(m);
  delay (1);

}

Currently when the code is ran, nothig happens, I have tested the stepper motor components in another sketch and then work, but even when assigning a number for the step (say 200) in this sketch, the stepper does not move.

Since a picture speaks a thousand words, I have included one below

Thanks for your help, Yew

Moderator edit: Quote tags swapped for code tags

Nothing happens at all? What about the serial monitor, does that at least give you "Basic Encoder Test:" ?
Do you ever get a print out for Serial.println(newPosition) ?
When does "m" ever change?

Currently when the code is ran, nothig happens

Do you mean that literally?- not even the serial write from setup()?

Edit.... seems HM and I on the same track here...

HazardsMind:
Nothing happens at all? What about the serial monitor, does that at least give you "Basic Encoder Test:" ?
Do you ever get a print out for Serial.println(newPosition) ?
When does "m" ever change?

by 'nothing happens' I mean there is no movement from the motor, and serial.println Gives a page of 0's, I'm unsure how to measure m, however. (as I'm still a beginner to arduino. :smiley:

Gives a page of 0's

It will, it never changes in that code

I'm unsure how to measure m

What is m in the first place?

The only way for this "Serial.println(newPosition);" to change is if the stepper moves, and the only way the stepper is going to move is if "m" changes. Stepper does not move equals no change in encoder, so your result will ALWAYS be 0.

Change "m" first.

JimboZA:

I'm unsure how to measure m

What is m in the first place?

In this code it is the output from the PID,

If I where to '//' out the later serial prints, I should be able to read the optical encoder output right (in this case, 'newPosition') and It would show the written value, plus any changes due to the encoder rotation, correct? However currently it gives;

Basic Encoder Test:
-180

However, since this is a single value, that doesnt not change with time or rotation, it suggests something is wrong with the read, or the connections. the encoder has channels A and B plugged into slots 20 and 21, I have changed the code to include the lines, 'int chanA = 20' and the same for channel B, the names on myEnc have been altered correspondingly. Unfortunately the Basic encoder test still returns the same written result.

On the bright side, I seem to be starting to think through this slightly more methodically :smiley:

EDIT: If I change 'myStepper.step(m)' to a value of 500, the motor does not move, however the same code in another sketch does...

ok then look at this.

const int m = 0;

This should NOT be a constant because you want m to change. And in order to change m, it should be getting the value from "myPID.Compute();"

So it should be,

m = myPID.Compute();

Added:
Where are these ever setup or used?
const int pwmA = 3;
const int pwmB = 11;
const int brakeA = 9;
const int brakeB = 8;

Does your code even know it has a stepper wired to the arduino?

HazardsMind:
ok then look at this.

const int m = 0;

This should NOT be a constant because you want m to change. And in order to change m, it should be getting the value from "myPID.Compute();"

So it should be,

m = myPID.Compute();

I thought something was wrong with that, however using 'int m - myPID.Compute();' I get an error saying the myPID is not declared in scope, if I put it before this, or 'm' is not declared if I put it afterwards. If i just use 'int m = 0' it says something about 'invalid conversion from 'int' or 'double*'

I think it would be best to start over and do tests to see if it works, and again after each additional code.

Syntax

PID(&Input, &Output, &Setpoint, Kp, Ki, Kd, Direction)
Parameters

Input: The variable we're trying to control (double)
Output: The variable that will be adjusted by the pid (double)
Setpoint: The value we want to Input to maintain (double)
Kp, Ki, Kd: Tuning Parameters. these affect how the pid will chage the output. (double>=0)
Direction: Either DIRECT or REVERSE. determines which direction the output will move when faced with a given error. DIRECT is most common.

http://playground.arduino.cc//Code/PIDLibraryConstructor
m needs to be a double to work correctly with the PID library
you can cast it to an int for your stepper motor like this if you want

myStepper.step((int)m);

HazardsMind:
I think it would be best to start over and do tests to see if it works, and again after each additional code.

Okay, i have gone back and and sorted out the stepper motor, this code was a second attempt, and the bits you mentioned I had forgotten to copy over. So now the motor will now step if commanded.

I'm still struggling to input a 'double*' however, I guess the code ' double int = (something) ' is used, but I am unsure what to put as the something. I guess a similar thing will happen for the variable 'newPosition'? to give a variable that has the double format required by the PID? (and hopefully fixing the encoder reader in the process)

long newPosition = myEnc.read();

Don't do this it will re-define newPosition which will no longer be associated with your PID.

double m = 0;
double newPosition = 0;
PID myPID(&newPosition, &m, 0.0, 1.0, 20.0, 100.0, DIRECT);
void loop(){
  newPosition = (double)myEnc.read();
}

You may not have to explicitly cast myEnc.read(), but I'm a little rusty on my automatic casts.

Thanks, The PID isnt throwing up any more errors :smiley:

I've looked with an oscilloscope, and the encoder changes its values when rotated in manner that I would expect, however unfortunately no change its still not showing in the serial monitor, with it sticking at the initial value (now -512) lke this:

-512.00
Basic Encoder Test
-ÿ-512.00
-512.00
-512.00
-512.00
-512.00

I'm not sure what the Y'' is, here is the most recent code for good measure.

#include <PID_v1.h>
#include <Encoder.h>
#include <Stepper.h>
const int pwmA = 3;
const int pwmB = 11;
const int brakeA = 9;
const int brakeB = 8;
const int dirA = 12;
const int dirB = 13;
const int STEPS = 200;
double m = 0;
double newPosition  = 0;
int chanA = 20;
int chanB = 21;
Stepper myStepper(STEPS, dirA, dirB);
Encoder myEnc(chanA, chanB);
PID myPID(&newPosition, &m, 0, 1, 20, 100, DIRECT);
void setup() {
  myStepper.setSpeed(90);
  Serial.begin(9600);
pinMode (pwmA, OUTPUT);
  digitalWrite(pwmA, HIGH);
  pinMode(pwmB, OUTPUT);
  digitalWrite(pwmB, HIGH);
  pinMode(brakeA, OUTPUT);
  digitalWrite(brakeA, LOW);
  pinMode(brakeB, OUTPUT);
  digitalWrite(brakeB, LOW);
}

long oldPosition  = -999;

void loop() {
  myEnc.write(-512);
  newPosition = myEnc.read();
  if (newPosition != oldPosition) {
    oldPosition = newPosition;
    }

Serial.println(newPosition);
     
  
  myPID.Compute();
  myStepper.step(int(m));
  //Serial.println(m);
  delay (1);

}

Thanks a lot for help everyone, every post puts me one step closer to an automatic breakfast machine :smiley:

(edited to fix broken quote '}' )

Try this, comment out "myEnc.write(-512);" and spin the encoder, does it give out readings. Whats happening is, your telling the encoder to be set to a value, and then without any actual movement, its reading back that same value.

I'm pretty sure you need to use an interrupt
attach the encoder to pins 2 and 4

Encoder encoder(2, 4);

        void setup() { 
            attachInterrupt(0, doEncoder, CHANGE); 
            Serial.begin (9600);
            Serial.println("start");
        } 

        void loop(){
            // do some stuff here - the joy of interrupts is that they take care of themselves
        }

        void doEncoder(){
            encoder.update();
            Serial.println( encoder.getPosition() );
        }

It looks like you are using a different encoder library. Seems to be several out there. Just try using the encoder on different interrupt pins.

Also try putting this bit in the setup

myEnc.write(-512);

You may be setting the encoder faster than the interrupt can update, also you won't get far from -512 as it is incremental and you are basically resetting it during the loop.

edit: and why -512 and not 0

Hey guys, I have disabled the line setting the encoder at -512, and now the encoder reads the value correctly. :smiley: Unfortunately the motor isnt working, the output from the PID is still constant , despite the Position changing

Any thoughts, I think its the PID causing the issues now, as the encoder reads correctly, and motor moves when an step is put in to it.

heres the code

#include <PID_v1.h>
#include <Encoder.h>
#include <Stepper.h>
const int pwmA = 3;
const int pwmB = 11;
const int brakeA = 9;
const int brakeB = 8;
const int dirA = 12;
const int dirB = 13;
const int STEPS = 200;
double m = 0;
double newPosition  = 0;
double Position = 0;
int chanA = 20;
int chanB = 21;
Stepper myStepper(STEPS, dirA, dirB);
Encoder myEnc(chanA, chanB);
PID myPID(&Position, &m, 0, 1, 20, 100, DIRECT);
void setup() {
  myStepper.setSpeed(90);
  Serial.begin(9600);
pinMode (pwmA, OUTPUT);
  digitalWrite(pwmA, HIGH);
  pinMode(pwmB, OUTPUT);
  digitalWrite(pwmB, HIGH);
  pinMode(brakeA, OUTPUT);
  digitalWrite(brakeA, LOW);
  pinMode(brakeB, OUTPUT);
  digitalWrite(brakeB, LOW);
}

long oldPosition  = -999;
void loop() {
  newPosition = myEnc.read();
  if (newPosition != oldPosition) {
    oldPosition = newPosition;
    }
    Position = ((newPosition/11.37)+180);
    if (Position > 180) {
    Position = Position -360;
    }
    if (Position < -180) {
    Position = Position +360;
    } 
//Serial.println(Position);
  myPID.Compute();
  myStepper.step(int(m));
  Serial.println(int(&m));
 
}

Just for referance, the PID wants the motor to move 536 steps no matter the actual location, I'm not sure why.

I'm glad that you got the encoder to work properly. To get the PID to work you are going to have to tune the PID. There are several ways to do this, but I'll borrow from wikipedia for the easiest way.
-Set the I and D terms to zero.
-Increase the P term until the system oscillates.
Then set your values based on the table found here

I would also suggest printing out the double value of m as well as the integer value of m. Maybe it is changing but not enough to make a difference.

Serial.println(int(&m));
Serial.println(&m);