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


}

[ComradeNikolai]

Originally Posted by billbo911

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.

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.

[billbo911]

Originally Posted by ComradeNikolai

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.

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.

[gohan22]

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?

[Nathan]

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.