Almost balancing Scooter

[gohan22]

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,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




//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>=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

/*makes sure that you remain forward or reverse and how we switch to reverse*/
/*if ((abs(new_angle-old_angle)>4) && (new_angle*old_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);
}


}