// --- setup ---
void setup()
{
pinMode(alarmPin, OUTPUT);
pinMode(pirPin, INPUT);
// pinMode(relayPin, OUTPUT);
kpd.addEventListener(kpdEvent); //keypad event listener
Serial.begin(9600); // serial debug
slcd.begin();
set_lcd("bonjour", "");
delay(3000);
set_lcd("demarre", "");
}
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 goto_on_if_password(){
if(checkPassword()) {
next_alarmState = wait_on;
set_lcd("wait_on", "");
wait_on_timer.reset();
}
}
void on_set_button_clicked(GtkObject *object, gpointer wn)
{
g_print("Set Button Clicked\n");
int32_t error_code = ERR_NONE;
if (0 > comm_init(com_port_name))
error_code = ERR_PORT_INIT;
if (ERR_NONE == error_code)
error_code = send_password("WIFIBOT123");
if (ERR_NONE == error_code)
{
int32_t motor_levels[NUMBER_OF_MOTOR_CHANNELS];
motor_levels[MOTOR_SPEED_CHANNEL] = get_motor_value_from_control(
wm->left_motor_slide);
motor_levels[MOTOR_DIRECTION_CHANNEL] = get_motor_value_from_control(
wm->right_motor_slide);
error_code = write_motor_levels(motor_levels[MOTOR_SPEED_CHANNEL],
motor_levels[MOTOR_DIRECTION_CHANNEL]);
}
if (ERR_NONE == error_code)
{
error_code = set_ir_led(get_ir_led_from_control(wm->ir_led_control));
}
if (ERR_NONE == error_code)
{
error_code = set_motor_timeout(get_motor_timeout_from_control(
wm->motor_timeout_control));
}
if (ERR_NONE == error_code)
{
StatusLedFlashState_t flash_state;
int32_t flash_rate;
get_led_state_from_control(wm->status_led_control, &flash_state,
&flash_rate);
write_status_led(flash_state, flash_rate);
}
if (ERR_NONE == error_code)
{
int32_t i;
for (i = 0; i < LCD_TEXT_LINES; i++)
{
gchar * lcd_line_text = get_lcd_text_from_control(wm->lcd_text, i);
set_lcd(i, lcd_line_text);
}
}
comm_close();
error_handler(wm->window, error_code);
}
void lcd_print (char *str)
{
unsigned char line = 0;
set_lcd (INST_REG,0x02); // Return Cursor to Home
for (; *str != '\0'; str ++)
{
if (*str == '\n')
{
line ++;
switch (line)
{
case 1: set_lcd (INST_REG, 0xC0); //C0
break;
case 2: set_lcd (INST_REG, 0x94); //94
break;
case 3: set_lcd (INST_REG, 0xD4); //D4
}
}
else
{
set_lcd (DATA_REG, *str);
}
}
}
static ssize_t lcd_proc_write(struct file *file, const char __user *buf,
size_t count, loff_t *pos)
{
struct toshiba_acpi_dev *dev = PDE(file->f_path.dentry->d_inode)->data;
char cmd[42];
size_t len;
int value;
int ret;
len = min(count, sizeof(cmd) - 1);
if (copy_from_user(cmd, buf, len))
return -EFAULT;
cmd[len] = '\0';
if (sscanf(cmd, " brightness : %i", &value) == 1 &&
value >= 0 && value < HCI_LCD_BRIGHTNESS_LEVELS) {
ret = set_lcd(dev, value);
if (ret == 0)
ret = count;
} else {
ret = -EINVAL;
}
return ret;
}
static int set_lcd_status(struct backlight_device *bd)
{
struct toshiba_acpi_dev *dev = bl_get_data(bd);
return set_lcd(dev, bd->props.brightness);
}
void init_lcd(void)
{
delay_ms(100);
RS = INST_REG;
RW = WRITE;
EN = 1;
// 0011 * 3 times
D7 = 0;
D6 = 0;
D5 = 1;
D4 = 1;
nop();
nop();
nop();
nop();
EN = 0;
delay_ms(5);
EN = 1;
nop();
nop();
nop();
nop();
EN = 0;
delay_us(200);
EN = 1;
nop();
nop();
nop();
nop();
EN = 0;
delay_us(200);
/************** 8 Bit Initialisation *******************
//data length:8 bit mode ; display no: 2 line ;char font : 5*8
set_lcd(INST_REG,0x38);
delay_us(100);
set_lcd(INST_REG,0x08);
delay_us(100);
/*******************************************************/
/************** 4 Bit Initialisation *******************/
//data length:4 bit mode ; display no: 2 line ;char font : 5*8
EN = 1;
D7 = 0;
D6 = 0;
D5 = 1;
D4 = 0;
nop();
nop();
nop();
nop();
EN = 0;
nop();
nop();
nop();
nop();
delay_us(100);
//display:Off ; cursor: off; blink of cursor: off
set_lcd(INST_REG,0x28);
delay_us(100);
/*****************************************************/
//clear display n return home position
set_lcd(INST_REG,0x01);
delay_ms(5);
set_lcd(INST_REG,0x02);
delay_ms(5);
//cursor move direction: decrement position; shift the display: move cursor
set_lcd(INST_REG,0x06);
delay_us(100);
//cursor move: display shift; shift direction: left
set_lcd(INST_REG,0x14);
delay_us(100);
// display : on cursor : off; blink of cursor: off
set_lcd(INST_REG,0x0C);
delay_us(100);
}
ErrorID_t button_mode(cwiid_wiimote_t *wiimote,
volatile WiimoteStatusDataType *wiimote_status)
{
int32_t speed = SPEED_NULL_VALUE;
int32_t direction = 0;
bool run = true;
cwiid_set_rpt_mode(wiimote, CWIID_RPT_BTN);
debug_print("Entering button mode:\n");
write_status_led(STATUS_LED_OFF, 0);
set_lcd(0, "Button Mode");
set_lcd(1, "2-fwd,1-back,dpad-steer");
do
{
wait_for_wiimotedata(wiimote_status, 25);
if ((wiimote_status->button_data & CWIID_BTN_2)
&& (wiimote_status->button_data & CWIID_BTN_1))
{
if (speed > 0)
{
speed -= MAX_FORWARD_SPEED / 10;
if (speed < 0)
speed = 0;
}
else if (speed < 0)
{
speed -= MAX_REVERSE_SPEED / 10;
if (speed > 0)
speed = 0;
}
}
else if (wiimote_status->button_data & CWIID_BTN_2)
{
speed += MAX_FORWARD_SPEED / 10;
if (speed > MAX_FORWARD_SPEED)
speed = MAX_FORWARD_SPEED;
}
else if (wiimote_status->button_data & CWIID_BTN_1)
{
speed += MAX_REVERSE_SPEED / 10;
if (speed < MAX_REVERSE_SPEED)
speed = MAX_REVERSE_SPEED;
}
else
{
speed = 0;
direction = 0;
run = false;
}
if (wiimote_status->button_data & CWIID_BTN_UP)
{
direction -= (MAX_LEFT_DIRECTION / 10);
if (direction < -MAX_LEFT_DIRECTION)
direction = -MAX_LEFT_DIRECTION;
}
else if (wiimote_status->button_data & CWIID_BTN_DOWN)
{
direction += (MAX_RIGHT_DIRECTION / 10);
if (direction > MAX_RIGHT_DIRECTION)
direction = MAX_RIGHT_DIRECTION;
}
else
direction = 0;
debug_print("@%u: Button (speed, heading) = %d %d\n", get_tick_count(),
speed, direction);
write_motor_levels(speed, ComputeDirectionMotor(direction));
} while (run);
return ERR_NONE;
}
ErrorID_t acceleration_mode(cwiid_wiimote_t *wiimote,
volatile WiimoteStatusDataType *wiimote_status)
{
typedef enum WiimoteAccelStateType
{
WIIMOTE_ACCEL_WAIT_FOR_START,
WIIMOTE_ACCEL_READ_CAL_DATA,
WIIMOTE_ACCEL_READ_DATA,
WIIMOTE_ACCEL_WAIT_FOR_EXIT,
WIIMOTE_ACCEL_EXIT,
} WiimoteAccelStateType;
const uint8_t RETRY_VALUE = 2;
uint8_t retry_count = 0;
ErrorID_t error_flag = ERR_NONE;
WiimoteAccelStateType state = WIIMOTE_ACCEL_WAIT_FOR_START;
debug_print("Entering acceleration mode...\n");
write_status_led(STATUS_LED_OFF, 0);
for (;;)
{
switch (state)
{
case WIIMOTE_ACCEL_WAIT_FOR_START:
if (0 > wait_for_wiimotedata(wiimote_status, INFINITE_TIMEOUT))
{
debug_print("No start signal received\n");
error_flag = WII_ERROR_DATA_TIMEOUT;
state = WIIMOTE_ACCEL_EXIT;
}
else if (0 == (wiimote_status->button_data & CWIID_BTN_A))
{
state = WIIMOTE_ACCEL_READ_CAL_DATA;
}
break;
case WIIMOTE_ACCEL_READ_CAL_DATA:
error_flag = WII_ERROR_ACCEL_CAL;
do
{
if (0
== cwiid_get_acc_cal(
wiimote,
CWIID_EXT_NONE,
(struct acc_cal *) &wiimote_status->accel_cal_data))
{
error_flag = ERR_NONE;
break;
}
} while (retry_count++ < RETRY_VALUE);
if (error_flag != ERR_NONE)
{
debug_print("Cannot read cal data\n");
return error_flag;
}
if (0 > cwiid_set_rpt_mode(wiimote, CWIID_RPT_ACC | CWIID_RPT_BTN))
{
debug_print("Cannot set report mode to ACCEL\n");
return false;
}
set_lcd(0, "Accel Mode...");
set_lcd(1, "P = %4d R = %4d", 0, 0);
state = WIIMOTE_ACCEL_READ_DATA;
break;
case WIIMOTE_ACCEL_READ_DATA:
if (0 > wait_for_wiimotedata(wiimote_status, INFINITE_TIMEOUT))
{
debug_print("@%u: WII_ERROR_DATA_TIMEOUT\n", get_tick_count());
error_flag = WII_ERROR_DATA_TIMEOUT;
state = WIIMOTE_ACCEL_EXIT;
}
else
{
if (wiimote_status->button_data & CWIID_BTN_A)
{
state = WIIMOTE_ACCEL_WAIT_FOR_EXIT;
}
else
{
error_flag = computer_motor_levels_accel(wiimote_status);
set_lcd(1, "P = %4d R = %4d",
wiimote_status->accel_computed_data.pitch / 100,
wiimote_status->accel_computed_data.roll / 100);
if (ERR_EXEC == error_flag)
cwiid_set_rumble(wiimote, true);
else
{
cwiid_set_rumble(wiimote, false);
if (error_flag < 0)
{
debug_print("@%u: CONTROL CAR COMM ERROR: %d\n",
get_tick_count(), error_flag);
state = WIIMOTE_ACCEL_EXIT;
}
}
}
}
break;
case WIIMOTE_ACCEL_WAIT_FOR_EXIT:
stop_motors();
if (0 < wait_for_wiimotedata(wiimote_status, INFINITE_TIMEOUT))
{
error_flag = WII_ERROR_DATA_TIMEOUT;
state = WIIMOTE_ACCEL_EXIT;
}
else if (0 == (wiimote_status->button_data & CWIID_BTN_A))
{
state = WIIMOTE_ACCEL_EXIT;
}
break;
case WIIMOTE_ACCEL_EXIT:
debug_print("Exiting acceleration mode.\n\n");
return error_flag;
}
}
return ERR_NONE;
}
/*!
@brief Determines motor parameters from IR data.
*/
ErrorID_t infrared_mode(cwiid_wiimote_t *wiimote,
volatile WiimoteStatusDataType *wiimote_status)
{
typedef enum WiimoteInfraredStateType
{
WIIMOTE_INFRARED_WAIT_FOR_START,
WIIMOTE_INFRARED_ENABLE,
WIIMOTE_INFRARED_READ_DATA,
WIIMOTE_INFRARED_WAIT_FOR_EXIT,
WIIMOTE_INFRARED_EXIT,
} WiimoteInfraredStateType;
ErrorID_t error_flag = ERR_NONE;
bool last_valid = false;
bool valid_points = false;
WiimoteInfraredStateType state = WIIMOTE_INFRARED_WAIT_FOR_START;
write_status_led(STATUS_LED_OFF, 0);
for (;;)
{
switch (state)
{
case WIIMOTE_INFRARED_WAIT_FOR_START:
if (0 < wait_for_wiimotedata(wiimote_status, 500))
{
error_flag = WII_ERROR_DATA_TIMEOUT;
state = WIIMOTE_INFRARED_EXIT;
}
else if (0 == (wiimote_status->button_data & CWIID_BTN_B))
{
state = WIIMOTE_INFRARED_ENABLE;
}
break;
case WIIMOTE_INFRARED_ENABLE:
set_ir_led(STATUS_LED_ON);
debug_print("Setting IR Report\n");
if (0 < cwiid_set_rpt_mode(wiimote, CWIID_RPT_IR | CWIID_RPT_BTN))
{
debug_print("Cannot set report mode to IR\n");
return false;
}
set_lcd(0, "Infrared Mode");
set_lcd(1, "Point wiimote at car");
write_status_led(STATUS_LED_ON, 0);
state = WIIMOTE_INFRARED_READ_DATA;
break;
case WIIMOTE_INFRARED_READ_DATA:
if (0 < wait_for_wiimotedata(wiimote_status, 500))
{
error_flag = WII_ERROR_DATA_TIMEOUT;
state = WIIMOTE_INFRARED_EXIT;
}
else if (wiimote_status->button_data & CWIID_BTN_B)
{
state = WIIMOTE_INFRARED_WAIT_FOR_EXIT;
}
else
{
error_flag = WiiComputeMotorLevelsInfrared(wiimote_status,
&valid_points);
if (valid_points != last_valid)
{
write_status_led(
valid_points ? STATUS_LED_OFF : STATUS_LED_ON, 0);
set_lcd(
1,
valid_points ?
"LED's seen" : "Point wiimote at car");
}
last_valid = valid_points;
if (ERR_EXEC == error_flag)
cwiid_set_rumble(wiimote, true);
else
{
cwiid_set_rumble(wiimote, false);
if (error_flag < 0)
state = WIIMOTE_INFRARED_EXIT;
}
}
break;
case WIIMOTE_INFRARED_WAIT_FOR_EXIT:
if (0 < wait_for_wiimotedata(wiimote_status, 500))
{
state = WIIMOTE_INFRARED_EXIT;
}
else if (0 == (wiimote_status->button_data & CWIID_BTN_B))
{
state = WIIMOTE_INFRARED_EXIT;
}
break;
case WIIMOTE_INFRARED_EXIT:
stop_motors();
write_status_led(STATUS_LED_OFF, 0);
set_ir_led(false);
return error_flag;
break;
break;
}
}
return true;
}
/*!
@brief Driver for Wiimote operation menu
Handles parsing wiimote input to determine correct operating mode.
This function will never exit once entered.
*/
ErrorID_t main_menu(cwiid_wiimote_t *wiimote,
volatile WiimoteStatusDataType *wiimote_status)
{
ErrorID_t error;
uint8_t menu_count;
uint16_t last_button_state = 0;
WII_OPERATE_STATE wii_operate_state = WII_OPERATE_INIT_MENU;
write_status_led(STATUS_LED_OFF, 0);
for (;;)
{
switch (wii_operate_state)
{
case WII_OPERATE_INIT_MENU:
menu_count = 0;
last_button_state = 0;
wii_operate_state = WII_OPERATE_WAIT_FOR_BUTTON_PRESS;
write_status_led(STATUS_LED_FLASH, 500);
if (0 > cwiid_set_led(wiimote, CWIID_LED1_ON))
return WII_ERROR_TRANSMIT;
if (0 > cwiid_enable(wiimote, CWIID_FLAG_MESG_IFC))
return WII_ERROR_TRANSMIT;
if (0 > cwiid_set_rpt_mode(wiimote, CWIID_RPT_BTN))
return WII_ERROR_TRANSMIT;
/* Menu */
debug_print("%s", MENU);
set_lcd(0, "Connected.");
set_lcd(1, "Select Mode.");
break;
case WII_OPERATE_WAIT_FOR_BUTTON_PRESS:
error = wait_for_wiimotedata(wiimote_status, 2000);
if (error == ERR_WII_DATA_TIMEOUT)
{
set_lcd(0, "Select Mode");
set_lcd(1, (char *) lcd_menu_strings[menu_count]);
if (++menu_count >= MENU_ENTRIES)
menu_count = 0;
}
else if (0 < error)
{
shutdown_application(error);
}
else if (wiimote_status->button_data != last_button_state)
{
last_button_state = wiimote_status->button_data;
if (last_button_state & CWIID_BTN_A) // collect acceleration data
{
wii_operate_state = WII_OPERATE_DISPLAY_ACCELEROMETER_INFO;
}
else if (last_button_state & CWIID_BTN_B) // collect IR data
{
wii_operate_state = WII_OPERATE_DISPLAY_IR_STATUS;
}
else if (last_button_state & CWIID_BTN_HOME) // reset
{
shutdown_application(0);
}
else if (last_button_state & CWIID_BTN_PLUS)
{
sensor_cutoff_fwd += 5;
if (sensor_cutoff_fwd > 100)
sensor_cutoff_fwd = 100;
#if REVERSE_SENSOR_PRESENT
sensor_cutoff_rev += 5;
if (sensor_cutoff_rev > 100)
sensor_cutoff_rev = 100;
#endif
}
else if (last_button_state & CWIID_BTN_MINUS)
{
sensor_cutoff_fwd -= 5;
if (sensor_cutoff_fwd < 0)
sensor_cutoff_fwd = 0;
#if REVERSE_SENSOR_PRESENT
sensor_cutoff_rev -= 5;
if (sensor_cutoff_rev < 0)
sensor_cutoff_rev = 0;
#endif
}
else if (last_button_state & (CWIID_BTN_2 | CWIID_BTN_1))
{
wii_operate_state = WII_OPERATE_DISPLAY_BUTTON_STATUS;
}
}
break;
case WII_OPERATE_DISPLAY_ACCELEROMETER_INFO:
if (ERR_NONE != acceleration_mode(wiimote, wiimote_status))
wii_operate_state = WII_OPERATE_ERROR_STATE;
else
wii_operate_state = WII_OPERATE_INIT_MENU;
break;
case WII_OPERATE_DISPLAY_IR_STATUS:
if (ERR_NONE != infrared_mode(wiimote, wiimote_status))
wii_operate_state = WII_OPERATE_ERROR_STATE;
else
wii_operate_state = WII_OPERATE_INIT_MENU;
break;
case WII_OPERATE_DISPLAY_BUTTON_STATUS:
if (ERR_NONE != button_mode(wiimote, wiimote_status))
wii_operate_state = WII_OPERATE_ERROR_STATE;
else
wii_operate_state = WII_OPERATE_INIT_MENU;
break;
case WII_OPERATE_ERROR_STATE:
if (ERR_NONE == error_mode(wiimote, wiimote_status))
wii_operate_state = WII_OPERATE_INIT_MENU;
break;
}
}
return ERR_NONE;
}
/*!
@brief High level task for wiimote.
Handles initializing wiimote.
*/
void control_tasks(char *dev_name)
{
WiimoteState_t WiimoteState = WII_PROMPT;
bdaddr_t bdaddr = *BDADDR_ANY; /* bluetooth device address */
init_tick_count();
#if _MUTEX_ENABLE
pthread_mutex_lock(&mutex);
#endif
#if HAVE_GTK
int argc_dummy = 0;
char **argv_dummy = NULL;
init_gui(argc_dummy, argv_dummy);
set_comm_trace(true);
set_diagnostic_mode(true);
#else
if (0 >= comm_init(dev_name))
{
debug_print("@%u: Cannot initialize %s\n", get_tick_count(), dev_name);
exit(4);
}
else
{
debug_print("@%u: %s initialized.\n", get_tick_count(), dev_name);
}
do
{
debug_print("Connecting to controller board... ");
if (0 > send_password("WIFIBOT123"))
{
debug_print("Error\n");
}
else
{
debug_print("Done\n");
break;
}
}while (1);
#endif
set_lcd(0, "%s", PACKAGE_NAME);
set_lcd(1, "Rev: %s", PACKAGE_VERSION);
// display startup for 1 s
sleep(1);
cwiid_set_err(err);
for (;;)
{
switch (WiimoteState)
{
case WII_PROMPT:
/* Connect to the wiimote */
debug_print(
"Put Wiimote in discoverable mode now (press 1+2)...\n");
set_lcd(0, "Connecting...");
set_lcd(1, "Press 1+2 now");
WiimoteState = WII_WAIT_FOR_CONNECTION;
break;
case WII_WAIT_FOR_CONNECTION:
if (!(wiimote = cwiid_open(&bdaddr, 0)))
{
fprintf(stderr, "Unable to connect to wiimote, retrying...\n");
WiimoteState = WII_PROMPT;
}
else if (cwiid_set_mesg_callback(wiimote, cwiid_callback))
{
fprintf(stderr,
"Unable to set message callback, retrying...\n");
WiimoteState = WII_PROMPT;
}
else
{
WiimoteState = WII_OPERATE;
}
break;
case WII_OPERATE:
default:
main_menu(wiimote, &wiimote_status_data);
debug_print("exiting.\n");
shutdown_application(0);
break;
}
}
}
int main(void)
{
cli();
set_lcd();
set_motors();
set_ADC();
sei();
int getValue1(void); //This function returns the value of the sensor mounted on the arm
void findagte(void);
void entergate(void);
int follow_wall(void);
int getValue2(void) ;
int row;
int state;
centre = ADC_Conversion(2);
sensorLeft = ADC_Conversion(3);
sensorRight = ADC_Conversion(1);
value_front = ADC_Conversion(11);
value_4sens= ADC_Conversion(12);
value_2sens= ADC_Conversion(10);
value_1sens=ADC_Conversion(9);
/*
lcd_print(2, 1, sensorLeft, 3);
lcd_print(2, 5, centre, 3);
lcd_print(2, 9, sensorRight, 3);
*/
if(sensorLeft <50 && sensorRight <50 && centre < 50) //when the bot enters the arena first
{
velocity(forwardLeftSpeed, forwardRightSpeed);
forward_mm(350);
stop();
_delay_ms(100);
}
//getValue2();
value_front = ADC_Conversion(11);
if(value_front>150)
{
centre = ADC_Conversion(2);
while(centre<150)
{
velocity(forwardLeftSpeed, forwardRightSpeed);
forward();
value_front = ADC_Conversion(11);
if(value_front<150)
{
findgate();
}
centre = ADC_Conversion(2);
}
entergate();
}
else if(value_front < 50)
{
left_degrees(90);
stop();
_delay_ms(500);
//turn the gripper
servo_2(90);
while(1)
{
row=follow_wall();
if(state)
{
soft_right();
centre = ADC_Conversion(2);
while(centre<150)
{
velocity(forwardLeftSpeed, forwardRightSpeed);
forward();
if(value_front<150)
{
findgate();
}
centre = ADC_Conversion(2);
}
entergate();
}
}
}
return(row);
}
// --- loop ---
void loop(){
char Key = kpd.getKey();
switch (alarmState) {
// off - system idle
case off:
set_sirene(false);
goto_on_if_password();
break;
// wait_on - delay before on (delai de sortie maison)
case wait_on:
set_sirene(false);
goto_off_if_password();
if(wait_on_timer.check() == 1){
set_lcd("alarm on", "");
next_alarmState = on;
}
break;
// on - system is running
case on:
set_sirene(false);
if(get_sensors()) {
next_alarmState = detection;
set_lcd("detection", "");
}
goto_off_if_password();
break;
// detection - movement detected
case detection:
set_sirene(false);
send_sms();
goto_off_if_password();
before_sirene_timer.reset();
next_alarmState = before_sirene;
break;
// before_sirene - delay before sirene (delai d'entrée dans maison)
case before_sirene:
set_sirene(false);
if(before_sirene_timer.check() == 1){
ring_sirene_timer.reset();
next_alarmState = ring_sirene;
set_lcd("sirene", "");
}
goto_off_if_password();
break;
// sirene - sirene is crying
case ring_sirene:
set_sirene(true);
goto_off_if_password();
if(ring_sirene_timer.check() == 1){
wait_on_timer.reset();
next_alarmState = wait_on;
set_lcd("wait_on", "");
}
break;
}
alarmState = next_alarmState;
}
int main(void)
{
cli();
set_lcd();
set_motors();
set_ADC();
sei();
PORTH |= (1<<2); //Turn off the proximity sensors.
DDRC |= (1 << 3); //Set the buzzer pin as output.
int error = 0, lastError = 0;
forward();
velocity(0,0);
while(1)
{
error = getError();
if(error > -100 && error < 100)
{
leftSpeed = forwardLeftSpeed - error*kp;
rightSpeed = forwardRightSpeed + error*kp;
velocity(leftSpeed, rightSpeed);
}
else if (error == 100)
{
if(lastError != error)
{
totalNodes++;
nodeMissTracker = 0;
if(totalNodes > 2)
{
switch(state)
{
case 0 :arenaLeft();
state++;
switch(checkForCube())
{
case 1 :buzzerBeep();
arenaUturn(); state = 5;
switch(checkForCube())
{
case 1 : buzzerBeep(); arenaLeft_2(); state = 9; break;
case 2 : buzzerBeep(); arenaLeft_2(); state = 9; break;
case 0 : break;
}
break;
case 2 :buzzerBeep();
arenaUturn(); state = 5;
switch(checkForCube())
{
case 1 : buzzerBeep(); arenaLeft_2(); state = 9; break;
case 2 : buzzerBeep(); arenaLeft_2(); state = 9; break;
case 0 : break;
}
break;
case 0 :break;
}
forward();
break;
case 1 : arenaStraight(); forward(); state++; break;
case 2 : arenaUturn();forward(); state++; break;
case 3 : arenaStraight(); forward(); state++; break;
case 4 : arenaStraight();
state++;
switch(checkForCube())
{
case 1 : buzzerBeep(); arenaLeft(); state = 9; break;
case 2 : buzzerBeep(); arenaLeft(); state = 9; break;
case 0 : break;
}
forward();
break;
case 5 : arenaStraight(); forward();state++; break;
case 6 : arenaUturn(); forward(); state++; break;
case 7 : arenaStraight(); forward();state++; break;
case 8 : arenaRight(); forward(); state++;
}
}
//This else block is executed only in the beginning. To skip the unwanted row (row 7)
else
{
/*
stop();
_delay_ms(500);
*/
velocity(forwardLeftSpeed, forwardRightSpeed);
forward();
}
if(state == 9)
{
state = 0;
row--;
velocity(forwardLeftSpeed, forwardRightSpeed);
forward();
if(row == 1)
{
stop();
while(1);
}
}
}
}
else
findLine();
lastError = error;
//This flag is set in the Interrupt service routine associated by the right optical encoder when the bot has skipped a node.
if(nodeMissedFlag == 1)
{
stop();
while(1);
}
}
return 0;
}
void goto_off_if_password(){
if(checkPassword()) {
next_alarmState = off;
set_lcd("alarm off", "");
}
}
