Swing Up Control of Reaction Wheel Inverted Pendulum

hello everyone,

i working on project stabilize reaction wheel pendulum. type of inverted pendulum unlike moving cart type pendulum has reaction wheel mounted on top.torque produced change of angular momentum of reaction wheel. non linear system. stabilize system there 3 control algorithms involved;

1. stabilizing pendulum upright position using pid, lqr,lqg etc.(linear control)
2. swinging pendulum stable unstable equilibrium(non linear control)
3. switching between 2 controllers @ proper angular position

as of have done first part i.e stabilizing upright position. made use of encoder , pid library developed fellow arduino forum members.in pid control set point set 180 degrees , error terms fed algorithm , appropriate gains found out. when take pendulum upright position able stabilize itself.

now have swing pendulum unstable position upright position , balance it. not easy because dynamics non linear. reading papers (by prof. spong , furuta)i have found out using bang bang controller energy based controller able swing appropriate velocity.then switching between stabilizing controller @ upright position can balanced.

a bang bang controller on , off controller.currently planning implement bang bang , later on integrate energy based controller. swing pendulum using bang bang controller need find rate of change of angle of pendulum i.e theta dot. swing pendulum apply torque in direction of swing. if reaction wheel moves clockwise pendulum moves anticlockwise , vice versa.we assume clockwise angle positive.so when pendulum moves clockwise rate of change of angle positive , negative when moving anticlockwise. motor driving reaction wheel can given voltage , direction motor driver , arduino accordingly. attached image give clear insight of planning do.

i hoping concepts right having hard time implementing algorithms in hardware.i have admit first time working extensively on microcontroller not great in arduino programming. hope fellow arduino forum members me one.thank much. code have made swing function.

code: [select]

#include <encoder.h>
#define dt 0.0001
//variable definition
double prevpos=0,currentpos;
double rate;
const int dira=8;
const int dirb=12;
const int motorpwm=11; // connected enable pin on h-bridge
encoder motenc(2,3);

void setup()
{
  serial.begin(9600);
  pinmode(dira,output);
  pinmode(dirb,output);
  pinmode(motorpwm,output);
}

void loop()
{
  currentpos = motenc.read()*0.09; // since encoder generates 4000 pulses in 1 revolution 4000 pules= 360 degree hence, 1 pulse=0.09 degree
  serial.println(rate);
  rate=(currentpos-prevpos)/dt;
  prevpos=currentpos;
  if(rate>0){
    digitalwrite(dira,high);
    digitalwrite(dirb,low);
    analogwrite(motorpwm,200);
    delay(50);
   
  }
  else if(rate<=0){
    digitalwrite(dira,low);
    digitalwrite(dirb,high);
    analogwrite(motorpwm,200);
    delay(50);
   
   
  }
 
  }
 




actually main problem right correctly measure rate of change of angle.i know way calculated rate of change of angle in above code totally wrong have randomly taken sample time. below latest code i've worked out on find rate of change of angle.

code: [select]


#include <encoder.h>
#include <timerone.h>
volatile double prevposition=0,newposition;
volatile double rate;
encoder myenc(18, 19);
void setup() {
  serial.begin(9600);
  serial.println("basic encoder test:");
  timer1.initialize(4000); // set timer of length 4000 microseconds (or 4 ms - or 250hz)
  timer1.attachinterrupt(callback); // attach service routine here
}
void loop() {

    serial.println(rate);
}

void callback()
{
    newposition = myenc.read(); 
    if(newposition>4000)// using 1000 ppr encoder 4x decoding
      myenc.write(0);
    if(newposition<-4000)
      myenc.write(0);
    rate=((newposition-prevposition)*0.01744*0.09)/0.004;//0.004 second sample time , converting velocity in radians per second 1 degree=0.01744 radians , 1 pulse=0.09 degree since 360 degree=4000 pulses
    prevposition=newposition;
}



this code i've worked out problem when move pendulum in anticlockwise direction stable direction shows value of 65535.

code: [select]

#define encoder0pina 2
#define encoder0pinb 3
volatile double dp,rate;
volatile double currentpos=0;
volatile unsigned previouspos;
volatile unsigned int encoder0pos = 0;
volatile unsigned long duration,lastcaptime;
volatile unsigned long captime=0;
void setup() {
  pinmode(encoder0pina, input);
  pinmode(encoder0pinb, input);
// encoder pin on interrupt 0 (pin 2)
  attachinterrupt(0, achange, change);
// encoder pin on interrupt 1 (pin 3)
  attachinterrupt(1, bchange, change); 
  serial.begin (9600);
}

void loop(){
  dp = (double)(currentpos - previouspos);
  duration = captime - lastcaptime; //get period in ms
  rate=dp/duration;
  serial.println(rate);
  }
 

void achange(){
  // low-to-high on channel a
  if (digitalread(encoder0pina) == high) {
    // check channel b see way encoder turning
    if (digitalread(encoder0pinb) == low) { 
      encoder0pos++;         // cw
    }
    else {
      encoder0pos-- ;        // ccw
    }
  }
  else   // must high-to-low edge on channel a                                       
  {
    // check channel b see way encoder turning 
    if (digitalread(encoder0pinb) == high) {   
      encoder0pos++;          // cw
    }
    else {
      encoder0pos--;          // ccw
    }
  previouspos=currentpos;
  currentpos=encoder0pos;
  lastcaptime = captime;
  captime = millis();
}
}

void bchange(){
  // low-to-high on channel b
  if (digitalread(encoder0pinb) == high) {   
   // check channel see way encoder turning
    if (digitalread(encoder0pina) == high) { 
      encoder0pos++;         // cw
    }
    else {
      encoder0pos--;         // ccw
    }
  }
  // high-to-low on channel b
  else {
    // check channel b see way encoder turning 
    if (digitalread(encoder0pina) == low) {   
      encoder0pos++;          // cw
    }
    else {
      encoder0pos--;          // ccw
    }
  }
  previouspos=currentpos;
  currentpos=encoder0pos;
  lastcaptime = captime;
  captime = millis();
}


which 1 above 2 better approach find out rate of change of pendulum angle? thank you.

Arduino Forum > Using Arduino > Project Guidance > Swing Up Control of Reaction Wheel Inverted Pendulum

arduino