CAN data receive problem

#include <string.h>
#include "inc/hw_memmap.h"

#include "inc/hw_types.h"
#include "inc/hw_can.h"
#include "inc/hw_ints.h"
#include "driverlib/can.h"
#include "driverlib/interrupt.h"
#include "driverlib/sysctl.h"
#include "driverlib/gpio.h"
#include "utils/uartstdio.h"

#include "candata.h"
#include "datastore.h"
#include "usbstick.h"

#define AANTAL 28

//***************************************************************************************
// Holds global flags for the start of the writing.
//***************************************************************************************
static unsigned long g_ulArray[AANTAL];

//***************************************************************************************
// Flags for the CAN bus.
//***************************************************************************************
volatile unsigned long g_ulMsgCount = 0;

//***************************************************************************************
// Initializes the CAN peripheral for receiving.
//***************************************************************************************
void CANDataInit(void)
{
 tCANMsgObject rCANMessage;

//Configure CAN0 Pins. CAN0RX -> PB4 CAN0TX -> PB5
 SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
 GPIOPinConfigure(GPIO_PB4_CAN0RX);
 GPIOPinConfigure(GPIO_PB5_CAN0TX);
 GPIOPinTypeCAN(GPIO_PORTB_BASE, GPIO_PIN_4 | GPIO_PIN_5);
 SysCtlPeripheralEnable(SYSCTL_PERIPH_CAN0);

// Enable all CAN functionability.
 CANInit(CAN0_BASE);
 CANBitRateSet(CAN0_BASE, SysCtlClockGet(), 500000);
 CANIntEnable(CAN0_BASE, CAN_INT_MASTER | CAN_INT_ERROR | CAN_INT_STATUS);
 IntEnable(INT_CAN0);
 IntMasterEnable();
 CANEnable(CAN0_BASE);

// Set the different Objects which can be recieved.

// Initialize the message object from controllers.
 rCANMessage.ulMsgID = 0x750; // CAN msg ID - 0 for any
 rCANMessage.ulMsgIDMask = 0; // mask is 0 for any ID
 rCANMessage.ulFlags = MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER; // enable interrupts MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER
 rCANMessage.ulMsgLen = 8;
 CANMessageSet(CAN0_BASE, 1, &rCANMessage, MSG_OBJ_TYPE_RX);

// Initialize the message object can sync.
 rCANMessage.ulMsgID = 0x730; // CAN msg ID - 0 for any
 rCANMessage.ulMsgIDMask = 0; // mask is 0 for any ID
 rCANMessage.ulFlags = MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER; // enable interrupts MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER
 rCANMessage.ulMsgLen = 8;
 CANMessageSet(CAN0_BASE, 2, &rCANMessage, MSG_OBJ_TYPE_RX);

// Initialize the message object BMS read.
 rCANMessage.ulMsgID = 0x710; // CAN msg ID - 0 for any
 rCANMessage.ulMsgIDMask = 0; // mask is 0 for any ID
 rCANMessage.ulFlags = MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER; // enable interrupts MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER
 rCANMessage.ulMsgLen = 8;
 CANMessageSet(CAN0_BASE, 3, &rCANMessage, MSG_OBJ_TYPE_RX);

// Initialize the message object torque feedback.
 rCANMessage.ulMsgID = 0x600; // CAN msg ID - 0 for any
 rCANMessage.ulMsgIDMask = 0; // mask is 0 for any ID
 rCANMessage.ulFlags = MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER; // enable interrupts MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER
 rCANMessage.ulMsgLen = 8;
 CANMessageSet(CAN0_BASE, 4, &rCANMessage, MSG_OBJ_TYPE_RX);

// Initialize the message object battery state.
 rCANMessage.ulMsgID = 0x540; // CAN msg ID - 0 for any
 rCANMessage.ulMsgIDMask = 0; // mask is 0 for any ID
 rCANMessage.ulFlags = MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER; // enable interrupts MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER
 rCANMessage.ulMsgLen = 8;
 CANMessageSet(CAN0_BASE, 5, &rCANMessage, MSG_OBJ_TYPE_RX);

// Initialize the message object brake pedal.
 rCANMessage.ulMsgID = 0x530; // CAN msg ID - 0 for any
 rCANMessage.ulMsgIDMask = 0; // mask is 0 for any ID
 rCANMessage.ulFlags = MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER; // enable interrupts MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER
 rCANMessage.ulMsgLen = 8;
 CANMessageSet(CAN0_BASE, 6, &rCANMessage, MSG_OBJ_TYPE_RX);

// Initialize the message object vertical acceleration.
 rCANMessage.ulMsgID = 0x520; // CAN msg ID - 0 for any
 rCANMessage.ulMsgIDMask = 0; // mask is 0 for any ID
 rCANMessage.ulFlags = MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER; // enable interrupts MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER
 rCANMessage.ulMsgLen = 8;
 CANMessageSet(CAN0_BASE, 7, &rCANMessage, MSG_OBJ_TYPE_RX);

// Initialize the message object force in suspension.
 rCANMessage.ulMsgID = 0x510; // CAN msg ID - 0 for any
 rCANMessage.ulMsgIDMask = 0; // mask is 0 for any ID
 rCANMessage.ulFlags = MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER; // enable interrupts MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER
 rCANMessage.ulMsgLen = 8;
 CANMessageSet(CAN0_BASE, 8, &rCANMessage, MSG_OBJ_TYPE_RX);

// Initialize the message object speed / tire.
 rCANMessage.ulMsgID = 0x500; // CAN msg ID - 0 for any
 rCANMessage.ulMsgIDMask = 0; // mask is 0 for any ID
 rCANMessage.ulFlags = MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER; // enable interrupts MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER
 rCANMessage.ulMsgLen = 8;
 CANMessageSet(CAN0_BASE, 9, &rCANMessage, MSG_OBJ_TYPE_RX);

// Initialize the message object temperature engines.
 rCANMessage.ulMsgID = 0x490; // CAN msg ID - 0 for any
 rCANMessage.ulMsgIDMask = 0; // mask is 0 for any ID
 rCANMessage.ulFlags = MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER; // enable interrupts MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER
 rCANMessage.ulMsgLen = 8;
 CANMessageSet(CAN0_BASE, 10, &rCANMessage, MSG_OBJ_TYPE_RX);

// Initialize the message object temperature cooling.
 rCANMessage.ulMsgID = 0x480; // CAN msg ID - 0 for any
 rCANMessage.ulMsgIDMask = 0; // mask is 0 for any ID
 rCANMessage.ulFlags = MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER; // enable interrupts MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER
 rCANMessage.ulMsgLen = 8;
 CANMessageSet(CAN0_BASE, 11, &rCANMessage, MSG_OBJ_TYPE_RX);

// Initialize the message object temperature brakes.
 rCANMessage.ulMsgID = 0x470; // CAN msg ID - 0 for any
 rCANMessage.ulMsgIDMask = 0; // mask is 0 for any ID
 rCANMessage.ulFlags = MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER; // enable interrupts MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER
 rCANMessage.ulMsgLen = 8;
 CANMessageSet(CAN0_BASE, 12, &rCANMessage, MSG_OBJ_TYPE_RX);

// Initialize the message object temperature tires.
 rCANMessage.ulMsgID = 0x460; // CAN msg ID - 0 for any
 rCANMessage.ulMsgIDMask = 0; // mask is 0 for any ID
 rCANMessage.ulFlags = MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER; // enable interrupts MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER
 rCANMessage.ulMsgLen = 8;
 CANMessageSet(CAN0_BASE, 13, &rCANMessage, MSG_OBJ_TYPE_RX);

// Initialize the message object suspension travel.
 rCANMessage.ulMsgID = 0x450; // CAN msg ID - 0 for any
 rCANMessage.ulMsgIDMask = 0; // mask is 0 for any ID
 rCANMessage.ulFlags = MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER; // enable interrupts MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER
 rCANMessage.ulMsgLen = 8;
 CANMessageSet(CAN0_BASE, 14, &rCANMessage, MSG_OBJ_TYPE_RX);

// Initialize the message object GPS parameters.
 rCANMessage.ulMsgID = 0x440; // CAN msg ID - 0 for any
 rCANMessage.ulMsgIDMask = 0; // mask is 0 for any ID
 rCANMessage.ulFlags = MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER; // enable interrupts MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER
 rCANMessage.ulMsgLen = 8;
 CANMessageSet(CAN0_BASE, 15, &rCANMessage, MSG_OBJ_TYPE_RX);

// Initialize the message object steering position.
 rCANMessage.ulMsgID = 0x430; // CAN msg ID - 0 for any
 rCANMessage.ulMsgIDMask = 0; // mask is 0 for any ID
 rCANMessage.ulFlags = MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER; // enable interrupts MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER
 rCANMessage.ulMsgLen = 8;
 CANMessageSet(CAN0_BASE, 16, &rCANMessage, MSG_OBJ_TYPE_RX);

// Initialize the message object engine revs.
 rCANMessage.ulMsgID = 0x420; // CAN msg ID - 0 for any
 rCANMessage.ulMsgIDMask = 0; // mask is 0 for any ID
 rCANMessage.ulFlags = MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER; // enable interrupts MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER
 rCANMessage.ulMsgLen = 8;
 CANMessageSet(CAN0_BASE, 17, &rCANMessage, MSG_OBJ_TYPE_RX);

// Initialize the message object pedal position.
 rCANMessage.ulMsgID = 0x410; // CAN msg ID - 0 for any
 rCANMessage.ulMsgIDMask = 0; // mask is 0 for any ID
 rCANMessage.ulFlags = MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER; // enable interrupts MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER
 rCANMessage.ulMsgLen = 8;
 CANMessageSet(CAN0_BASE, 18, &rCANMessage, MSG_OBJ_TYPE_RX);

// Initialize the message object accelerometer x.
 rCANMessage.ulMsgID = 0x320; // CAN msg ID - 0 for any
 rCANMessage.ulMsgIDMask = 0; // mask is 0 for any ID
 rCANMessage.ulFlags = MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER; // enable interrupts MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER
 rCANMessage.ulMsgLen = 8;
 CANMessageSet(CAN0_BASE, 19, &rCANMessage, MSG_OBJ_TYPE_RX);

// Initialize the message object accelerometer y.
 rCANMessage.ulMsgID = 0x310; // CAN msg ID - 0 for any
 rCANMessage.ulMsgIDMask = 0; // mask is 0 for any ID
 rCANMessage.ulFlags = MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER; // enable interrupts MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER
 rCANMessage.ulMsgLen = 8;
 CANMessageSet(CAN0_BASE, 20, &rCANMessage, MSG_OBJ_TYPE_RX);

// Initialize the message object accelerometer z.
 rCANMessage.ulMsgID = 0x300; // CAN msg ID - 0 for any
 rCANMessage.ulMsgIDMask = 0; // mask is 0 for any ID
 rCANMessage.ulFlags = MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER; // enable interrupts MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER
 rCANMessage.ulMsgLen = 8;
 CANMessageSet(CAN0_BASE, 21, &rCANMessage, MSG_OBJ_TYPE_RX);

// Initialize the message object position torque.
 rCANMessage.ulMsgID = 0x100; // CAN msg ID - 0 for any
 rCANMessage.ulMsgIDMask = 0; // mask is 0 for any ID
 rCANMessage.ulFlags = MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER; // enable interrupts MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER
 rCANMessage.ulMsgLen = 8;
 CANMessageSet(CAN0_BASE, 22, &rCANMessage, MSG_OBJ_TYPE_RX);

// Initialize the message object datapoint error.
 rCANMessage.ulMsgID = 0x090; // CAN msg ID - 0 for any
 rCANMessage.ulMsgIDMask = 0; // mask is 0 for any ID
 rCANMessage.ulFlags = MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER; // enable interrupts MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER
 rCANMessage.ulMsgLen = 8;
 CANMessageSet(CAN0_BASE, 23, &rCANMessage, MSG_OBJ_TYPE_RX);

// Initialize the message object controller error.
 rCANMessage.ulMsgID = 0x080; // CAN msg ID - 0 for any
 rCANMessage.ulMsgIDMask = 0; // mask is 0 for any ID
 rCANMessage.ulFlags = MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER; // enable interrupts MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER
 rCANMessage.ulMsgLen = 8;
 CANMessageSet(CAN0_BASE, 24, &rCANMessage, MSG_OBJ_TYPE_RX);

// Initialize the message object dash error.
 rCANMessage.ulMsgID = 0x070; // CAN msg ID - 0 for any
 rCANMessage.ulMsgIDMask = 0; // mask is 0 for any ID
 rCANMessage.ulFlags = MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER; // enable interrupts MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER
 rCANMessage.ulMsgLen = 8;
 CANMessageSet(CAN0_BASE, 25, &rCANMessage, MSG_OBJ_TYPE_RX);

// Initialize the message object fatal error.
 rCANMessage.ulMsgID = 0x060; // CAN msg ID - 0 for any
 rCANMessage.ulMsgIDMask = 0; // mask is 0 for any ID
 rCANMessage.ulFlags = MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER; // enable interrupts MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER
 rCANMessage.ulMsgLen = 8;
 CANMessageSet(CAN0_BASE, 26, &rCANMessage, MSG_OBJ_TYPE_RX);

// Initialize the message object torque error.
 rCANMessage.ulMsgID = 0x050; // CAN msg ID - 0 for any
 rCANMessage.ulMsgIDMask = 0; // mask is 0 for any ID
 rCANMessage.ulFlags = MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER; // enable interrupts MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER
 rCANMessage.ulMsgLen = 8;
 CANMessageSet(CAN0_BASE, 27, &rCANMessage, MSG_OBJ_TYPE_RX);

// Initialize the message object BMS error.
 rCANMessage.ulMsgID = 0x040; // CAN msg ID - 0 for any
 rCANMessage.ulMsgIDMask = 0; // mask is 0 for any ID
 rCANMessage.ulFlags = MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER; // enable interrupts MSG_OBJ_RX_INT_ENABLE | MSG_OBJ_USE_ID_FILTER
 rCANMessage.ulMsgLen = 8;
 CANMessageSet(CAN0_BASE, 28, &rCANMessage, MSG_OBJ_TYPE_RX);
}

