Almost balancing Scooter

[billbo911]

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);
}


}