//servo_1 states
//init servo at 90 deg
void elevate(unsigned char angle, unsigned char side)
{
if (side == 0)
servo_1(angle);
else if (side == 1)
servo_1((angle == 45) ? 0 : 45);
}
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);
}
}
/* Rotating the panel in the plane of servo hinge by one degree each till 165
Here we have rotated the servo only upto 165 not 180 due to large size of panel, that is when rotated by more than
165 it start touching the upper plate of bot.
Here first the servo is rotated upto 165 each by one degree and the value corresponding to voltage is noted in the array
then maximum value is calculated from the array and the corresponding angle.
Then the panel is aligned to that angle.
Delay time for each rotation is 400 milli-seconds.
*/
void servo_rotation_165()
{
init_devices_servo();
unsigned int i = 0;
float max_panel_volt=0.0;
int counter=0;
float panel_voltage_s[165];
for (i = 0; i <165; i++)
{
servo_1(i);
_delay_ms(delay_time);
panel_voltage_s[i]=value_in_volt(ADC_Conversion(10)); // ADC_Conversion gives analog value of voltage through channel 10
lcd_print(1,13,batt_volt(ADC_Conversion(0)),4); // Printing the battery voltage.
lcd_print(2,9,panel_voltage_s[i],3); // Printing voltage of panel at specific angles.
// _delay_ms(50);
}
// finding the maximum intensity of value and corresponding angle
for(int j=0;j<165;j++)
{
if(panel_voltage_s[j]> max_panel_volt)
{
max_panel_volt= panel_voltage_s[j];
counter=j; // identifier counter contains the angle for which the intensity is maximum
}
}
lcd_print(2,9,panel_voltage_s[counter],3); // Printing maximum voltage.
// setting the panel at that angle of maximum intensity
servo_panel_0();
for (int j = 0; j<counter;j++) // Setting the panel at maximum voltage.
{
servo_1(j);
_delay_ms(delay_time);
float panel_volt=value_in_volt(ADC_Conversion(10));
lcd_print(2,13,panel_volt,3);
}
lcd_print(2,13,panel_voltage_s[counter],3); // Checking the value to be exact by printing again.
_delay_ms(1000);
servo_1_free();
}
void reset_servo() {
servo_3(150);
servo_2(120);
servo_1(30);
_delay_ms(1000);
servo_1_free();
servo_2_free();
servo_3_free();
}
// aligning the panel on servo at 45 degree
void servo_panel_45(void)
{
init_devices_servo();
for (int i = 0; i <45; i++) //FIRST ALINGING THE PANEL AT 45 DEGREE FROM GROUND TO HAVE THE LOCAL MAXIMUM INTENSITY IN ONE PLANE
{
servo_1(i);
_delay_ms(100);
}
_delay_ms(1000);
servo_1_free();
}
void close(unsigned char side)
{
if (side == 0)
for(i=0;i<=40;i++)
{
servo_3(i);
_delay_ms(10);
}
else if (side == 1)
for(i=0;i<=60;i++)
{
servo_1(i);
_delay_ms(10);
}
}
void open(unsigned char side)
{
if (side == 0)
for(i=55;i>0;i--)
{
servo_3(i);
_delay_ms(10);
}
else if (side == 1)
for(i=55;i>0;i--)
{
servo_1(i);
_delay_ms(10);
}
}
//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;
}
void open(unsigned char side)
{
if (side == 0)
for(i=40;i>0;i--)
{
servo_3(i);
_delay_ms(10);
}
else if (side == 1)
for(i=60;i>0;i--)
{
servo_1(i);
_delay_ms(10);
}
if(side==0)
servo_3_free();
else
servo_1_free();
}
/**
* Function called using matlab, used to detect red/green/blue poles.
*/
int locate()
{
unsigned char i = 0, sharp;
unsigned int value=0, cm;
if (rotate == 1) {
for (i = prevI; i < 180; i++) {
servo_1(i);
_delay_ms(300);
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 calculated in a variable "value".
lcd_print(2,1,value,3);
lcd_print(2,6,i,3);
if (rotate == 0) {
prevI = i;
/*
* Distance measured by sharp sensor is in mm, convert to cm for sending send.
* for example 230 is send as 23; 233 as 230; 238 as 24 so max error is 5 mm
*/
cm = value/10;
if (value%cm > 5) {
cm += 1;
}
while (sendDistance==0); //wait till matlab is ready to accept distance
sendNumber(cm);
sendDistance = 0;
while (sendAngle==0); // wait till matlab is ready to receive angle.
sendNumber(i);
sendAngle = 0; // disable sendNext,
i=180; //any random number to get us out of for loop
}
if (i == 179) {
prevI = 0;
}
}
_delay_ms(500);
}
}
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);
}
int main(void)
{
port_init();
init_devices();
int k=240;
int i=0;
while(i<3)
{
servo_1(180);//for opening the gripper
_delay_ms(200);
backward_mm_ext(290);//for releasing the thread as gripper goes down
stop();
_delay_ms(300);
servo_1(100);//for closing the gripper
_delay_ms(300);
forward_mm_ext(225);//for winding the thread so that gripper comes up
_delay_ms(200);
servo_1_free();//making servo free to rotate
_delay_ms(300);
velocity(170,200);
back_mm(k);
stop();
_delay_ms(300);
velocity(190,240);
right_degrees(100);
_delay_ms(200);
stop();
velocity(145,200);
forward_mm(400);//for moving robot in x direction
stop();
_delay_ms(300);
backward_mm_ext(225);
_delay_ms(600);
_delay_ms(1200);
servo_1(180);
_delay_ms(300);
forward_mm_ext(290);
stop();
_delay_ms(100);
velocity(170,200);
back_mm(400);
_delay_ms(300);
stop();
velocity(190,240);
left_degrees(90);
stop();
_delay_ms(60);
velocity(190,240);
forward_mm(k);//for moving robot in y direction
stop();
_delay_ms(300);
k=k-120;
i=i+1;//for 3 iteration
}
stop();
}
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()
{
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;
}
void object_grip (void) // relase the object
{
servo_1 (145);
_delay_ms(1000);
}
void object_ungrip (void) // grip the object
{
servo_1 (10);
_delay_ms(1000);
}
