8 years ago.

Help with school project needed

For a schoolproject i'm making a wheelchair with adjustable backrest. I have a PWM controlled H-Bridge and a lineair motor with a potmeter as position sensor.

This backrest needs to be positioned in three modes: active mode (straight up), semi-active mode (half way) , and rest mode (laid back). Also after activating a button this backrest needs to be moved slowly back and forth within the three different modes as setpoint.

E.g. mode 'active' belongs to position input of 0.200 (from 0.000 to 1.000). The motor moves to the point where position input is 0.200. When a button for 'moving' is pressed, the motor needs to move back and forth between 0 and 0.300.

Mode 'semi-active' belongs to position input of 0.500 and needs to be moving between 0.400 and 0.700.

Mode 'rest' belongs to 0.800 and needs to be moved between 0.800 and 1.000.

Right now I came to the point where I have the motor following a certain setpoint, moving in a sinus. I have this done by a PI controller. But i'm stuck in implemeting this PI controller in the desired workform discribed above.

Can somebody help me translating this workform into a C++ code? I'm not good at C programming..

Working PI Controller code:

PI controller

#include "mbed.h"
#include "TextLCD.h"

TextLCD lcd(p26, p25, p24, p23, p22, p20, p19, TextLCD::LCD20x4);
PwmOut LM (p21);
DigitalOut Dir (p7);
AnalogIn   AP (p16);
Ticker flipper;
DigitalOut led1(LED1);
DigitalOut led2(LED2);

long int tick=0;
float SP,Kp,Ki;
float delta,output;
float integral=0;

void flip() 
{
    tick++;
    SP=0.2+0.3*(1-cos(6.28*tick*0.003*0.5));
    delta=SP-AP.read();
    integral=integral+delta*0.5;
    if (integral>1) integral=1;
    output=Kp*delta+Ki*integral;
    if (output>0)Dir=0;
    if (output<0)
    {
        Dir=1; 
        output=-output;
    }
    if (output>1) output=1; 
    if ((SP<0.31)&&(tick>100)) tick=0;
    LM=0.3+output;
    led1=!led1;
}

int main() 
{
    led1=1;Kp=6;Ki=1.2;
    flipper.attach(&flip, 0.5);
    LM.period(0.0001);
    
/*
    lcd.cls();Dir=0;
    lcd.locate(0,0);   
    lcd.printf("Test lineaire motor");
    while(1) 
    {
       lcd.locate(0,1);
       lcd.printf("Act position %2.3f",AP.read());
       lcd.locate(0,2);
       lcd.printf("Set position %2.3f",SP);
       lcd.locate(0,3);
       lcd.printf("Output value %2.3f",output);
    }

*/
}

My code so far (none working):

Projectbegin

#include "mbed.h"
#include "TextLCD.h"

AnalogIn positie(p16);
AnalogIn snelheid(p17);
// AnalogIn frequentie(p18);

DigitalIn bewegen(p23);
DigitalIn actief(p24);
DigitalIn semi(p25);
DigitalIn rust(p26);
 
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut Dir (p7);

PwmOut LM (p21);

Ticker flipper;

long int tick=0;
float SP,Kp,Ki;
float delta,output;
float integral=0;
float speed;
               


int main() 
{
    LM.period(0.0001);
    
    if (actief==1) {
                    goto knoppositie;
                    }
     else {
            while (0.15<positie) {
                                   LM.write (0.5);
                                   Dir=1;
                                 }
                                 
            while (0.2>positie)  {
                                   LM.write (0.5);
                                   Dir=0;
                                 }
          }                                                  // Controle of de rugleuning wel in actieve stand staat

knoppositie:

    if (actief==1) {
    
                while (0.15<positie) {
                                   LM.write (0.5);
                                   Dir=1;
                                  }
                                 
                while (0.2>positie)  {
                                   LM.write (0.5);
                                   Dir=0;
                                 }
                                 
                goto knopbewegen;              
                }
                    
     if (semi==1) {
                
                while (0.5<positie) {
                                    LM.write (0.5);
                                    Dir=1;                                 
                                    }
                                    
                while (0.6>positie) {
                                    LM.wire (0.5);
                                    Dir=0;
                                    }
                goto knopbewegen;
                }
                
     if (rust==1) {
     
                while (0.8<positie) {
                                    LM.write (0.5);
                                    Dir=1;
                                    }
                                    
                while (1.0>positie) {
                                    LM.write (0.5);
                                    Dir=0;
                                    }
                goto knopbewegen;
                }
                
knopbewegen:

    if (bewegen==1) {
    
    led1=1;Kp=6;Ki=0.5;
    flipper.attach(&flip, 0.5);
    LM.period(0.0001);
    
                    void flip() {
                        
                                    tick++;
                                    speed=0.01*snelheid;                             // percentage snelheid analog, max is nu 0.01
                                    SP=0.2+0.3*(1-cos(6.28*tick*speed*0.5));        //speed = snelheid 
                                    delta=SP-AP.read();
                                    integral=integral+delta*0.5;
                                        if (integral>1) integral=1;
                                    output=Kp*delta+Ki*integral;
                                        if (output>0)Dir=0;
                                        if (output<0)
                                            {
                                                Dir=1; 
                                                output=-output;
                                            }
                                        if (output>1) output=1; 
                                        if ((SP<0.31)&&(tick>100)) tick=0;
                                    LM=0.3+output;
                                    led1=!led1;
                    }
                    
                    
    else {
            goto knoppositie;
         }
         
         
}




                    
    
/*
    lcd.cls();Dir=0;
    lcd.locate(0,0);   
    lcd.printf("Test lineaire motor");
    while(1) 
    {
       lcd.locate(0,1);
       lcd.printf("Act position %2.3f",AP.read());
       lcd.locate(0,2);
       lcd.printf("Set position %2.3f",SP);
       lcd.locate(0,3);
       lcd.printf("Output value %2.3f",output);
    }

*/
}



