Permalink
Switch branches/tags
Nothing to show
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
1349 lines (1179 sloc) 53.2 KB
/*
* Team Id: eYRCPlus-PS1#2678
* Author List: Avick Dutta (team leader), Arpan Das, Subhendu Hazra, Asesh Basu
* Filename: Puzzle_Solver.c
* Theme: Puzzle Solver Robot (GLCD) - eYRCPlus
* Functions: left_led_on, left_led_off, right_led_on, right_led_off,
* get_point_cost, get_nearest_point, follow_black_line, follow_black_line_mm,
* turn_robot, change_direction, move_one_cell, match_column, match_row,
* go_to_coordinate, go_to_cell_no, get_pickup_direction, pickup, deposit
* Global Variables: ADC_Value, flag, Left_white_line, Center_white_line, Right_white_line,
* ShaftCountLeft, ShaftCountRight, Degrees, data, input_str, d1_position_map,
* d2_position_map, current_velocity, current_direction,
* pickup_direction, current_grid, current_cell_no, current_coordinate,
* BNW_Thresh, left_velocity_float, right_velocity_float,
* left_velocity, right_velocity
*/
// Note: that predefined function definitions are written in Allman indent style
// and our function definitions are written in K&R indent style
// reference: https://en.wikipedia.org/wiki/Indent_style
#define F_CPU 14745600
#include <avr/io.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h> //included to support power function
#include "lcd.h"
#include "glcd.h" //User defined LCD library which contains the lcd routines
#include "glcd.c"
#include "glcd_big_font_msg.h" // big ubuntu font stored as image
#include "glcd_big_font_msg.c"
#include <string.h>
void port_init();
void timer5_init();
void velocity(unsigned char, unsigned char);
void motors_delay();
unsigned char ADC_Conversion(unsigned char);
unsigned char ADC_Value;
unsigned char flag = 0;
unsigned char Left_white_line = 0;
unsigned char Center_white_line = 0;
unsigned char Right_white_line = 0;
volatile unsigned long int ShaftCountLeft = 0; //to keep track of left position encoder
volatile unsigned long int ShaftCountRight = 0; //to keep track of right position encoder
volatile unsigned int Degrees; //to accept angle in degrees for turning
char data; //to store received data from UDR1
char input_str[100] = ""; // stores the raw input string
//Function to configure LCD port
void lcd_port_config (void)
{
DDRC = DDRC | 0xF7; //all the LCD pin's direction set as output
PORTC = PORTC & 0x80; // all the LCD pins are set to logic 0 except PORTC 7
}
//ADC pin configuration
void adc_pin_config (void)
{
DDRF = 0x00;
PORTF = 0x00;
DDRK = 0x00;
PORTK = 0x00;
}
//Function to configure ports to enable robot's motion
void motion_pin_config (void)
{
DDRA = DDRA | 0x0F;
PORTA = PORTA & 0xF0;
DDRL = DDRL | 0x18; //Setting PL3 and PL4 pins as output for PWM generation
PORTL = PORTL | 0x18; //PL3 and PL4 pins are for velocity control using PWM.
}
//Function to configure INT4 (PORTE 4) pin as input for the left position encoder
void left_encoder_pin_config (void)
{
DDRE = DDRE & 0xEF; //Set the direction of the PORTE 4 pin as input
PORTE = PORTE | 0x10; //Enable internal pull-up for PORTE 4 pin
}
//Function to configure INT5 (PORTE 5) pin as input for the right position encoder
void right_encoder_pin_config (void)
{
DDRE = DDRE & 0xDF; //Set the direction of the PORTE 4 pin as input
PORTE = PORTE | 0x20; //Enable internal pull-up for PORTE 4 pin
}
//Function to initialize Buzzer
void buzzer_pin_config (void)
{
DDRC = DDRC | 0x08; //Setting PORTC 3 as output
PORTC = PORTC & 0xF7; //Setting PORTC 3 logic low to turnoff buzzer
}
//Function to configure Interrupt switch
void interrupt_switch_config (void)
{
DDRE = DDRE & 0x7F; //PORTE 7 pin set as input
PORTE = PORTE | 0x80; //PORTE7 internal pull-up enabled
}
//Function to configure LED port i.e. pg0 and pg1 for left and right LEDs respectively
void LED_pin_config (void) {
DDRG |= 0b00000011; // all the LED pin's direction set as output
PORTG &= 0b11111100; // all the LED pins are set to logic 0
}
//Function to Initialize PORTS
void port_init()
{
lcd_port_config();
adc_pin_config();
motion_pin_config();
left_encoder_pin_config(); //left encoder pin config
right_encoder_pin_config(); //right encoder pin config
buzzer_pin_config();
LED_pin_config();
interrupt_switch_config();
}
void left_position_encoder_interrupt_init (void) //Interrupt 4 enable
{
cli(); //Clears the global interrupt
EICRB = EICRB | 0x02; // INT4 is set to trigger with falling edge
EIMSK = EIMSK | 0x10; // Enable Interrupt INT4 for left position encoder
sei(); // Enables the global interrupt
}
void right_position_encoder_interrupt_init (void) //Interrupt 5 enable
{
cli(); //Clears the global interrupt
EICRB = EICRB | 0x08; // INT5 is set to trigger with falling edge
EIMSK = EIMSK | 0x20; // Enable Interrupt INT5 for right position encoder
sei(); // Enables the global interrupt
}
void buzzer_on (void)
{
unsigned char port_restore = 0;
port_restore = PINC;
port_restore = port_restore | 0x08;
PORTC = port_restore;
}
void buzzer_off (void)
{
unsigned char port_restore = 0;
port_restore = PINC;
port_restore = port_restore & 0xF7;
PORTC = port_restore;
}
//ISR for right position encoder
ISR(INT5_vect)
{
ShaftCountRight++; //increment right shaft position count
}
//ISR for left position encoder
ISR(INT4_vect)
{
ShaftCountLeft++; //increment left shaft position count
}
// Timer 5 initialized in PWM mode for velocity control
// Prescale:256
// PWM 8bit fast, TOP=0x00FF
// Timer Frequency:225.000Hz
void timer5_init()
{
TCCR5B = 0x00; //Stop
TCNT5H = 0xFF; //Counter higher 8-bit value to which OCR5xH value is compared with
TCNT5L = 0x01; //Counter lower 8-bit value to which OCR5xH value is compared with
OCR5AH = 0x00; //Output compare register high value for Left Motor
OCR5AL = 0xFF; //Output compare register low value for Left Motor
OCR5BH = 0x00; //Output compare register high value for Right Motor
OCR5BL = 0xFF; //Output compare register low value for Right Motor
OCR5CH = 0x00; //Output compare register high value for Motor C1
OCR5CL = 0xFF; //Output compare register low value for Motor C1
TCCR5A = 0xA9; /*{COM5A1=1, COM5A0=0; COM5B1=1, COM5B0=0; COM5C1=1 COM5C0=0}
For Overriding normal port functionality to OCRnA outputs.
{WGM51=0, WGM50=1} Along With WGM52 in TCCR5B for Selecting FAST PWM 8-bit Mode*/
TCCR5B = 0x0B; //WGM12=1; CS12=0, CS11=1, CS10=1 (Prescaler=64)
}
void adc_init()
{
ADCSRA = 0x00;
ADCSRB = 0x00; //MUX5 = 0
ADMUX = 0x20; //Vref=5V external --- ADLAR=1 --- MUX4:0 = 0000
ACSR = 0x80;
ADCSRA = 0x86; //ADEN=1 --- ADIE=1 --- ADPS2:0 = 1 1 0
}
//Function For ADC Conversion
unsigned char ADC_Conversion(unsigned char Ch)
{
unsigned char a;
if(Ch>7)
{
ADCSRB = 0x08;
}
Ch = Ch & 0x07;
ADMUX= 0x20| Ch;
ADCSRA = ADCSRA | 0x40; //Set start conversion bit
while((ADCSRA&0x10)==0); //Wait for conversion to complete
a=ADCH;
ADCSRA = ADCSRA|0x10; //clear ADIF (ADC Interrupt Flag) by writing 1 to it
ADCSRB = 0x00;
return a;
}
//Function To Print Sensor Values At Desired Row And Column Location on LCD
void print_sensor(char row, char coloumn,unsigned char channel)
{
ADC_Value = ADC_Conversion(channel);
lcd_print(row, coloumn, ADC_Value, 3);
}
//Function for velocity control
void velocity (unsigned char left_motor, unsigned char right_motor)
{
OCR5AL = (unsigned char)left_motor;
OCR5BL = (unsigned char)right_motor;
}
//Function used for setting motor's direction
void motion_set (unsigned char Direction)
{
unsigned char PortARestore = 0;
Direction &= 0x0F; // removing upper nibbel for the protection
PortARestore = PORTA; // reading the PORTA original status
PortARestore &= 0xF0; // making lower direction nibbel to 0
PortARestore |= Direction; // adding lower nibbel for forward command and restoring the PORTA status
PORTA = PortARestore; // executing the command
}
void forward (void) //both wheels forward
{
motion_set(0x06);
}
void back (void) //both wheels backward
{
motion_set(0x09);
}
void left (void) //Left wheel backward, Right wheel forward
{
motion_set(0x05);
}
void right (void) //Left wheel forward, Right wheel backward
{
motion_set(0x0A);
}
void soft_left (void) //Left wheel stationary, Right wheel forward
{
motion_set(0x04);
}
void soft_right (void) //Left wheel forward, Right wheel is stationary
{
motion_set(0x02);
}
void soft_left_2 (void) //Left wheel backward, right wheel stationary
{
motion_set(0x01);
}
void soft_right_2 (void) //Left wheel stationary, Right wheel backward
{
motion_set(0x08);
}
void stop (void)
{
motion_set(0x00);
}
//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)
{
if((ShaftCountRight >= ReqdShaftCountInt) | (ShaftCountLeft >= ReqdShaftCountInt))
break;
}
stop(); //Stop robot
}
//Function used for moving robot forward by specified distance
void linear_distance_mm(unsigned int DistanceInMM)
{
float ReqdShaftCount = 0;
unsigned long int ReqdShaftCountInt = 0;
ReqdShaftCount = DistanceInMM / 5.338; // division by resolution to get shaft count
ReqdShaftCountInt = (unsigned long int) ReqdShaftCount;
ShaftCountRight = 0;
while(1)
{
if(ShaftCountRight > ReqdShaftCountInt)
{
break;
}
}
stop(); //Stop robot
}
void forward_mm(unsigned int DistanceInMM)
{
forward();
linear_distance_mm(DistanceInMM);
}
void back_mm(unsigned int DistanceInMM)
{
back();
linear_distance_mm(DistanceInMM);
}
void left_degrees(unsigned int Degrees)
{
// 88 pulses for 360 degrees rotation 4.090 degrees per count
left(); //Turn left
angle_rotate(Degrees);
}
void right_degrees(unsigned int Degrees)
{
// 88 pulses for 360 degrees rotation 4.090 degrees per count
right(); //Turn right
angle_rotate(Degrees);
}
void soft_left_degrees(unsigned int Degrees)
{
// 176 pulses for 360 degrees rotation 2.045 degrees per count
soft_left(); //Turn soft left
Degrees=Degrees*2;
angle_rotate(Degrees);
}
void soft_right_degrees(unsigned int Degrees)
{
// 176 pulses for 360 degrees rotation 2.045 degrees per count
soft_right(); //Turn soft right
Degrees=Degrees*2;
angle_rotate(Degrees);
}
void soft_left_2_degrees(unsigned int Degrees)
{
// 176 pulses for 360 degrees rotation 2.045 degrees per count
soft_left_2(); //Turn reverse soft left
Degrees=Degrees*2;
angle_rotate(Degrees);
}
void soft_right_2_degrees(unsigned int Degrees)
{
// 176 pulses for 360 degrees rotation 2.045 degrees per count
soft_right_2(); //Turn reverse soft right
Degrees=Degrees*2;
angle_rotate(Degrees);
}
//Function To Initialize UART2
// desired baud rate:9600
// actual baud rate:9600 (error 0.0%)
// char size: 8 bit
// parity: Disabled
void uart2_init(void)
{
UCSR2B = 0x00; //disable while setting baud rate
UCSR2A = 0x00;
UCSR2C = 0x06;
UBRR2L = 0x5F; //set baud rate lo
UBRR2H = 0x00; //set baud rate hi
UCSR2B = 0x98;
}
SIGNAL(SIG_USART2_RECV) { // ISR for receive complete interrupt
data = UDR2; // making copy of data from UDR2 in 'data' variable
UDR2 = data; // echo data back to PC
strcat(input_str, &data); // concatenate each ascii character received to string input_str
//GLCD_DisplayChar(data);
}
void init_devices (void)
{
cli(); //Clears the global interrupts
port_init();
lcd_set_4bit();
lcd_init();
GLCD_Init();
uart2_init(); //Initialize UART2 for serial communication
adc_init();
timer5_init();
left_position_encoder_interrupt_init();
right_position_encoder_interrupt_init();
sei(); //Enables the global interrupts
}
// #########################################################################################################
// ################################ my functions and variables start #######################################
// #########################################################################################################
// ################################ declaring global variables start #######################################
// mapping / setting cell co-ordinates in D1
// 12 cells, 4 points in each cell, 2 index for row-column in each point
// 4 points for each cell are sorted in this order: top-left, top-right, bottom-right, bottom-left
int d1_position_map[12][4][2] = {
{{0, 0}, {0, 1}, {1, 1}, {1, 0}}, {{0, 1}, {0, 2}, {1, 2}, {1, 1}}, {{0, 2}, {0, 3}, {1, 3}, {1, 2}}, {{0, 3}, {0, 4}, {1, 4}, {1, 3}},
{{1, 0}, {1, 1}, {2, 1}, {2, 0}}, {{1, 1}, {1, 2}, {2, 2}, {2, 1}}, {{1, 2}, {1, 3}, {2, 3}, {2, 2}}, {{1, 3}, {1, 4}, {2, 4}, {2, 3}},
{{2, 0}, {2, 1}, {3, 1}, {3, 0}}, {{2, 1}, {2, 2}, {3, 2}, {3, 1}}, {{2, 2}, {2, 3}, {3, 3}, {3, 2}}, {{2, 3}, {2, 4}, {3, 4}, {3, 3}}
};
// mapping / setting cell co-ordinates in D2
// 24 cells, 4 points in each cell, 2 index for row-column in each point
// 4 points for each cell are sorted in this order: top-left, top-right, bottom-right, bottom-left
int d2_position_map[24][4][2] = {
{{0, 0}, {0, 1}, {1, 1}, {1, 0}}, {{0, 1}, {0, 2}, {1, 2}, {1, 1}}, {{0, 2}, {0, 3}, {1, 3}, {1, 2}}, {{0, 3}, {0, 4}, {1, 4}, {1, 3}}, {{0, 4}, {0, 5}, {1, 5}, {1, 4}}, {{0, 5}, {0, 6}, {1, 6}, {1, 5}},
{{1, 0}, {1, 1}, {2, 1}, {2, 0}}, {{1, 1}, {1, 2}, {2, 2}, {2, 1}}, {{1, 2}, {1, 3}, {2, 3}, {2, 2}}, {{1, 3}, {1, 4}, {2, 4}, {2, 3}}, {{1, 4}, {1, 5}, {2, 5}, {2, 4}}, {{1, 5}, {1, 6}, {2, 6}, {2, 5}},
{{2, 0}, {2, 1}, {3, 1}, {3, 0}}, {{2, 1}, {2, 2}, {3, 2}, {3, 1}}, {{2, 2}, {2, 3}, {3, 3}, {3, 2}}, {{2, 3}, {2, 4}, {3, 4}, {3, 3}}, {{2, 4}, {2, 5}, {3, 5}, {3, 4}}, {{2, 5}, {2, 6}, {3, 6}, {3, 5}},
{{3, 0}, {3, 1}, {4, 1}, {4, 0}}, {{3, 1}, {3, 2}, {4, 2}, {4, 1}}, {{3, 2}, {3, 3}, {4, 3}, {4, 2}}, {{3, 3}, {3, 4}, {4, 4}, {4, 3}}, {{3, 4}, {3, 5}, {4, 5}, {4, 4}}, {{3, 5}, {3, 6}, {4, 6}, {4, 5}}
};
unsigned char current_velocity = 127; // value can be 0-255, default velocity 127
char current_direction = 'N'; // value can be E/W/N/S i.e. east, west, north or south respectively, Default N
char pickup_direction = '\0'; // value can be L or R i.e. left or right respectively
int current_grid = -1; // value can be 1 or 2 i.e. D1 or D2, initially -1 as invalid
int current_cell_no = -1; // value can be 0 to 11 for D1, 0-23 for D2, initially -1 as invalid
int current_coordinate[2] = {-1, -1}; // co-ordinate of the cell i.e. row-column number from position map, initially -1, -1 as invalid
int BNW_Thresh = 40; // black and white threshold value, default 28 [formula: ((W+B)/2)-(B/3)]
float left_velocity_float, right_velocity_float; // stores velocity of left and right wheel respectively as float
unsigned char left_velocity, right_velocity; // stores velocity of left and right wheel respectively as unsigned char
// ################################ declaring global variables end #########################################
/*
* Function Name: left_led_on
* Input: None
* Output: It turns on the left RGB LED
* Logic: PORT G0 is configured for left RGB LED, writing logic high to PG0
* Example Call: left_led_on()
*/
void left_led_on (void) {
unsigned char port_restore = 0;
port_restore = PING;
port_restore |= 0b00000001;
PORTG = port_restore;
}
/*
* Function Name: left_led_off
* Input: None
* Output: It turns of the left RGB LED
* Logic: PORT G0 is configured for left RGB LED, writing logic low to PG0
* Example Call: left_led_on()
*/
void left_led_off (void) {
unsigned char port_restore = 0;
port_restore = PING;
port_restore &= 0b11111110;
PORTG = port_restore;
}
/*
* Function Name: right_led_on
* Input: None
* Output: It turns on the right RGB LED
* Logic: PORT G1 is configured for right RGB LED, writing logic high to PG1
* Example Call: right_led_on()
*/
void right_led_on (void) {
unsigned char port_restore = 0;
port_restore = PING;
port_restore |= 0b00000010;
PORTG = port_restore;
}
/*
* Function Name: right_led_off
* Input: None
* Output: It turns on the right RGB LED
* Logic: PORT G1 is configured for right RGB LED, writing logic low to PG1
* Example Call: right_led_off()
*/
void right_led_off (void) {
unsigned char port_restore = 0;
port_restore = PING;
port_restore &= 0b11111101;
PORTG = port_restore;
}
/*
* Function Name: get_point_cost
* Input: current_point - int array containing row & column number of current coordinate point,
* target_point - int array containing row & column number of target coordinate point
* Output: total_cost - int which is the calculated cost between current and target coordinate point
* Logic: It calculates the cost of traveling between two points i.e. calculating row difference and
* column difference and make sum of them
* Example Call: get_point_cost(current_point, target_point)
*/
int get_point_cost (int current_point[2], int target_point[2]) {
int total_cost;
total_cost = abs(current_point[0] - target_point[0]) + abs(current_point[1] - target_point[1]);
return total_cost;
}
// It receives current point and target cell which contains 4 points and returns nearest point from current point among those 4 points
/*
* Function Name: get_nearest_point
* Input: current_point - int array containing row & column number of current coordinate point,
* target_cell - 2D int array contains target cell's 4 points with coordinate based on row & column
* Output: nearest_point - int array containing row & column number of nearest coordinate point among 4 points of target_cell
* Logic: We are calculating traveling cost i.e. distance between current point and each of target cell's 4 points (top-left,
* top-right, bottom-right, bottom-left) and returning that point with lowest cost
* Example Call: get_nearest_point(current_coordinate, d1_position_map[target_cell_no])
*/
int * get_nearest_point (int current_point[2], int target_cell[4][2]) {
int * nearest_point = malloc(2 * sizeof(int));
int i, current_cost, lowest_cost = 100;
for (i=0; i<4; i++) {
current_cost = get_point_cost(current_point, target_cell[i]);
if (current_cost < lowest_cost) {
nearest_point[0] = target_cell[i][0];
nearest_point[1] = target_cell[i][1];
lowest_cost = current_cost;
}
}
return nearest_point;
}
// It follows a 1cm thick black line on white surface
/*
* Function Name: read_wl_sensor_values
* Input: Global variables Left_white_line, Center_white_line, Right_white_line
* Output: It updates global variables Left_white_line, Center_white_line, Right_white_line
* Logic: It reads all 3 white line sensor values by using function ADC_Conversion()
* Example Call: read_wl_sensor_values()
*/
void read_wl_sensor_values () {
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
}
/*
* Function Name: follow_black_line
* Input: Left_white_line, Center_white_line, Right_white_line are global variables,
* direction can be 'F' or 'B' as forward or backward respectively
* Output: It follows a 1cm thick black line on white surface
* Logic: To follow a 1cm black line, we can linearly forward the robot if all three white line sensors
* are on white surface or the center white line sensor is on the black surface. We decrease velocity of the
* right wheel and increase velocity of the left wheel if right white line sensor is on the black surface and
* other twos are on the white surface. Similarly we decrease velocity of the left wheel and increase velocity
* of the right wheel if left white line sensor is on the black surface and other twos are on the white surface.
* This way the robot follows a 1cm thick black line.
*
* Example Call: follow_black_line('F')
*/
void follow_black_line (char direction) {
flag = 0;
if (((Left_white_line <= BNW_Thresh) && (Center_white_line <= BNW_Thresh) && (Right_white_line <= BNW_Thresh)) || (Center_white_line > BNW_Thresh)) {
flag = 1;
if (direction == 'F') forward();
else back();
velocity(left_velocity, right_velocity);
}
if((Left_white_line <= BNW_Thresh) && (Center_white_line <= BNW_Thresh) && (Right_white_line > BNW_Thresh) && (flag == 0)) {
flag = 1;
if (direction == 'F') {
forward();
velocity(left_velocity+30, right_velocity-50);
} else {
back();
velocity(left_velocity-50, right_velocity+30);
}
}
if((Left_white_line > BNW_Thresh) && (Center_white_line <= BNW_Thresh) && (Right_white_line <= BNW_Thresh) && (flag == 0)) {
flag = 1;
if (direction == 'F') {
forward();
velocity(left_velocity-50, right_velocity+30);
} else {
back();
velocity(left_velocity+30, right_velocity-50);
}
}
}
// It follows a black line up to a fixed distance
/*
* Function Name: follow_black_line_mm
* Input: DistanceInMM - is a unsigned integer,
* direction - is a char variable
* Output: It follows a 1cm thick black line up to a specific distance
* Logic: It calculates required shaft count to travel up to DistanceInMM
* and follows the black line. ShaftCountRight and ShaftCountLeft are initially
* set to 0. While moving the robot, we increase them and When the ShaftCountRight
* or ShaftCountLeft reaches as required, we stop the robot
* Example Call: follow_black_line_mm(100, 'F')
*/
void follow_black_line_mm (unsigned int DistanceInMM, char direction) {
float ReqdShaftCount = 0;
unsigned long int ReqdShaftCountInt = 0;
ReqdShaftCount = DistanceInMM / 5.338; // division by resolution to get shaft count
ReqdShaftCountInt = (unsigned long int) ReqdShaftCount;
ShaftCountRight = 0;
ShaftCountLeft = 0;
while(1) {
read_wl_sensor_values();
if(ShaftCountRight > ReqdShaftCountInt || ShaftCountLeft > ReqdShaftCountInt) {
break;
} else {
follow_black_line(direction);
}
}
stop(); //Stop robot
}
// It turns the robot left/right by 5 degrees until encounters a black line.
/*
* Function Name: turn_robot
* Input: direction is a char variable which can be 'L' or 'R' as left or right respectively
* Output: This function rotates the robot perfect 90 degrees left or right
* Logic: At first it reads all white line sensor values and increase velocity to current velocity + 10
* left_degrees(90)3is static and does not always as per hardware and physical limitations.
* So, we have to rotate the robot dynamically. We rotate the robot by 5 degrees until
* the center white line sensor detects a black line. But to do that we have to cross/overcome
* current black line and the thick black number inside a cell. If direction is 'L' then
* we rotate the robot left by 75 degrees using left_degrees(75) and if direction is 'R'
* then rotate the robot right by 75 degrees using right_degrees(75). After that, we rotate
* the robot by 5 degrees in mentioned direction until the center white line sensor detects
* a black line.
* Example Call: turn_robot('L')
*/
void turn_robot (char direction) {
read_wl_sensor_values();
velocity(current_velocity+10, current_velocity+10);
if (direction == 'L') {
left_degrees(75);
read_wl_sensor_values();
while (Center_white_line <= BNW_Thresh) {
read_wl_sensor_values();
left_degrees(5);
}
} else {
right_degrees(75);
read_wl_sensor_values();
while (Center_white_line <= BNW_Thresh) {
read_wl_sensor_values();
right_degrees(5);
}
}
velocity(current_velocity-10 ,current_velocity-10);
_delay_ms(500);
}
// It changes the direction of the robot i.e. east/west/north/south.
/*
* Function Name: change_direction
* Input: desired_direction is a unsigned char variable which can be 'E'/'W'/'N'/'S' as
* ease, west, north or south respectively
* Output: It changes the robots direction to ease, west, north or south and updates its current
* direction which is stored in global variable current_direction
* Logic: if current direction is north and desired direction is west then
* we turn the robot left by 90 degrees using turn_robot('L') and updates current direction to west
* if current direction is north and desired direction is east then
* we turn the robot right by 90 degrees using turn_robot('R') and updates current direction to east
* if current direction is north and desired direction is south then
* if current column is 0 then we turn the robot right by 90 degrees using turn_robot('R') two times
* else we turn the robot left by 90 degrees using turn_robot('L') two times
* and update current direction to south
* if current direction is south and desired direction is north then
* if current column is 0 then we turn the robot left by 90 degrees using turn_robot('L') two times
* else we turn the robot right by 90 degrees using turn_robot('R') two times
* and update current direction to north
* if current direction is south and desired direction is east then
* we turn the robot left by 90 degrees using turn_robot('L') and updates current direction to east
* if current direction is south and desired direction is west then
* we turn the robot right by 90 degrees using turn_robot('R') and updates current direction to west
* if current direction is east and desired direction is north then
* we turn the robot left by 90 degrees using turn_robot('L') and updates current direction to north
* if current direction is east and desired direction is west then
* if current row is 0 then we turn the robot right by 90 degrees using turn_robot('R') two times
* else we turn the robot left by 90 degrees using turn_robot('L') two times
* and update current direction to west
* if current direction is east and desired direction is south then
* we turn the robot right by 90 degrees using turn_robot('R') and updates current direction to south
* if current direction is west and desired direction is north then
* we turn the robot right by 90 degrees using turn_robot('R') and updates current direction to north
* if current direction is west and desired direction is east then
* if current row is 0 then we turn the robot left by 90 degrees using turn_robot('L') two times
* else we turn the robot right by 90 degrees using turn_robot('R') two times
* and update current direction to east
* if current direction is west and desired direction is south then
* we turn the robot left by 90 degrees using turn_robot('L') and updates current direction to south
* Example Call: change_direction('N')
*/
void change_direction (unsigned char desired_direction) {
if (current_direction == desired_direction) return;
if (current_direction == 'N' && desired_direction == 'W') { // north
turn_robot('L');
current_direction = 'W';
} else if (current_direction == 'N' && desired_direction == 'E') { // north
turn_robot('R');
current_direction = 'E';
} else if (current_direction == 'N' && desired_direction == 'S') { // north
if (current_coordinate[1] == 0) {
turn_robot('R');
turn_robot('R');
} else {
turn_robot('L');
turn_robot('L');
}
current_direction = 'S';
} else if (current_direction == 'S' && desired_direction == 'N') { //south
if (current_coordinate[1] == 0) {
turn_robot('L');
turn_robot('L');
} else {
turn_robot('R');
turn_robot('R');
}
current_direction = 'N';
} else if (current_direction == 'S' && desired_direction == 'E') { //south
turn_robot('L');
current_direction = 'E';
} else if (current_direction == 'S' && desired_direction == 'W') { //south
turn_robot('R');
current_direction = 'W';
} else if (current_direction == 'E' && desired_direction == 'N') { //east
turn_robot('L');
current_direction = 'N';
} else if (current_direction == 'E' && desired_direction == 'W') { //east
if (current_coordinate[0] == 0) {
turn_robot('R');
turn_robot('R');
} else {
turn_robot('L');
turn_robot('L');
}
current_direction = 'W';
} else if (current_direction == 'E' && desired_direction == 'S') { //east
turn_robot('R');
current_direction = 'S';
} else if (current_direction == 'W' && desired_direction == 'N') { //west
turn_robot('R');
current_direction = 'N';
} else if (current_direction == 'W' && desired_direction == 'E') { //west
if (current_coordinate[0] == 0) {
turn_robot('L');
turn_robot('L');
} else {
turn_robot('R');
turn_robot('R');
}
current_direction = 'E';
} else if (current_direction == 'W' && desired_direction == 'S') { //west
turn_robot('L');
current_direction = 'S';
}
}
// It follows a black line until encounter a 3x3 cm black square.
/*
* Function Name: move_one_cell
* Input: Reads global variables Left_white_line, Center_white_line, Right_white_line, BNW_Thresh
* Output: It moved the robot 1 cell in forward direction i.e. robot forwards until get a 3x3 black square
* Logic: If right and center white line sensors are on black surface and left white line sensor is on white
* surface, we also detect it as a 3x3cm black square.
* If left and center white line sensors are on the black surface and right white line sensor is on
* white surface, we detect it as a 3x3cm black square.
* Again, if all three white line sensors are on black surface, we also detect it as a 3x3cm black square.
* After all, forward the robot 5 cm to make the wheels on the 3x3cm black squares.
* Example Call: move_one_cell()
*/
void move_one_cell () {
read_wl_sensor_values();
while (!(((Left_white_line > BNW_Thresh) && (Center_white_line > BNW_Thresh)) || ((Center_white_line > BNW_Thresh) && (Right_white_line > BNW_Thresh)) // 1-2 or 3-2 on white
|| ((Left_white_line > BNW_Thresh) && (Center_white_line > BNW_Thresh) && (Right_white_line > BNW_Thresh)))) { // center on black
read_wl_sensor_values();
follow_black_line('F');
}
// this delay is important: if disabled, the bot may get our of line
_delay_ms(200); // delay
// adjust 10/2 cm forward to make the wheels on the 3x3cm black squares.
follow_black_line_mm(50, 'F');
}
// It is a helper function, used to match columns.
/*
* Function Name: match_column
* Input: target_coordinate is a int array which holds a co-ordinate i.e. row, column number
* Output: It updates current co-ordinate's column number i.e. the global variable current_coordinate[1]
* Logic: Until current co-ordinate's column > target co-ordinates's column, we repeat the process indented below
* change direction to west by using change_direction('W') and then move the robot by one cell
* forward by using move_one_cell(). After that we update current co-ordinate's column number by decreasing 1
* All after above, Until current co-ordinate's column < target co-ordinates's column, we repeat the process indented below
* change direction to east by using change_direction('E') and then move the robot by one cell
* forward by using move_one_cell(). After that we update current co-ordinate's column number by increase 1
* Example Call: match_column((int){1, 3})
*/
void match_column (int target_coordinate[]) {
while (current_coordinate[1] > target_coordinate[1]) {
change_direction('W');
move_one_cell();
current_coordinate[1]--;
}
while (current_coordinate[1] < target_coordinate[1]) {// go east/west until both position on same column
change_direction('E');
move_one_cell();
current_coordinate[1]++;
}
}
// It is a helper function, used to match rows.
/*
* Function Name: match_row
* Input: target_coordinate is a int array which holds a co-ordinate i.e. row, column number
* Output: It updates current co-ordinate's row number i.e. the global variable current_coordinate[0]
* Logic: Until current co-ordinate's row > target co-ordinates's row, we repeat the process indented below
* change direction to north by using change_direction('N') and then move the robot by one cell
* forward by using move_one_cell(). After that we update current co-ordinate's column number by decreasing 1
* All after above, Until current co-ordinate's row < target co-ordinates's row, we repeat the process indented below
* change direction to south by using change_direction('S') and then move the robot by one cell
* forward by using move_one_cell(). After that we update current co-ordinate's column number by increase 1
* Example Call: match_row((int){1, 3})
*/
void match_row (int target_coordinate[]) {
while (current_coordinate[0] > target_coordinate[0]) {// go north/south until both position on same row
change_direction('N');
move_one_cell();
current_coordinate[0]--;
}
while (current_coordinate[0] < target_coordinate[0]) {// go north/south until both position on same row
change_direction('S');
move_one_cell();
current_coordinate[0]++;
}
}
// It is used to move the robot to a particular coordinate.
// constraint: when we are in D1, then we cannot put co-ordinate of D2 and vice-versa. To do that, first cross the bridge.
/*
* Function Name: go_to_coordinate
* Input: target_coordinate is a int array which holds a co-ordinate i.e. row, column number
* Output: It moves the robot to a specific co-ordinate by using match_column and match_row functions
* Logic: If the robot is currently in D1 then
* If current direction is east or west then
* move the robot and match robot's and target co-ordinate's column
* move the robot and match robot's and target co-ordinate's row
* else
* move the robot and match robot's and target co-ordinate's row
* move the robot and match robot's and target co-ordinate's column
* else
* If target co-ordinate's row number is 2 and column number is 0 then
* move the robot and match robot's and target co-ordinate's row
* move the robot and match robot's and target co-ordinate's column
* else
* move the robot and match robot's and target co-ordinate's column
* move the robot and match robot's and target co-ordinate's row
* Example Call: go_to_coordinate((int){1, 3})
*/
void go_to_coordinate (int target_coordinate[]) {
if (current_grid == 1) {
if (current_direction == 'E' || current_direction == 'W') { // match column then row
match_column(target_coordinate);
match_row(target_coordinate);
} else { // current_direction = N/S // match row then column
match_row(target_coordinate);
match_column(target_coordinate);
}
} else { // if robot is in d2, and target is bridge point, match row, then column
if (target_coordinate[0] == 2 && target_coordinate[1] == 0) {
match_row(target_coordinate);
match_column(target_coordinate);
} else {
match_column(target_coordinate);
match_row(target_coordinate);
}
}
}
// It is used to go to nearest co-ordinate point of a cell from current point's co-ordinate.
/*
* Function Name: go_to_cell_no
* Input: target_division - is an int variable which can be 1 or 2 as D1 or D2 respectively,
* target_cell_no - is an int variable which can be 0-11 in D1 and 0-23 in D2
* Output: It moves the robot to nearest among 4 points (top-left, top-right, bottom-right, bottom-left)
* of a given cell number with target division
* Logic: It creates an integer pointer to store a co-ordinate. Then if the target division is 1 i.e. D1
* then copy returned data from get_nearest_point(current_coordinate, d1_position_map[target_cell_no])
* to nearest_point. Note that we are using d1_position_map here
* else copy returned data from get_nearest_point(current_coordinate, d2_position_map[target_cell_no])
* to nearest_point. Note that we are using d2_position_map here this time
* Example Call: go_to_cell_no(2, 5)
*/
void go_to_cell_no (int target_division, int target_cell_no) {
int * nearest_point = (int *) malloc(2 * sizeof(int));
if (target_division == 1) { // go to cell no in D1
memcpy(nearest_point, get_nearest_point(current_coordinate, d1_position_map[target_cell_no]), 2 * sizeof(int));
} else { // go to cell no in D2
memcpy(nearest_point, get_nearest_point(current_coordinate, d2_position_map[target_cell_no]), 2 * sizeof(int));
}
go_to_coordinate(nearest_point);
// after reaching, update current_cell_no
current_cell_no = target_cell_no;
}
// It is used to get the pickup direction i.e. L or R i.e. left or right respectively.
/*
* Function Name: get_pickup_direction
* Input: Global variable current_direction.
* Output: It updates the global variable pickup_direction which can be 'L' or 'R' as left or right respectively
* Logic: Integer left and right is initialized with 0
* if current direction of the robot is north then
* i = 0
* if i < 4 then
* if current co-ordinate's column number > d1_position_map[current_cell_no][i][1] then left = left + 1
* else current co-ordinate's column number < d1_position_map[current_cell_no][i][1] then right = right + 1
* i = i + 1
* if current direction of the robot is south then
* i = 0
* if i < 4 then
* if current co-ordinate's column number > d1_position_map[current_cell_no][i][1] then right = right + 1
* else current co-ordinate's column number < d1_position_map[current_cell_no][i][1] then left = left + 1
* i = i + 1
* if current direction of the robot is east then
* i = 0
* if i < 4 then
* if current co-ordinate's row number > d1_position_map[current_cell_no][i][0] then left = left + 1
* else current co-ordinate's row number < d1_position_map[current_cell_no][i][0] then right = right + 1
* i = i + 1
* if current direction of the robot is west then
* i = 0
* if i < 4 then
* if current co-ordinate's row number > d1_position_map[current_cell_no][i][0] then right = right + 1
* else current co-ordinate's row number < d1_position_map[current_cell_no][i][0] then left = left + 1
* i = i + 1
* if left > right then set pickup direction to 'L' as left
* else set pickup direction to 'R' as right
* Example Call: get_pickup_direction()
*/
void get_pickup_direction () {
int i, left = 0, right = 0;
if (current_direction == 'N') { // if north, just compare columns
for (i=0; i<4; i++) {
if (current_coordinate[1] > d1_position_map[current_cell_no][i][1]) left++;
else if (current_coordinate[1] < d1_position_map[current_cell_no][i][1]) right++;
}
} else if (current_direction == 'S') { // if south, just compare columns
for (i=0; i<4; i++) {
if (current_coordinate[1] > d1_position_map[current_cell_no][i][1]) right++;
else if (current_coordinate[1] < d1_position_map[current_cell_no][i][1]) left++;
}
} else if (current_direction == 'E') { // if east, just compare rows
for (i=0; i<4; i++) {
if (current_coordinate[0] > d1_position_map[current_cell_no][i][0]) left++;
else if (current_coordinate[0] < d1_position_map[current_cell_no][i][0]) right++;
}
} else { // if west, just compare rows
for (i=0; i<4; i++) {
if (current_coordinate[0] > d1_position_map[current_cell_no][i][0]) right++;
else if (current_coordinate[0] < d1_position_map[current_cell_no][i][0]) left++;
}
}
if (left > right) pickup_direction = 'L';
else pickup_direction = 'R';
}
// It is used to pickup a number from D1.
/*
* Function Name: pickup
* Input: num - is an integer number which can be 0-9 present in D1,
* skip_over - is an integer number which can be 0 or 1. It decides move forward of backward after pickup.
* Output: It picks up a number from D1 by forwarding the robot half of the cell and then pickup by
* glowing left or right RGB LEDs and showing the number in the GLCD and after pickup the
* robot forward and skip over the cell or backwards and returns to previous node as per travel cost
* Logic: Then we forward the robot by 50 mm to get to the center of the cell before pickup a number
* the we calculate pickup direction.
* If pickup direction is left, then turn on left RGB LED
* Else, turn on right RGB LED
* After that clear GLCD screen and display the number we have picked up and delay for 500 ms
* Then, if skip_over is equals to 1 then call move_one_cell()
* else follow black line forward for 25 mm by follow_black_line_mm(25, 'B')
* Example Call: pickup(9, 1)
*/
void pickup (int num, int skip_over) {
follow_black_line_mm(50, 'F');
get_pickup_direction();
// turn on the left/right led
if (pickup_direction == 'L') left_led_on();
else right_led_on();
GLCD_Clear();
GLCD_DisplayBitmap(num); // show the num in big ubuntu font
_delay_ms(500);
if (skip_over == 1) {
move_one_cell();
} else {
follow_black_line_mm(25, 'B');
}
}
// It is used to deposit a number in D2.
/*
* Function Name: deposit
* Input: completed - is an integer variable which can be 0 or 1. It determines if a number in D2 is completed or not,
* isEnd - is an integer variable which can be 0 or 1. It determines if robot has ended the whole task or not.
* Output: It deposits the number that picked from D1 to a cell in D2 by maintaining pickup and deposit direction equal.
* While depositing a number, the robot turns the RGB LED off, shows a deposit message on GLCD and a buzzes for
* for 1000ms if a number in D2 is completed, and continuous buzzer if whole task is completed.
* Logic: If current_coordinate is on top left of the cell then
* If pickup direction is left then change robot's direction to south
* else change robot's direction to east
* If current_coordinate is on top right of the cell then
* If pickup direction is left then change robot's direction to west
* else change robot's direction to south
* If current_coordinate is on bottom right of the cell then
* If pickup direction is left then change robot's direction to north
* else change robot's direction to west
* If current_coordinate is on bottom left of the cell then
* If pickup direction is left then change robot's direction to east
* else change robot's direction to north
* After that, follow black line for 50 mm to get to the center of the cell
* If pickup direction is left, then turn off the left RGB LED, else turn off the right RGB LED
* Clear the GLCD and show the deposit message
* If completed is equal to 1 and isEnd is equal to 0 then turn on buzzer for 1000 ms
* else, if completed is equal to 1 and isEnd is equal to 1 turn on buzzer for lifetime
* else delay for 1000 ms
* After all these steps, clear the GLCD
* If isEnd is equal to 0 then follow black line in backward direction for 25 mm
* Example Call: deposit(1, 0)
*/
void deposit (int completed, int isEnd) {
if ((current_coordinate[0] == d2_position_map[current_cell_no][0][0]) && // current_coordinate is on top left of the cell
(current_coordinate[1] == d2_position_map[current_cell_no][0][1])) {
if (pickup_direction == 'L') change_direction('S');
else change_direction('E');
} else if ((current_coordinate[0] == d2_position_map[current_cell_no][1][0]) && // current_coordinate is on top right of the cell
(current_coordinate[1] == d2_position_map[current_cell_no][1][1])) {
if (pickup_direction == 'L') change_direction('W');
else change_direction('S');
} else if ((current_coordinate[0] == d2_position_map[current_cell_no][2][0]) && // current_coordinate is on bottom right of the cell
(current_coordinate[1] == d2_position_map[current_cell_no][2][1])) {
if (pickup_direction == 'L') change_direction('N');
else change_direction('W');
} else if ((current_coordinate[0] == d2_position_map[current_cell_no][3][0]) && // current_coordinate is on bottom left of the cell
(current_coordinate[1] == d2_position_map[current_cell_no][3][1])) {
if (pickup_direction == 'L') change_direction('E');
else change_direction('N');
}
follow_black_line_mm(50, 'F');
//turn off led
if (pickup_direction == 'L') left_led_off();
else right_led_off();
GLCD_Clear();
GLCD_DisplayBitmap(-1); // show DEPOSIT message in big font
if (completed == 1 && isEnd == 0) { // 1000ms buzzer if number in D2 completed but task is not
buzzer_on();
_delay_ms(1000);
buzzer_off();
} else if (completed == 1 && isEnd == 1) { // the robot has finished the task
// continuous buzzer
buzzer_on();
while(1);
} else {
_delay_ms(1000);
}
GLCD_Clear();
if (isEnd == 0) { // the robot has not finished the task yet
follow_black_line_mm(25, 'B');
}
}
// my functions and variables end ##########################################################################
//Main Function start ######################################################################################
/*
* Function Name: main
* Input: input_str - is a string which is a global variable. It contains comma separated numbers.
* Output: int to inform the caller that the program exited correctly or incorrectly (C code standard)
* Logic: At first, we initialize devices. Then we make the robot busy until pressing interrupt key.
* If interrupt key is pressed then initialize integer array path_points -1 set to all elements.
* Then we convert the comma separated string input_str and store into integer array path_points.
* Set current_velocity = 135 and synchronize wheel speed if needed and set synchronized velocity.
* Now, follow from the SRART position, follow black line for 50 mm to skip thick black line of START.
* Then move one cell and update robots current grid to 1, direction to north, and position
* coordinate to [3, 2]. After that start traversal on the grids. Now set variable j = 0,
* and sum = 0 initially. Now iterate through i in range from 0 to 99, follow the indented steps below:
* If i th element in path_points is not equal to -1, then do the following indented steps below:
* If j is even, then do the following indented steps below:
* sum = sum + (i+1) th element in path_points. (target cell is in D1 i.e. pickup operation)
* if current_grid is 1 i.e. robot is already in D1, then do the following indented steps below:
* Go to the cell no which is the i th element of path_points.
* if current direction is east of (current direction is north and current row is 3) then:
* Pickup i+1 th element of path_points i.e. number in D1 and skip over the cell.
* If current direction is north, then update current row = current row - 1
* If current direction is east, then update current column = current column + 1
* Else, pickup i+1 th element of path_points i.e. number in D1 and move back to previous node.
* Else, do as the following indention:
* Move to bridge point (2, 0) in D2.
* Go to west one cell.
* Update current grid to 1 i.e. D1 and current coordinate = (2, 4) (D1 bridge point)
* Update current coordinate to (2, 4).
* Go to cell no in D1 which is i th element of path_points.
* Pickup i+1 th element of path_points i.e. number in D1 and move back to previous node.
* Else (j is odd i.e. position is in D2), as as the following indention:
* Move to bridge point (2, 4) in D1.
* Go east one cell.
* Update current_grid=2 and current_coordinate=(2, 0) (D2 bridge point).
* Update current_coordinate to (2, 0).
* Go to cell no in D2 which is i th element of path_points.
* If sum is equal to i+1 th element of path_points, then do as following indention:
* If i+2 th element of path_points is equal to -1 then do as following indention:
* Deposit as number in D2 is completed and whole task is completed.
* Else, do as following indention:
* Deposit as number in D2 is completed but whole task is not completed.
* sum = 0
* Else, deposit as number in D2 is not completed and whole task is not completed as well.
* If i+2 th element in path_points is -1, then continuous buzzer as robot has finished the task.
* Example Call: Called automatically by the Operating System
*/
int main() {
init_devices();
// make the robot busy until detecting boot switch i.e. interrupt is pressed
while (1) {
if((PINE | 0x7F) == 0x7F) { // interrupt switch is pressed
//GLCD_Clear();
break;
}
}
/*************************** converting input string to int array start ****************************/
// it must be done after pressing the interrupt key else data will be lost
int i, j;
int path_points[100]; // -1 refers to invalid/null point; odd index = the point is in D1, even index = the point is in D2
for (i=0; i<100; i++) path_points[i] = -1;
char * token;
token = strtok(input_str, ",");
i = 0;
while (token != '\0') {
path_points[i++] = atoi((char *)token);
token = strtok('\0', ",");
}
/*************************** converting input string to int array end ******************************/
// set velocity
current_velocity = 135; // 127 on full charge
// synchronize wheels
// suppose left wheel is physically 7.18% slower than the right wheel, so increase velocity of left wheel
left_velocity_float = current_velocity + current_velocity * 0/100.0; // replace 0 with patch
right_velocity_float = current_velocity + current_velocity * 0/100.0; // replace 0 with patch
left_velocity = (unsigned char) left_velocity_float;
right_velocity = (unsigned char) right_velocity_float;
// go to 9th cell from start
velocity(left_velocity, right_velocity);
follow_black_line_mm(50, 'F');
move_one_cell();
_delay_ms(500);
current_grid = 1;
current_direction = 'N';
current_coordinate[0] = 3;
current_coordinate[1] = 2;
// start traversal
// iterate through all positions
// to get value, add 1 to i, e.g.: path_points[i+1]
// use j to count iteration number, even value of j = position is in D1, odd value of j = position is in D2
j = 0; // iteration counter
int sum = 0; // used to track when a number in D2 get completed
for (i=0; i<100; i+=2) {
if (path_points[i] != -1) {
if (j%2 == 0) { // j is even i.e. position is in D1
// target cell is in D1 i.e. pickup operation
sum += path_points[i+1]; // add D1 number's value to sum
if (current_grid == 1) { // if robot is already in D1
go_to_cell_no(1, path_points[i]);
// decides just pickup ad back or pickup and forward
if (current_direction == 'E' || (current_direction == 'N' && current_coordinate[0] == 3)) {
pickup(path_points[i+1], 1);
if (current_direction == 'N') current_coordinate[0]--; // current row = current row - 1
if (current_direction == 'E') current_coordinate[1]++; // current column = current column + 1
} else {
pickup(path_points[i+1], 0);
}
} else { // robot is in D2, need to cross the bridge to go to D1
// 1. move to bridge point (2, 0) in D2
go_to_coordinate((int[]){2, 0});
// 2. go to west one cell
change_direction('W');
move_one_cell();
// 3. update current_grid=1 and current_coordinate=(2, 4) (D1 bridge point)
current_grid = 1;
// update current_coordinate to (2, 4)
current_coordinate[0] = 2;
current_coordinate[1] = 4;
// 4. go_to_cell_no()
go_to_cell_no(1, path_points[i]);
// 5. pickup
pickup(path_points[i+1], 0);
}
} else { // j is odd i.e. position is in D2
// target cell is in D2 i.e. deposit operation
// 1. move to bridge point (2, 4) in D1
go_to_coordinate((int[]){2, 4});
// 2. go east one cell
change_direction('E');
move_one_cell();
// 3. update current_grid=2 and current_coordinate=(2, 0) (D2 bridge point)
current_grid = 2;
// update current_coordinate to (2, 0)
current_coordinate[0] = 2;
current_coordinate[1] = 0;
// 4. go_to_cell_no()
go_to_cell_no(2, path_points[i]);
// 5. deposit
if (sum == path_points[i+1]) {
if (path_points[i+2] == -1) {
deposit(1, 1); // 1 = number in D2 is completed, 1 = task completed respectively
} else {
deposit(1, 0); // 1 = number in D2 is completed, 0 = task not completed respectively
}
sum = 0;
} else {
deposit(0, 0); // 0 = number in D2 is not completed, 0 = task not completed respectively
}
// it should be deleted before video submission/final
if (path_points[i+2] == -1) {
// continuous buzzer on when finished the task
buzzer_on();
while(1);
}
}
j++;
}
}
}
//Main Function end ########################################################################################