void beep()
{
wyswietl_LCD("BEEP..BEEP");
buzzer_on();
_delay_ms(500);
buzzer_off();
_delay_ms(500);
buzzer_on();
_delay_ms(700);
buzzer_off();
}
void onNode()
{
static unsigned int nodeCount = 0;
nodeCount++;
if (nodeCount == 1)
{
lcd_print(2, 1, 3, 1);
forward();
//_delay_ms(5000);
}
else if (nodeCount == 2 || nodeCount == 3)
{
lcd_print(2, 1, 4, 1);
nodeInd();
}
else if (nodeCount == 4)
{
nodeLeft();
}
else if (nodeCount == 5)
{
nodeRight();
}
else if (nodeCount == 6)
{
stop();
buzzer_on();
while (1);
}
}
void mymain(void)
{
// set GPKCON0: output (led1-led4: use GPK4-GPK7)
GPKCON0 = 0x11110000;
// set all led off (write 1 to be off)
GPKDAT = 0x000000f0;
// set GPF14 as output
GPFCON |= 1<<28;
GPFCON &= ~(1<<29);
// set GPN0 as input
GPNCON = 0;
while(1)
{
if ( !(GPNDAT & (1<<0)) )
{
buzzer_on();
led_on(0);
}
else
{
buzzer_off();
led_off(0);
}
}
}
void buzzer()
{
buzzer_on();
////_delay_ms(1000);
buzzer_off();
}
void buzzer_sound( uint8_t time )
{
#ifndef REVX
buzzer_on() ;
Buzzer_count = time ;
#endif
}
//--------------------------------------------------------------------------------
// start the main function here
//--------------------------------------------------------------------------------
int main(void)
{
float battery_low = 0;
init_devices();
packet_valid = 0;// initialise valid packet variable to 0
// initially calculate the battery voltage
UCSRB = 0x00;
ADMUX = 0x26; //select ADC input 6
battery_voltage = ADC_conversion();
UCSRB = 0x98;
flag2 = 0;
while(1)
{
// flag2 will rise once in 10sec and calculates the Battery voltage
// and enables the UART communication
if(flag2 == 1)
{
ADMUX = 0x26; //select ADC input 6
battery_voltage = ADC_conversion();
UCSRB = 0x98;
flag2 = 0;
}
// if battery voltage is less then battery threshold value ,buzzer beeps
battery_low = (battery_voltage * 0.041) + 0.7; // battery voltage caluculation
if(battery_low < BATTERY_TRHRESHOLD)
{ buzzer_on(); _delay_ms(200); buzzer_off();_delay_ms(200);}
// following function beeps the buzzer twice after sucessfully reciving 10 packets
// from the master(FireBird-V Hexapod robot)
if(flag1 == 0)
{
if(communication_proper_count > 10)
{
flag1 = 1;
buzzer_on(); _delay_ms(400); buzzer_off();_delay_ms(400);
buzzer_on(); _delay_ms(400); buzzer_off();_delay_ms(400);
}
}
}
}
//function for indication of eye-bling and raw values when removed from head
void Eye_Blink ()
{
if (Eye_Enable)
{
if (On_Flag==1 && Off_Flag==0)
{
if ((Avg_Raw>Theshold_Eyeblink) && (Avg_Raw<350)) //eye-blink detected
{
p++;
buzzer_on();
_delay_ms(500);
buzzer_off();
if(p==2){
stop();
velocity(240,240);
left();
_delay_ms(650);
stop();
run();
p=0;
}
}
else
{
if (Avg_Raw>350) //Raw data values indication
{
buzzer_on();_delay_ms(50);buzzer_off(); //Sensor removed from head, bot stops.
stop();
On_Flag==0;Off_Flag==1;
}
}
}
else
{
PORTJ=0x00;
}
}
else //Device is paired
{
PORTJ=0x01;
stop();
}
}
void checkState(int t)
{
while(t>0)
{
buzzer_on();
_delay_ms(50);
buzzer_off();
_delay_ms(200);
t--;
}
}
//Main Function
void buzzer(void)
{
init_devices_buzzer();
for(int i=0;i<1;i++)
{
buzzer_on();
_delay_ms(1000); //delay
buzzer_off();
_delay_ms(1000); //delay
}
}
void sh_cmd1(void)
{
// time_tt t = {};
// clean_lcd(BACK_COL);
// gettime(&t);
buzzer_init();
buzzer_on();
udelay(1000*1000);
buzzer_off();
udelay(1000*1000);
}
static void work_handler(struct work_struct *data)
{
// buzzer_beep(1);
// get current time and set next call time
mod_timer(&mytimer, jiffies + HZ / 50);
// start timer and buzzer on
buzzer_on();
return;
}
static void tasklet_handler(unsigned long data)
{
// buzzer_beep(1);
// get current time and set next call time
mod_timer(&mytimer, jiffies + HZ / 50);
// start timer and buzzer on
buzzer_on();
return;
}
void buzzer_beep(int n)
{
int i;
for (i = 0; i < n; i++)
{
buzzer_on();
mdelay(100);
buzzer_off();
mdelay(100);
}
}
/**
****************************************************************************************
* @brief Show proxr alert. User can add their own code here to show alert.
****************************************************************************************
*/
static void usr_proxr_alert(struct proxr_alert_ind *param)
{
// If it is PROXR_LLS_CHAR writting indication, don't alert,
// otherwise (PROXR_IAS_CHAR, disconnect) to alert
if (param->char_code == PROXR_IAS_CHAR || app_proxr_env->enabled == false)
{
if(param->alert_lvl == 2)
{
buzzer_on(BUZZ_VOL_HIGH);
ke_timer_set(APP_PROXR_ALERT_STOP_TIMER, TASK_APP, 500); // 5 seconds
}
else if(param->alert_lvl == 1)
{
buzzer_on(BUZZ_VOL_LOW);
ke_timer_set(APP_PROXR_ALERT_STOP_TIMER, TASK_APP, 500); // 5 seconds
}
else
{
buzzer_off();
ke_timer_clear(APP_PROXR_ALERT_STOP_TIMER, TASK_APP);
}
}
}
void keypad_buzzer(void)
{
unsigned int keyStatus;
while(1)
{
keyStatus = keyScan();
if(keyStatus)
{
buzzer_on();
delay();
buzzer_off();
}
else
buzzer_off();
}
}
// reset model to defaults
static void menu_reset_model(u8 action) {
// change value
if (action == MLA_CHG) menu_tmp_flag ^= 1;
// select next value, reset when flag is set
else if (action == MLA_NEXT) {
if (menu_tmp_flag) {
menu_tmp_flag = 0;
config_model_set_default();
buzzer_on(60, 0, 1);
}
}
// show value
lcd_7seg(L7_R);
lcd_chars(menu_tmp_flag ? "YES" : "NO ");
}
//Main Function
int main(void)
{
init_devices();
while(1)
{
if((PINE & 0x80) == 0x80) //switch is not pressed
{
buzzer_off(); //Turn off buzzer
PORTJ = 0x00; //Turn off bargraph LEDs
}
else
{
buzzer_on(); //Turn on buzzer
PORTJ = 0xFF; //Turn on bargraph LEDs
}
}
}
int isPlus()
{
if((Left_white_line >0x20 && Center_white_line>0x20) || (Right_white_line >0x20 && Center_white_line>0x20))
{
buzzer_on();
_delay_ms(100);
buzzer_off();
_delay_ms(100);
return 1;
}
else
{
return 0;
}
}
int detect_program_bin(void)
{
int ret, size, count;
u32 *buf;
buf = (u32 *)0xe00000;
fat_register_device(&mmc_blk_dev,1);
//buzzer_off();
size = file_fat_read ("program.bin", buf, 128 * 1024);
if (size > 1 && (buf[0] == 0xea000012 || buf[0] == 0x120000ea))
{
buzzer_on(0);
run_command("go e00000", 0);
buzzer_off();
}
//buzzer_off();
return 0;
}
void detect(){
if(USART1_RX_vect()==0xAA){ //check SYNC byte 1
if(USART1_RX_vect()==0xAA){ //check SYNC byte 2
payloadlength=USART1_RX_vect();
if(payloadlength < 169) { //Payload length can not be greater than 169
generatedChecksum = 0;
for(int i = 0; i < payloadlength; i++) {
payloaddata[i] = USART1_RX_vect(); //Read payload into memory
generatedChecksum += payloaddata[i];
}
checksum = USART1_RX_vect(); //Read checksum byte from stream
generatedChecksum = 255 - generatedChecksum;
if(checksum == generatedChecksum) { //Checking whether checksum received is correct. If buzzer turns on then packet is correct.
buzzer_on();_delay_ms(100);buzzer_off();_delay_ms(100);
}
}
}
}
}
int test(void)
{
int i = 0;
led_init();
buzzer_init();
k1_init();
while(1)
{
if(k1_is_down())
{
buzzer_on();
led_on(i%4 + 1);
delay(1);
buzzer_off();
led_off(i%4 + 1);
delay(1);
i++;
}
}
return 0;
}
/**
Go forward by a certain specified number of steps.
*/
void go_distance(unsigned char x)
{
reset_shaft_counters();
forward();
velocity(100,100);
PORTJ = 0x00;
while(1){
read_sensors();
print_sensor_data();
if( Front_IR_Sensor<0xF0)
{
stop();
buzzer_on();
}
else
{
forward();
buzzer_off();
}
if((ShaftCountLeft + ShaftCountRight)*5 > x*10)
break;
}
velocity(0,0);
}
int main(void)
{
data = UDR0; //making copy of data from UDR0 in 'data' variable
UDR0 = data; //echo data back to PC
init_devices();
velocity (246, 250); //Set robot velocity here. Smaller the value lesser will be the velocity
//Try different valuse between 0 to 255
while(1)
{
if(data == 0x38) //ASCII value of 8
{
forward_mm(90); //Moves robot forward 100mm
stop();
_delay_ms(700);
}
if(data == 0x32) //ASCII value of 2
{
back_mm(90); //Moves robot backward 100mm
stop();
_delay_ms(700);
}
if(data == 0x34) //ASCII value of 4
{
left_degrees(90); //Rotate robot left by 90 degrees
stop();
_delay_ms(700);
}
if(data == 0x36) //ASCII value of 6
{
right_degrees(90); //Rotate robot right by 90 degrees
stop();
_delay_ms(700);
}
if(data == 0x67) //ASCII value of g
{
soft_left_degrees(90); //Rotate robot left by 90 degrees
stop();
_delay_ms(500);
}
if(data == 0x68) //ASCII value of h
{
soft_right_degrees(90); //Rotate robot right by 90 degrees
stop();
_delay_ms(500);
}
if(data == 0x69) //ASCII value of i
{
left_degrees(180); //Rotate robot left by 90 degrees
stop();
_delay_ms(500);
}
if(data == 0x70) //ASCII value of j
{
right_degrees(180); //Rotate robot right by 90 degrees
stop();
_delay_ms(500);
}
if(data == 0x35) //ASCII value of 5
{
stop();
_delay_ms(500);//stop
}
if(data == 0x37) //ASCII value of 7
{
buzzer_on();
}
if(data == 0x39) //ASCII value of 9
{
buzzer_off();
}
if(data == 0x61) //ASCII value of a
{
redled_on();
}
if(data == 0x62) //ASCII value of b
{
redled_off();
}
if(data == 0x63) //ASCII value of c
{
blueled_on();
}
if(data == 0x64) //ASCII value of d
{
blueled_off();
}
if(data == 0x65) //ASCII value of e
{
greenled_on();
}
if(data == 0x66) //ASCII value of f
{
greenled_off();
}
}
}
// calibrate menu
void menu_calibrate(u8 at_poweron) {
u8 channel = 1;
u16 last_val = 0xffff;
u16 val;
u8 seg;
u8 bat_volts;
u16 update_time = 0;
u16 update_val = 0;
menu_adc_wakeup = 1;
// cleanup screen and disable possible low bat warning
buzzer_off();
if (at_poweron) buzzer_on(30, 30, 2);
else key_beep();
menu_battery_low = 0; // it will be set automatically again
battery_low_shutup = 0;
backlight_set_default(BACKLIGHT_MAX);
backlight_on();
lcd_clear();
btnra();
// show intro text
lcd_chars("CAL");
lcd_update();
delay_menu_always(2);
// show channel number and not-yet calibrated values
lcd_segment(LS_SYM_CHANNEL, LS_ON);
lcd_7seg(channel);
lcd_menu(LM_MODEL | LM_NAME | LM_REV | LM_TRIM | LM_DR | LM_EXP);
lcd_set_blink(LMENU, LB_SPC);
while (1) {
// check keys
if (btnl(BTN_BACK | BTN_ENTER)) break;
if (btn(BTN_END | BTN_ROT_ALL)) {
if (btn(BTN_END)) key_beep();
// change channel number
channel = (u8)menu_change_val(channel, 1, 4, 1, 1);
lcd_7seg(channel);
lcd_update();
update_time = 0;
}
else if (btn(BTN_ENTER)) {
// save calibrate value for channels 1 and 2
// select actual voltage for channel 4
if (channel == 1) {
key_beep();
val = ADC_OVS(steering);
if (val < CALIB_ST_LOW_MID) {
cg.calib_steering_left = val;
seg = LS_MENU_MODEL;
}
else if (val <= CALIB_ST_MID_HIGH) {
cg.calib_steering_mid = val;
seg = LS_MENU_NAME;
}
else {
cg.calib_steering_right = val;
seg = LS_MENU_REV;
}
lcd_segment(seg, LS_OFF);
lcd_update();
}
else if (channel == 2) {
key_beep();
val = ADC_OVS(throttle);
if (val < CALIB_TH_LOW_MID) {
cg.calib_throttle_fwd = val;
seg = LS_MENU_TRIM;
}
else if (val <= CALIB_TH_MID_HIGH) {
cg.calib_throttle_mid = val;
seg = LS_MENU_DR;
}
else {
cg.calib_throttle_bck = val;
seg = LS_MENU_EXP;
}
lcd_segment(seg, LS_OFF); // set corresponding LCD off
lcd_update();
}
else if (channel == 4) {
key_beep();
// allow to set actual battery voltage
lcd_segment(LS_SYM_DOT, LS_ON);
lcd_segment(LS_SYM_VOLTS, LS_ON);
bat_volts = (u8)(((u32)adc_battery * 100 + 300) / cg.battery_calib);
lcd_char_num3(bat_volts);
lcd_update();
while (1) {
btnra();
stop();
if (btnl(BTN_BACK) || btn(BTN_ENTER | BTN_END)) break;
if (btn(BTN_ROT_ALL)) {
if (btn(BTN_ROT_L)) bat_volts--;
else bat_volts++;
lcd_char_num3(bat_volts);
lcd_update();
}
}
key_beep();
lcd_segment(LS_SYM_DOT, LS_OFF);
lcd_segment(LS_SYM_VOLTS, LS_OFF);
last_val = 0xffff; // show ADC value
if (!btn(BTN_END)) {
// recalculate calibrate value for 10V
cg.battery_calib = (u16)(((u32)adc_battery * 100 + 40) / bat_volts);
if (btnl(BTN_BACK | BTN_ENTER)) break;
}
}
}
// show ADC value if other than last val
switch (channel) {
case 1:
val = ADC_OVS(steering);
break;
case 2:
val = ADC_OVS(throttle);
break;
case 3:
val = ADC_OVS(ch3);
break;
case 4:
val = adc_battery;
break;
default: // to eliminate compiler warning
val = 0;
}
// only update display every 1s
if (time_sec >= update_time) {
update_time = time_sec + 1;
update_val = val;
}
if (update_val != last_val) {
last_val = update_val;
lcd_char_num3(val);
lcd_update();
}
btnra();
stop();
}
menu_adc_wakeup = 0;
beep(60);
lcd_menu(0);
lcd_update();
config_global_save();
apply_global_config();
}
//Main Function
int main()
{
init_devices();
lcd_set_4bit();
lcd_init();
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
Front_Sharp_Sensor = ADC_Conversion(11);
Front_IR_Sensor = ADC_Conversion(6);
flag1=0;
flag2=0;
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,4,11); //Prints Value of Front Sharp Sensor
print_sensor(2,8,6); //Prints Value of Front IR Sensor
if(Front_Sharp_Sensor>0x82 || Front_IR_Sensor<0xF0)
{
flag2=1;
stop();
buzzer_on();
}
if((Center_white_line<0x28) && (flag2==0))
{
flag1=1;
buzzer_off();
forward();
velocity(150,150);
}
if((Left_white_line>0x28) && (flag1==0) && (flag2==0))
{
flag1=1;
buzzer_off();
forward();
velocity(150,50);
}
if((Right_white_line>0x28) && (flag1==0) && (flag2==0))
{
flag1=1;
buzzer_off();
forward();
velocity(50,150);
}
if((Center_white_line>0x28) && (Left_white_line>0x28) && (Right_white_line>0x28) && (flag2==0))
{
buzzer_off();
forward();
velocity(0,0);
}
}
}
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);
}
//This is a non-blocking function. It is called once on every iteration of the main loop if "white_line_flag" is ON
//It reads the whiteline sensor values and determines what it should do next to stay on the white line
void whiteline_follow_continue() {
read_sensors();
flag=0;
print_sensor_data();
if(Center_white_line<W_THRESHOLD_STOP && Left_white_line<W_THRESHOLD_STOP && Right_white_line<W_THRESHOLD_STOP ){
if (whiteline_stop_intersection_flag) {
whiteline_follow_end();
send_char(SUCCESS);
}
/*else {
forward();
velocity(100,100);
stop_on_timer4_overflow = 1;
start_timer4();
while (stop_on_timer4_overflow != 0) {;}
}
return;*/
}
if( Front_IR_Sensor<0xF0)
{
stop();
buzzer_on();
}
//Sensor config : 010
else if(Left_white_line > W_THRESHOLD && Center_white_line < W_THRESHOLD && Right_white_line > W_THRESHOLD)
{
forward();
velocity(150,150);
black_flag = 0;
buzzer_off();
}
//Sensor config : 110
else if(Left_white_line < W_THRESHOLD && Center_white_line < W_THRESHOLD && Right_white_line > W_THRESHOLD)
{
forward();
velocity(120,150);
black_flag = 0;
buzzer_off();
}
//Sensor config : 100
else if(Left_white_line < W_THRESHOLD && Center_white_line > W_THRESHOLD && Right_white_line > W_THRESHOLD)
{
PORTA = 0x05;
velocity(50,130);
last_on = LEFT_SENSOR;
black_flag = 0;
buzzer_off();
}
//Sensor config : 011
else if(Left_white_line > W_THRESHOLD && Center_white_line < W_THRESHOLD && Right_white_line < W_THRESHOLD)
{
forward();
velocity(150,120);
black_flag = 0;
buzzer_off();
}
//Sensor config : 001
else if(Left_white_line > W_THRESHOLD && Center_white_line > W_THRESHOLD && Right_white_line < W_THRESHOLD)
{
PORTA = 0x0A;
velocity(130,50);
last_on = RIGHT_SENSOR;
black_flag = 0;
buzzer_off();
}
//Sensor config : 000
else
{
buzzer_off();
if(black_flag >= CONT_BLACK) {
if(last_on == LEFT_SENSOR)
motion_set(0x05);
else if(last_on == RIGHT_SENSOR)
motion_set(0x0A);
velocity(100,100);
while(1){
print_sensor_data();
read_sensors();
if(Center_white_line < W_THRESHOLD) break;
}
}
black_flag = (black_flag < CONT_BLACK)?black_flag+1:CONT_BLACK;
forward();
velocity(0,0);
}
}
//Main Function
int main()
{
init_devices();
init_encoders();
lcd_set_4bit();
lcd_init();
int value=0;
forward();
velocity(130,130);
lcd_print(2,1,130,3);
lcd_print(2,5,130,3);
lcd_print(2,9,pathindex,2);
lcd_print(2,13,dirn,3);
while(1)
{
read_sensor();
follow();
if(isPlus())
{
read_sensor();
value = path[pathindex++];
// Code inserted for calculation of actual location wrt initial starting point ,
// It will consider direction also.
if (value == F)
{
// Move the bot forward, for location only , No movement on ground.
move_bot(FR);
}
else if (value == L)
{
// Move the bot left , for location only , No movement on ground.
move_bot(LT);
}
else if (value == R)
{
// Move the bot right, for location only , No movement on ground.
move_bot(RT);
}
else if (value == M)
{
// To stop the Bot and then break out
stop();
break;
}
orient(value);
/* lcd_print(2,9,pathindex,2);
lcd_print(2,13,dirn,3);
lcd_print(1,13,turnL,1);
lcd_print(1,15,turnR,1);
*/
}
if(turnL == 1)
{/*
lcd_print(1,13,turnL,1);
forward_mm(20);
stop();
velocity(180,180);
left_degrees(95);
//_delay_ms(120);
read_sensor();
// while(Left_white_line <0x40)
// {
// read_sensor();
// left();
// }
stop();
forward();
velocity(180,180);
turnL = 0;
*/
back_mm(50);
//stop();
//velocity(130,130);
stop();
left_degrees(50);
rotate_left_slowly();
forward();
velocity(130,130);
turnL = 0;
}
if(turnR == 1)
{
/*
lcd_print(1,15,turnR,1);
forward_mm(20);
stop();
velocity(180,180);
right_degrees(95);
//_delay_ms(200);
read_sensor();
// while(Right_white_line <0x30)
// {
// read_sensor();
// right();
// }
stop();
forward();
//follow();
velocity(180,180);
turnR = 0;
*/
back_mm(50);
//stop();
//velocity(130,130);
stop();
right_degrees(50);
rotate_right_slowly();
forward();
velocity(130,130);
turnR = 0;
}
}
// Three Beeps for Interval
buzzer_on();
_delay_ms(100);
buzzer_off();
_delay_ms(100);
buzzer_on();
_delay_ms(100);
buzzer_off();
_delay_ms(100);
buzzer_on();
_delay_ms(100);
buzzer_off();
_delay_ms(100);
//code to head-back to starting position , i.e. Origin
return_path_counter = reach_origin();
forward();
velocity(130,130);
int counter = 0;
int intermediate_value = 0;
while(counter < return_path_counter)
{
read_sensor();
follow();
if(isPlus())
{
read_sensor();
value = path_to_origin[counter];
counter++;
// Code inserted for calculation of actual location wrt initial starting point ,
// It will consider direction also.
if (intermediate_value == FR)
{
// Move the bot forward, for location only , No movement on ground.
value = F;
}
else if (value == LT)
{
// Move the bot left , for location only , No movement on ground.
value = L;
}
else if (value == RT)
{
// Move the bot right, for location only , No movement on ground.
value = R;
}
else if (value == ST)
{
value = M;
// specially inserted as break will not allow the bot to stop using "orient(value)".
orient(value);
break;
}
orient(value);
}
if(turnL == 1)
{
back_mm(50);
//stop();
//velocity(130,130);
stop();
left_degrees(50);
rotate_left_slowly();
forward();
velocity(130,130);
turnL = 0;
}
if(turnR == 1)
{
back_mm(50);
//stop();
//velocity(130,130);
stop();
right_degrees(50);
rotate_right_slowly();
forward();
velocity(130,130);
turnR = 0;
}
}
// Three beeps for Finish
buzzer_on();
_delay_ms(100);
buzzer_off();
_delay_ms(100);
buzzer_on();
_delay_ms(100);
buzzer_off();
_delay_ms(100);
buzzer_on();
_delay_ms(100);
buzzer_off();
_delay_ms(100);
}
/*
bbb node
bwb impossible
wbb DO
bbw DO
bww DOne
wwb DOne
wbw OK!
www Death
*/
void node()
{
buzzer_on();
_delay_ms(100);
buzzer_off();
}
//The main invoker routine. It takes as argument the next command to execute and does what is necessary
//Self-explanatory code!
void my_invoker (unsigned char command) {
if(command == BUZZER_ON){
buzzer_on();
return;
}
else if(command == BUZZER_OFF){
buzzer_off();
return;
}
else if(command == MOVE_FORWARD)
{
forward(); //forward
return;
}
else if(command == MOVE_BACKWARD)
{
back(); //back
return;
}
else if(command == MOVE_LEFT)
{
left(); //left
return;
}
else if(command == MOVE_RIGHT)
{
right(); //right
return;
}
else if(command == STOP)
{
stop(); //stop
return;
}
else if(command == SET_VELOCITY)
{
int numargs;
unsigned char * ch = recieve_args(&numargs);
//assert(numargs == 1);
int velleft = (int)*(ch);
int velright = (int)*(ch+1);
velocity(velleft,velright);
return;
}
else if(command == MOVE_BY)
{
int numargs;
unsigned char * ch = recieve_args(&numargs);
int pos_a = (int)*(ch);
int pos_b = (int)*(ch+1);
//int pos = 10;
//while (pos_b--) pos *= 10;
//pos *= pos_a;
//forward_mm(pos);
pos_a += (pos_b << 8);
forward();
velocity(120,120);
while (pos_a--) {
//delay on 5 ms
stop_on_timer4_overflow = 1;
start_timer4();
while (stop_on_timer4_overflow != 0) {;}
}
stop();
send_char(SUCCESS);
leftInt = 0;
rightInt = 0;
return;
}
else if(command == MOVE_BACK_BY)
{
int numargs;
unsigned char * ch = recieve_args(&numargs);
int pos_a = (int)*(ch);
int pos_b = (int)*(ch+1);
//int pos = 10;
//while (pos_b--) pos *= 10;
//pos *= pos_a;
//forward_mm(pos);
pos_a += (pos_b << 8);
back();
velocity(120,120);
while (pos_a--) {
//delay on 5 ms
stop_on_timer4_overflow = 1;
start_timer4();
while (stop_on_timer4_overflow != 0) {;}
}
stop();
send_char(SUCCESS);
leftInt = 0;
rightInt = 0;
return;
}
else if(command == TURN_LEFT_BY)
{
int numargs;
unsigned char * ch = recieve_args(&numargs);
already_stopped = 0;
int pos_a = (int)*(ch);
int pos_b = (int)*(ch+1);
pos_a += (pos_b << 8);
_delay_ms(500);
left();
velocity(200,200);
while (pos_a--) {
//delay on 5 ms
stop_on_timer4_overflow = 1;
start_timer4();
while (stop_on_timer4_overflow != 0) {;}
}
stop();
send_char(SUCCESS);
leftInt = 0;
rightInt = 0;
already_modified_stopped = 0;
return;
}
else if(command == TURN_RIGHT_BY)
{
int numargs;
unsigned char * ch = recieve_args(&numargs);
//assert(numargs == 2);
int pos_a = (int)*(ch);
int pos_b = (int)*(ch+1);
pos_a += (pos_b << 8);
_delay_ms(500);
right();
velocity(200,200);
while (pos_a--) {
//delay on 5 ms
stop_on_timer4_overflow = 1;
start_timer4();
while (stop_on_timer4_overflow != 0) {;}
}
stop();
send_char(SUCCESS);
leftInt = 0;
rightInt = 0;
already_modified_stopped = 0;
return;
}
else if(command == LCD_SET_STRING)
{
int numargs;
unsigned char * ch = recieve_args(&numargs);
int i =0;
lcd_clear();
for(;i<numargs;i++)
{
lcd_wr_char(*(ch+i));
}
return;
}
else if (command == SET_PORT){
int numargs;
unsigned char * ch = recieve_args(&numargs); ;
if (numargs != 2){
}
int portnum = (int) *(ch);
unsigned char value = (unsigned char) *(ch+1);
setPort(portnum,value);
}
else if(command == GET_SENSOR_VALUE)
{
int numargs;
unsigned char * ch = recieve_args(&numargs); ;
if (numargs != 1){
}
int sensornum = (int) *(ch);
//setPort(portnum,value);
getSensorValue(sensornum);
}
else if(command == GET_PORT)
{
int numargs;
unsigned char * ch = recieve_args(&numargs); ;
if (numargs != 1){
}
int portnum = (int) *(ch);
getPort(portnum);
}
else if (command == WHITELINE_FOLLOW_START) {
whiteline_follow_start();
}
else if(command == PRINT_STATE){
buzzer_on();
lcd_num(state);
_delay_ms(1000);
buzzer_off();
}
else if (command == WHITELINE_FOLLOW_END) {
whiteline_follow_end();
}
else if (command == WHITELINE_STOP_INTERSECTION) {
whiteline_stop_intersection_flag = 1;
}
else if(command == ACC_START) {
acc_flag = 1;
}
else if(command == ACC_STOP) {
acc_flag = 0;
acc_modified_flag = 0;
buzzer_off();
}
else if(command == ACC_MODIFIED){
acc_modified_flag = 1;
already_modified_stopped = 0;
}
else if(command == ACC_CHECK){
if (acc_modified_flag == 1 && already_modified_stopped == 1){
send_char((char)1);
}
else {
char value = PORTA;
if (value == 0) send_char((char)2);
else send_char((char)0);
}
}
else if (command == ENABLE_LEFT_WHEEL_INTERRUPT) {
leftInt = 0;
left_position_encoder_interrupt_init();
}
else if (command == ENABLE_RIGHT_WHEEL_INTERRUPT) {
rightInt = 0;
right_position_encoder_interrupt_init();
}
else if (command == GET_LEFT_WHEEL_INTERRUPT_COUNT) {
send_int (leftInt);
leftInt = 0;
}
else if (command == GET_RIGHT_WHEEL_INTERRUPT_COUNT) {
send_int (rightInt);
rightInt = 0;
}
else if (command == SET_TIMER) {
int numargs;
unsigned char * ch = recieve_args(&numargs); ;
if (numargs != 1){
}
int time = (int) *(ch);
timer4_init2(time);
}
else if (command == DISCONNECT) {
disconnect();
}
else { //Error!!! Unrecognized Command
buzzer_on();
_delay_ms(1000);
buzzer_off();
}
}