Desired workform: /media/uploads/Dynasit/hsd_sfc_v0.2.png

2 Answers

8 years ago.

What is exactly the problem? You could make it move a sine movement, while using a PID controller? What is then the problem with simply not using a sine as input for the PID, but a fixed value? This might also help: http://mbed.org/cookbook/PID

My first impression is that it is probably best to start with making a PID loop that is called by a ticker, and which moves it to a global variable (the PID page/code might help with that). When that works it is fairly trivial to change that global variable to a new value it should move to.

Thanks, I haven't thought about that. That makes it a lot easier.

I will test it out tomorrow.

posted by Dynasit Chair 11 Jun 2013
8 years ago.

Ok I compiled the code, but it doesn't seem to work. When coming to the part where manual input is needed for selecting the mode, it doesn't do anything. The motor isn't moving to the point where AP=0.200 e.g.

#include "mbed.h"
#include "TextLCD.h"

TextLCD lcd(p26, p25, p24, p23, p22, p20, p19, TextLCD::LCD20x4);
PwmOut LM (p21);

AnalogIn   AP (p16);
AnalogIn snelheid(p17);

Ticker flipper;

DigitalOut Dir (p7);
DigitalOut led1(LED1);
DigitalOut led2(LED2);

DigitalIn bewegen(p23);
DigitalIn actief(p24);
DigitalIn semi(p25);
DigitalIn rust(p26);

long int tick=0;
float SP,Kp,Ki;
float delta,output;
float integral=0;

void flip() 
{
    tick++;
    delta=SP-AP.read();
    integral=integral+delta*0.5;
    if (integral>1) integral=1;
    output=Kp*delta+Ki*integral;
    if (output>0)Dir=0;
    if (output<0)
    {
        Dir=1; 
        output=-output;
    }
    if (output>1) output=1; 
    if ((SP<0.31)&&(tick>100)) tick=0;
    LM=0.3+output;
    led1=!led1;
}

int main() 
{

    led1=1;Kp=4;Ki=1;
    flipper.attach(&flip, 0.5);
    LM.period(0.0001);
    
    
    
    if (0.180<AP<0.200) {
                        goto knoppositie;
                        }
        else {
                SP=0.200;
                wait(5);
                goto knoppositie; 
             }                                                  // Controle of de rugleuning wel in actieve stand staat

knoppositie:

     while (actief==1) {
                    SP=0.200;
                    wait(10);
                    goto knopbewegen;
                    }
                    
     while (semi==1) {
                    SP=0.500;
                    wait(10);
                    goto knopbewegen;
                    }                                        
                
     while (rust==1) {
                    SP=0.800;
                    wait(10);
                    goto knopbewegen;
                    }

     goto knoppositie;
            
                                                                            
knopbewegen:

    while ((bewegen==1) && (actief==1)) {
                                    SP=0.2+0.3*(1-cos(6.28*tick*0.003*0.5));
                                 }
                                 
    while ((bewegen==1) && (semi==1))   {
                                    SP=0.5+0.3*(1-cos(6.28*tick*0.003*0.5));
                                 }
                                 
    while ((bewegen==1) && (rust==1))   {                                                                                                               
                                    SP=0.8+0.3*(1-cos(6.28*tick*0.003*0.5));
                                 }
                                                      
                     

    goto knoppositie;
  

}




/*    

    lcd.cls();Dir=0;
    lcd.locate(0,0);   
    lcd.printf("Test lineaire motor");
    while(1)  
    {
       lcd.locate(0,1);
       lcd.printf("Act position %2.3f",AP.read());
       lcd.locate(0,2);
       lcd.printf("Set position %2.3f",SP);
       lcd.locate(0,3);
       lcd.printf("Output value %2.3f",output);
    }

*/

What do you mean it doesn't do anything, and where doesn't it do anything exactly? (This might be more a forum topic btw, since you can only put one real answer here).

General advice: Don't use goto's: God kills a kitten everytime you use them. Also especially if you want to share your code with others later, or like this need help, it is really handy to have English names/comments (comments at all also help), since most won't be able to read Dutch. For comments my favorite way is just making a library with not too large functions, then you don't need to comment much besides in the .h file telling what each function is for.

For example line 65 to 83, you can do it many ways without using a goto, one of them:

while(1) {
  if (actief == 1) {
    SP = 0.2;
    wait(10); //No idea why
    break;
  } elseif (semi==1) {
    SP = 0.5;
    wait(10); 
    break;
  } elseif (rust==1) {
    SP = 0.8;
    wait(10); 
    break;
  }
}

// Here your next code

Edit: And btw, I would first just use a fixed value for SP, and see if it moves to that value. If not, debug that :)

posted by Erik - 12 Jun 2013

Nevermind, it moves to the correct setpoint now. Thanks for the tip. I only know a little bit of C programming, let alone C++...

I will make a new topic on the forums! Thanks!

posted by Dynasit Chair 12 Jun 2013