void red2()
{
while(true)
{
cleargyro();
if(SensorValue(sonar) <50) // Loop while robot's Ultrasonic sensor is further than 20 inches away from an object
{
while(SensorValue[gyro] < 500){
turn_left();
}
go_straight();
}
else
{
motor[right] = 127;
motor[left] = 127;
}
}
}
int main(void)
{
PORTG=255;
_delay_ms(2000);
#define F_CPU 16000000UL
unsigned char temp;
//Init_LED();
uart_1_ini();
while (temp=='P')
{
temp=USART1_Recieve();
}
temp=0;
while (temp<5)
{
uart_1_Transmit('o');
temp++;
}
UCSR1B=((1<<RXEN1)|(1<<TXEN1)|(1<<RXCIE1));
PORTG=0;
TWI_init_master();
//kalmaninit();
PORTG=255;
uart_1_Transmit('B');
_delay_ms(200);
cli(); //Initialize Gyro
InitHMC();
PORTG=0; //Enable Global Interrupt
DDRA=0;
DDRD=0b00001011;
PORTA=255;
DDRF=0;
//ADCSRA=(1<<ADEN);//|(1<<ADFR);
_delay_ms(5);
cleargyro();
zero_error();
sei();
PORTG=0;
//uart_1_Transmit('C');
//uart_1_Transmit('L');
//init_timer();
//_delay_ms(500);
/*_delay_ms(2000);
setspeed(front,100);
setspeed(left,0);
setspeed(right,0);
setspeed(back,0);
_delay_ms(500);
setspeed(front,0);
setspeed(left,100);
setspeed(right,0);
setspeed(back,0);
_delay_ms(1000);
setspeed(front,0);
setspeed(left,0);
setspeed(right,100);
setspeed(back,0);
_delay_ms(1000);
setspeed(front,0);
setspeed(left,0);
setspeed(right,0);
setspeed(back,100);
_delay_ms(2000);
setspeed(front,0);
setspeed(left,0);
setspeed(right,0);
setspeed(back,0);
*/
//OCR0=18;
int i,countera=0;
//
//while(1)
//{
//PORTG=255;
//_delay_ms(1000);
// PORTG=0;
//_delay_ms(1000);
//}
while(1)
{
/* PORTG=!(PORTG);
frontmotor=ontime_1;
backmotor=ontime_1;
rightmotor=ontime_1;
leftmotor=ontime_1;
*/
//if (X_angle>4 || X_angle < (-4))
//{
//
//leftmotor=leftmotor-Y_angle*0.3;
//rightmotor=rightmotor+Y_angle*0.3;
//}
if(X_angle>X_angle_prev)
X_error=X_angle-X_angle_prev;
else
X_error=X_angle_prev-X_angle;
X_integ =X_integ+Xangle;
if (X_integ>=5000)
X_integ=5000;
if (X_integ<=-5000)
X_integ=-5000;
countera++;
if (X_angle>2)
{
float kp=0.8;
float kd=0.6;
float ki=0.005;
leftmotor=ontime_1+kp*X_angle+kd*X_error+ki*X_integ;
if(leftmotor<0)
leftmotor=0;
rightmotor=ontime_1-kp*X_angle-kd*X_error-ki*X_integ;
if(rightmotor<0)
rightmotor=0;
X_angle_prev=X_angle;
}
else if (X_angle<-2)
{
float kp=0.8;
float kd=0.6;
float ki=0.005; // 0.0012
leftmotor=ontime_1+kp*X_angle-kd*X_error+ki*X_integ;
if(leftmotor<0)
leftmotor=0;
rightmotor=ontime_1-kp*X_angle+kd*X_error-ki*X_integ;
if(rightmotor<0)
rightmotor=0;
X_angle_prev=X_angle;
}
else
{
rightmotor=ontime_1;
leftmotor=ontime_1;
}
leftmotor=ontime_1;
rightmotor=ontime_1;
//setspeed(front,frontmotor);
setspeed(left,leftmotor);
setspeed(right,rightmotor);
////setspeed(back,backmotor);
//
//uart_1_Transmit('Y');
//uartWriteInt(Y_angle,4);
//uart_1_Transmit('y');
//uartWriteInt(Yangle,4);
//uart_1_Transmit('X');
//uartWriteInt(Xangle,4);
uart_1_Transmit('X');
uartWriteInt(Xangle,4);
uart_1_Transmit('x');
uartWriteInt(yangle,4);
//uart_1_Transmit('Y');
//uartWriteInt(Yangle,4);
uart_1_Transmit('Y');
uartWriteInt(Yangle,4);
uart_1_Transmit('y');
uartWriteInt(xangle,4);
//uart_1_Transmit('Z');
//uartWriteInt(zacc*100,4);
//uart_1_Transmit('I');
//uartWriteInt(X_integ,6);
//uart_1_Transmit('C');
//uartWriteInt(countera,6);
uart_1_Transmit(13);
//uart_1_Transmit('Y');
//uartWriteInt(yacc*100,4);
//uart_1_Transmit('X');
//uartWriteInt(xacc*100,4);
//// */
/*uart_1_Transmit('F');
uartWriteInt(frontmotor,4);
uart_1_Transmit('B');
uartWriteInt(backmotor,4);
uart_1_Transmit('R');
uartWriteInt(load,1);
//// */
/*
uart_1_Transmit('L');
uartWriteInt(load,1);
uart_1_Transmit('L');
uart_1_Transmit('&');
uartWriteInt(mean_1,3);
uart_1_Transmit('&');
uartWriteInt(mean_2,3);
uart_1_Transmit('&');
uartWriteInt(ontime_1,4);
uart_1_Transmit('&');
uartWriteInt(ontime_2,4);
uart_1_Transmit('&');
uartWriteInt(ontime_3,4);
*/
}
}