[gohan22]

Ok so my code has changed quite a bit. Currently i am having problems with vibrations which is causing my sensors to go crazy and not allowing me to balance. I have coded my PWM to increase smoothly and decrease smoothly.
Any suggestions on how to solve this? My code is below. If you require any more infomation please let me know.

Should i code a deadzone between -2 to 2 degrees?






#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)

//#define kp 0.4
//#define kd 0.5


void main ()
{

float motor=0,angle,last_motor=0;
long lmotor=0,rmotor=0,value;
long gz_adc,ax_adc, ax_offset=512,gz_offset=494; /* Analog values */
signed long gz_deg,ax_deg;
long dt=0, i;


/*Setup PWM timers*/
setup_ccp1(CCP_PWM);
setup_ccp2(CCP_PWM);
setup_timer_2(T2_DIV_BY_16, 255, 1);



output_bit(pin_b6,0);//Disable pin enable=0 and disable=1 goes
to pin 4 of cable
output_bit(pin_b5,1); //AHI and BHI always pulled high goes to
pin7 and pin 5

/*Timer0, 8 bit, prescaler of 256, 20Mhz/4=5,000. Inverse 5,000
times the prescaler value, this will be the increment by which your
timer counts 'one tic'in our case it's 56.2 ms */
//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 );
set_adc_channel( 0 );
delay_ms(10);

while(1)
{
dt=get_timer1();
set_timer1(0x0000);

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

//Setup read 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.97* (angle + (float)(gz_deg*0.00000016*dt));
angle += 0.03*(ax_deg);
}

else { gz_deg=((((gz_adc-gz_offset)*41)*-1)>>6)*-1;
angle = 0.97* (angle + (((float)gz_deg)*0.0000016*(float)dt));
angle += 0.03*((float)ax_deg);

}

//Calculate motor value for PD controller
if (angle>3 && angle<50)
{motor=(((angle)/30)*345)+(0.2*(float)(gz_deg));
last_motor=motor;
}
else if (angle<-2 && angle>=-45)
{

motor=(((angle)/(-25))*285)+(0.6*(float)(gz_deg));
last_motor=motor;

}

else
{ motor=(long)(motor);
last_motor=motor;}

if(motor>255)
{motor=255;}
else {motor=(long)(motor);}

if(motor>(last_motor+4) && motor<(last_motor-4))
{motor=last_motor;}
else {motor=motor;}

lmotor= rmotor=(long)(motor);
delay_us(10);




//lmotor=((56523+lmotor)>>8);
//rmotor=((56523+rmotor)>>8);
//Control the motors
if (angle>=2 && angle<70)
{
output_b(0b00100001);
output_d(0b00100010);
set_pwm1_duty(rmotor);
set_pwm2_duty(lmotor);
delay_us(100);
}
//Reverse

else if (angle<-2 && angle>=-45)
{
output_b(0b00100010);
output_d(0b00100001);
set_pwm1_duty(rmotor);
set_pwm2_duty(lmotor);
delay_us(100);
}
//Turn off motors

else
{
output_b(0b00100001);
output_d(0b00100010);
rmotor=lmotor=0;
set_pwm1_duty(rmotor);
set_pwm2_duty(lmotor);
delay_us(100);


}

//dt=get_timer0();
//gz_adc=gz_adc+5;
//ax_adc=ax_adc+3;


//printf ("\rAX_ANGLE=%ld \n\rANGLE=%f \n\rdt=%ld \n\rMOTOR=%f
\n",ax_deg,angle,dt,motor ); // d for int and f for
//printf (lcd_putc,"\f%ld %ld \n
%ld",gz_deg,ax_deg,gyrointegral); // d for int and f for
//delay_ms(10);



}

}