void lower(unsigned char side)
{
if(side==1)
{
servo_2(60);
}
else
servo_2(120);
return;
//
// if (side == 1)
// {
// for(i=90;i>=60;i--)
// {
// servo_2(i);
// _delay_ms(10);
// }
// cur_angle=60;
// }
// else
// {
// for(i=90;i<=120;i++)
// {
// servo_2(i);
// _delay_ms(10);
// }
// cur_angle=120;
// }
}
int kkkk(void)
{
velocity(255,255);
forward();
while(1)
{
servo_1(0);
servo_2(0);
servo_3(0);
forward();
red_read(); //display the pulse count when red filter is selected
_delay_ms(500);
green_read(); //display the pulse count when green filter is selected
_delay_ms(500);
blue_read(); //display the pulse count when blue filter is selected
_delay_ms(500);
lcd_wr_command(0x01);
print_sensor(1,1,3); //Prints value of White Line Sensor1
print_sensor(1,5,2); //Prints Value of White Line Sensor2
print_sensor(1,9,1); //Prints Value of White Line Sensor3
print_sensor(2,1,11);
servo_1(100);
servo_2(100);
servo_3(100);
_delay_ms(7000);
}
}
void close(unsigned char side)
{
if (side == 0)
servo_2(0);
else if (side == 1)
servo_3(0);
}
void open(unsigned char side)
{
if (side == 0)
servo_2(180);
else if (side == 1)
servo_3(180);
}
void elevate()
{
servo_2(90);
cur_angle=90;
_delay_ms(1000);
servo_2_free();
//return;
// if(cur_angle==60)
// {
// for(i=60;i<=90;i++)
// {
// servo_2(i);
// _delay_ms(10);
// }
// }
// else
// {
// for(i=120;i>=90;i--)
// {
// servo_2(i);
// _delay_ms(10);
// }
// }
// cur_angle=90;
}
void reset_servo() {
servo_3(150);
servo_2(120);
servo_1(30);
_delay_ms(1000);
servo_1_free();
servo_2_free();
servo_3_free();
}
void elevate()
{
if(cur_angle==60)
{
for(i=60;i<=90;i++)
{
servo_2(i);
_delay_ms(10);
}
}
else
{
for(i=120;i>=90;i--)
{
servo_2(i);
_delay_ms(10);
}
}
cur_angle=90;
}
/**************************************************
******************SERVO****************************
**************************************************/
void lower(unsigned char side)
{
if (side == 1)
{
for(i=90;i>=60;i--)
{
servo_2(i);
_delay_ms(10);
}
cur_angle=60;
}
else
{
for(i=90;i<=120;i++)
{
servo_2(i);
_delay_ms(10);
}
cur_angle=120;
}
}
//Main function
int main(void)
{
//servo 1 & 3 0 open 40 close
//servo 1 60 close
unsigned char i = 0;
init_devices();
servo_3(0);
servo_2(90);
servo_1(0);
_delay_ms(2000);
// servo_1(45);
// _delay_ms(2000);
//servo_1(60);
//while(1);
//while(1);
// lower(1);
//_delay_ms(3000);
// elevate();
// while(1);
pick(0);
_delay_ms(4000);
drop(0);
while(1);
_delay_ms(2000);
pick(1);
_delay_ms(2000);
drop(1);
//servo_3(0);
//pick(1);
_delay_ms(2000);
pick(0);
while(1);
drop(0);
return 0;
}
//Main Function
int main(void)
{
//all initializations
init_devices_motion();
init_devices();
init_devices_lf();
lcd_set_4bit();
lcd_init();
//setting gripper arm
initial_angle = 120;
servo1_init();
servo_2(initial_angle);
servo_3(180);
//UDR0 = 0x31;
//line_follower();
//pluck_fruit();
while(1);
}
void main()
{
unsigned int value,value1;
int a=0,b=0;
cli();
INIT_PORTS(); //Initialize ports
uart0_init(); //Initialize UART0 for xbee communication
timer5_init();
sei();
INIT_PORTS_ROTATE(); //Initialize ports
right_position_encoder_interrupt_init(); //Initialize control registers for wheel
left_position_encoder_interrupt_init(); // encoders.
init_devices();
lcd_set_4bit(); //LCD initialization functions.
lcd_init();
unsigned char angle = 0;
init_devices_servo(); //Initialize servo motors.
data='0';
sharp = ADC_Conversion(11); //Stores the Analog value of front sharp connected to ADC channel 11 into variable "sharp"
value = Sharp_GP2D12_estimation(sharp); //Stores Distance calsulated in a variable "value".
// lcd_print(1,1,value,3);
// sharp1 = ADC_Conversion(10); //Stores the Analog value of front sharp connected to ADC channel 11 into variable "sharp"
// value1 = Sharp_GP2D12_estimation(sharp1); //Stores Distance calsulated in a variable "value".
// lcd_print(1,5,value1,3);
// set servo for camera to ground zero
servo_1(0); // code to bring the camera to ground state
_delay_ms(1000);
// set servo for incline to ground zero
servo_2(90); // align camera
_delay_ms(500);
// the bot rotating slowly trying to find the bot
int terminate = 0;
while(data=='0' && terminate < 150)
{
velocity(150,150); //If no ball is detected the rotate and scan
angle_rotate_left(3); // for ball in the arena.
_delay_ms(500);
stop();
_delay_ms(500);
sharp = ADC_Conversion(11); //Stores the Analog value of front sharp connected to ADC channel 11 into variable "sharp"
value = Sharp_GP2D12_estimation(sharp); //Stores Distance calsulated in a variable "value".
terminate++;
// lcd_print(1,1,value,3);
}
if(terminate == 60)
{
buzzer_on();
return;
}
/*If a ball(red colour) is detected then matlab code sends a '5' signal through
zigbee.If a '5' is received then the robot stops rotating and moves towards the
ball*/
value2 = 100;
if(atoi(data) > 5)
{
lcd_print(1,5,value2,2);
}
velocity(160,184);
// change value
while(value>140)
{
sharp = ADC_Conversion(11); //Stores the Analog value of front sharp connected to ADC channel 11 into variable "sharp"
value = Sharp_GP2D12_estimation(sharp); //Stores Distance calsulated in a variable "value".
lcd_print(1,1,value,3);
forward();
}
stop();
_delay_ms(2000);
/* the servo_1 is for the camera and the servo_2 is for the inclined plane */
// rotate 90 at left
left90_at_place();
_delay_ms(1000);
// rotate the camera 90 degrees to the right
servo_1(90);
_delay_ms(1000);
//rotate in circle
rotate_in_circle();
int perceived_height;
perceived_height = atoi(data);
lcd_print(1,5,perceived_height,5);
// rotate 90 degrees to the right to face the ball again
right90_at_place();
stop();
_delay_ms(2000);
// rotate camera also to ground state to face the ball again
servo_1(0); // code to bring the camera to ground state
_delay_ms(1000);
// set servo for incline
int angle_of_inclination = get_angle(perceived_height);
lcd_print(1,2,angle_of_inclination,5);
servo_2(angle_of_inclination);
_delay_ms(1000);
while(1);
}
void Lift() {
//Open and come to pick
for (i = 0; i <120; i++)
{
servo_1(i);
_delay_ms(30);
servo_2(i);
_delay_ms(30);
servo_3(i);
_delay_ms(30);
}
/*for (i = 120; i >90; i--)
{
servo_2(i);
_delay_ms(30);
}*/
for (i = 120; i <150; i++)
{
servo_1(i);
_delay_ms(30);
//servo_2(i);
//_delay_ms(30);
servo_3(i);
_delay_ms(30);
}
//Grab
for (i = 150; i >30; i--)
{
servo_1(i);
_delay_ms(30);
// servo_2(i);
// _delay_ms(30);
// servo_3(i);
// _delay_ms(30);
}
//pick
for (i = 150; i >120; i--)
{
// servo_1(i);
// _delay_ms(30);
servo_3(i);
_delay_ms(30);
}
for (i = 120; i >0; i--)
{
// servo_1(i);
// _delay_ms(30);
servo_2(i);
_delay_ms(30);
servo_3(i);
_delay_ms(30);
}
// _delay_ms(2000);
//
}
int main(void)
{
cli();
set_lcd();
set_motors();
set_ADC();
sei();
int getValue1(void); //This function returns the value of the sensor mounted on the arm
void findagte(void);
void entergate(void);
int follow_wall(void);
int getValue2(void) ;
int row;
int state;
centre = ADC_Conversion(2);
sensorLeft = ADC_Conversion(3);
sensorRight = ADC_Conversion(1);
value_front = ADC_Conversion(11);
value_4sens= ADC_Conversion(12);
value_2sens= ADC_Conversion(10);
value_1sens=ADC_Conversion(9);
/*
lcd_print(2, 1, sensorLeft, 3);
lcd_print(2, 5, centre, 3);
lcd_print(2, 9, sensorRight, 3);
*/
if(sensorLeft <50 && sensorRight <50 && centre < 50) //when the bot enters the arena first
{
velocity(forwardLeftSpeed, forwardRightSpeed);
forward_mm(350);
stop();
_delay_ms(100);
}
//getValue2();
value_front = ADC_Conversion(11);
if(value_front>150)
{
centre = ADC_Conversion(2);
while(centre<150)
{
velocity(forwardLeftSpeed, forwardRightSpeed);
forward();
value_front = ADC_Conversion(11);
if(value_front<150)
{
findgate();
}
centre = ADC_Conversion(2);
}
entergate();
}
else if(value_front < 50)
{
left_degrees(90);
stop();
_delay_ms(500);
//turn the gripper
servo_2(90);
while(1)
{
row=follow_wall();
if(state)
{
soft_right();
centre = ADC_Conversion(2);
while(centre<150)
{
velocity(forwardLeftSpeed, forwardRightSpeed);
forward();
if(value_front<150)
{
findgate();
}
centre = ADC_Conversion(2);
}
entergate();
}
}
}
return(row);
}
int main()
{
int i=0;
init_devices();
lcd_set_4bit();
lcd_init();
color_sensor_scaling();
/*
//variable 'i' scales at 13,14 for sharp sensor for velocitty 240 240
//u turn 1600ms at 200,200 velocity
velocity(200,200);
left();
_delay_ms(1600);
stop();
while(1);
threshold=6000;
right();
while(ADC_Conversion(11)<65)
{
i++;
lcd_print(1,11,i,3);
}
stop();
lcd_print(2,11,scan(),1);
stop();
while(1);
*/
setIndicatorAndColor();
threshold=6000;
ct = 0; adj = 2;
//lcd("Begin");
forwardJaa();
stop();
servo_1(0);
servo_2(90);
servo_3(0);
while (sorted<total)
{
canDrop();
//buzzer_on();
//_delay_ms(500);
//buzzer_off();
if (visitedCount == 3)
predict();
if (sorted == total)
break;
pickup();
traverseToSort(ct, ct % 2 + 4);
sortCheck();
}
for (i = 0; i<4; i++);
while(1);
//..printf("%d %d\n", term[i][0], term[i][1]);
//..printf("Sort 0=%dSort 1=%d\nArm 0=%dArm 1=%d\n", sort[0], sort[1], arm[0], arm[1]);
//..printf("Cost=%d\nSORTED!!!!!\n", cost + 7);
//getch();
return 0;
}
