Almost balancing Scooter

Technical Programming
1

visit http://www.youtube.com/watch?v=VLHPlXa6e3M to see the video.

I’ve increased Kp until i noticed vibrations and then back off
that…i added a delay at the end of the of the loop to also reduce
the vibrations, Currently the loop runs at 50hz. Then i added Kd… if
set too high it causes the lithium ion battery to put out too much
current causing the battery to shut down.

So as you can see in the video there is a lot of oscillation at the
zero point and i don’t see discontinuity. Any ideas of how to remove
them… Also i noticed that once you pass a certain threshold the
motors cant bring you back to your zero point, did you face a similar
issue.

Thanks
Ahmed

Code:

#include<16f877a.h>

#DEVICE ADC=10
#fuses hs,nowdt,noprotect,noput,nobrownout
#use delay(clock=20000000)

#use rs232(baud=9600,xmit=PIN_C6,rcv=PIN_C7,bits=8,parity=N)

int BIN (float x)
{
if (x==0)
return(0);
if (x>0)
return (1);
if (x<0)
return(-1);
}

void main ()
{

float angle=0,motor=0;
long  lmotor=0,rmotor=0;
long gz_adc=0,ax_adc=0, ax_offset=516,gz_offset=494; /* Analog values */
signed long gz_deg=0,ax_deg=0,old_angle=0,new_angle=0;
long dt=0; // do nothing
    int flag=0;


/*Setup PWM timers*/
setup_ccp1(CCP_PWM);
setup_ccp2(CCP_PWM);
setup_timer_2(T2_DIV_BY_4,82 , 1); //15KHz frequency of the motor




    //setup_timer_0(RTCC_INTERNAL | RTCC_8_BIT| RTCC_DIV_256);

//sets up timer
setup_timer_1(T1_INTERNAL|T1_DIV_BY_8);

//Setup the ADC
setup_adc(ADC_CLOCK_INTERNAL );//setup clock before setup
setup_adc_ports( ALL_ANALOG );

//Initialize the PWM to 0
set_pwm1_duty(0);
set_pwm2_duty(0);

delay_ms(3000);

while(1)
{

    dt=get_timer1();
set_timer1(0x0000);
    old_angle=angle;

//setup ADC for accel
set_adc_channel(0);
delay_us(10);
ax_adc=read_adc();

 delay_us(60);

 //Setup ADC for gyro
set_adc_channel(1);
delay_us(10);
gz_adc = read_adc();



 if (ax_adc&gt;=ax_offset){ax_deg=(((ax_adc-ax_offset)*14)&gt;&gt;5);
     }
 else { ax_deg=((((ax_adc-ax_offset)*14)*-1)&gt;&gt;5)*-1;}

 if (gz_adc&gt;=gz_offset)
  {gz_deg=(((gz_adc-gz_offset)*41)&gt;&gt;6);
  angle = 0.91* (angle + (float)(gz_deg*0.00000016*dt));
  angle += 0.09*(ax_deg);
  }

 else { gz_deg=((((gz_adc-gz_offset)*41)*-1)&gt;&gt;6)*-1;
      angle = 0.91* (angle + (((float)gz_deg)*0.0000016*(float)dt));
      angle += 0.09*((float)ax_deg);
 }

           if (angle&lt;0.6 && angle&gt;-0.7)
   angle=0;
   else angle=angle;

//flag will be 1 if angle>0 and flag will be -1 if angle<0 and 0
flag = BIN(angle);
new_angle = angle;

 //Calculate motor value for PD controller


    motor=((4.3*angle)+(0.2*(float)(gz_deg)));




   //Make sure the "motor" value does not exceed 250 or -250
      if(motor&gt;178)
  motor=178;
  else if (motor&lt;-178)
  motor=-178;
      else  {motor=(signed long)(motor);}


 //Control the motors direction and set its PWM

/makes sure that you remain forward or reverse and how we switch to reverse/
/if ((abs(new_angle-old_angle)>4) && (new_angleold_angle)<0)
flag = -flag;
else
flag=flag;
/
/
if (flag=1)
motor=motor;
else
motor=(255-abs(motor));
*/

//flag=1 => forward
if (flag==1)
{

                              rmotor=(long)((motor))+15;

                                      rmotor=((float)(rmotor)*1.128);

                                      lmotor=(long)((motor)+15);
                  output_b(0b10000000);
                  //delay_us(10);


                                           set_pwm1_duty(rmotor);
                                           set_pwm2_duty(lmotor);

                // delay_us(50);
                  }

//Reverse

 else if (flag==-1)
                 {

               lmotor=(abs(motor)+15);
               rmotor=(abs(motor)+15);
                                   rmotor=((float)(rmotor)*1.128);
                                   //rmotor=(long)(rmotor);
               output_b(0b00000001);
               //delay_us(10);

                               set_pwm1_duty(rmotor);
                               set_pwm2_duty(lmotor);

               //delay_us(50);
               }
           else {
                           lmotor=rmotor=0;

           output_b(0b00000000);
           set_pwm1_duty(rmotor);
                           set_pwm2_duty(lmotor);}

old_angle = angle;
delay_ms(20);
}

}

