Revision 0:5b9f87a086ce, committed 2015-09-22

Comitter:

baraki

Date:

Tue Sep 22 21:49:44 2015 +0000

Commit message:

for use with blue mbed

Changed in this revision

diff -r 000000000000 -r 5b9f87a086ce NeoStrip.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/NeoStrip.lib	Tue Sep 22 21:49:44 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aswild/code/NeoStrip/#f531a2be180d

diff -r 000000000000 -r 5b9f87a086ce USBDevice.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/USBDevice.lib	Tue Sep 22 21:49:44 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/USBDevice/#0f216c4e75e5

diff -r 000000000000 -r 5b9f87a086ce bluetoothComm.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/bluetoothComm.cpp	Tue Sep 22 21:49:44 2015 +0000
@@ -0,0 +1,93 @@
+#include "bluetoothComm.h"
+
+//USBSerial console1; // tx, rx
+Serial bt(p9,p10); // tx, rx
+bool bluetoothConnected;
+bool validBluetoothData;
+char bluetoothData[50]={0};
+int getIndex=0;
+// Read data from the bluetooth
+// Stores data in the bluetoothData array
+// Messages must end in the null termination character ('\0')
+// Will return true if a complete bluetooth message has been received (and thus if the data in bluetoothData is valid)
+// This method DOES NOT BLOCK
+//   it will read as long as data is available and then stop, whether or not the message is complete
+//   at the next call, it will continue reading and adding to the array until '\0' is received
+//  Thus, this method is meant to be called once per loop
+bool getBluetoothData()
+{
+  if(validBluetoothData) // reset array
+  {
+    getIndex = 0;
+    validBluetoothData = false;
+  }
+
+  if(!bt.readable())
+    return false;
+
+  while(bt.readable() && !validBluetoothData)
+  {
+    bluetoothData[getIndex] = getBluetoothChar();
+    validBluetoothData = (bluetoothData[getIndex] == '\0');
+    getIndex++;
+  }
+  return validBluetoothData;
+}
+
+// Returns the value of bluetoothConnected
+// This variable must be set somewhere, probably in processBluetoothData
+bool isBluetoothConnected()
+{
+  return bluetoothConnected;
+}
+
+void setBluetoothConnected(bool btCon){
+    bluetoothConnected = btCon;
+}
+
+// Read bluetooth data and then process it
+void processBluetoothData()
+{
+  if(!getBluetoothData())
+    return;
+  // DO SOMETHING WITH bluetoothData HERE
+  // If it is a valid message, set bluetoothConnected = true
+  bluetoothConnected = true;
+}
+
+char* returnBluetoothData(){
+  return bluetoothData;
+}
+
+bool isBluetoothDataValid()
+{
+  return validBluetoothData;
+}
+
+void robotLoop()
+{
+  //robotPrintlnDebug();
+  processBluetoothData();
+}
+
+void robotSetup(int baud_rate)
+{
+  bt.baud(baud_rate);
+  bluetoothConnected = false;
+  validBluetoothData = false;
+  bluetoothData[0] = '\0';
+}
+
+void sendBluetoothData(const char* data)
+{
+  //robotPrintDebug("Sending BT data <"); robotPrintDebug(data); robotPrintlnDebug(">");
+  int index = 0;
+  for(; index < length(data); index++)
+    {
+      sendBluetoothChar(data[index]);
+      wait_ms(5);
+    }
+  if(data[index-1] != '\0')
+    sendBluetoothChar('\0');
+  //robotPrintDebug("Sent BT data <"); robotPrintDebug(data); robotPrintlnDebug(">");
+}
\ No newline at end of file

