#include <math.h>
#define SERIAL_IN Serial1
#define LABVIEW_SERIAL Serial3
#define BYTE_SERIAL_INCOMING_PACKET 30
#define BYTE_DATA_PACKET 34
uint8_t dataPacket[BYTE_DATA_PACKET];
IntervalTimer SerialINCOMINGTimer;
#define SERIAL_INCOMING_TIMER_INTERVAL 1000
volatile unsigned long TIMER;
volatile unsigned long CYCLES;
volatile int16_t ENC_1_pos_raw;
volatile int16_t ENC_2_pos_raw;
volatile int16_t ENC_1_omega_raw;
volatile int16_t ENC_2_omega_raw;
volatile int16_t LC_1_raw;
volatile int16_t LC_2_raw;
volatile int16_t POT_pos_raw;
volatile int16_t POT_punto_raw;
volatile float ENC_1_pos;
volatile float ENC_2_pos;
volatile float ENC_1_omega;
volatile float ENC_2_omega;
volatile float LC_1;
volatile float LC_2;
volatile float POT_pos;
volatile float POT_punto;
struct structPacket
{
uint8_t Start_bytes[3];
uint8_t timestamp[4];
uint8_t cycles[4];
uint8_t encoder_1_pos[2];
uint8_t encoder_2_pos[2];
uint8_t encoder_1_omega[2];
uint8_t encoder_2_omega[2];
uint8_t load_cell_1[2];
uint8_t load_cell_2[2];
uint8_t pot_pos[2];
uint8_t pot_punto[2];
uint8_t Stop_bytes[3];
};
union unionIncoming
{
uint8_t vData[BYTE_SERIAL_INCOMING_PACKET];
structPacket Raw;
}
SERIAL_Incoming_Data;
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// SD card
#define BYTE_SD_PACKET 512
uint8_t SDPacket[BYTE_SD_PACKET];
#define packets_number 15
uint8_t packets_counter = 0;
uint16_t close_file_counter = 0;
#include "SdFat.h"
SdFatSdioEX SD;
File myFile;
char LogFileName[] = "test001.bin";
IntervalTimer SDcardTimer;
#define SD_CARD_TIMER_INTERVAL 1000
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
// other variables and stuff..
// ...
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void setup()
{
Serial.begin(115200);
SERIAL_IN.begin(115200);
SERIAL_IN.setTimeout(100);
SERIAL_IN.clear();
delay(100);
LABVIEW_SERIAL.begin(115200);
LABVIEW_SERIAL.setTimeout(100);
delay(100);
// START
dataPacket[0] = 0x01;
dataPacket[1] = 0x02;
dataPacket[2] = 0x03;
// DATA
for (uint i = 3; i < sizeof(dataPacket); i++) dataPacket[i - 3] = 0x00;
// STOP
dataPacket[31] = 0x04;
dataPacket[32] = 0x05;
dataPacket[33] = 0x06;
// other variables and stuff..
// ...
// SD CARD
for(uint i=0;i<sizeof(SDPacket);i++) SDPacket[i]=0x00;
if (!(SD.begin()))
{
// stop here if no SD card, but print a message
while (1) {
Serial.println("Unable to access the SD card");
delay(100);
}
}
else
{
Serial.println("SD card ready");
delay(100);
}
SerialINCOMINGTimer.begin(functionSerialINCOMINGTimer, SERIAL_INCOMING_TIMER_INTERVAL);
SDcardTimer.begin(functionSDcardTimer, SD_CARD_TIMER_INTERVAL);
// other two timers and stuff..
// ...
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void loop()
{
// some computations here...
// ...
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
boolean checkPacket(uint8_t *buffer)
{
return (buffer[0] == 0x01 && buffer[1] == 0x02 && buffer[2] == 0x03 && buffer[BYTE_SERIAL_INCOMING_PACKET - 3] == 0x4
&& buffer[BYTE_SERIAL_INCOMING_PACKET - 2] == 0x5 && buffer[BYTE_SERIAL_INCOMING_PACKET - 1] == 0x6);
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void extractData(structPacket Raw)
{
TIMER = *(unsigned long *)&Raw.timestamp;
CYCLES = *(unsigned long *)&Raw.cycles;
ENC_1_pos_raw = *(int16_t *)&Raw.encoder_1_pos;
ENC_2_pos_raw = *(int16_t *)&Raw.encoder_2_pos;
ENC_1_omega_raw = *(int16_t *)&Raw.encoder_1_omega;
ENC_2_omega_raw = *(int16_t *)&Raw.encoder_2_omega;
LC_1_raw = *(int16_t *)&Raw.load_cell_1;
LC_2_raw = *(int16_t *)&Raw.load_cell_2;
POT_pos_raw = *(int16_t *)&Raw.pot_pos;
POT_punto_raw = *(int16_t *)&Raw.pot_punto;
ENC_1_pos = ((float)(ENC_1_pos_raw / 100.0f));
ENC_2_pos = ((float)(ENC_2_pos_raw / 100.0f));
ENC_1_omega = ((float)(ENC_1_omega_raw / 100.0f));
ENC_2_omega = ((float)(ENC_2_omega_raw / 100.0f));
LC_1 = ((float)(LC_1_raw / 100.0f));
LC_2 = ((float)(LC_2_raw / 100.0f));
POT_pos = ((float)(POT_pos_raw / 100.0f));
POT_punto = ((float)(POT_punto_raw / 100.0f));
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void functionSerialINCOMINGTimer()
{
uint8_t bSERIAL_IN;
if (SERIAL_IN.available() > 0)
{
bSERIAL_IN = 0;
while (!(bSERIAL_IN >= BYTE_SERIAL_INCOMING_PACKET))
{
if ((SERIAL_IN.available() > 0) && (bSERIAL_IN < BYTE_SERIAL_INCOMING_PACKET))
{
SERIAL_Incoming_Data.vData[bSERIAL_IN] = SERIAL_IN.read();
bSERIAL_IN++;
}
}
if (checkPacket(SERIAL_Incoming_Data.vData))
{
extractData(SERIAL_Incoming_Data.Raw);
for (uint8_t idx = 3; idx < BYTE_SERIAL_INCOMING_PACKET - 3; idx++)
{
dataPacket[idx] = SERIAL_Incoming_Data.vData[idx];
}
noInterrupts();
uint16_t channel_1_local = channel_1;
uint16_t channel_2_local = channel_2;
interrupts();
uint8_t* pointer;
int16_t val;
//var_controllo_1
val = int16_t(channel_1_local);
pointer = (uint8_t*)&val;
dataPacket[27] = pointer[0];
dataPacket[28] = pointer[1];
//var_controllo_2
val = int16_t(channel_2_local);
pointer = (uint8_t*)&val;
dataPacket[29] = pointer[0];
dataPacket[30] = pointer[1];
}
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void functionSDcardTimer()
{
if (packets_counter < packets_number - 1)
{
noInterrupts();
for (uint8_t idx = 0; idx < BYTE_DATA_PACKET; idx++)
{
SDPacket[packets_counter * BYTE_DATA_PACKET + idx] = dataPacket[idx];
}
interrupts();
packets_counter++;
}
else //if (packets_counter==packets_number)
{
noInterrupts();
for (uint8_t idx = 0; idx < BYTE_DATA_PACKET; idx++)
{
SDPacket[packets_counter * BYTE_DATA_PACKET + idx] = dataPacket[idx];
}
interrupts();
packets_counter = 0;
if (close_file_counter < 1)
{
myFile = SD.open(LogFileName, O_CREAT | O_WRITE | O_APPEND);
myFile.write(SDPacket, BYTE_SD_PACKET);
}
else if (close_file_counter == 199 )
{
myFile.write(SDPacket, BYTE_SD_PACKET);
myFile.close();
}
else if (close_file_counter == 200 )
{
close_file_counter = 1;
myFile = SD.open(LogFileName, O_CREAT | O_WRITE | O_APPEND);
myFile.write(SDPacket, BYTE_SD_PACKET);
}
else
{
myFile.write(SDPacket, BYTE_SD_PACKET);
}
close_file_counter++;
}
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void functionTimer3()
{
// some computations here
// ...
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
void functionTimer4()
{
// some computations here
// ...
}
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%