2

great job guys, we tried building a segway using a pic controller. We started out prototyping with the Vex controller, but there was too much “play” in the vex gears.

3

Thanks…almost there we just need to get rid of those vibrations…im just out of ideas.

4

unfortunately im not a programmer so i can help you but i would like to know about the specs on your segway like what motors and gearboxes you used and what speed your assuming it to run at because im looking into making my own homemade segway.

5

Are you using a proportional algorithm, or PID?

6

right now we are using a PD

7

Maybe you could do a “delay” in the other direction. Do a simple prediction of what the angle is likely be the next time through the loop, and use that as your feedback value instead of the current angle.

I didn’t look closely enough at your code to be certain, but are you sure you have the right sign on your Kd term? It should act to reduce motor speed, but you describe it as causing more current to be drawn.

Also, are you using Victor speed controllers? They have a dead band around the neutral point that makes them a bit unsuited for applications requiring the motors to hold a given position. If you’re very careful, you can account for the dead band in software, but you have to make sure everything is appropriately calibrated.

8

Im using OSMC to drive my motors and using a pic16f877A for the control.

9

Toyota may be beating you to the punch:

Toyota brings a Segway to the masses.

10

My god those are ugly.

11

Personally I am not a fan of almost any Toyota styling. My recommendation would be to wait for the Lexus version to come out. It will be black with tan “interior”.

12

I am not a great programmer, but I have a little experience.
I hope you don’t mind, but I have modified your code slightly to make it more readable (well, at least for me).
I found two places where it looks like some brackets may have been left out. This is where my lack of expertise shows it’s ugly head. If I am correct, the code will not execute as desired if left this way. One of the places will prevent the “angle” to ever be set to “0” if it is in the window you defined. That being the case, the “flag” will never be set to “0” either. This looks like it may affect your stability around the balance point.

Below is my modified code. I added color to the sections that needed brackets (in my opinion).

#include<16f877a.h>

#DEVICE ADC=10
#fuses hs,nowdt,noprotect,noput,nobrownout
#use delay(clock=20000000)

#use rs232(baud=9600,xmit=PIN_C6,rcv=PIN_C7,bits=8,pari ty=N)

int BIN (float x)
{
if (x==0)
return(0);
if (x>0)
return (1);
if (x<0)
return(-1);
}