//***************************************************************************************
// Function that receives the data caused by the interrupt. This data will be identified
// and added to the char array.
//***************************************************************************************
void CANDataReceive(int ulState)
{
 tCANMsgObject rCANMessage;
 unsigned char rMsgData[8];
 unsigned long int ID = 0;

// Increment the counter for messages.
 g_ulMsgCount++;

// Reuse the same message object that was used earlier to configure the CAN for receiving messages.
 // A buffer for storing the received data must also be provided, so set the buffer pointer within the message object.
 rCANMessage.pucMsgData = rMsgData;

// Read the message from the CAN. Message object number 1 is used (which is not the same thing as CAN ID).
 // The interrupt clearing flag is not set because this interrupt was already cleared in the interrupt handler.
 CANMessageGet(CAN0_BASE, ulState, &rCANMessage, 0);

// Check to see if there is an indication that some messages were lost.
 if(rCANMessage.ulFlags & MSG_OBJ_DATA_LOST)
 {
 // If data loss, no message needed.
 }

ID = rCANMessage.ulMsgID;

UARTprintf("%s : %c : %d : %x \n", rMsgData, rMsgData, rMsgData, rMsgData); // adress of data
 UARTprintf("%s : %c : %d : %x \n", *rMsgData, *rMsgData, *rMsgData, *rMsgData); // data itself

// Switch on the correct Object. This way we can store the data in the correct temp variabels.
 // Ignore all warnings occuring in this function. Data types are resolved in datastore.c
 switch(ID)
 {
 // ID from controllers
 case 0x750:
 {
 SetControllers(rMsgData);
 break;
 }
 // ID can sync
 case 0x730:
 {
 SetCANsync(rMsgData);
 break;
 }
 // ID bms read
 case 0x710:
 {
 SetBMSread(rMsgData);
 break;
 }
 // ID torque feedback
 case 0x600:
 {
 SetTorqueFeedback(rMsgData);
 break;
 }
 // ID battery state
 case 0x540:
 {
 SetBatteryState(rMsgData);
 break;
 }
 // ID position brake pedal
 case 0x530:
 {
 SetPoisitonBrakePedal(rMsgData);
 break;
 }
 // ID versnelling verticaal per wiel
 case 0x520:
 {
 SetAccelVertical(rMsgData);
 break;
 }
 // ID krachten in ophangingarmen ( met mask per wiel)
 case 0x510:
 {
 SetForceSuspension(rMsgData);
 break;
 }
 // ID snelheid wiel mask met wiel
 case 0x500:
 {
 SetSpeedTire(rMsgData);
 break;
 }
 // ID temperatuur motoren
 case 0x490:
 {
 SetTempEngines(rMsgData);
 break;
 }
 // ID temperatuur koelwater
 case 0x480:
 {
 SetTempCooling(rMsgData);
 break;
 }
 // ID brake temp
 case 0x470:
 {
 SetTempBrake(rMsgData);
 break;
 }
 // ID tire temp
 case 0x460:
 {
 SetTempTire(rMsgData);
 break;
 }
 // ID suspension travel
 case 0x450:
 {
 SetSuspensionTravel(rMsgData);
 break;
 }
 // ID gps parameters
 case 0x440:
 {
 SetGPSparameter(rMsgData);
 break;
 }
 // ID steering position
 case 0x430:
 {
 SetSteerPos(rMsgData);
 break;
 }
 // ID toerental
 case 0x420:
 {
 SetRevsEngine(rMsgData);
 break;
 }
 // ID pedal position
 case 0x410:
 {
 SetPedalPos(rMsgData);
 break;
 }
 // ID accelerometer z
 case 0x320:
 {
 SetAccelX(*rMsgData);
 break;
 }
 // ID accelerometer y
 case 0x310:
 {
 SetAccelY(rMsgData);
 break;
 }
 // ID accelerometer x
 case 0x300:
 {
 SetAccelZ(rMsgData);
 break;
 }
 // ID torque
 case 0x100:
 {
 SetTorquePos(rMsgData);
 break;
 }
 // ID meetpunt error
 case 0x090:
 {
 SetDatapointError(rMsgData);
 break;
 }
 // ID controller error
 case 0x080:
 {
 SetControlError(rMsgData);
 break;
 }
 // dash error
 case 0x070:
 {
 SetDashError(rMsgData);
 break;
 }
 // ID algemene error
 case 0x060:
 {
 SetFatalError(rMsgData);
 break;
 }
 // torque error
 case 0x050:
 {
 SetTorqueError(rMsgData);
 break;
 }
 // ID bms error
 case 0x040:
 {
 SetBMSerror(rMsgData);
 break;
 }
 // default
 default:
 {
 break;
 }
 }
}

