I am using the micro dual controller on my MSc. Project to control two 6 VDC motors driving a mobile tank like robotic platform motors working on an AVR Mega128. I already have a working interface module for the controller and it has been working fine for the last month as I am dealing with the rest of my project. This week I added a new module to interface the robotic platform to a PC via UART and as a download the code and test it the controller stop functioning and never does it again!!!
I am quite sure that is the new code and it cost me 3 controller already!
I do not know what to do :? .
Right now I am testing the rest of the software but in the very, very, near future need the controllers running and do not what to burn them!
Can anyone help me? Does anyone have or had a similar problem?
Here is the controller killer code:
//********************************************************************************
// File name: platformUartInterface.c
//
// Description: Module that interface the mobile & sensor platform with a PC
// via the uC serial port uart0 in assynchronous mode.
// The module allows the control and aquisition of the platform
// sensor data via Tx of predifined commands.
// All commands are defined in this file
//
//
// All functions are non blocking
// an if the platformUartInterface application is run at the main
// function it can run simultaneously with other applications.
//
// Target Micro: Atmel AVR Mega128;
//
// Created: 09 July 2007;
//
// Last modified: 11 July 2007;
//
// Project: Msc. Final project "Army of Ants" - University of Sussex 2007;
//
// Author: Pedro M. Lau Semedo.
//********************************************************************************
//********************************************************************************
// Include files
//********************************************************************************
#include "uart.h"
#include "platformUartInterface.h"
// #include "stdio.h"
//********************************************************************************
// Global variables
//********************************************************************************
char aux[50];
char introString[] = "MSc. Final Project - ''Army of Ants'': Mobile & Sensor Platform UART Interface:\n\r>: \n\r\0";
char helpIntroString[] = "Interface Help:\n\r\n\r\0";
char newLine[] = "\n\r\0";
char separatorString[] = ": \0";
char invalidCommandString[] = "Invalid Command\n\r\0";
command commandHelp, commandGetSensorsData;
command * commands[NCOMMANDS] = {&commandGetSensorsData, &commandHelp};
command * actualCommand;
char commandGetSensorsDataString[] = "getSensorsData\0" ;
char commandGetSensorsDataHelpString[] = "Obtains all platform sensors data\n\r\0";
char commandHelpString[] = "help\0";
char commandHelpHelpString[] = "Shows all commands help information\n\r\0";
unsigned char platformUartInterfaceAppState, commandFunctionState,
commandFunctionAux, uartInterfaceRxStringState, uartInterfaceAppAux,
uartInterfaceAppAux2;
//********************************************************************************
// Function name: platformUartInterfaceInit;
//
// In. arguments: void
//
//
// Out. arguments: void;
//
// Description: Initializes the platformUartInterface application variables.
//********************************************************************************
void platformUartInterfaceInit(void)
{
// Initialize commands
newCommand(&commandGetSensorsData, commandGetSensorsDataString, commandGetSensorsDataHelpString, &getSensorsDataFunction);
newCommand(&commandHelp, commandHelpString, commandHelpHelpString, &helpFunction);
// Initialize application global variables
platformUartInterfaceAppState = 0;
commandFunctionState = 0;
commandFunctionAux = 0;
uartInterfaceRxStringState = 0;
uartInterfaceAppAux = 0;
uartInterfaceAppAux2 = 0;
}
//********************************************************************************
// Function name: platformUartInterfaceApp;
//
// In. arguments: void;
//
// Out. arguments: void
//
// Description: mobile & sensor uart interface application. This function
// needs to be run in the main function regularly in order for
// the interface to process and obtain sensor data.
//********************************************************************************
void platformUartInterfaceApp(void)
{
switch ( platformUartInterfaceAppState ){
// Write intro string
case 0:
// If string Tx was sucssefully initiated
if ( uartSendString(introString) )
platformUartInterfaceAppState = 1; // Go to next state
break;
// Obtain a command string from the uart and save it to "aux" string
case 1:
// If string Rx was sucessfully initialized
if ( uartInterfaceRxString(aux, 50) )
{
// while( !uartTxChar('$') );
// while ( !uartTxString(aux) );
platformUartInterfaceAppState = 2; // Go to next state
}
break;
// State to wait for command string Rx completion
case 2:
// if ( uartRxStringReady() ) // If a string was Rx
// {
// while( !uartTxChar('$') );
// uartSendString(aux);
// if ( uartSendString(newLine) ) // Write a new line indicating that a new command was Rx
// {
while ( !uartTxChar( '?' ) );
platformUartInterfaceAppState = 3; // Go to state to test validity of command inserted
// }
break;
// Test if a valid command was inserted
case 3:
// Test Rx string
actualCommand = getInsertedCommand(aux, commands);
if ( actualCommand ) // If a valid command was inserted
platformUartInterfaceAppState = 4; // Go to state that run command specific function
else // If comand is not valid
platformUartInterfaceAppState = 5; // Go to state to send error msg
break;
// Run command specific function
case 4:
// If function finnished running (return 1)
if ( (*actualCommand->ptrCommandFunction)() )
{
// while ( !uartTxChar('F') );
platformUartInterfaceAppState = 1; // Go to state to Rx a new command string
}
break;
// Send error msg
case 5:
// uartSendString(invalidCommandString); // Send error msg
// while ( !uartTxChar('E') );
platformUartInterfaceAppState = 1; // Go to state to Rx a new command string
break;
}
}
//********************************************************************************
// Function name: commandHelpFunction;
//
// In. arguments: void;
//
// Out. arguments: Returns a 'unsigned char' indicating all command help strings
// were written to uart orreading has finnish or writting in
// progress;
//
// Writing finnish - return '1',
// Writing in progress - return '0';
//
// Description: Writes all command help srings to uart in order to give help
// info on the commands to the user;
//
// This function is non blocking and is used by the
// sensorsInterface application only!
//********************************************************************************
unsigned char helpFunction(void)
{
switch ( commandFunctionState )
{
// Write help intro string
case 0:
// If help string Tx was sucssefully initiated
if ( uartSendString( helpIntroString ) )
{
commandFunctionAux = 0; // Initiale auxiliary variable for indexing porpuses
commandFunctionState = 1; // Go to next state
}
break;
// Check if all command help strings were written
case 1:
// If There are still strings left to write
if ( commandFunctionAux < NCOMMANDS )
commandFunctionState = 2; // Go to next state
else
{
commandFunctionState = 0; // Initialize command function state
return 1; // Return value indicating that function processing was finnished
}
break;
// Write command string of presently indexed command
case 2:
// If command string Tx was sucssefully initiated
if ( uartSendString( commands[commandFunctionAux]->ptrCommandString ) )
commandFunctionState = 3; // Go to next state
break;
// Write separator strings
case 3:
// If separator string Tx was sucssefully initiated
if ( uartSendString( separatorString ) )
commandFunctionState = 4; // Go to next state
break;
// Write command help string of presently indexed command
case 4:
if ( uartSendString( commands[commandFunctionAux]->ptrCommandHelpString ) ) // If a string was Rx
{
commandFunctionState = 1; // Go back to teste state 1
++commandFunctionAux; // Increment index to point to next command
}
break;
}
return 0; // Return value indicating that function processing did not finnish
}
//********************************************************************************
// Function name: commandGetSensorsDataFunction;
//
// In. arguments: void;
//
// Out. arguments: Returns a 'unsigned char' indicating that all the sensors data
// was aquired and Tx to uart:
//
// Aquisition and Tx finnish - return '1',
// Aquisition or Tx in progress - return '0';
//
// This function is incomplete;
//
// Description: Aquires and Tx all platform sensors data to uart;
//
// This function is non blocking and is used by the
// sensorsInterface application only!
//********************************************************************************
unsigned char getSensorsDataFunction(void)
{
double t = 10.24;
switch ( commandFunctionState )
{
// Initiate aquisition of platform sensors data
case 0:
// If aquisition was sucssefully initiated
//if ( getSensorsData( &sensDataPtr ) )
commandFunctionState = 1; // Go to next state
break;
// Wait until data is aquired
case 1:
// If There are still strings left to write
//if ( isSensorsDataReady() ) // If data was aquired
commandFunctionState = 2; // Go to next state
break;
//
case 2:
// If sensor data was obtained
if ( uartSendString("Ha Ha Ha" ) ) // If header txt was Tx
{
// Convert and save into a string the string representation of the aquired sensor data
//sprintf( charArray, "%e", (float)(sensDataPtr.irShortRangeSensorFront) );
//sprintf( charArray, "%e", t );
commandFunctionState = 0; // go to next state
return 1;
}
break;
// Tx sensor data in txt format
case 3:
// If sensor data was obtained
// if ( uartSendString( charArray ) )
// {
// commandFunctionState = 0; // Initialize command function state
// return 1; // Return value indicating that function processing was finnished
// }
break;
}
return 0; // Return value indicating that function processing did not finnish
}
//********************************************************************************
// Function name: getInsertedCommand;
//
// In. arguments: unsigned char *ptrInserterdString - Pointer to Rx string,
// command *commandsArray[] - Pointer to array with
// all interface commands
//
// Out. arguments: Returns a pointer to the command structure which command
// string is equalt to string which pointer is given as input
// argument. If no match was found between Rx string and
// interface commands a null pointer will be returned.
//
// Description: Check string which pointer is Rx as input argument and returns
// a pointer to the command that match, if there is no match a
// null pointer will be returned.
//********************************************************************************
command * getInsertedCommand(char *ptrInserterdString, command *commandsArray[])
{
unsigned char i, j, nMatches;
unsigned char commandMatch[NCOMMANDS];
command *retCommand;
for (i=0; i<NCOMMANDS; ++i)
commandMatch[i] = 1;
j = 0;
while (1)
{
if ( *(ptrInserterdString + j) == 0 )
break;
for (i=0; i<NCOMMANDS; ++i)
{
if ( commandMatch[i] == 1 )
{
retCommand = commandsArray[i];
if ( *( retCommand->ptrCommandString + j ) != *( ptrInserterdString + j ) )
commandMatch[i] = 0;
}
}
++j;
}
nMatches = 0;
for (i=0; i<NCOMMANDS; ++i)
{
if ( commandMatch[i] == 1 )
{
++nMatches;
retCommand = commandsArray[i];
}
}
if ( nMatches != 1 )
retCommand = 0;
return retCommand;
}
//********************************************************************************
// Function name: uartRxString;
//
// In. arguments: char *ptrString - Pointer to String to store
// Rx characters,
// unsigned char maxSize - Maximum size of Rx string;
// Contains Rx character index.
//
// Out. arguments: Returns a 'unsigned char' indicating if the status of the
// string reading
//
// Reception in progress - return '0',
// Reception finished - return '1';
// Reception error - return '2' (Maximum size reached);
//
// Description: Stores all characters received until the "Enter" character
// '0x0D'is received. This function is non blocking and is used
// by the uart application only!
//********************************************************************************
unsigned char uartInterfaceRxString(char *ptrString, unsigned char maxSize)
{
switch ( uartInterfaceRxStringState )
{
// Initialize index variable
case 0:
uartInterfaceAppAux = 0; // aux = Rx character index
uartInterfaceRxStringState = 1;
break;
// Obtain a character
case 1:
uartInterfaceAppAux2 = uartGetChar(); // obtain character from uart
while ( !uartTxChar( uartInterfaceAppAux2 ) );
if ( uartInterfaceAppAux2 ) // If the character is valid
uartInterfaceRxStringState = 2; // Go to state to echo Rx character
break;
// Echo Rx character
case 2:
if ( uartSendChar( uartInterfaceAppAux2 ) ) // If any character was received
uartInterfaceRxStringState = 3; // Go to state to save character to string
break;
// Test if it is a Enter, Backspace or printable character
case 3:
if (uartInterfaceAppAux2 == ENTER ) // If a "Enter" character is received
{
if ( !uartInterfaceAppAux2 )
return 0; // If no character received but a enter, continue reading
else
{
*(ptrString + uartInterfaceAppAux) = 0; // write string terminator
uartInterfaceRxStringState = 0; // reinitialize function machine state
// rxStringFlag = 1; // Set Rx String flag
return 1; // String reading finnish
}
}
else if (uartInterfaceAppAux2 == BSC) // If a "Backspace" character is received
{
uartInterfaceRxStringState = 4; // Go to state to delete last character on string
// while ( !uartTxChar( 'B' ) );
return 0;
}
else
{
if ( uartInterfaceAppAux < (maxSize-1) ) // If the string does not exceed the maximum size
{
*(ptrString + uartInterfaceAppAux) = uartInterfaceAppAux2; // Write character to string
++uartInterfaceAppAux; // Increment index
uartInterfaceRxStringState = 1; // Go to state to obtain a new character
return 0;
}
else // If string does exceed maximum size
{
*(ptrString + uartInterfaceAppAux) = 0; // write string terminator
uartInterfaceRxStringState = 0; // reinitialize function machine state
return 2; // return Rx string error
}
}
break;
// Delete the last character on string in response to the Rx of a Backspace
case 4:
if ( uartInterfaceAppAux > 0) // If any character was received
--uartInterfaceAppAux; // Delete last character by moving back the string pointer
uartInterfaceRxStringState = 1; // Continue reading
break;
}
return 0; // continue reading
}
// command.c
// Implementação de uma estructura para suporte a comandos tipo batch
void newCommand(command *ptr, char *commandString, char *commandHelpString, void *commandFunction)
{
ptr->ptrCommandString = commandString;
ptr->ptrCommandHelpString = commandHelpString;
ptr->ptrCommandFunction = commandFunction;
}
//******************************* End of file ************************************
Here is my controller interface:
//********************************************************************************
// File name: motorController.c
//
// Description: Module for interface with Pololu micro dual serial motor
// controller communicating using the Atmel Mega128 USART1 in
// assynchronous mode.Contains functions to send commands to
// control the DC motors direction and speed and higher level
// functions to go forward, backwards at acertain speed raging
// from 0-127 and turn right or left with ? degrees accuracy.
//
// All functions are non blocking
// an if the Uart application is run at the main function it can
// run simultaneously with other applications.
//
// Target Micro: Atmel AVR Mega128;
//
// Created: 26 Jun 2007;
//
// Last modified: 17 July 2007;
//
// Project: Msc. Final project "Army of Ants" - University of Sussex 2007;
//
// Author: Pedro M. Lau Semedo.
//********************************************************************************
//********************************************************************************
// Include files
//********************************************************************************
#include "io.h"
#include "interrupt.h"
#include "math.h"
#include "stdio.h"
#include "motorController.h"
#include "softwareTimers.h"
#include "uart.h"
//********************************************************************************
// Constants definitions
//********************************************************************************
#define APPTIMERINDEX 0
#define MOTORCONTROLERRST 0x20
#define MOTORSMAXIMUMVELOCITY 80
#define MOTORSMINIMUMOPERATINGVELOCITY 40
//********************************************************************************
// Global variables
//********************************************************************************
char configurationCommand[4] = {0x80, 0x02, 0x02, 0xFF};
char standardCommand[5] = {0x80, 0x00, 0x00, 0x00, 0xFF};
char nl[] = {0x0A, 0x0D, 0x00};
char chArray[30];
char chArray2[30];
unsigned char motorControllerAppFunctionIndex, motorControllerAppAux,
motorControllerAppAux2, txCommandStringState,
driveSelectedDirectionMachineState, motorControlAppAuxFlag,
motorControlAppAuxFlag2, breakMotorsMachineState,
turnSelectedDirectionMachineState, motorControllerInitMachineState;
unsigned int motorControllerAppAux3;
char* motorControllerAppAuxPtr;
float yPos, xPos, tetha, deltaTetha = 0.1;
unsigned long c, d;
volatile unsigned int rightTrackTicks, leftTrackTicks;
//********************************************************************************
// Function name: motorControllerInit;
//
// In. arguments: void
//
//
// Out. arguments: void;
//
// Description: Initializes the Uart1 hardware for assynchronous operation at
// 19200 baud, 8 bit data, no parity bit, one start bit and
// one stop bit t.Initializes a command Tx software buffer for
// the controller and all necessary state and auxiliary variables
// required to run the application. Innitializes the hardware
// motor controller by sending a configuration frame to configure
// the module to dual motor control with motor numbers
// 2 (Left Motor) and 3 (Rigth motor).
//********************************************************************************
void motorControllerInit(void)
{
// Set baud rate to 19200 Bps with CPU clk@16 MHz
UBRR0L = 51;
// Enable receiver and transmitter
UCSR0B = (1<<RXEN0)|(1<<TXEN0);
// Set Uart for assynchronous mode, no parity, 1 stop bit, 8 bit data
UCSR0C = (1<<UCSZ00)|(1<<UCSZ01);
// Initialize application to go to motor controller initialization state
motorControllerAppFunctionIndex = 1;
// Initialize Timer1
/*TCCR1A = 0; // Set to normal mode of operation without capture compare
TCCR1B = (1<<ICES1)|(1<<CS10); // Set to rising edge capture timer mode with source clock = clkIO/1
TCCR1C = 0; // Set without any compare modes selected
TIMSK = (1<<TOIE1); // Set Timer1 overflow interrupt enable
TCNT1 = 0; // Clear Timer1 Timer/Counter register
// Initialize Timer3
TCCR3A = 0; // Set to normal mode of operation without capture compare
TCCR3B = (1<<ICES3)|(1<<CS30); // Set to rising edge capture timer mode with source clock = clkIO/1
TCCR3C = 0; // Set without any compare modes selected
ETIMSK = (1<<TOIE3); // Set Timer3 overflow interrupt enable
TCNT3 = 0; // Clear Timer3 Timer/Counter register
EIMSK = (1<<INT0)|(1<<INT1); // Enable external interupt in pin PD0 (INT0) & PD1 (INT1)
EICRA = (1<<ISC01)|(1<<ISC00)|(1<<ISC11)|(1<<ISC10); // Selects INT0 external interrupt to be set in a risng edge
*/ // to count right and left track ticks
//Initialize global variables;
motorControllerAppAux = 0;
motorControllerAppAux2 = 0;
motorControllerAppAuxPtr = 0;
txCommandStringState = 0;
driveSelectedDirectionMachineState = 0;
motorControlAppAuxFlag = 0;
motorControlAppAuxFlag2 = 0;
breakMotorsMachineState = 0;
turnSelectedDirectionMachineState = 0;
motorControllerInitMachineState = 0;
rightTrackTicks = 0;
leftTrackTicks = 0;
xPos = 0.0;
yPos = 0.0;
tetha = (M_PI/2);
DDRB = (DDRB | MOTORCONTROLERRST); // Set motor controller reset pin as output
PORTB &= (~MOTORCONTROLERRST); // Activate motor controler reset
c = 0;
d = 0;
}
//********************************************************************************
// Function name: txCommandChar;
//
// In. arguments: unsigned char character - character to transmit,
//
//
// Out. arguments: Returns a 'unsigned char' indicating if the writing to the
// uart1 Tx buffer was sucessful.
//
// Sucessful - return '1',
// Not sucessful - return '0';
//
// Description: Tx a character through the Uart1 to the motor controller.
// This function is non blocking and is used by
// the motorController application only!
//********************************************************************************
unsigned char txCommandChar( unsigned char character )
{
if ( UCSR0A & (1<<UDRE0) ){
UDR0 = character ;
return 1; // Character sucessfully loaded to uart Tx buffer
}
return 0; // Addition to uart Tx buffer not successful
}
//********************************************************************************
// Function name: txCommandString;
//
// In. arguments: char* ptrString - pointer to string to transmit,
//
//
// Out. arguments: Returns a 'unsigned char' indicating if the string transmition
// is finnish. (I.e. if all characters within string are sent and
// terminator character '0' is found)
//
// Transmition finnished - return '1',
// transmitting in progress - return '0';
//
// Description: Transmit a string which pointer is given as parameter.
// This function is non blocking and is used by
// the motorController application only!
//********************************************************************************
unsigned char txCommandString(char* ptrString )
{
switch ( txCommandStringState ){
// Initialize index variable
case 0:
motorControllerAppAux = 0; // Clear index variable
txCommandStringState = 1; // Go to next state
break;
// Send elements on command string
case 1:
// If the present indexed element is no the string terminator
if ( *(ptrString + motorControllerAppAux) != 0xFF )
{
// If the element is sucessfully Tx
if ( txCommandChar( *(ptrString + motorControllerAppAux) ) )
++motorControllerAppAux; // Point to next element
}
// If the present element is the string terminator
else {
txCommandStringState = 0; // Initialize machine state variable
return 1; // Finnished sending command string
}
break;
}
return 0; // Continue sending
}
//********************************************************************************
// Function name: motorControllerInitMachine;
//
// In. arguments: void;
//
// Out. arguments: Returns an "unsigned char" indicating the status of the
// function process:
//
// Initialization finnished - return '1',
// Initialization in progress - return '0';
//
// Description: State macine that resets and Tx configuration commnad to motor
// controller.
//********************************************************************************
unsigned char motorControllerInitMachine(void)
{
switch ( motorControllerInitMachineState )
{
// Set reset time state
case 0:
if ( setTime(APPTIMERINDEX, 1, 1) ) // If timer is sucessfuly set
motorControllerInitMachineState = 1; // Go to nesxt state
break;
// Reset time wait state
case 1:
if ( !getTime(APPTIMERINDEX, 1) ) // If reset time ended
{
PORTB |= MOTORCONTROLERRST; // Deactivate motor controler reset
motorControllerInitMachineState = 2; // Go to next state
}
break;
// Set motor controller warm up time state
case 2:
if ( setTime(APPTIMERINDEX, 1, 1) ) // If timer is sucessfuly set
motorControllerInitMachineState = 3; // Go to nesxt state
break;
// Reset warm up time wait state
case 3:
if ( !getTime(APPTIMERINDEX, 1) ) // If reset time ended
motorControllerInitMachineState = 4; // Go to next state
break;
// Tx configuration command
case 4:
// Set auxiliary application pointer to the configuration command frame
// The command configures the motor controller for dual motor control
// with motor numbers 2 & 3
if ( txCommandString(configurationCommand) ) // If configuration commnad was Tx
{
PORTB &= (~MOTORCONTROLERRST); // Activate motor controler reset
motorControllerInitMachineState = 5; // Go to Idle state
}
break;
// Set reset time state
case 5:
if ( setTime(APPTIMERINDEX, 1, 1) ) // If timer is sucessfuly set
motorControllerInitMachineState = 6; // Go to nesxt state
break;
// Reset time wait state
case 6:
if ( !getTime(APPTIMERINDEX, 1) ) // If reset time ended
{
PORTB |= MOTORCONTROLERRST; // Deactivate motor controler reset
motorControllerInitMachineState = 0; // Initialize machine state
return 1; // Return value indicating motor controler initialization completed
}
break;
}
return 0;
}
//********************************************************************************
// Function name: driveSelectedDirectionMachine;
//
// In. arguments: unsigned char velocity - Sets the motors velocity,
// unsigned char direction - Sets the motors driving in
// selected direction,
// unsigned int distance - Sets the distance to be traveled.
//
// Out. arguments: Returns a 'unsigned char' indicating if the string transmition
// is finnish. (I.e. if all characters within string are sent and
// terminator character '0' is found)
//
// Transmition finnished - return '1',
// transmitting in progress - return '0';
//
// Description: Transmit commnad strings to drive both motors in the direction,
// velocity and distance received as parameters and contained in
// the gobal auxiliaruy functions already described.
// This function is non blocking and is used by
// the motorController application only!
//
// The distance controlling functionality is not yet implemented.
//
//********************************************************************************
unsigned char driveSelectedDirectionMachine(unsigned char velocity, unsigned char direction, unsigned int distance)
{
switch ( driveSelectedDirectionMachineState ){
// Set velocity, direction (forward) and motor N. for the
// first motor to be activated
case 0:
// If the speed value received as parameter does not exceed
// the controller maximum
if ( motorControllerAppAux <= 127 )
standardCommand[3] = velocity; // Set to speed given as parameter
else
standardCommand[3] = 0x7F; // Otherwise set speed to maximum
// Use of variable "motorControlAppAuxFlag" necessary to change the
// first motor to be activated in order to minimize effects like slight
// change of direction by changing the order in which the motors are
// activated each time a forward or backwards command is Tx
if (motorControlAppAuxFlag & motorControlAppAuxFlag2)
{
// Select the first motor (No. 2)
standardCommand[2] = 0x04;
}
else
{
// Select the second motor (No. 3)
standardCommand[2] = 0x06;
}
// Set the motor to the direction selected
standardCommand[2] = standardCommand[2] | ( direction & 0x01 );
driveSelectedDirectionMachineState = 1;
break;
// Tx commnad to activate first motor
case 1:
// If command Tx for one of the motor is finnnish
if ( txCommandString(standardCommand) )
driveSelectedDirectionMachineState = 2; // Send command for the other motor
break;
// Direction (forward) and motor N. for the second motor to be activated
case 2:
if (motorControlAppAuxFlag & motorControlAppAuxFlag2)
{
// Select the second motor (No. 3)
standardCommand[2] = 0x06;
}
else
{
// Select the first motor (No. 2)
standardCommand[2] = 0x04;
}
// Set the motor to the direction selected
standardCommand[2] = standardCommand[2] | ( direction & 0x01 );
driveSelectedDirectionMachineState = 3;
break;
// Tx commnad to activate second motor
case 3:
// If command Tx for the other motor is finnnish
if ( txCommandString(standardCommand) )
{
driveSelectedDirectionMachineState = 0; // Initialize driveBackwardMachine
motorControlAppAuxFlag=~motorControlAppAuxFlag; // Invert flag to change initial motor
// Sets last direction flag to the direction just set
motorControlAppAuxFlag2 = motorControllerAppAux2;
return 1; // Return value indicating that both motors were
// sucssefully set to driver forward with
// the selected velocity
}
break;
}
return 0;
}
//********************************************************************************
// Function name: turnSelectedDirectionMachine;
//
// In. arguments: unsigned char velocity - Sets the motors velocity,
// unsigned char direction - Sets the motors turning in
// selected direction,
// unsigned int angle - Sets angle of rotation.
//
// Out. arguments: Returns a 'unsigned char' indicating if the string transmition
// is finnish. (I.e. if all characters within string are sent and
// terminator character '0' is found)
//
// Transmition finnished - return '1',
// transmitting in progress - return '0';
//
// Description: Transmit commnad strings to drive both motors in order to turn
// the platform with the velocity to angle received as parameters
// and contained in the gobal auxiliaruy functions already
// described. This function is non blocking and is used by
// the motorController application only!
//
// The angle controlling functionality is not yet implemented.
//********************************************************************************
unsigned char turnSelectedDirectionMachine(unsigned char velocity, unsigned char direction, unsigned int angle)
{
switch ( turnSelectedDirectionMachineState ){
// Set velocity, direction (left/right) and motor N. for the
// first motor to be activated
case 0:
// If the speed value received as parameter does not exceed
// the controller maximum
if ( motorControllerAppAux <= 127 )
standardCommand[3] = velocity; // Set to speed given as parameter
else
standardCommand[3] = 0x7F; // Otherwise set speed to maximum
// Select the first motor (No. 2)
standardCommand[2] = 0x04;
// Sets the right direction for the No. 4 motor (left motor) in order
// for the oplatform to turn in the selected direction
if (direction)
{
// Set the motor to the forward direction (turn right)
standardCommand[2] = ( standardCommand[2] | 0x01 );
}
else
{
// Set the motor to the reverse direction (turn left)
standardCommand[2] = ( standardCommand[2] & 0xFE );
}
turnSelectedDirectionMachineState = 1;
break;
// Tx commnad to activate first motor
case 1:
// If command Tx for one of the motor is finnnish
if ( txCommandString(standardCommand) )
turnSelectedDirectionMachineState = 2; // Send command for the other motor
break;
// Direction (forward) and motor N. for the second motor to be activated
case 2:
// Select the second motor (No. 3)
standardCommand[2] = 0x06;
// Sets the right direction for the No. 5 motor (right motor) in order
// for the platform to turn in the selected direction
if (direction)
{
// Set the motor to the reverse direction (turn rigth)
standardCommand[2] = ( standardCommand[2] & 0xFE );
}
else
{
// Set the motor to the forward direction (turn leftt)
standardCommand[2] = ( standardCommand[2] | 0x01 );
}
turnSelectedDirectionMachineState = 3;
break;
// Tx commnad to activate second motor
case 3:
// If command Tx for the other motor is finnnish
if ( txCommandString(standardCommand) )
{
turnSelectedDirectionMachineState = 0; // Initialize driveBackwardMachine
return 1; // Return value indicating that both motors were
// sucssefully set to driver turn with
// the selected direction and velocity
}
break;
}
return 0;
}
//********************************************************************************
// Function name: breakMotorsMachine;
//
// In. arguments: void;
//
// Uses following global variables as input:
//
// - motorControllerAppAuxPtr: Used to save pointer to the
// stardard command string to be
// used by the "txCommandString"
// function to Tx a command.
//
// Out. arguments: Returns a 'unsigned char' indicating if the string transmition
// is finnish. (I.e. if all characters within string are sent and
// terminator character '0' is found)
//
// Transmition finnished - return '1',
// transmitting in progress - return '0';
//
// Description: Transmit commnad strings to stop both motors.
// This function is non blocking and is used by
// the motorController application only!
//********************************************************************************
unsigned char breakMotorsMachine(void)
{
switch ( breakMotorsMachineState ){
// Set velocity, direction (forward) and motor N. for the
// first motor to be activated
case 0:
standardCommand[3] = 0x00; // Set to speed to '0'
// Set auxiliary application pointer to the standard command frame to
// be modified with all the input arguments
motorControllerAppAuxPtr = standardCommand;
// Use of variable "motorControlAppAuxFlag" necessary to change the
// first motor to be stopped in order to minimize effects like slight
// change of direction by changing the order in which the motors are
// stopped each time break command is Tx
if (motorControlAppAuxFlag)
{
// Select the first motor (No. 2)
standardCommand[2] = 0x04;
}
else
{
// Select the second motor (No. 3)
standardCommand[2] = 0x06;
}
// Set the motor to the direction to reverse
standardCommand[2] = standardCommand[2] & 0xFE ;
breakMotorsMachineState = 1;
break;
// Tx commnad to stop first motor
case 1:
// If command Tx for one of the motor is finnnish
if ( txCommandString(motorControllerAppAuxPtr) )
breakMotorsMachineState = 2; // Send command for the other motor
break;
// Direction (forward) and motor N. for the second motor to be activated
case 2:
if (motorControlAppAuxFlag)
{
// Select the second motor (No. 3)
standardCommand[2] = 0x06;
}
else
{
// Select the first motor (No. 2)
standardCommand[2] = 0x04;
}
// Set the motor to the direction to reverse
// standarCommand[2] = standarCommand[2] | ( motorControllerAppAux2 & 0x01 ); - Is there need to do this again???
breakMotorsMachineState = 3;
break;
// Tx commnad to stop second motor
case 3:
// If command Tx for the other motor is finnnish
if ( txCommandString(motorControllerAppAuxPtr) )
{
breakMotorsMachineState = 0; // Initialize driveBackwardMachine
motorControlAppAuxFlag = ~motorControlAppAuxFlag; // Invert flag to change initial motor
return 1; // Return value indicating that both motors were
// sucssefully set to driver forward with
// the selected velocity
}
break;
}
return 0;
}
//********************************************************************************
// Function name: motorControllerApp;
//
// In. arguments: void;
//
// Out. arguments: void
//
// Description: Motor control application. This function needs to be run in
// the main function regularly in order for the controller to
// process the commands.
//********************************************************************************
void motorControllerApp(void)
{
switch ( motorControllerAppFunctionIndex )
{
// Idle state
case 0:
break;
// Initialization state
case 1:
if ( motorControllerInitMachine() ) // If motor controler initialization is complete
motorControllerAppFunctionIndex = 0; // Go to Idle state
break;
// Runs "driveSelectedDirectionMachine" function to Tx forward and
// reverse drive commands at a user selected speed and distance
case 2:
if ( driveSelectedDirectionMachine(motorControllerAppAux, motorControllerAppAux2, motorControllerAppAux3) )
motorControllerAppFunctionIndex = 0; // Go to Idle state
break;
// Runs "turnSelectedDirectionMachine" function to Tx turn right and
// turn left commands at a user selected speed and angle
case 3:
if ( turnSelectedDirectionMachine(motorControllerAppAux, motorControllerAppAux2, motorControllerAppAux3) )
motorControllerAppFunctionIndex = 0; // Go to Idle state
break;
case 4:
if ( breakMotorsMachine() )
motorControllerAppFunctionIndex = 0; // Go to Idle state
break;
}
}
//********************************************************************************
// Function name: driveForward;
//
// In. arguments: unsigned char velocity - Sets the motors velocity
// int distance - Sets the distance to be traveled
//
//
// Out. arguments: Returns a 'unsigned char' indicating if the command was
// sucessfully initiatlized;
//
// Addition sucessfull - return '1',
// Adition not sucessfull - return '0';
//
// Description: Initializes the string command Tx which contains the command
// frame to set the motors driving in the forward direction with
// the velocity selected.
//
// The distance controlling functionality is not yet implemented.
//
// This function is non blocking and is to be used by
// external applications.
//********************************************************************************
unsigned char driveForward(unsigned char velocity, int distance)
{
// If the controller is in Idle state
if ( motorControllerAppFunctionIndex == 0 )
{
// If velocity exceeds the maximum
if ( velocity > MOTORSMAXIMUMVELOCITY)
velocity = (MOTORSMAXIMUMVELOCITY + MOTORSMINIMUMOPERATINGVELOCITY); // Set to real motor controller maximum
else
velocity += MOTORSMINIMUMOPERATINGVELOCITY; // Otherwise set to real motor controller range of
// velocity that make the plataform motors run
// Save selected velocity to global variable to be used by the
// "driveSelectedDirectionMachine" function that will send the
// commands to activate the motors
motorControllerAppAux = velocity;
// Save selected direction (forward) to global variable to be used by the
// "driveSelectedDirectionMachine" function that will send the
// commands to activate the motors
motorControllerAppAux2 = 1;
// Save distance to be travelled to global variable to be used by the
// "turnSelectedDirectionMachine" function that will send the
// commands to activate the motors
motorControllerAppAux3 = distance;
// Sets the motorController application to the state that runs the
// "driveSelectedDirectionMachine" function
motorControllerAppFunctionIndex = 2;
return 1; // Return indication of sucessefull command initialization
}
return 0; // Otherwise return indication of unsucessefull initialization
}
//********************************************************************************
// Function name: driveReverse;
//
// In. arguments: unsigned char velocity - Sets the motors velocity
// int distance - Sets the distance to be traveled
//
//
// Out. arguments: Returns a 'unsigned char' indicating if the command was
// sucessfully initiatlized;
//
// Initiatlization sucessfull - return '1',
// Initiatlization not sucessfull - return '0';
//
// Description: Initializes the string command Tx which contains the command
// frame to set the motors driving in the reverse direction with
// the velocity selected.
//
// The distance controlling functionality is not yet implemented.
//
// This function is non blocking and is to be used by
// external applications.
//********************************************************************************
unsigned char driveReverse(unsigned char velocity, unsigned int distance)
{
// If the controller is in Idle state
if ( motorControllerAppFunctionIndex == 0 )
{
// If velocity exceeds the maximum
if ( velocity > MOTORSMAXIMUMVELOCITY)
// Set to real motor controller maximum
velocity = (MOTORSMAXIMUMVELOCITY + MOTORSMINIMUMOPERATINGVELOCITY);
else
// Otherwise set to real motor controller range of velocity that make the plataform motors run
velocity += MOTORSMINIMUMOPERATINGVELOCITY;
// Save selected velocity to global variable to be used by the
// "driveSelectedDirectionMachine" function that will send the
// commands to activate the motors
motorControllerAppAux = velocity;
// Save selected direction (reverse) to global variable to be used by the
// "driveSelectedDirectionMachine" function that will send the
// commands to activate the motors
motorControllerAppAux2 = 0;
// Save distance to be travelled to global variable to be used by the
// "turnSelectedDirectionMachine" function that will send the
// commands to activate the motors
motorControllerAppAux3 = distance;
// Sets the motorController application to the state that runs the
// "driveSelectedDirectionMachine" function
motorControllerAppFunctionIndex = 2;
return 1; // Return indication of sucessefull command initialization
}
return 0; // Otherwise return indication of unsucessefull initialization
}
//********************************************************************************
// Function name: turnRight;
//
// In. arguments: unsigned char velocity - Sets the motors velocity
// unsigned char angle - Sets the angle of turn
//
//
// Out. arguments: Returns a 'unsigned char' indicating if the command was
// sucessfully initiatlized;
//
// Addition sucessfull - return '1',
// Adition not sucessfull - return '0';
//
// Description: Initializes the string command Tx which contains the command
// frame to set the motors to turn right with
// the velocity selected.
//
// The angle controlling functionality is not yet implemented.
//
// This function is non blocking and is to be used by
// external applications.
//********************************************************************************
unsigned char turnRight(unsigned char velocity, unsigned int angle)
{
// If the controller is in Idle state
if ( motorControllerAppFunctionIndex == 0 )
{
// If velocity exceeds the maximum
if ( velocity > MOTORSMAXIMUMVELOCITY)
// Set to real motor controller maximum
velocity = (MOTORSMAXIMUMVELOCITY + MOTORSMINIMUMOPERATINGVELOCITY);
else
// Otherwise set to real motor controller range of velocity that make the plataform motors run
velocity += MOTORSMINIMUMOPERATINGVELOCITY; // velocity that make the plataform motors run
// Save selected velocity to global variable to be used by the
// "turnSelectedDirectionMachine" function that will send the
// commands to activate the motors
motorControllerAppAux = velocity;
// Save selected turning direction (right) to global variable to be used
// by the "turnSelectedDirectionMachine" function that will send the
// commands to activate the motors
motorControllerAppAux2 = 1;
// Save selected rotation angle to global variable to be used by the
// "turnSelectedDirectionMachine" function that will send the
// commands to activate the motors
motorControllerAppAux3 = angle;
// Sets the motorController application to the state that runs the
// "turnSelectedDirectionMachine" function
motorControllerAppFunctionIndex = 3;
return 1; // Return indication of sucessefull command initialization
}
return 0; // Otherwise return indication of unsucessefull initialization
}
//********************************************************************************
// Function name: turnLeft;
//
// In. arguments: unsigned char velocity - Sets the motors velocity
// unsigned char angle - Sets the angle of turn
//
//
// Out. arguments: Returns a 'unsigned char' indicating if the command was
// sucessfully initiatlized;
//
// Addition sucessfull - return '1',
// Adition not sucessfull - return '0';
//
// Description: Initializes the string command Tx which contains the command
// frame to set the motors to turn left with
// the velocity selected.
//
// The angle controlling functionality is not yet implemented.
//
// This function is non blocking and is to be used by
// external applications.
//********************************************************************************
unsigned char turnLeft(unsigned char velocity, unsigned int angle)
{
// If the controller is in Idle state
if ( motorControllerAppFunctionIndex == 0 )
{
// If velocity exceeds the maximum
if ( velocity > MOTORSMAXIMUMVELOCITY)
// Set to real motor controller maximum
velocity = (MOTORSMAXIMUMVELOCITY + MOTORSMINIMUMOPERATINGVELOCITY);
else
// Otherwise set to real motor controller range of velocity that make the plataform motors run
velocity += MOTORSMINIMUMOPERATINGVELOCITY;
// Save selected velocity to global variable to be used by the
// "turnSelectedDirectionMachine" function that will send the
// commands to activate the motors
motorControllerAppAux = velocity;
// Save selected turning direction (left) to global variable to be used
// by the "turnSelectedDirectionMachine" function that will send the
// commands to activate the motors
motorControllerAppAux2 = 0;
// Save selected rotation angle to global variable to be used by the
// "turnSelectedDirectionMachine" function that will send the
// commands to activate the motors
motorControllerAppAux3 = angle;
// Sets the motorController application to the state that runs the
// "turnSelectedDirectionMachine" function
motorControllerAppFunctionIndex = 3;
return 1; // Return indication of sucessefull command initialization
}
return 0; // Otherwise return indication of unsucessefull initialization
}
//********************************************************************************
// Function name: breakMotors;
//
// In. arguments: void;
//
// Out. arguments: Returns a 'unsigned char' indicating if the command was
// sucessfully added to the command Tx buffer;
//
// Addition sucessfull - return '1',
// Adition not sucessfull - return '0';
//
// Description: Initializes the string command Tx which contains the command
// frame to breake the motors.
//
// The distance controlling functionality is not yet implemented.
//
// This function is non blocking and is to be used by
// external applications.
//********************************************************************************
unsigned char breakMotors(void)
{
// If the controller is in Idle state
if ( motorControllerAppFunctionIndex == 0 )
{
// Sets the motorController application to the state that runs the
// "breakMotorsMachine" function
motorControllerAppFunctionIndex = 4;
return 1; // Return indication of sucessefull command initialization
}
return 0; // Otherwise return indication of unsucessefull initialization
}
//********************************************************************************
// Function name: calculateDeadReckonPosition;
//
// In. arguments: unsigned char direction - Sets the direction of the
// platform, forward, revers,
// turning right or turning left:
//
// Forward - direction = 0x00,
// Reverse - direction = 0x01,
// Turning right - direction = 0x02,
// Turning left - direction = 0x03;
//
// Out. arguments: void;
//
// Description: Calculates the curent position of the platform in relation to
// the last known position and the new traveled distance and
// direction.
//********************************************************************************
void calculateDeadReckonPosition(unsigned char direction, unsigned char rightTrackTicks, unsigned char leftTrackTicks)
{
switch ( direction )
{
// Forward
case 0:
if ( rightTrackTicks == leftTrackTicks )
{
xPos += rightTrackTicks*cos(tetha);
yPos += rightTrackTicks*sin(tetha);
}
else
{
tetha += (rightTrackTicks-leftTrackTicks)*deltaTetha;
if ( rightTrackTicks > leftTrackTicks )
{
xPos += leftTrackTicks*cos(tetha);
yPos += leftTrackTicks*sin(tetha);
}
else
{
xPos += rightTrackTicks*cos(tetha);
yPos += rightTrackTicks*sin(tetha);
}
}
break;
// Reverse
case 1:
if ( rightTrackTicks == leftTrackTicks )
{
xPos -= rightTrackTicks*cos(tetha);
yPos -= rightTrackTicks*sin(tetha);
}
else
{
tetha -= (rightTrackTicks-leftTrackTicks)*deltaTetha;
if ( rightTrackTicks > leftTrackTicks )
{
xPos -= leftTrackTicks*cos(tetha);
yPos -= leftTrackTicks*sin(tetha);
}
else
{
xPos -= rightTrackTicks*cos(tetha);
yPos -= rightTrackTicks*sin(tetha);
}
}
break;
// Turning right
case 2:
if ( rightTrackTicks == leftTrackTicks )
tetha -= rightTrackTicks*deltaTetha;
else
{
if ( rightTrackTicks > leftTrackTicks )
{
tetha -= leftTrackTicks*deltaTetha;
xPos += (rightTrackTicks-leftTrackTicks)*cos(tetha);
yPos += (rightTrackTicks-leftTrackTicks)*sin(tetha);
}
else
{
tetha -= rightTrackTicks*deltaTetha;
xPos += (leftTrackTicks-rightTrackTicks)*cos(tetha);
yPos += (leftTrackTicks-rightTrackTicks)*sin(tetha);
}
}
break;
// Turning left
case 3:
if ( rightTrackTicks == leftTrackTicks )
tetha += rightTrackTicks*deltaTetha;
else
{
if ( rightTrackTicks > leftTrackTicks )
{
tetha += leftTrackTicks*deltaTetha;
xPos += (rightTrackTicks-leftTrackTicks)*cos(tetha);
yPos += (rightTrackTicks-leftTrackTicks)*sin(tetha);
}
else
{
tetha += rightTrackTicks*deltaTetha;
xPos += (leftTrackTicks-rightTrackTicks)*cos(tetha);
yPos += (leftTrackTicks-rightTrackTicks)*sin(tetha);
}
}
break;
}
}
//********************************************************************************
// Function name: ISR(TIMER2_OVF_vect);
//
// In. arguments: NA;
//
// Out. arguments: NA;
//
// Description: Hardware timer interrupt handler used to generate timers ticks.
//********************************************************************************
ISR(TIMER1_OVF_vect)
{
// cli();
++c;
if ( c > 200 )
{
if ( PORTB & 0x80 )
PORTB &= 0x7F;
else
PORTB |= 0x80;
c = 0;
}
// sei();
}
//********************************************************************************
// Function name: ISR(TIMER2_OVF_vect);
//
// In. arguments: NA;
//
// Out. arguments: NA;
//
// Description: Hardware timer interrupt handler used to generate timers ticks.
//********************************************************************************
ISR(TIMER3_OVF_vect)
{
// cli();
++d;
if ( d > 100 )
{
if ( PORTB & 0x40 )
PORTB &= 0xB0;
else
PORTB |= 0x40;
d = 0;
/*calculateDeadReckonPosition(1, 1, 2);
sprintf(chArray, "%f", xPos);
// uartSendString( "Actual X position: " );
uartSendString( chArray );
uartSendString( "| " );
sprintf(chArray2, "%f", yPos);
// uartSendString( "Actual Y position: /0" );
uartSendString( chArray2 );
uartSendString( nl );*/
}
// sei();
}
ISR(INT0_vect)
{
// ++rightTrackTicks;
}
ISR(INT1_vect)
{
// ++leftTrackTicks;
}
//******************************* End of file ************************************