void main ()
{

float angle=0,motor=0;
long lmotor=0,rmotor=0;
long gz_adc=0,ax_adc=0, ax_offset=516,gz_offset=494; /* Analog values */
signed long gz_deg=0,ax_deg=0,old_angle=0,new_angle=0;
long dt=0; // do nothing
int flag=0;


/*Setup PWM timers*/
setup_ccp1(CCP_PWM);
setup_ccp2(CCP_PWM);
setup_timer_2(T2_DIV_BY_4,82 , 1); //15KHz frequency of the motor




//sets up timer
setup_timer_1(T1_INTERNAL|T1_DIV_BY_8);



//Setup the ADC
setup_adc(ADC_CLOCK_INTERNAL );//setup clock before setup
setup_adc_ports( ALL_ANALOG );


//Initialize the PWM to 0
set_pwm1_duty(0);
set_pwm2_duty(0);

delay_ms(3000);


while(1)
{

dt=get_timer1();
set_timer1(0x0000);
old_angle=angle;


//setup ADC for accel
set_adc_channel(0);
delay_us(10);
ax_adc=read_adc();

delay_us(60);

//Setup ADC for gyro
set_adc_channel(1);
delay_us(10);
gz_adc = read_adc();



if (ax_adc>=ax_offset)
{
ax_deg=(((ax_adc-ax_offset)*14)>>5);
}
else
{ 
ax_deg=((((ax_adc-ax_offset)*14)*-1)>>5)*-1;
}

if (gz_adc>=gz_offset)
{
gz_deg=(((gz_adc-gz_offset)*41)>>6);
angle = 0.91* (angle + (float)(gz_deg*0.00000016*dt));
angle += 0.09*(ax_deg);
}

else
{ 
gz_deg=((((gz_adc-gz_offset)*41)*-1)>>6)*-1;
angle = 0.91* (angle + (((float)gz_deg)*0.0000016*(float)dt));
angle += 0.09*((float)ax_deg);
}
*******************************
if (angle<0.6 && angle>-0.7)
{
angle=0;
}
else
{
angle=angle;
}
*********************************
//flag will be 1 if angle>0 and flag will be -1 if angle<0 and 0
flag = BIN(angle);
new_angle = angle;


//Calculate motor value for PD controller


motor=((4.3*angle)+(0.2*(float)(gz_deg)));



**********************************************************
//Make sure the "motor" value does not exceed 250 or -250
if(motor>178)
{
motor=178;
}
else if (motor<-178)
{
motor=-178;
}
else
{
motor=(signed long)(motor);
}
*********************************************************

//Control the motors direction and set its PWM



//flag=1 => forward
if (flag==1)
{

rmotor=(long)((motor))+15;

rmotor=((float)(rmotor)*1.128);

lmotor=(long)((motor)+15);
output_b(0b10000000);
//delay_us(10);


set_pwm1_duty(rmotor);
set_pwm2_duty(lmotor);

// delay_us(50);
}
//Reverse

else if (flag==-1)
{

lmotor=(abs(motor)+15);
rmotor=(abs(motor)+15);
rmotor=((float)(rmotor)*1.128);
//rmotor=(long)(rmotor);
output_b(0b00000001);
//delay_us(10);

set_pwm1_duty(rmotor);
set_pwm2_duty(lmotor);

//delay_us(50);
}
else
{
lmotor=rmotor=0;

output_b(0b00000000);
set_pwm1_duty(rmotor);
set_pwm2_duty(lmotor);
}


old_angle = angle;
delay_ms(20);
}


}
13

The brackets can actually be left out in this case, since in C an if-then structure is formatted as “if-conditional statement;” where “statement” is the one line executed if the conditional is true. However, since one usually needs more than one statement per conditional, the brackets allow you to put multiple statements into a block, which C thent treats as one statement for the purpose of the conditional.

That having been said, the addition of the brackets certainly makes it more readable and, if you ever have to add another statement into the block, it makes it much easier to do it quickly.

14

This is the best thing about CD. One can admit their shortcomings and not be ridiculed for them. Instead, they receive an education.

Thanks for helping me (us) out on this!

Gohan22, ignore what I said. :o

15

no its all good… that was actually new to me as well. I always thought that you needed the brackets. So anyone have any ideas of how to remove the vibrations from the scooter?

16

Yes, a Kalman filter.

http://www.cs.unc.edu/~welch/kalman/

Can’t help you out with it though, still haven’t figured it out for myself.

17

I think the strategy of modelling the response may not work since the time step is so small… and if you were to model it for too many timesteps you get problems with response.

maybe you can introduce a decay coefficient which basically says if the error hasn’t changed significantly in the previous N iterations (where N is large enough to account for oscillation type behaviour), and the error is below some constant decay the output by a growing coefficient.