//***************************************************************************************
// Collect the data from the temp variables. This data is used to log the current or
// last value of the parameters.
//***************************************************************************************
void CANDataLog(void)
{
 int i = 0;

// Collect the data.
 g_ulArray[0] = GetControllers();
 g_ulArray[1] = GetCANsync();
 g_ulArray[2] = GetBMSread();
 g_ulArray[3] = GetTorqueFeedback();
 g_ulArray[4] = GetBatteryState();
 g_ulArray[5] = GetPoisitonBrakePedal();
 g_ulArray[6] = GetAccelVertical();
 g_ulArray[7] = GetForceSuspension();
 g_ulArray[8] = GetSpeedTire();
 g_ulArray[9] = GetTempEngines();
 g_ulArray[10] = GetTempCooling();
 g_ulArray[11] = GetTempBrake();
 g_ulArray[12] = GetTempTire();
 g_ulArray[13] = GetSuspensionTravel();
 g_ulArray[14] = GetGPSparameter();
 g_ulArray[15] = GetSteerPos();
 g_ulArray[16] = GetRevsEngine();
 g_ulArray[17] = GetPedalPos();
 g_ulArray[18] = GetAccelX();
 g_ulArray[19] = GetAccelY();
 g_ulArray[20] = GetAccelZ();
 g_ulArray[21] = GetTorquePos();
 g_ulArray[22] = GetDatapointError();
 g_ulArray[23] = GetControlError();
 g_ulArray[24] = GetDashError();
 g_ulArray[25] = GetFatalError();
 g_ulArray[26] = GetTorqueError();
 g_ulArray[27] = GetBMSerror();

// Print for Debugging purposes.
 UARTprintf("Data");

for(i = 0; i < AANTAL; i++)
 {
 UARTprintf(" : %d", g_ulArray[i]);
 }
 UARTprintf("\n");

// Run the USB functionability to store the data in a file.
 USBStickRun(&g_ulArray);
}