diff -r 000000000000 -r 5b9f87a086ce bluetoothComm.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/bluetoothComm.h	Tue Sep 22 21:49:44 2015 +0000
@@ -0,0 +1,36 @@
+#ifndef INCL_ROBOTLIBRARY_H
+#define INCL_ROBOTLIBRARY_H
+
+#include "mbed.h"
+#include "USBSerial.h"
+
+#define bluetoothAvailable() (bt.readable())
+#define sendBluetoothChar(toSend) (bt.putc((char)toSend))
+#define getBluetoothChar() ((char) bt.getc())
+#include "string_functions.h"
+//#define robotPrint(toPrint) console1.printf(toPrint)
+//#define robotPrintln(toPrint) console1.printf(toPrint)
+
+#define PRINT_DEBUG 0
+#if PRINT_DEBUG == 1
+//#define robotPrintDebug(toPrint) robotPrint(toPrint)
+//#define robotPrintlnDebug(toPrint) robotPrintln(toPrint)
+#else
+//#define robotPrintDebug(toPrint)
+//#define robotPrintlnDebug(toPrint)
+#endif
+
+extern Serial bt; // tx, rx
+//extern USBSerial console1;
+extern char bluetoothData[50];
+bool getBluetoothData();
+bool isBluetoothConnected();
+void setBluetoothConnected(bool btCon);
+void processBluetoothData();
+char* returnBluetoothData();
+bool isBluetoothDataValid();
+void robotLoop();
+void robotSetup(int baud_rate);
+void sendBluetoothData(const char* data);
+
+#endif
\ No newline at end of file

