/*

Command codes for the various commands that the firebird can accept
*/
2#define BUZZER_ON 65
#define BUZZER_OFF 66
#define MOVE_FORWARD 67
#define MOVE_BACKWARD 68
#define MOVE_RIGHT 69
#define MOVE_LEFT 70
#define STOP 71
#define
#define
#define
#define
#define
#define
#define
#define
#define

WHITELINE_FOLLOW_START 74
WHITELINE_FOLLOW_END 75
WHITELINE_STOP_INTERSECTION 76
ACC_START 77
ACC_STOP 78
ACC_CHECK 79
ACC_MODIFIED 80
PRINT_STATE 90
DISCONNECT 120

#define
#define
#define
#define
#define
#define
#define
#define
#define

LCD_SET_STRING 32
SET_PORT 33
GET_SENSOR_VALUE 34
GET_PORT 35
SET_VELOCITY 36
MOVE_BY 37
TURN_LEFT_BY 38
TURN_RIGHT_BY 39
MOVE_BACK_BY 40

//! velocity control

/*!
Function for setting velocities of both wheels. pulse width modulation
*/
void velocity (unsigned char left_motor, unsigned char right_motor)
{
OCR5AL = (unsigned char)left_motor;
OCR5BL = (unsigned char)right_motor;
leftVel=left_motor;
rightVel=right_motor;
}
//! setting motor's direction
/*!
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 resto
ring the PORTA status
PORTA = PortARestore;
// executing the command
}

//! setting motor's forward

/*!
Function used for setting motor's direction forward
*/
void forward (void)
{
motion_set (0x06);
}
//! setting motor's direction back
/*!
Function used for setting motor's direction back
*/
void back (void) //both wheels backward
{
motion_set(0x09);
}
//! setting motor's direction left
/*!
Function used for setting motor's direction left
*/
void left (void) //Left wheel backward, Right wheel forward
{
motion_set(0x05);
}
//! setting motor's direction right
/*!
Function used for setting motor's direction right
*/
void right (void) //Left wheel forward, Right wheel backward
{
motion_set(0x0A);
}
//! setting motor's direction soft left
/*!
Function used for setting motor's direction soft left
*/
void soft_left (void) //Left wheel stationary, Right wheel forward
{
motion_set(0x04);
}
//! setting motor's direction soft right
/*!
Function used for setting motor's direction soft right
Left wheel forward, Right wheel is stationary
*/
void soft_right (void) //Left wheel forward, Right wheel is stationary
{
motion_set(0x02);
}
//! setting motor's direction soft left
/*!
Function used for setting motor's direction soft left
Left wheel backward, right wheel stationary
*/

void soft_left_2 (void) //Left wheel backward, right wheel stationary

{
motion_set(0x01);
}
//! setting motor's direction soft right
/*!
Function used for setting motor's direction
Left wheel stationary, Right wheel backward
*/
void soft_right_2 (void) //Left wheel stationary, Right wheel backward
{
motion_set(0x08);
}
//! stop the bot
/*!
Function used for stoping the bot.
*/
void stop (void)
{
motion_set (0x00);
}

//Function to split and send an integer over the communication channel

void send_int (int i) {
if (USE_BLUETOOTH) {
unsigned char c = (i >> 24) & 0xFF;
send_data_bt (c);
c = (i >> 16) & 0xFF;
send_data_bt (c);
c = (i >> 8) & 0xFF;
send_data_bt (c);
c = i & 0xFF;
send_data_bt (c);
}
else {
char str[10];
itoa(i,str,10);
serial_sendString(str);
}
}