/**
Reads all relevant sensor values and stores it in appropriate global variables.
*/
void read_sensors()
{
Left_white_line = ADC_Conversion(LEFT_SENSOR);
Center_white_line = ADC_Conversion(CENTER_SENSOR);
Right_white_line = ADC_Conversion(RIGHT_SENSOR);
Front_IR_Sensor = ADC_Conversion(FRONT_IR_SENSOR);
}
void arenaLeft_2()
{
nodeMissEnable = 0;
/*
stop();
_delay_ms(1000);
soft_left();
_delay_ms(1000);
*/
velocity(forwardLeftSpeed, forwardRightSpeed);
left();
_delay_ms(1000);
centre = ADC_Conversion(2);
while(centre < 15)
{
centre = ADC_Conversion(2);
}
stop();
_delay_ms(100);
nodeMissEnable = 1;
}
int getValue1(void)
{
centre = ADC_Conversion(2);
sensorLeft = ADC_Conversion(3);
sensorRight = ADC_Conversion(1);
}
//++++++===Servo __init__ ========++++++++
void set_color()
{
Left_white_line = ADC_Conversion(3); //Getting data of Left WL Sensor
Center_white_line = ADC_Conversion(2); //Getting data of Center WL Sensor
Right_white_line = ADC_Conversion(1); //Getting data of Right WL Sensor
}
int scan()//return the color no.
{
red_read();
blue_read();
green_read();
_delay_ms(100);
if(!(ADC_Conversion(11)>55 && ADC_Conversion(11)<100))
return EMPTY;
if (red<threshold && green < threshold && blue < threshold)
{
return BLACK;
}
else
{
if (red > blue)
{
if (red > green)
return RED;
else
return GREEN;
}
else if (blue > green)
return BLUE;
else return GREEN;
}
}
//-------------------------------------------------------------------------------
//Main Function
//-------------------------------------------------------------------------------
int main(void)
{
init_devices();
while(1)
{
sensor_data_interpretation();
//print_sensor(1,1,1); //Prints IR Proximity Sensor 1
BATT_V = ADC_Conversion(0);
BATT_Voltage = ((ADC_Conversion(0)*100)*0.07902) + 0.7; //Prints Battery Voltage Status
lcd_print(1,1,BATT_Voltage,4);
//print_sensor(1,1,0); //Prints Battery voltage binary value
print_sensor(1,6,5); //Prints IR Proximity Sensor 1
print_sensor(1,10,6); //Prints vlaue of Analog IR Proximity Sensor 2
print_sensor(1,14,7); //Prints value of Analog IR Proximity Sensor 3
print_sensor(2,2,3); //Prints value of White Line Sensor1
print_sensor(2,6,2); //Prints Value of White Line Sensor2
print_sensor(2,10,1); //Prints Value of White Line Sensor3
//print_sensor(2,9,11); //Analog Value Of Front Sharp Sensor
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(2,14,value,3);
}
}
/*
This function first rotates the bot by 5 degree till 360
Stores the voltage at each angle in array
Find maximum value of voltage from the array
Aligns the bot at that value of voltage.
*/
void bot_rotation360()
{
init_devices_motion();
init_devices_adc();
int bot_ang=0;
float panel_voltage[72];
float max_panel_volt=0;
float bat_voltage = 0.0;
int j;
for(j=0;j<72;j++)
{
soft_right();
// halting right wheel and moving only the left wheel for specified time to get the desired angle of rotation(according to calculation)
_delay_ms(rot_time); // rot_time is calculated according to power from the battery
stop();
_delay_ms(delay_time);
panel_voltage[j]=value_in_volt(ADC_Conversion(10)); // channel 10 contains voltage reading
lcd_print(2,1,panel_voltage[j],3);
bat_voltage=batt_volt(ADC_Conversion(0));
lcd_print(1,13,bat_voltage,4); // Printing battery voltage.
}
_delay_ms(1000); // Stopping the bot momentarily.
// for getting the angle of bot corresponding to maximum voltage and maximum voltage also.
for( j=0;j<72;j++)
{
if(panel_voltage[j]>max_panel_volt)
{
max_panel_volt=panel_voltage[j];
bot_ang=j;
}
}
lcd_print(2,1,panel_voltage[bot_ang],3); // Printing maximum voltage.
// Realigning the bot at the maximum angle of intensity in circular plane
j=1;
while(j!=bot_ang) // Since the bot_ang variable contains the angle for which the intensity was maximum
{
soft_right(); //left wheel forward leaving the right wheel at rest to get
//soft rotation at the axis of right wheel
_delay_ms(rot_time2);
float servo_volt=value_in_volt(ADC_Conversion(10));
lcd_print(2,5,servo_volt,3);
j++;
stop();
_delay_ms(delay_time);
}
lcd_print(2,5,panel_voltage[bot_ang],3);
max_angle_of_bot=bot_ang;
}
void arenaUturn(void)
{
nodeMissEnable = 0;
/*
stop();
_delay_ms(1000);
*/
// velocity(forwardLeftSpeed, forwardRightSpeed);
/*
line_follow_mm(30);
stop();
_delay_ms(1000);
soft_right();
_delay_ms(1000);
*/
stop();
_delay_ms(100);
velocity(forwardLeftSpeed, forwardRightSpeed);
left();
_delay_ms(2500);
centre = ADC_Conversion(2);
while(centre < 15)
{
centre = ADC_Conversion(2);
}
stop();
_delay_ms(100);
nodeMissEnable = 1;
}
//Function used for turning robot by specified degrees
void angle_rotate(unsigned int Degrees)
{
float ReqdShaftCount = 0;
unsigned long int ReqdShaftCountInt = 0;
ReqdShaftCount = (float) Degrees/ 4.090; // division by resolution to get shaft count
ReqdShaftCountInt = (unsigned int) ReqdShaftCount;
ShaftCountRight = 0;
ShaftCountLeft = 0;
while (1)
{
Left_white_line = ADC_Conversion(3); //Getting data of Left WL Sensor
Center_white_line = ADC_Conversion(2); //Getting data of Center WL Sensor
Right_white_line = ADC_Conversion(1); //Getting data of Right WL Sensor
if((ShaftCountRight >= ReqdShaftCountInt) | (ShaftCountLeft >= ReqdShaftCountInt))
{
break;
}
if((Left_white_line<0x20) && (Center_white_line>0x20) && (Right_white_line<0x20))//Black
{
break;
}
}
stop(); //Stop robot
}
int getValue2(void)
{
value_front = ADC_Conversion(11);
value_4sens= ADC_Conversion(12);
value_2sens= ADC_Conversion(10);
value_1sens=ADC_Conversion(9);
}
void get_vector() {
Left_white_line = ADC_Conversion(3);
Center_white_line = ADC_Conversion(4);
Right_white_line = ADC_Conversion(5);
print_sensor(2,1,3); //Prints value of White Line Sensor Left
print_sensor(2,5,4); //Prints value of White Line Sensor Center
print_sensor(2,9,5); //Prints Value of White Line Sensor Right
}
void sensor_data_interpretation(void)
{
SHARP_1 = ADC_Conversion(9);
Left_white_line = ADC_Conversion(3); //Getting data of Left WL Sensor
Center_white_line = ADC_Conversion(2); //Getting data of Center WL Sensor
Right_white_line = ADC_Conversion(1); //Getting data of Right WL Sensor
}
/*--functions--*/
void set_color()
{
Left_white_line = ADC_Conversion(3); //Getting data of Left WL Sensor
Center_white_line = ADC_Conversion(2); //Getting data of Center WL Sensor
Right_white_line = ADC_Conversion(1); //Getting data of Right WL Sensor
/*lcd_print(1,1,Left_white_line,3); //Prints value of White Line Sensor1
lcd_print(1,5,Center_white_line,3); //Prints Value of White Line Sensor2
lcd_print(1,9,Right_white_line,3); //Prints Value of White Line Sensor3
*/
}
int getError2(void)
{
int error;
centre = ADC_Conversion(2);
sensorLeft = ADC_Conversion(3);
sensorRight = ADC_Conversion(1);
lcd_print(2, 1, sensorLeft, 3);
lcd_print(2, 5, centre, 3);
lcd_print(2, 9, sensorRight, 3);
if(sensorLeft > 40 && sensorRight > 40)
error = 100;
else if(centre > 90)
error = 0;
else if(sensorLeft > 45)
error = 7;
else if(sensorLeft > 25)
error = 6;
else if(sensorLeft > 17)
error = 5;
else if(sensorLeft > 9)
error = 4;
else if(sensorRight > 110)
error = -7;
else if(sensorRight > 85)
error = -6;
else if(sensorRight > 55)
error = -5;
else if(sensorRight > 35)
error = -4;
else if(sensorRight > 25)
error = -3;
else if(sensorRight > 18)
error = -2;
else if(sensorRight > 15)
error = -1;
else
error = -100;
return -error;
}
void terminalCheck2()
{
if (flag == 0)
{
if (dir == 0)
if (ot == 0 || ot == 1)
forward_mm(30);
else back_mm(30);
else if (ot == 0 || ot == 1)
back_mm(30);
else forward_mm(30);
flag = 1;
}
if (((ct == 0 || ct == 1) && dir == 0) || ((ct == 2 || ct == 3) && dir == 2))
{
left_degrees(30);
velocity(turn_v, turn_v);
while (ADC_Conversion(1)<70)
left();
// _delay_ms(100);
stop();
}
else if (((ct == 0 || ct == 1) && dir == 2) || ((ct == 2 || ct == 3) && dir == 0))
{
right_degrees(30);
velocity(turn_v, turn_v);
while (ADC_Conversion(1)<70)
right();
// _delay_ms(100);
stop();
}
else {
left_degrees(150);
velocity(turn_v, turn_v);
while (ADC_Conversion(1)<70)
left();
// _delay_ms(100);
stop();
}
//printf("Enter term[%d][%d]\n", ct, 1);
//scanf("%d", &term[ct][1]);
term[ct][1] = scan();
if(term[ct][1]==-1)
lcd_print(2,11,9, 1);
else
lcd_print(2,11, term[ct][1], 1);
_delay_ms(1000);
if (term[ct][1] == -1 || term[ct][1] == color[ct])
total--;
visited[ct] = 1;
visitedCount++;
}
void Read_And_Print() {
stop();
Left_white_line = ADC_Conversion(3); //Getting data of Left WL Sensor
Center_white_line = ADC_Conversion(2); //Getting data of Center WL Sensor
Right_white_line = ADC_Conversion(1); //Getting data of Right WL Sensor
/*lcd_print(2, 1, Left_white_line, 3);
lcd_print(2, 7, Center_white_line, 3);
lcd_print(2, 13, Right_white_line, 3);*/
}
/* 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 read_sensor()
{
Left_white_line = ADC_Conversion(3); //Getting data of Left WL Sensor
Center_white_line = ADC_Conversion(2); //Getting data of Center WL Sensor
Right_white_line = ADC_Conversion(1); //Getting data of Right WL Sensor
/* 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
*/
lcd_print(1,1,Left_white_line,3);
lcd_print(1,5,Center_white_line,3);
lcd_print(1,9,Right_white_line,3);
}
int main(void)
{
cli();
set_ADC();
set_lcd();
sei();
PORTH |= 0x08; //Turn off the IR sharp sensors.
PORTG |= 0x04; //Turn off the White line sensors.
int sensors[5];
int i;
while(1)
{
for(i=0; i<5; i++)
{
sensors[i] = ADC_Conversion(i+4);
if(i==2)
lcd_print(1, 7, sensors[i], 4);
else if(i < 2)
lcd_print(2, 4*(i) + 1, sensors[i], 3);
else
lcd_print(2, 4*(i-1) + 2, sensors[i], 3);
}
}
return 0;
}
void turn() //turn robo by 180 degree
{
if (dir == 0 && (ot == 2 || ot == 3))
{
velocity(turn_v, turn_v);
left_degrees(180);
}
else if (dir == 2 && (ot == 0 || ot == 1))
{
velocity(turn_v, turn_v);
left_degrees(180);
}
else
{
velocity(turn_v, turn_v);
left_degrees(150);
while (ADC_Conversion(2)<70)
left();
stop();
}
lcd("turn");
//_delay_ms(2000);
dir = (dir + 2) % 4;
//printf("Turn\n");
angle += 180;
}
//Main Function
int main(void)
{
double error_p = 0.0;
double error_i = 0.0;
double error_d = 0.0;
double angle;
kalman_state k = kalman_init(1, 16, 1, 137);
init_devices();
lcd_set_4bit();
lcd_init();
double preverror = 0.0;
while(1)
{
sensor2 = ADC_Conversion(2);
kalman_update(&k,(double)sensor2);
angle = k.x;
lcd_print(1,5,(int)angle,3);
error_p = (angle - 137);
error_i += error_p*dt;
error_d = (error_p - preverror)/dt;
lcd_print(1,1,abs((int)error_p),3);
Disturbance = ((error_p*kp) + (error_i*ki) + (error_d*kd));
if(angle > 137) back();
else { forward();}
if (Disturbance > MAX) Disturbance = MAX;
if (Disturbance < MIN) Disturbance = MIN;
lcd_print(2,6,abs((int)Disturbance),5);
velocity(35+abs((int)Disturbance),35+abs((int)Disturbance));
preverror = error_p;
}
}
//Function To Print Sesor Values At Desired Row And Coloumn Location on LCD
int print_sensor(char row, char coloumn,unsigned char channel)
{
ADC_Value = ADC_Conversion(channel);
lcd_print(row, coloumn, ADC_Value, 3);
return ADC_Value ;
}
uint16_t ADC_BlockMeasure(uint8_t channel, uint8_t block) {
uint16_t ret = 0;
while(block > 0) {
ret += ADC_Conversion(channel);
block--;
}
return(ret);
}
void threshold(void)
{ while(x<=50){
threshold1 = ADC_Conversion(3); //Getting data of Left WL Sensor
threshold2 = ADC_Conversion(4); //Getting data of Center WL Sensor
threshold3 = ADC_Conversion(5); //Getting data of Right WL Sensor
_delay_ms(20);
x++;
}
if((threshold1>threshold2)&&(threshold1>threshold3)){
THRESHOLD=threshold1;}
else if((threshold2>threshold3)&&(threshold2>threshold3)){
THRESHOLD=threshold2;}
else{
THRESHOLD=threshold3;}//Getting maximum value of threshold1,2 & 3
THRESHOLD=THRESHOLD+20;
lcd_print (2,9,THRESHOLD,3);
}
int getError(void)
{
int error;
centre = ADC_Conversion(2);
sensorLeft = ADC_Conversion(3);
sensorRight = ADC_Conversion(1);
lcd_print(2, 1, sensorLeft, 3);
lcd_print(2, 5, centre, 3);
lcd_print(2, 9, sensorRight, 3);
if(sensorLeft > 15 && sensorRight > 15)
error = 100;
else if(centre > 30)
error = 0;
else if(sensorLeft > 75)
error = 4;
else if(sensorLeft > 50)
error = 3;
else if(sensorLeft > 11)
error = 2;
else if(sensorLeft > 9)
error = 1;
else if(sensorRight > 80)
error = -4;
else if(sensorRight > 35)
error = -3;
else if(sensorRight > 10)
error = -2;
else
error = -100;
return error;
}
/*************************GULLA CODE**Blackline Forward***********/
char Delay(int tim)
{
int i;
for(i=0;i<tim && ADC_Conversion(2)<=40;i++)
{
//set_color();
_delay_ms(1);
}
stop();
if(i<tim)
return 1;
else return 0;
}
//Main Function
int main(void)
{
init_devices();
while(1)
{
while((SPSR & 0x80) == 0x00); //wait for data reception to complete
data = SPDR;
//_delay_ms(1);
//data = data + 1;
ADC_Value = ADC_Conversion(data);
SPDR = ADC_Value;
while((SPSR & 0x80) == 0x00); //wait for data transmission to complete
}
}
//rotate right degree
void angle_rotate_right()
{
ShaftCountRight = 0;
ShaftCountLeft = 0;
while (1)
{
Center_white_line = 0;
for(temp=0; temp<AVERAGE_WL_COUNT; ++temp){
Center_white_line += ADC_Conversion(2);
}
Center_white_line/=AVERAGE_WL_COUNT;
if((ShaftCountRight >= COUNT_LEFT_4) | (ShaftCountLeft >= COUNT_LEFT_4) | (Center_white_line > THRESHOLD_WL_C1) )
break;
}
motion_set(0x00); //Stop robot
}
/*
* Main function, will call different phases, locate/clean
*/
int main(void)
{
unsigned char sharp;
unsigned int value=0;
init_devices();
lcd_set_4bit();
lcd_init();
lcd_wr_command(0x0C);// Display ON Cursor OFF
while(1) {
if (phase == 1) {
locate();
}
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(2,1,value,3);
}
servo_1_free();
return 0;
}
void turnLeft() //turns the robo left
{
if ((dir == 1 && (ot == 0 || ot == 1)) || (dir == 3 && (ot == 2 || ot == 3)))
{
velocity(turn_v, turn_v);
left_degrees(90);
}
else
{
left_degrees(30);
velocity(turn_v,turn_v);
while (ADC_Conversion(2)<70)
left();
// _delay_ms(100);
stop();
}
lcd("Left turn");
//_delay_ms(2000);
dir = (dir + 3) % 4;
//printf("Turn Left\n");
angle += 90;
}