diff -r 000000000000 -r 5b9f87a086ce main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Sep 22 21:49:44 2015 +0000
@@ -0,0 +1,323 @@
+#include "mbed.h"
+#include "math.h"
+#include "bluetoothComm.h"
+#include "NeoStrip.h"
+
+#define BT_BAUD 9600
+#define NUM_LRAS 5
+#define NUM_ENS NUM_LRAS
+#define NUM_LEVELS 7
+#define NUM_PULSES 5
+
+#define NUM_LIDAR 7 //number of lidar
+
+#define LED_NUM NUM_LIDAR
+#define LED_PIN p16
+
+NeoStrip leds(LED_PIN, LED_NUM);
+
+PwmOut lra[NUM_LRAS] = {
+    PwmOut(p25), PwmOut(p24),PwmOut(p23),PwmOut(p22),
+    PwmOut(p21)
+};
+
+DigitalOut myLED(LED1);
+
+Ticker motorPulse;
+Timeout motorOff;
+
+// given a number between 0 and NUM_PULSES, returns the number of times
+// the LRA should be pulsed during the maximum pulse interval
+int pulse[NUM_PULSES] = {9, 8, 4, 2, 1};
+
+// given a number between 0 and NUM_LEVELS, returns the
+// corresponding intensity of the LRA
+float intensity[8] = {0.0, 150.0/255.0, 170.0/255.0, 190.0/255.0,210.0/255.0,230.0/255.0, 250.0/255.0,255.0/255.0};
+
+DigitalOut lra_en[NUM_ENS] = {
+   DigitalOut(p7), DigitalOut(p8),DigitalOut(p12),DigitalOut(p13),
+    DigitalOut(p14)
+};
+
+//length of the longest period in milliseconds
+int longPeriod=600; //1024
+int shortPeriod=longPeriod/pulse[0];
+int onPeriod=60; //100
+
+bool isOn[NUM_LRAS+2];
+volatile int lraPulse[NUM_LRAS + 2];
+volatile int lraCount;
+volatile char lraIntensity[NUM_LRAS + 2];
+// currently three modes of vibrating
+// vibration period is divided into three sections
+// motor can be: on/mid ; on/off ; on/mid
+// MODE 1: ON ON ON
+// MODE 2: MID OFF ON
+// MODE 3: ON OFF MID
+int lraMode[NUM_LRAS+2];
+
+Serial pc(USBTX,USBRX);
+
+void motorOff3_cb(void);
+void motorOff2_cb(void);
+void motorOff1_cb(void);
+void processData(char *n);
+
+void setColor(int num, int power){
+    int lednum = num*2;
+    if(num == 5)
+        lednum = 3;
+    if(num == 6)
+        lednum = 5;
+    if(power >= 240)
+         leds.setPixel(lednum, 100, 0, 0);
+    else if(power >= 220 && power < 240)
+        leds.setPixel(lednum, 0, 100,0);
+    else if(power >= 200 && power < 220)
+        leds.setPixel(lednum,100,0,100);
+    else if(power >= 100 && power < 200)
+        leds.setPixel(lednum,0,0,100);
+    else
+        leds.setPixel(lednum,0,0,0);
+}
+
+void processData(char *n) {
+    //try to take these variables out and make them globals to speed processing
+    int i = 0;
+    int index = 0;
+    
+    char input =  n[i];
+    
+    unsigned char newIntensity = 0;
+    
+    int m1pulse = 0;
+    int m1int = 0;
+    
+    int m2pulse = 0;
+    int m2int = 0;
+    
+    while(input !='\0'){
+        switch ( index ) {
+            case 0: {
+                // bits 0-2: m1 pulse
+                // bits 3-5: m1 intensity
+                // bits 6-7: first 2 bits of m1 mode
+                lraPulse[0] = pulse[(int)((input & 0xE0) >> 5) - 1]; //0b11100000
+                m1pulse = ((input & 0xE0) >> 5);
+                m1int = ((input & 0x1C) >> 2);
+                lraIntensity[0] = intensity[(int) ((input & 0x1C) >> 2)]; //0b00011100
+                setColor(0, lraIntensity[0]);
+                lraMode[0] = (input & 0x3) << 1; //0b00000011
+                break;
+            }
+            case 1: {
+                // bit 0: last bit of m1 mode
+                // bits 1-3: m2 pulse
+                // bits 4-6: m2 intensity
+                // bit 7: first bit of m2 mode
+                lraMode[0] = lraMode[0] | ((input & 0x80) >> 7); //0b10000000
+                //printf("m1 pulse: %d, intensity: %d, mode: %d \n",m1pulse,m1int,lraMode[0]);
+                printf("m1 pulse: %d, intensity: %f, mode: %d \n",lraPulse[0],lraIntensity[0],lraMode[0]);
+                m2pulse = ((input & 0x70) >> 4);
+                m2int = ((input & 0xE) >> 1);
+                lraPulse[1] = pulse[(int)((input & 0x70) >> 4) - 1]; //0b01110000
+                lraIntensity[1] = intensity[(int)((input & 0xE) >> 1)]; //0b00001110
+                setColor(1, lraIntensity[1]);
+                lraMode[1] = (input & 0x01) << 2; //0b00000001
+                break;
+            }
+            case 2: {
+                // bits 0-1: last 2 bits of m2 mode
+                // bits 2-4: m3 pulse
+                // bits 5-7: m3 intensity
+                lraMode[1] = lraMode[1] | ((input & 0xC0) >> 6); //0b11000000
+                //printf("m2 pulse: %d, intensity: %d, mode: %d \n",m2pulse,m2int,lraMode[1]);
+                printf("m2 pulse: %d, intensity: %f, mode: %d \n",lraPulse[1],lraIntensity[1],lraMode[1]);
+                lraPulse[2] = pulse[(int)((input & 0x38) >> 3) - 1]; //0b00111000
+                lraIntensity[2] = intensity[(int)(input & 0x7)]; //0b00000111
+                setColor(2, lraIntensity[2]);
+                break;
+            }
+            case 3: {
+                // bits 0-2: m3 mode
+                // bits 3-5: m4 pulse
+                // bits 6-7: first 2 bits of m4 intensity
+                lraMode[2] = (input & 0xE0) >> 5; //0b11100000
+                //printf("m3 pulse: %d, intensity: %d, mode: %d \n",((input & 0x38) >> 3),(input & 0x7),lraMode[2]);
+                printf("m3 pulse: %d, intensity: %f, mode: %d \n",lraPulse[2],lraIntensity[2],lraMode[2]);
+                lraPulse[3] = pulse[(int)((input & 0x1C) >> 2) - 1]; //0b00011100
+                newIntensity = (input & 0x03) << 1; //0b00000011
+                break;
+            }
+            case 4: {
+                // bit 0: last bit of m4 intensity
+                // bits 1-3: m4 mode
+                // bits 4-6: m5 pulse
+                // bit 7: first bit of m5 intensity
+                newIntensity = newIntensity | ((input & 0x80) >> 7); //0b10000000
+                lraIntensity[3] = intensity[(int)newIntensity];
+                setColor(3, lraIntensity[3]);
+                lraMode[3] = (input & 0x70) >> 4; //0b01110000
+                lraPulse[4] = pulse[(int)((input & 0xE) >> 1) - 1]; //0b00001110
+                newIntensity = (input & 0x01) << 2; //0b00000001
+                break;
+            }
+            case 5: {
+                // bits 0-1: last 2 bits of m5 intensity
+                // bits 2-4: m5 mode
+                // bits 5-7: 3 bit checksum supposedly
+                newIntensity = newIntensity | ((input & 0xC0) >> 6); //0b11000000
+                lraIntensity[4] = intensity[(int)newIntensity];
+                setColor(4, lraIntensity[4]);
+                lraMode[4] = (input & 0x38) >> 3; //0b00111000
+                break;
+            }
+            case 6: { //UP
+                // bits 0-2: m3 pulse UP
+                // bits 3-5: m3 intensity UP
+                lraPulse[5] = pulse[(int)((input & 0xE0) >> 5) - 1]; //0b11100000
+                //m3pulse = ((input & 0xE0) >> 5);
+                //m3int = ((input & 0x1C) >> 2);
+                lraIntensity[5] = intensity[(int) ((input & 0x1C) >> 2)]; //0b00011100
+                setColor(5, lraIntensity[5]);
+                break;
+            }
+            case 7: { //DOWN
+                // bits 0-2: m3 pulse DOWN
+                // bits 3-5: m3 intensity1 mode DOWN
+                lraPulse[6] = pulse[(int)((input & 0xE0) >> 5) - 1]; //0b11100000
+                //m3pulse = ((input & 0xE0) >> 5);
+                //m3int = ((input & 0x1C) >> 2);
+                lraIntensity[6] = intensity[(int) ((input & 0x1C) >> 2)]; //0b00011100
+                setColor(6, lraIntensity[6]);
+                
+                //if lraPulse 5 or 6 are >3, that means set middle motor to highest pulse
+                if(lraPulse[5] < 4 || lraPulse[6] < 4){
+                    lraPulse[2] = pulse[NUM_PULSES-1];
+                    lraIntensity[2] = intensity[NUM_LEVELS];
+                }
+                break;
+            }
+            default: {
+                // do nothing
+                break;
+            }
+        }
+        index++;
+        i++;
+        input = n[i];
+     }
+}
+
+void motorPulse_cb(void){      
+    pc.printf("hi\n");
+    for(int n=0; n<NUM_LRAS; n++) {
+        //pulse motor if lraCount is 0
+        if(lraCount % lraPulse[n] == 0) {
+            isOn[n] = true;
+            //Set LRA PWM to desired intensity
+            //if((lraMode[n] == 1) || (lraMode[n] == 3)) 
+            lra[n] = lraIntensity[n]; //full intensity 
+            pc.printf("M%d : %f\n",n,lraIntensity[n]);
+            //else if(lraMode[n] == 2)
+             //   lra[n] = lraIntensity[n] - 0.1; //"mid" intensity
+            //Turn LRA On by setting enable pin to 1
+            lra_en[n] = 1;
+        }
+    }
+    lraCount++;
+    if(lraCount > 8)
+        lraCount = 1;
+    motorOff.detach();
+    motorOff.attach(&motorOff3_cb,((float)onPeriod)/1300.0);
+}
+
+int main (void)
+{
+    //Init communication
+    robotSetup(BT_BAUD); //set baud rate of bluetooth connection
+    pc.baud(9600);
+    
+    leds.clear();
+    leds.setBrightness(0.15);
+
+    //initialize and start everything
+    lraCount = 0;
+    for(int i = 0; i < NUM_LRAS; i++) {
+        //set pwm frequency
+        lra[i].period_us(90);
+        //initialize values
+        //set starting vibration
+        lraIntensity[i] = 0.5f;
+        lraPulse[i] = 0;
+        lraMode[i] = 0;
+        
+        lra_en[i] = 0;
+        lra[i] = lraIntensity[i]; //set initial intensity
+        isOn[i] = false;
+    }
+    
+    motorPulse.attach(&motorPulse_cb, ((float) shortPeriod)/1000.0);
+    
+    while(1){
+        if(getBluetoothData()){ //if the bluetooth data has finished sending (there is a \0 detected)
+            processData(bluetoothData);
+        }
+    }
+}
+void motorOff1_cb(void) {
+    for(int n=0;n<NUM_LRAS;n++){
+        if(isOn[n]) {
+            if(lraMode[n] == 2 || lraMode[n] == 3){//turn off motor
+                //do NOT set isOn to 0
+                //isOn is used to tell if the motor is in the midst of a pulse
+                //even if it is turned off during this phase
+                //Set LRA PWM to 0.5
+                lra[n] = 0.5f; // that turns off the motor!
+                //Turn LRA Off by setting enable pin to 0
+                lra_en[n] = 0; // no braking happening
+            }
+            //if mode is 1, leave it on
+        }
+        //motorOff[n].detach();
+        //motorOff[n].attach(&motorOff2_cb,((float)onPeriod)/3000.0);
+    }
+}
+
+void motorOff2_cb(void) {
+    for(int n=0;n<NUM_LRAS;n++){
+        if(isOn[n]) {
+            if(lraMode[n] == 2){ //turn motor back on
+                lra[n] = lraIntensity[n];
+                lra_en[n] = 1;
+            }
+            else if(lraMode[n] == 3){//turn motor to mid intensity
+                lra[n] = lraIntensity[n] - 0.1;
+                lra_en[n] = 1;
+            }
+        }
+        //motorOff[n].detach();
+        //motorOff[n].attach(&motorOff3_cb,((float)onPeriod)/3000.0);
+    }
+}
+
+void motorOff3_cb(void) {
+    lra[0] = 0.2f;
+    lra[1] = 0.2f;
+    lra[2] = 0.2f;
+    lra[3] = 0.2f;
+    lra[4] = 0.2f;
+    lra_en[0] = 0.2f;
+    lra_en[1] = 0.2f;
+    lra_en[2] = 0.2f;
+    lra_en[3] = 0.2f;
+    lra_en[4] = 0.2f;
+    //for(int n=0;n<NUM_LRAS;n++){
+    //    if(isOn[n]) {
+    //        //turn em off
+    //        isOn[n] = 0;
+    //        lra[n] = 0.2f;
+    //        lra_en[n] = 0;
+    //    }
+    //}
+}
\ No newline at end of file

