blob: ac392b76ccea569798e8f90a73657d9cef4af45a [file] [log] [blame]
/*
* Copyright (c) 2017 FH Dortmund.
* All rights reserved. This program and the accompanying materials
* are made available under the terms of the Eclipse Public License v1.0
* which accompanies this distribution, and is available at
* http://www.eclipse.org/legal/epl-v10.html
*
* Description:
* A4MCAR Project - Low-level Module Main Processing Task that processes driving commands over Bluetooth (UART) and Ethernet interface
*
* Authors:
* M. Ozcelikors <mozcelikors@gmail.com>
*
* Update History:
*
*/
#include "rn42_driver.h"
#include "core_debug.h"
/***
* Function Name: GetLightStateFromCommands
* Function Description : This function uses the speed, steering, and direction information in order to derive the lighting system light state.
*
* Argument Type Description
* speed int Speed value 0-99
* steering int Steering value 0-99
* direction int Direction value FORWARD or REVERSE
*/
short int GetLightStateFromCommands (int speed, int steering, int direction)
{
short int lightstate;
if (steering >= 40 && steering <= 60)
{
// Not steering
if (direction == FORWARD)
{
// Going forward
lightstate = 1;
}
else if (direction == REVERSE)
{
// Going back
lightstate = 1;
}
}
else
{
// Steering
if (steering < 40)
{
// Going left
lightstate = 4;
}
else if (steering > 60)
{
// Going right
lightstate = 5;
}
}
return lightstate;
}
/***
* Function Name: WriteData
* Function Description : This utility function writes a string over uart_tx.
*
* Argument Type Description
* uart_tx client uart_tx_if UART TX Interface
* data char[] string to be sent
*/
void WriteData (client uart_tx_if uart_tx, char data[])
{
int i;
for (i = 0 ; i < strlen(data); i++)
{
uart_tx.write(data[i]); //Write one byte
}
}
/***
* Function Name: isDigit
* Function Description : Checks if a character is a digit
*
* Argument Type Description
* digit char Character to be checked
*
*/
int isDigit (char digit)
{
switch (digit)
{
case '0': case '1': case '2': case '3': case '4': case '5': case '6': case '7': case '8': case '9':
return 1;
default:
return -1;
break;
}
return -1;
}
/***
* Function Name: CharToDigit
* Function Description : Returns the decimal value digit from ASCII
*
* Argument Type Description
* digit char Character to be converted to decimal
*
*/
int CharToDigit (char digit)
{
return digit - '0';
}
/***
* Function Name: ParseRCCommandString
* Function Description : Parses the command string and returns steering, speed, and direction values, respectively.
*
* Argument Type Description
* data char[] Data to be parsed
*
*/
{int, int, int} ParseRCCommandString (char data[])
{
// Data is valid, now parse it..
int speed = 0;
int steering = 50;
int direction = FORWARD;
speed = CharToDigit(data[1])*10 + CharToDigit(data[2]);
steering = CharToDigit(data[4])*10 + CharToDigit(data[5]);
if(data[6] == 'F')
{
direction = FORWARD;
}
else
{
direction = REVERSE;
}
return {speed, steering, direction};
}
/***
* Function Name: InitializeRN42asSlave
* Function Description : This function sends some commands to RN42 Module through UART
* to initialize the module as a slave, and sets the bluetooth device name and password.
* Function is activated by uncommenting the RN42_INITIAL_CONFIG in the header file and
* the configuration should only be done once for a RN42 module.
*
* Argument Type Description
* uart_tx client uart_tx_if UART TX Interface
*/
void InitializeRN42asSlave(client uart_tx_if uart_tx)
{
char buffer[]= "$$$"; // Enter Command mode to configure the device
WriteData(uart_tx, buffer);
delay_microseconds(300000); //0.03sec //Wait for the response
char buffer1[]= "SF,1\r"; // Software reset
WriteData(uart_tx, buffer1);
delay_microseconds(300000); //0.03sec //Wait for the response
char buffer2[]= "SM,0\r"; // set to slave mode
WriteData(uart_tx, buffer2);
delay_microseconds(300000); //0.03sec //Wait for the response
char buffer6[]= "SA,0\r"; // set to open mode (no authentication)
WriteData(uart_tx, buffer6);
delay_microseconds(300000); //0.03sec //Wait for the response
char buffer3[]= "SN,RCCar Bluetooth\r"; // Setting bluetooth name
WriteData(uart_tx, buffer3);
delay_microseconds(300000); //0.03sec //Wait for the response
char buffer4[]= "SP,1234\r"; //Setting Bluetooth pin
WriteData(uart_tx, buffer4);
delay_microseconds(300000); //0.03sec //Wait for the response
char buffer5[]= "R,1\r"; // Reboot
WriteData(uart_tx, buffer5);
delay_microseconds(300000); //0.03sec //Wait for the response
}
/***
* Function Name: CheckIfCommandFormatIsValid
* Function Description : This function checks the format of the driving command and
* returns 1 if the driving command is as expected
*
* Argument Type Description
* command char* Command string to be checked
*/
int CheckIfCommandFormatIsValid (char* command)
{
if (command[0] == 'S' && command[3] == 'A' && (command[6] == 'F' || command[6] == 'R'))
{
if((isDigit(command[1]) != -1) && (isDigit(command[2]) != -1) && (isDigit(command[4]) != -1) && (isDigit(command[5]) != -1) )
{
return 1;
}
else
{
return 0;
}
}
else
{
return 0;
}
}
/***
* Function Name: Task_MainProcessingAndBluetoothControl
* Function Description : This function communicates with Bluetooth module via UART to obtain
* control interface and steering_inferface values
*
* Argument Type Description
* uart_tx client uart_tx_if UART TX Interface
* uart_rx client uart_rx_if UART RX Interface
* control_interface client control_if Motor speed control value (high level)
* steering_interface client steering_if Steering control value (high level)
* cmd_from_ethernet_to_override server ethernet_to_cmdparser_if Server that reads the driving command if an overriding command is received from the high-level module.
* lightstate_interface client lightstate_if Client side of the light state interface to send the light state information to the light system task.
*/
[[combinable]]
void Task_MainProcessingAndBluetoothControl(client uart_tx_if uart_tx,
client uart_rx_if uart_rx,
client control_if control_interface,
client steering_if steering_interface,
server ethernet_to_cmdparser_if cmd_from_ethernet_to_override,
client lightstate_if lightstate_interface)
{
//Debugging related definitions
timer tmr;
unsigned int time, delay = RCCAR_STATUS_UPDATE_RATE;
int set_val = 0;
int direction_val = REVERSE;
int speed_val = 0;
tmr :> time;
char CommandLine_Buffer[COMMANDLINE_BUFSIZE];
int char_index = 0;
char command[COMMANDLINE_BUFSIZE];
int command_line_ready = 0;
int speed = 0;
int steering = 50;
int direction = FORWARD;
int previous_direction = FORWARD;
int ctr = 0;
char data;
PrintCoreAndTileInformation("Task_GetRemoteCommandsViaBluetooth");
// Timing measurement/debugging related definitions
timer debug_timer;
uint32_t start_time, end_time;
char buffer2[]= "SA,0\r"; // set to open mode (no authentication)
WriteData(uart_tx, buffer2);
delay_microseconds(300000); //0.03sec //Wait for the response
//Light system state
short int lightstate = 2;
short int previous_lightstate = 2;
#ifdef RN42_INITIAL_CONFIG
// Send initialization commands to RN42
// Do for only first time. Change RN42_INITIAL CONFIG section in rn42_driver.h to do so.
InitializeRN42asSlave(uart_tx);
#endif
while (1) {
select
{
case uart_rx.data_ready(): //Read when data is available
//Measure start time
//debug_timer :> start_time;
//Receive the data over uart_rx interface.
data = uart_rx.read();
// This section is basically dedicated to use the bytes and when the end of line reached,
// construct the command line.
if(data != 'E')
{
CommandLine_Buffer[char_index] = data;
if(char_index >= COMMANDLINE_BUFSIZE-1) char_index = 0;
else char_index++;
}
else // data == E
{
char_index = 0;
ctr = 0;
//Copy the string to construct the command
for(int x = 0; x < COMMANDLINE_BUFSIZE; x++)
command[x] = CommandLine_Buffer[x];
//Raise the command line ready flag
command_line_ready = 1;
}
//Measure end time
////debug_timer :> end_time;
////printf("RN42 t: %u", end_time - start_time);
break;
//Whenever a driving command comes over ethernet to override local driving commands,
//it needs to be processed.
case cmd_from_ethernet_to_override.SendCmd(char* override_command, int cmd_length):
command_line_ready = 1;
for(int k=0; k<cmd_length; k++)
{
command[k] = override_command[k];
}
break;
//Process the commands received above in a timer event
case tmr when timerafter(time) :> void : // Timer event
time += delay;
if ( command_line_ready )
{
// Check if incoming data is as expected..
if ( CheckIfCommandFormatIsValid(command) == 1 )
{
{speed, steering, direction} = ParseRCCommandString (command);
//In order to enable the light system, uncomment below:
/*lightstate = GetLightStateFromCommands (speed, steering, direction);
if (lightstate != previous_lightstate)
{
lightstate_interface.ShareLightSystemState (lightstate);
}*/
if (previous_direction == FORWARD && direction == REVERSE)
{
//Commands to cheat into REVERSE mode, so that motor driver is ready for REVERSE movement
steering_interface.ShareSteeringValue(50);
control_interface.ShareDirectionValue(REVERSE);
control_interface.ShareSpeedValue(11);
delay_milliseconds(20);
steering_interface.ShareSteeringValue(50);
control_interface.ShareDirectionValue(REVERSE);
control_interface.ShareSpeedValue(53);
delay_milliseconds(150);
steering_interface.ShareSteeringValue(50);
control_interface.ShareDirectionValue(REVERSE);
control_interface.ShareSpeedValue(6);
delay_milliseconds(50);
}
steering_interface.ShareSteeringValue(steering);
control_interface.ShareDirectionValue(direction);
control_interface.ShareSpeedValue(speed);
command_line_ready = 0;
previous_direction = direction;
previous_lightstate = lightstate;
}
}
break;
}
}
}