diff -r 000000000000 -r 5b9f87a086ce mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Sep 22 21:49:44 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/4f6c30876dfa
\ No newline at end of file

diff -r 000000000000 -r 5b9f87a086ce string_functions.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/string_functions.h	Tue Sep 22 21:49:44 2015 +0000
@@ -0,0 +1,220 @@
+#ifndef INCL_STRING_FUNCTIONS
+#define INCL_STRING_FUNCTIONS
+
+#include <stdio.h>
+#include <stdlib.h>
+
+inline int length(const char* source);
+inline int indexOf(const char* source, const char* target);
+inline bool contains(const char* source, const char* target);
+inline void substring(const char* source, int startIndex, int endIndex, char* dest);
+inline void substring(const char* source, int startIndex, char* dest);
+inline void substring(const char* source, int startIndex, int endIndex, char* dest, int destLength);
+inline void trim(char* source, int sourceLength);
+inline bool equals(const char* first, const char* second);
+inline bool equalsIgnoreCase(const char* first, const char* second);
+inline void strcpy(const char* source, char* dest, int destLength);
+inline void concat(const char* first, const char* second, char* dest, int destLength);
+inline void concatInt(const char* first, int second, char* dest, int destLength);
+
+
+
+
+
+//============================================================================================
+// Gets the length of the character array
+//============================================================================================
+int length(const char* source)
+{
+  int length = 0;
+  for(; source[length] != '\0'; length++);
+  return length;
+}
+
+//============================================================================================
+// Gets the index of the given string, or -1 if not found
+//============================================================================================
+int indexOf(const char* source, const char* target)
+{
+  int targetLength = length(target);
+  int sourceLength = length(source);
+  int index = -1;
+  for(int i = 0; i <= sourceLength - targetLength && index == -1; i++)
+  {
+    bool foundTarget = true;
+    for(int n = 0; n < targetLength && i+n < sourceLength; n++)
+    {
+      if(source[i+n] != target[n])
+        foundTarget = false;
+    }
+    if(foundTarget)
+      index = i;
+  }
+  return index;
+}
+
+//============================================================================================
+// Checks if the given string is contained in the source string
+//============================================================================================
+bool contains(const char* source, const char* target)
+{
+  return indexOf(source, target) >= 0;
+}
+
+//============================================================================================
+// Returns a substring of the character array
+// First is inclusive, second is exclusive
+// Returns itself if bounds are bad
+// Assumes beginning / end if either bound is negative
+//============================================================================================
+void substring(const char* source, int startIndex, int endIndex, char* dest)
+{
+  substring(source, startIndex, endIndex, dest, 20);
+}
+
+//============================================================================================
+// Returns a substring of the character array
+// First is inclusive, second is exclusive
+// Returns itself if bounds are bad
+// Assumes beginning / end if either bound is negative
+//============================================================================================
+void substring(const char* source, int startIndex, char* dest)
+{
+  substring(source, startIndex, length(source), dest, 30);
+}
+
+//============================================================================================
+// Returns a substring of the character array
+// First is inclusive, second is exclusive
+// Returns itself if bounds are bad
+// Assumes beginning / end if either bound is negative
+//============================================================================================
+void substring(const char* source, int startIndex, int endIndex, char* dest, int destLength)
+{
+  char temp[destLength];
+  if(startIndex < 0)
+    startIndex = 0;
+  if(endIndex < 0)
+    endIndex = 0;
+  if(endIndex < startIndex)
+  {
+    dest[0] = '\0';
+    return;
+  }
+  if(endIndex >= length(source))
+    endIndex = length(source);
+  if(destLength < endIndex - startIndex + 1)
+  {
+    dest[0] = '\0';
+    return;
+  }
+  for(int i = 0; i < endIndex - startIndex; i++)
+  {
+    temp[i] = source[startIndex + i];
+  }
+  for(int i = 0; i < endIndex - startIndex; i++)
+    dest[i] = temp[i];
+  dest[endIndex - startIndex] = '\0';
+}
+
+//============================================================================================
+// Removing leading and trailing spaces or new lines
+//============================================================================================
+void trim(char* source, int sourceLength)
+{
+  char temp[sourceLength];
+  int startIndex = 0;
+  int endIndex = length(source)-1;
+  for(; startIndex < length(source) && (source[startIndex] == ' ' || source[startIndex] == '\n' || source[startIndex] == '\t'); startIndex++);
+  for(; endIndex >= 0 && (source[endIndex] == ' ' || source[endIndex] == '\n' || source[startIndex] == '\t'); endIndex--);
+  endIndex++;  
+  substring(source, startIndex, endIndex, temp, sizeof(temp)-1);
+  strcpy(temp, source, sourceLength-1);
+}
+
+//============================================================================================
+// Tests if two character arrays are equal
+//============================================================================================
+bool equals(const char* first, const char* second)
+{
+  if(length(first) != length(second))
+    return false;
+  for(int i = 0; i < length(first); i++)
+  {
+    if(first[i] != second[i])
+      return false;
+  }
+  return true;
+}
+
+//============================================================================================
+// Tests if two character arrays are equal, ignoring case
+//============================================================================================
+bool equalsIgnoreCase(const char* first, const char* second)
+{
+  if(length(first) != length(second))
+    return false;
+  for(int i = 0; i < length(first); i++)
+  {
+    int firstChar = first[i];
+    int secondChar = second[i];
+    // Make them lowercase
+    if(firstChar >= 'A' && firstChar <= 'Z')
+      firstChar += 'a' - 'A';
+    if(secondChar >= 'A' && secondChar <= 'Z')
+      secondChar += 'a' - 'A';
+    if(firstChar != secondChar)
+      return false;
+  }
+  return true;
+}
+
+//============================================================================================
+// Copies one array into the other
+//============================================================================================
+void strcpy(const char* source, char* dest, int destLength)
+{
+  if(destLength < length(source) + 1)
+  {
+    dest[0] = '\0';
+    return;
+  }
+  for(int i = 0; i < length(source); i++)
+  {
+    dest[i] = source[i];
+  }
+  dest[length(source)] = '\0';
+}
+
+//============================================================================================
+// Concatenates two character arrays
+//============================================================================================
+void concat(const char* first, const char* second, char* dest, int destLength)
+{
+  char temp[destLength];
+  if(destLength < length(first) + length(second) + 1)
+  {
+    dest[0] = '\0';
+    return;
+  }
+  for(int i = 0; i < length(first); i++)
+    temp[i] = first[i];
+  for(int i = 0; i < length(second); i++)
+    temp[i + length(first)] = second[i];
+  temp[length(second) + length(first)] = '\0';  
+  for(int i = 0; i < length(second) + length(first); i++)
+    dest[i] = temp[i];
+  dest[length(second) + length(first)] = '\0';  
+}
+
+//============================================================================================
+// Concatenates a character array with an integer
+//============================================================================================
+void concatInt(const char* first, int second, char* dest, int destLength)
+{
+  char secondChar[10];
+  sprintf(secondChar,"%d",second); //itoa(second, secondChar, 10);
+  concat(first, secondChar, dest, destLength);  
+}
+
+#endif
\ No newline at end of file