diff --git a/.gitignore b/.gitignore
index 9fa8248..8ad9f07 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,4 +1,5 @@
Firmware/carteHRI.uvguix.*
Firmware/doc/html/
Firmware/lst/
-Firmware/output/
\ No newline at end of file
+Firmware/output/
+MATLAB/logs/*
\ No newline at end of file
diff --git a/Firmware/carteHRI.uvoptx b/Firmware/carteHRI.uvoptx
index fc80484..f524f98 100644
--- a/Firmware/carteHRI.uvoptx
+++ b/Firmware/carteHRI.uvoptx
@@ -1,762 +1,747 @@
1.0
### uVision Project, (C) Keil Software
*.c
*.s*; *.src; *.a*
*.obj
*.lib
*.txt; *.h; *.inc
*.plm
*.cpp
0
0
0
Target 1
0x4
ARM-ADS
168000000
1
1
0
1
0
1
65535
0
0
0
79
66
8
.\lst\
1
1
1
0
1
1
0
1
0
0
0
0
1
1
1
1
1
1
1
0
0
1
0
1
18
0
User Manual (MCBSTM32F400)
C:\Keil_v5\ARM\PACK\Keil\STM32F4xx_DFP\2.5.0\MDK\Boards\Keil\MCBSTM32F400\Documentation\mcbstm32f200.chm
1
Schematics (MCBSTM32F400)
C:\Keil_v5\ARM\PACK\Keil\STM32F4xx_DFP\2.5.0\MDK\Boards\Keil\MCBSTM32F400\Documentation\mcbstm32f400-schematics.pdf
2
Getting Started (STM32F4-Discovery)
C:\Keil_v5\ARM\PACK\Keil\STM32F4xx_DFP\2.5.0\MDK\Boards\ST\STM32F4-Discovery\Documentation\DM00037368.pdf
3
User Manual (STM32F4-Discovery)
C:\Keil_v5\ARM\PACK\Keil\STM32F4xx_DFP\2.5.0\MDK\Boards\ST\STM32F4-Discovery\Documentation\DM00039084.pdf
4
Bill of Materials (STM32F4-Discovery)
C:\Keil_v5\ARM\PACK\Keil\STM32F4xx_DFP\2.5.0\MDK\Boards\ST\STM32F4-Discovery\Documentation\stm32f4discovery_bom.zip
5
Gerber Files (STM32F4-Discovery)
C:\Keil_v5\ARM\PACK\Keil\STM32F4xx_DFP\2.5.0\MDK\Boards\ST\STM32F4-Discovery\Documentation\stm32f4discovery_gerber.zip
6
Schematics (STM32F4-Discovery)
C:\Keil_v5\ARM\PACK\Keil\STM32F4xx_DFP\2.5.0\MDK\Boards\ST\STM32F4-Discovery\Documentation\stm32f4discovery_sch.zip
7
MCBSTM32F400 Evaluation Board Web Page (MCBSTM32F400)
http://www.keil.com/mcbstm32f400/
8
STM32F4-Discovery Web Page (STM32F4-Discovery)
http://www.st.com/web/catalog/tools/FM116/SC959/SS1532/LN1199/PF252419
0
1
1
1
1
1
1
1
1
1
0
1
1
1
0
1
1
1
1
0
0
11
STLink\ST-LINKIII-KEIL_SWO.dll
0
ARMRTXEVENTFLAGS
-L50 -Z18 -C0 -M0 -T0
0
DLGDARM
(1010=-1,-1,-1,-1,0)(1007=-1,-1,-1,-1,0)(1008=-1,-1,-1,-1,0)(1009=-1,-1,-1,-1,0)(1012=-1,-1,-1,-1,0)
0
DLGTARM
(1010=-1,-1,-1,-1,0)(1007=-1,-1,-1,-1,0)(1008=-1,-1,-1,-1,0)(1009=-1,-1,-1,-1,0)(1012=-1,-1,-1,-1,0)
0
ARMDBGFLAGS
-T0
0
DLGUARM
(105=-1,-1,-1,-1,0)
0
ST-LINKIII-KEIL_SWO
-U303030303030303030303031 -O8398 -S1 -C0 -A0 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO19 -TC168000000 -TP21 -TDS8053 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC800 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407VG$CMSIS\Flash\STM32F4xx_1024.FLM)
0
UL2CM3
UL2CM3(-S0 -C0 -P0 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024 -FS08000000 -FL0100000 -FP0($$Device:STM32F407VG$Flash\STM32F4xx_1024.FLM))
0
1
uart_txBuffer[0]
1
1
uart_txBuffer[1]
-
- 2
- 1
- uart_currentTxBufferToWriteTo
-
-
- 3
- 1
- testVar
-
-
- 4
- 1
- uart_txBufferIndex
-
0
2
motorCurrentDebug
1
2
motorTargetCurrentDebug
2
2
ctrl_motorTorque_mNm_c
3
2
motorTorque_mNm_dbg
4
2
motorTorque_mNm
5
2
nCycles
6
2
motorPosCod,0x0A
7
2
CurrentSensOffset,0x0A
8
2
motorCurrent_mA_mean
9
2
motorCurrent_mA
10
2
ADCValuesBuffer,0x0A
11
2
motorCurrent_mA
12
2
motorCurrentSum
13
2
debugVar01,0x0A
14
2
adcValues1,0x0A
15
2
motorCurrent_mA
0
0
1
1
0
0
0
0
1
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
0
STM32_files
1
0
0
0
1
1
1
0
0
0
0
.\src\system_stm32f4xx.c
system_stm32f4xx.c
0
0
1
2
2
0
0
0
0
.\startup_stm32f4xx.s
startup_stm32f4xx.s
0
0
SRC
1
0
0
0
2
3
1
0
0
0
0
.\src\main.c
main.c
0
0
2
4
1
0
0
0
0
.\src\communication.c
communication.c
0
0
2
5
1
0
0
0
0
.\src\controller.c
controller.c
0
0
STM32_libs
1
0
0
0
3
6
1
0
0
0
0
C:\STM32F4-Discovery_FW_V1.1.0\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_gpio.c
stm32f4xx_gpio.c
0
0
3
7
1
0
0
0
0
C:\STM32F4-Discovery_FW_V1.1.0\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_rcc.c
stm32f4xx_rcc.c
0
0
3
8
1
0
0
0
0
C:\STM32F4-Discovery_FW_V1.1.0\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_tim.c
stm32f4xx_tim.c
0
0
3
9
1
0
0
0
0
C:\STM32F4-Discovery_FW_V1.1.0\Libraries\STM32F4xx_StdPeriph_Driver\src\misc.c
misc.c
0
0
3
10
1
0
0
0
0
C:\STM32F4-Discovery_FW_V1.1.0\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_exti.c
stm32f4xx_exti.c
0
0
3
11
1
0
0
0
0
C:\STM32F4-Discovery_FW_V1.1.0\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_syscfg.c
stm32f4xx_syscfg.c
0
0
3
12
1
0
0
0
0
C:\STM32F4-Discovery_FW_V1.1.0\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_adc.c
stm32f4xx_adc.c
0
0
3
13
1
0
0
0
0
C:\STM32F4-Discovery_FW_V1.1.0\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_dac.c
stm32f4xx_dac.c
0
0
3
14
1
0
0
0
0
C:\STM32F4-Discovery_FW_V1.1.0\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_dma.c
stm32f4xx_dma.c
0
0
3
15
1
0
0
0
0
C:\STM32F4-Discovery_FW_V1.1.0\Libraries\STM32F4xx_StdPeriph_Driver\src\stm32f4xx_usart.c
stm32f4xx_usart.c
0
0
3
16
4
0
0
0
0
C:\STM32F4-Discovery_FW_V1.1.0\Libraries\CMSIS\Lib\ARM\arm_cortexM4lf_math.lib
arm_cortexM4lf_math.lib
0
0
SRC_drivers
1
0
0
0
4
17
1
0
0
0
0
.\src\drivers\adc.c
adc.c
0
0
4
18
1
0
0
0
0
.\src\drivers\callback_timers.c
callback_timers.c
0
0
4
19
1
0
0
0
0
.\src\drivers\dac.c
dac.c
0
0
4
20
1
0
0
0
0
.\src\drivers\h_bridge.c
h_bridge.c
0
0
4
21
1
0
0
0
0
.\src\drivers\incr_encoder.c
incr_encoder.c
0
0
4
22
1
0
0
0
0
.\src\drivers\uart.c
uart.c
0
0
4
23
1
0
0
0
0
.\src\drivers\hall.c
hall.c
0
0
4
24
1
0
0
0
0
.\src\drivers\strain_gauge.c
strain_gauge.c
0
0
4
25
1
0
0
0
0
.\src\drivers\tachometer.c
tachometer.c
0
0
SRC_lib
1
0
0
0
5
26
1
0
0
0
0
.\src\lib\basic_filter.c
basic_filter.c
0
0
5
27
1
0
0
0
0
.\src\lib\pid.c
pid.c
0
0
5
28
1
0
0
0
0
.\src\lib\utils.c
utils.c
0
0
5
29
1
0
0
0
0
.\src\lib\circular_buffer.c
circular_buffer.c
0
0
diff --git a/Firmware/src/communication.c b/Firmware/src/communication.c
index a3d2b8a..afe9546 100644
--- a/Firmware/src/communication.c
+++ b/Firmware/src/communication.c
@@ -1,378 +1,713 @@
#include "communication.h"
#include "controller.h"
#include "drivers/callback_timers.h"
#include "drivers/adc.h"
#include "drivers/dac.h"
#include "drivers/incr_encoder.h"
#include "drivers/hall.h"
#include "drivers/strain_gauge.h"
#include "drivers/tachometer.h"
#include "drivers/uart.h"
#include "lib/basic_filter.h"
#include "lib/pid.h"
#include "lib/utils.h"
#include
-extern uint32_t statusReg, ctrl_timestamp, ctrl_currentFilterType;
-extern float32_t ctrl_motorPosCod, ctrl_motorPosCod_c;
-extern float32_t ctrl_motorTorque_Nm_c, ctrl_motorPosCodFilt, ctrl_firstOrderFilterTau;
-extern float32_t hall_angle, tac_speed, sg_force;
-extern pid_Pid ctrl_positionPid;
-extern bfilt_BasicFilter ctrl_encoderFilter;
-extern float32_t ctrl_sineAmplitude, ctrl_sinePeriod;
-
uint32_t selectedVariablesToStream; // Bitfield that indicates for each variable if it should be streamed or not.
-void* variablesArray[N_VARIABLES]; // Pointers to variables to stream.
uint8_t txBuffer[1024];
#define COMM_BUFFER_SIZE 1024
#define DEBUG_MESSAGE_BUFFER_SIZE 1024
uint8_t comm_packetTxBuffer[COMM_BUFFER_SIZE];
char comm_debugMessageBuffer[DEBUG_MESSAGE_BUFFER_SIZE];
cb_CircularBuffer *comm_rxQueue;
uint8_t rxCurrentMessageType = PC_MESSAGE_DO_NOTHING; // Current message type for RX bytes.
uint32_t rxBytesCount; // Number of received bytes for the current message.
uint8_t firstHalfByte; // First half of the data byte to receive.
uint8_t rxDataBytesBuffer[32]; // Data bytes received (ready to use, bytes already merged).
+// SyncVar-related vars.
+#define N_SYNCVARS_MAX 50
+comm_SyncVar comm_syncVars[N_SYNCVARS_MAX];
+uint8_t comm_nSyncVars;
+volatile bool comm_varListLocked;
+uint8_t comm_streamId;
+uint8_t comm_nVarsToStream;
+comm_SyncVar const* comm_streamedVars[N_SYNCVARS_MAX];
+extern volatile uint32_t ctrl_timestamp; // [us].
+
+// Private functions.
void comm_SendPacket(uint8_t messageType, uint8_t *data, uint16_t dataLength);
void comm_HandleByte(uint8_t rxData);
void comm_Stream(void);
uint32_t comm_GetVar(uint8_t variableIndex);
+void comm_SendVarsList(void);
/**
* @brief Init the communication manager.
*/
void comm_Init(void)
{
- // Initialize the array with all the pointers on variables.
- // Note that this array contains pointers only to uint32_t, even if the
- // variables are actually float32_t. This does not matter, since we only use
- // these pointers to perform byte-to-byte copies.
- // Do not modify this by hand, copy-paste code generated by the excel sheet.
- variablesArray[VAR_TIMESTAMP] = (uint32_t*) &ctrl_timestamp;
- variablesArray[VAR_POS_REG_PID_P] = (uint32_t*) &ctrl_positionPid.kp;
- variablesArray[VAR_POS_REG_PID_I] = (uint32_t*) &ctrl_positionPid.ki;
- variablesArray[VAR_POS_REG_PID_D] = (uint32_t*) &ctrl_positionPid.kd;
- variablesArray[VAR_POS_REG_PID_ARW] = (uint32_t*) &ctrl_positionPid.arw;
- variablesArray[VAR_POS_REG_PID_INTEGRATOR] = (uint32_t*) &ctrl_positionPid.integrator;
- variablesArray[VAR_POS_REG_PID_OUTPUT] = (uint32_t*) &ctrl_motorTorque_Nm_c;
- variablesArray[VAR_POS_REG_SAMPLING_PERIOD] = NULL;
- variablesArray[VAR_WALL_POSITION] = (uint32_t*) &ctrl_motorPosCod_c;
- variablesArray[VAR_MEASURED_ANGLE_RAW] = NULL;
- variablesArray[VAR_MEASURED_ANGLE_FILTERED] = (uint32_t*) &ctrl_motorPosCodFilt;
- variablesArray[VAR_COMM_TX_PERIOD] = NULL;
- variablesArray[VAR_ACTIVE_FILTER] = (uint32_t*) &ctrl_currentFilterType;
- variablesArray[VAR_1ST_FILT_TAU] = (uint32_t*) &ctrl_encoderFilter.tau;
- variablesArray[VAR_DAC1_VOLTAGE] = NULL;
- variablesArray[VAR_DAC2_VOLTAGE] = NULL;
- variablesArray[VAR_ADC1_VOLTAGE] = NULL;
- variablesArray[VAR_ADC2_VOLTAGE] = NULL;
- variablesArray[VAR_HALL_ANGLE] = NULL;
- variablesArray[VAR_TACHO_SPEED] = NULL;
- variablesArray[VAR_STRAIN_GAUGE_FORCE] = NULL;
- variablesArray[VAR_USER_VAR_1] = NULL;
- variablesArray[VAR_USER_VAR_2] = NULL;
- variablesArray[VAR_USER_VAR_3] = NULL;
- variablesArray[VAR_USER_VAR_4] = NULL;
- variablesArray[VAR_USER_VAR_5] = NULL;
-
+ comm_nSyncVars = 0;
+ comm_varListLocked = false;
+ comm_streamId = 0;
+ comm_nVarsToStream = 0;
// Setup the UART peripheral, and specify the function that will be called
// each time a byte is received.
uart_Init();
comm_rxQueue = uart_GetRxQueue();
rxCurrentMessageType = PC_MESSAGE_DO_NOTHING;
// Make the streaming function periodically called by the timer 7.
cbt_SetCommLoopTimer(comm_Stream, TE_DATA_LOOP_DEFAULT_VAL);
}
void comm_Step(void)
{
// Send the bytes in the TX queue, even if it is not full, to avoid latency.
uart_FlushTx();
// Interpret the bytes in the RX queue.
uart_Step();
while(!cb_IsEmpty(comm_rxQueue))
comm_HandleByte(cb_Pull(comm_rxQueue));
}
+void comm_NotifyReady(void)
+{
+ comm_SendPacket(STM_MESSAGE_START_INFO, NULL, 0);
+}
+
/**
* @brief UART data streaming "loop" function.
*/
void comm_Stream()
{
// If the data streaming is enabled, send a stream packet to the PC.
- if(statusReg & DATA_UPSTREAM_ENB)
+ if(comm_nVarsToStream > 0)
{
- uint32_t i;
+ uint8_t i;
int nDataBytesToSend = 0;
+
+ // Stream ID.
+ txBuffer[nDataBytesToSend] = comm_streamId;
+ nDataBytesToSend++;
+
+ // Timestamp.
+ memcpy(&txBuffer[nDataBytesToSend], (uint32_t*)&ctrl_timestamp,
+ sizeof(ctrl_timestamp));
+ nDataBytesToSend += sizeof(ctrl_timestamp);
- for(i=0; iaddress, sv->size);
+ nDataBytesToSend += sv->size;
}
comm_SendPacket(STM_MESSAGE_STREAMING_PACKET, txBuffer, nDataBytesToSend);
}
}
-/**
- * @brief Get the four bytes of a monitored variable, from its index.
- * @param variableIndex: index of the variable to get.
- * @return the variable bytes as a uint32_t (even if actually float32_t).
- * @note This function returns 0 if the variableIndex does not correspond to a
- * valid variable to read.
- */
-uint32_t comm_GetVar(uint8_t variableIndex)
-{
- float32_t tmp;
- uint32_t variableValue;
-
- // Note that some variable cannot be read directly, and are
- // treated as exceptions, because a function call is necessary.
- switch(variableIndex)
- {
- // Special case, function call needed.
- case VAR_POS_REG_SAMPLING_PERIOD:
- variableValue = cbt_GetPositionLoopPeriod();
- break;
- case VAR_COMM_TX_PERIOD:
- variableValue = cbt_GetCommLoopPeriod();
- break;
- case VAR_DAC1_VOLTAGE:
- tmp = dac_GetVoltage1();
- variableValue = *(uint32_t*)&tmp;
- break;
- case VAR_DAC2_VOLTAGE:
- tmp = dac_GetVoltage2();
- variableValue = *(uint32_t*)&tmp;
- break;
- case VAR_ADC1_VOLTAGE:
- tmp = adc_GetChannelVoltage(ANIN1, ADC_SCALE_10_V);
- variableValue = *(uint32_t*)&tmp;
- break;
- case VAR_ADC2_VOLTAGE:
- tmp = adc_GetChannelVoltage(ANIN2, ADC_SCALE_10_V);
- variableValue = *(uint32_t*)&tmp;
- break;
- case VAR_MEASURED_ANGLE_RAW:
- tmp = enc_GetPosition();
- variableValue = *(uint32_t*)&tmp;
- break;
-
- // All the others are normal 32-bits variables.
- default:
- if((variableIndex < N_VARIABLES) && (variablesArray[variableIndex] != NULL)) // Check if the index is OK.
- variableValue = *((uint32_t*)variablesArray[variableIndex]);
- else
- variableValue = 0;
- break;
- }
-
- return variableValue;
-}
-
-/**
- * @brief Set the four bytes of a monitored variable, from its index.
- * @param variableIndex: index of the variable to get.
- * @param newValue: new value for the variable.
- * @note This function does nothing if the variableIndex does not correspond to
- * a valid variable to write.
- */
-void comm_SetVar(uint8_t variableIndex, uint32_t newValue)
-{
- // Copy the new value to the selected 32-bits variable.
- // Note that some variable cannot be changed directly, and
- // are treated as exceptions, because a function call is necessary.
- switch(variableIndex)
- {
- // Special case, function call needed.
- case VAR_POS_REG_SAMPLING_PERIOD:
- cbt_SetPositionLoopPeriod(newValue);
- break;
- case VAR_COMM_TX_PERIOD:
- cbt_SetCommLoopPeriod(newValue);
- break;
- case VAR_DAC1_VOLTAGE:
- dac_SetVoltage1(*(float32_t*)&newValue);
- break;
- case VAR_DAC2_VOLTAGE:
- dac_SetVoltage2(*(float32_t*)&newValue);
- break;
- case VAR_MEASURED_ANGLE_RAW:
- enc_SetPosition(*(float32_t*)&newValue);
- break;
-
- // All the others are normal 32-bits variables.
- default:
- if((variableIndex < N_VARIABLES) && (variablesArray[variableIndex] != NULL)) // Check if the index is OK.
- memcpy(variablesArray[variableIndex], &newValue, 4);
- break;
- }
-}
-
/**
* @brief Sends Packet with a header and data bytes.
* @param type: message type ("header" of the message).
* @param data: array of data bytes to be sent ("content" of the message).
* @param dataLength: number of data bytes to be sent.
*/
void comm_SendPacket(uint8_t type, uint8_t *data, uint16_t dataLength)
{
int i=0;
uint8_t *p = comm_packetTxBuffer;
*p = ((1<<7) | type);
p++;
for(i=0; i> 4); // Most significant bits.
p++;
*p = ((data[i]&0x0f)); // Least significant bits.
p++;
}
uart_SendBytesAsync(comm_packetTxBuffer, dataLength*2+1);
}
/**
* @brief Process the received byte to interpret the messages.
* @param rxData: the byte to be processed and interpreted.
*/
void comm_HandleByte(uint8_t rxData)
{
if(rxData & (1<<7)) // The start byte has the most significant bit high.
{
rxCurrentMessageType = (rxData & ~(1<<7)); // Remove the start bit.
rxBytesCount = 0;
}
else
rxBytesCount++;
if(rxBytesCount % 2 == 1) // First half of the data byte has been received.
firstHalfByte = rxData; // Store it until the second half arrives.
else // Second half of the data byte has been received (or no data bytes yet).
{
int dataBytesReady = rxBytesCount/2;
rxDataBytesBuffer[dataBytesReady-1] = (firstHalfByte<<4) + (rxData & 0xf);
switch(rxCurrentMessageType)
{
case PC_MESSAGE_DO_NOTHING:
if(dataBytesReady == 0)
; // Do nothing.
break;
-
- case PC_MESSAGE_SEND_STATUS:
- if(dataBytesReady == 0)
- comm_SendPacket(STM_MESSAGE_STATUS, (uint8_t*)(&statusReg), 4);
- break;
-
- case PC_MESSAGE_START_STREAMING:
+
+ case PC_MESSAGE_PING:
if(dataBytesReady == 0)
- {
- statusReg |= DATA_UPSTREAM_ENB;
- TIM_SetCounter(TIM7, 0);
- TIM_Cmd(TIM7, ENABLE);
- }
+ comm_SendPacket(STM_MESSAGE_PINGBACK, NULL, 0);
break;
-
- case PC_MESSAGE_STOP_STREAMING:
- if(dataBytesReady == 0)
+
+ case PC_MESSAGE_GET_VARS_LIST:
+ if(dataBytesReady == 0 && comm_varListLocked)
{
- TIM_Cmd(TIM7, DISABLE);
- statusReg &= ~DATA_UPSTREAM_ENB;
- }
- break;
-
- case PC_MESSAGE_GET_VAR:
- if(dataBytesReady == 1)
+ int16_t i, length;
+ uint8_t *p = &txBuffer[0];
+
+ *p = (uint8_t)comm_nSyncVars;
+ p++;
+
+ for(i=0; i= N_VARIABLES)
- return;
+ *p = (uint8_t)comm_syncVars[i].access;
+ p++;
+
+ *p = (uint8_t)comm_syncVars[i].size;
+ p++;
+ }
- // Prepare the message to be sent to the PC.
- // First byte: variable index.
- txBuffer[0] = variableIndex;
+ length = p - &txBuffer[0];
+ comm_SendPacket(STM_MESSAGE_VARS_LIST, txBuffer, length);
+ }
+ break;
+
+ case PC_MESSAGE_GET_VAR:
+ if(dataBytesReady == 1)
+ {
+ comm_SyncVar *v;
+
+ // Extract the index, that indicates which variable will be sent.
+ uint8_t variableIndex = rxDataBytesBuffer[0];
+
+ // If the variable index is out of range, ignore the request.
+ if(variableIndex >= comm_nSyncVars)
+ return;
+
+ v = &comm_syncVars[variableIndex];
- // Four following bytes: actual value of the variable.
- variableValue = comm_GetVar(variableIndex);
+ // If the variable is not readable, ignore the request.
+ if((v->usesVarAddress && v->access == WRITEONLY) ||
+ (!v->usesVarAddress && v->getFunc == NULL))
+ {
+ break;
+ }
- memcpy(&txBuffer[1], &variableValue, 4);
-
- // Send the message to the PC.
- comm_SendPacket(STM_MESSAGE_VAR, txBuffer, 5);
+ // Prepare the message to be sent to the PC.
+ // First byte: variable index.
+ txBuffer[0] = variableIndex;
+
+ // Following bytes: actual value of the variable.
+ if(v->usesVarAddress)
+ {
+ v = &comm_syncVars[variableIndex];
+ memcpy(&txBuffer[1], v->address, v->size);
}
- break;
-
- case PC_MESSAGE_SET_VAR:
- if(dataBytesReady == 5)
+ else
{
- // Extract the index (that indicates which variable will change),
- // and the new value of the variable.
- uint32_t newValue;
- uint8_t variableIndex = rxDataBytesBuffer[0];
- memcpy(&newValue, &rxDataBytesBuffer[1], 4);
-
- // Set the selected variable with the new value.
- comm_SetVar(variableIndex, newValue);
+ switch(v->type)
+ {
+ case BOOL:
+ {
+ bool tmpBool = ((bool (*)(void))v->getFunc)();
+ memcpy(&txBuffer[1], &tmpBool, v->size);
+ }
+ break;
+ case UINT8:
+ {
+ uint8_t tmpUint8 = ((uint8_t (*)(void))v->getFunc)();
+ memcpy(&txBuffer[1], &tmpUint8, v->size);
+ }
+ break;
+ case INT8:
+ {
+ int8_t tmpInt8 = ((int8_t (*)(void))v->getFunc)();
+ memcpy(&txBuffer[1], &tmpInt8, v->size);
+ }
+ break;
+ case UINT16:
+ {
+ uint16_t tmpUint16 = ((uint16_t (*)(void))v->getFunc)();
+ memcpy(&txBuffer[1], &tmpUint16, v->size);
+ }
+ break;
+ case INT16:
+ {
+ int16_t tmpInt16 = ((int16_t (*)(void))v->getFunc)();
+ memcpy(&txBuffer[1], &tmpInt16, v->size);
+ }
+ break;
+ case UINT32:
+ {
+ uint32_t tmpUint32 = ((uint32_t (*)(void))v->getFunc)();
+ memcpy(&txBuffer[1], &tmpUint32, v->size);
+ }
+ break;
+ case INT32:
+ {
+ int32_t tmpInt32 = ((int32_t (*)(void))v->getFunc)();
+ memcpy(&txBuffer[1], &tmpInt32, v->size);
+ }
+ break;
+ case UINT64:
+ {
+ uint32_t tmpUint64 = ((uint64_t (*)(void))v->getFunc)();
+ memcpy(&txBuffer[1], &tmpUint64, v->size);
+ }
+ break;
+ case INT64:
+ {
+ int32_t tmpInt64 = ((int64_t (*)(void))v->getFunc)();
+ memcpy(&txBuffer[1], &tmpInt64, v->size);
+ }
+ break;
+ case FLOAT32:
+ {
+ float32_t tmpFloat = ((float32_t (*)(void))v->getFunc)();
+ memcpy(&txBuffer[1], &tmpFloat, v->size);
+ }
+ break;
+ case FLOAT64:
+ {
+ double tmpDouble = ((double (*)(void))v->getFunc)();
+ memcpy(&txBuffer[1], &tmpDouble, v->size);
+ }
+ break;
+ }
}
- break;
-
- case PC_MESSAGE_SET_STREAMED_VAR:
- if(dataBytesReady == 4)
+
+ // Send the message to the PC.
+ comm_SendPacket(STM_MESSAGE_VAR, txBuffer, 1 + v->size);
+ }
+ break;
+
+ case PC_MESSAGE_SET_VAR:
+ if(dataBytesReady >= 1)
+ {
+ comm_SyncVar *v;
+ uint8_t variableIndex;
+
+ // Extract the index (that indicates which variable will change),
+ // and the new value of the variable.
+ variableIndex = rxDataBytesBuffer[0];
+
+ // If the variable index is out of range, ignore the request.
+ if(variableIndex >= comm_nSyncVars)
+ return;
+
+ v = &comm_syncVars[variableIndex];
+
+ // Set the selected variable with the new value.
+ if(dataBytesReady == 1 + v->size)
{
- uint8_t *newSelectedVars = &rxDataBytesBuffer[0];
- memcpy(&selectedVariablesToStream, newSelectedVars, 4);
+ if(v->usesVarAddress)
+ {
+ if(v->access != READONLY)
+ {
+ v = &comm_syncVars[variableIndex];
+ memcpy(v->address, &rxDataBytesBuffer[1], v->size);
+ }
+ }
+ else
+ {
+ if(v->setFunc == NULL)
+ break;
+
+ switch(v->type)
+ {
+ case BOOL:
+ {
+ bool tmp;
+ memcpy(&tmp, &rxDataBytesBuffer[1], v->size);
+ ((void (*)(bool))v->setFunc)(tmp);
+ }
+ break;
+ case UINT8:
+ {
+ uint8_t tmp;
+ memcpy(&tmp, &rxDataBytesBuffer[1], v->size);
+ ((void (*)(uint8_t))v->setFunc)(tmp);
+ }
+ break;
+ case INT8:
+ {
+ int8_t tmp;
+ memcpy(&tmp, &rxDataBytesBuffer[1], v->size);
+ ((void (*)(int8_t))v->setFunc)(tmp);
+ }
+ break;
+ case UINT16:
+ {
+ uint16_t tmp;
+ memcpy(&tmp, &rxDataBytesBuffer[1], v->size);
+ ((void (*)(uint16_t))v->setFunc)(tmp);
+ }
+ break;
+ case INT16:
+ {
+ int16_t tmp;
+ memcpy(&tmp, &rxDataBytesBuffer[1], v->size);
+ ((void (*)(int16_t))v->setFunc)(tmp);
+ }
+ break;
+ case UINT32:
+ {
+ uint32_t tmp;
+ memcpy(&tmp, &rxDataBytesBuffer[1], v->size);
+ ((void (*)(uint32_t))v->setFunc)(tmp);
+ }
+ break;
+ case INT32:
+ {
+ int32_t tmp;
+ memcpy(&tmp, &rxDataBytesBuffer[1], v->size);
+ ((void (*)(int32_t))v->setFunc)(tmp);
+ }
+ break;
+ case UINT64:
+ {
+ uint64_t tmp;
+ memcpy(&tmp, &rxDataBytesBuffer[1], v->size);
+ ((void (*)(uint64_t))v->setFunc)(tmp);
+ }
+ break;
+ case INT64:
+ {
+ int64_t tmp;
+ memcpy(&tmp, &rxDataBytesBuffer[1], v->size);
+ ((void (*)(int64_t))v->setFunc)(tmp);
+ }
+ break;
+ case FLOAT32:
+ {
+ float32_t tmp;
+ memcpy(&tmp, &rxDataBytesBuffer[1], v->size);
+ ((void (*)(float32_t))v->setFunc)(tmp);
+ }
+ break;
+ case FLOAT64:
+ {
+ double tmp;
+ memcpy(&tmp, &rxDataBytesBuffer[1], v->size);
+ ((void (*)(double))v->setFunc)(tmp);
+ }
+ break;
+ }
+ }
}
- break;
+ }
+ break;
+
+ case PC_MESSAGE_SET_STREAMED_VAR:
+ if(dataBytesReady >= 1)
+ {
+ int i;
+ comm_nVarsToStream = rxDataBytesBuffer[0];
+
+ if(dataBytesReady != 1 + 1 + comm_nVarsToStream)
+ return;
+
+ comm_streamId = rxDataBytesBuffer[1];
+
+ for(i=0; i SYNCVAR_NAME_SIZE - 1)
+ {
+ memcpy(v.name, name, SYNCVAR_NAME_SIZE - 1);
+ v.name[SYNCVAR_NAME_SIZE - 1] = '\0';
+ }
+ else
+ strcpy(v.name, name);
+
+ //
+ v.address = address;
+ v.type = type;
+ v.size = size;
+ v.access = access;
+ v.usesVarAddress = true;
+
+ // Add the SyncVar to the list.
+ comm_syncVars[comm_nSyncVars] = v;
+ comm_nSyncVars++;
+ }
+}
+
+void comm_monitorVarFunc(const char name[], comm_VarType type, uint8_t size,
+ void (*getFunc)(void), void (*setFunc)(void))
+{
+ // Adding a variable to the list is not allowed if it has been locked.
+ if(!comm_varListLocked)
+ {
+ comm_SyncVar v;
+
+ // Trim the name if it is too long.
+ if(strlen(name) > SYNCVAR_NAME_SIZE - 1)
+ {
+ memcpy(&v.name, &name, SYNCVAR_NAME_SIZE - 1);
+ v.name[SYNCVAR_NAME_SIZE - 1] = '\0';
+ }
+ else
+ strcpy(v.name, name);
+
+ //
+ v.getFunc = getFunc;
+ v.setFunc = setFunc;
+ v.type = type;
+ v.size = size;
+ v.usesVarAddress = false;
+
+ // Determine the variable access.
+ if(getFunc != NULL && setFunc == NULL)
+ v.access = READONLY;
+ else if(getFunc == NULL && setFunc != NULL)
+ v.access = WRITEONLY;
+ else if(getFunc != NULL && setFunc != NULL)
+ v.access = READWRITE;
+ else
+ return; // No function provided at all, ignoring var.
+
+ // Add the SyncVar to the list.
+ comm_syncVars[comm_nSyncVars] = v;
+ comm_nSyncVars++;
+ }
+}
+
+void comm_monitorBool(const char name[], bool *address,
+ comm_VarAccess access)
+{
+ comm_monitorVar(name, address, BOOL, 1, access);
+}
+
+void comm_monitorUint8(const char name[], uint8_t *address,
+ comm_VarAccess access)
+{
+ comm_monitorVar(name, address, UINT8, 1, access);
+}
+
+void comm_monitorInt8(const char name[], int8_t *address,
+ comm_VarAccess access)
+{
+ comm_monitorVar(name, address, INT8, 1, access);
+}
+
+void comm_monitorUint16(const char name[], uint16_t *address,
+ comm_VarAccess access)
+{
+ comm_monitorVar(name, address, UINT16, 2, access);
+}
+
+void comm_monitorInt16(const char name[], int16_t *address,
+ comm_VarAccess access)
+{
+ comm_monitorVar(name, address, INT16, 2, access);
+}
+
+void comm_monitorUint32(const char name[], uint32_t *address,
+ comm_VarAccess access)
+{
+ comm_monitorVar(name, address, UINT32, 4, access);
+}
+
+void comm_monitorInt32(const char name[], int32_t *address,
+ comm_VarAccess access)
+{
+ comm_monitorVar(name, address, INT32, 4, access);
+}
+
+void comm_monitorUint64(const char name[], uint64_t *address,
+ comm_VarAccess access)
+{
+ comm_monitorVar(name, address, UINT64, 8, access);
+}
+
+void comm_monitorInt64(const char name[], int64_t *address,
+ comm_VarAccess access)
+{
+ comm_monitorVar(name, address, INT64, 8, access);
+}
+
+void comm_monitorFloat(const char name[], float *address,
+ comm_VarAccess access)
+{
+ comm_monitorVar(name, address, FLOAT32, 4, access);
+}
+
+void comm_monitorDouble(const char name[], double *address,
+ comm_VarAccess access)
+{
+ comm_monitorVar(name, address, FLOAT64, 8, access);
+}
+
+void comm_monitorBoolFunc(const char name[],
+ bool (*getFunc)(void), void (*setFunc)(bool))
+{
+ comm_monitorVarFunc(name, BOOL, 1, (void (*)(void))getFunc, (void (*)(void))setFunc);
+}
+
+void comm_monitorUint8Func(const char name[],
+ uint8_t (*getFunc)(void), void (*setFunc)(uint8_t))
+{
+ comm_monitorVarFunc(name, UINT8, 1, (void (*)(void))getFunc, (void (*)(void))setFunc);
+}
+
+void comm_monitorInt8Func(const char name[],
+ int8_t (*getFunc)(void), void (*setFunc)(int8_t))
+{
+ comm_monitorVarFunc(name, INT8, 1, (void (*)(void))getFunc, (void (*)(void))setFunc);
+}
+
+void comm_monitorUint16Func(const char name[],
+ uint16_t (*getFunc)(void),
+ void (*setFunc)(uint16_t))
+{
+ comm_monitorVarFunc(name, UINT16, 2, (void (*)(void))getFunc, (void (*)(void))setFunc);
+}
+
+void comm_monitorInt16Func(const char name[],
+ int16_t (*getFunc)(void), void (*setFunc)(int16_t))
+{
+ comm_monitorVarFunc(name, INT16, 2, (void (*)(void))getFunc, (void (*)(void))setFunc);
+}
+
+void comm_monitorUint32Func(const char name[],
+ uint32_t (*getFunc)(void),
+ void (*setFunc)(uint32_t))
+{
+ comm_monitorVarFunc(name, UINT32, 4, (void (*)(void))getFunc, (void (*)(void))setFunc);
+}
+
+void comm_monitorInt32Func(const char name[],
+ int32_t (*getFunc)(void), void (*setFunc)(int32_t))
+{
+ comm_monitorVarFunc(name, INT32, 4, (void (*)(void))getFunc, (void (*)(void))setFunc);
+}
+
+void comm_monitorUint64Func(const char name[],
+ uint64_t (*getFunc)(void),
+ void (*setFunc)(uint64_t))
+{
+ comm_monitorVarFunc(name, UINT64, 8, (void (*)(void))getFunc, (void (*)(void))setFunc);
+}
+
+void comm_monitorInt64Func(const char name[],
+ int64_t (*getFunc)(void), void (*setFunc)(int64_t))
+{
+ comm_monitorVarFunc(name, INT64, 8, (void (*)(void))getFunc, (void (*)(void))setFunc);
+}
+
+void comm_monitorFloatFunc(const char name[],
+ float (*getFunc)(void), void (*setFunc)(float))
+{
+ comm_monitorVarFunc(name, FLOAT32, 4, (void (*)(void))getFunc, (void (*)(void))setFunc);
+}
+
+void comm_monitorDoubleFunc(const char name[],
+ double (*getFunc)(void), void (*setFunc)(double))
+{
+ comm_monitorVarFunc(name, FLOAT64, 8, (void (*)(void))getFunc, (void (*)(void))setFunc);
+}
+
+void comm_LockSyncVarsList(void)
+{
+ comm_varListLocked = true;
+}
+
+void comm_SendVarsList(void)
+{
+ int i;
+ uint8_t *p;
+ uint16_t buffLength = 1 + comm_nSyncVars * (SYNCVAR_NAME_SIZE + 1 + 1);
+
+ p = &txBuffer[0];
+
+ *p = comm_nSyncVars;
+ p++;
+
+ for(i=0; i
/** @defgroup Communication Main / Communication
* @brief Control the communication with the computer.
*
* This module controls all the communication logic between the board and the
* computer. It uses a specific communication protocol between the
* computer and the board, with a system of messages. Thanks to this, the
* the MATLAB application is able to get the value of selected variables on the
* STM, and is even capable of modifiying them remotely.
*
* Make sure that the files communication.h/.c are up-to-date with the Excel
* spreadsheet "Protocol description.xlsx".
*
* Call comm_Init() to setup this module. Its interrupt function will be called
* automatically when a message arrives, or periodically when the data
* streaming is enabled.
*
* @addtogroup Communication
* @{
*/
+typedef struct
+{
+ char name[SYNCVAR_NAME_SIZE];
+ void *address;
+ comm_VarType type;
+ uint8_t size;
+ comm_VarAccess access;
+ bool usesVarAddress;
+ void (*getFunc)(void);
+ void (*setFunc)(void);
+} comm_SyncVar;
+
+
void comm_Init(void);
void comm_Step(void);
+
+void comm_NotifyReady(void);
+
+void comm_monitorVar(const char name[], void *address, comm_VarType type,
+ uint8_t size, comm_VarAccess access);
+void comm_monitorVarFunc(const char name[], comm_VarType type, uint8_t size,
+ void (*getFunc)(void), void (*setFunc)(void));
+
+void comm_monitorBool(const char name[], bool *address,
+ comm_VarAccess access);
+void comm_monitorUint8(const char name[], uint8_t *address,
+ comm_VarAccess access);
+void comm_monitorInt8(const char name[], int8_t *address,
+ comm_VarAccess access);
+void comm_monitorUint16(const char name[], uint16_t *address,
+ comm_VarAccess access);
+void comm_monitorInt16(const char name[], int16_t *address,
+ comm_VarAccess access);
+void comm_monitorUint32(const char name[], uint32_t *address,
+ comm_VarAccess access);
+void comm_monitorInt32(const char name[], int32_t *address,
+ comm_VarAccess access);
+void comm_monitorUint64(const char name[], uint64_t *address,
+ comm_VarAccess access);
+void comm_monitorInt64(const char name[], int64_t *address,
+ comm_VarAccess access);
+void comm_monitorFloat(const char name[], float *address,
+ comm_VarAccess access);
+void comm_monitorDouble(const char name[], double *address,
+ comm_VarAccess access);
+
+void comm_monitorBoolFunc(const char name[],
+ bool (*getFunc)(void), void (*setFunc)(bool));
+void comm_monitorUint8Func(const char name[],
+ uint8_t (*getFunc)(void), void (*setFunc)(uint8_t));
+void comm_monitorInt8Func(const char name[],
+ int8_t (*getFunc)(void), void (*setFunc)(int8_t));
+void comm_monitorUint16Func(const char name[],
+ uint16_t (*getFunc)(void),
+ void (*setFunc)(uint16_t));
+void comm_monitorInt16Func(const char name[],
+ int16_t (*getFunc)(void), void (*setFunc)(int16_t));
+void comm_monitorUint32Func(const char name[],
+ uint32_t (*getFunc)(void),
+ void (*setFunc)(uint32_t));
+void comm_monitorInt32Func(const char name[],
+ int32_t (*getFunc)(void), void (*setFunc)(int32_t));
+void comm_monitorUint64Func(const char name[],
+ uint64_t (*getFunc)(void),
+ void (*setFunc)(uint64_t));
+void comm_monitorInt64Func(const char name[],
+ int64_t (*getFunc)(void), void (*setFunc)(int64_t));
+void comm_monitorFloatFunc(const char name[],
+ float (*getFunc)(void), void (*setFunc)(float));
+void comm_monitorDoubleFunc(const char name[],
+ double (*getFunc)(void), void (*setFunc)(double));
+
+void comm_LockSyncVarsList(void);
+
void comm_SendDebugMessage(const char *format, ...);
/**
* @brief Sends a debug message to the computer, with decimation.
* This macro is useful to print human-readable text, in a fast loop, to avoid
* overloading the communication bus, or the computer.
* @param decimation this macro will actually print once out of decimation, and
* do nothing otherwise.
* @param format format string. See the printf() documentation for format
* specification.
* @param ... variables to be printed in the format string.
*/
#define comm_SendDebugMessageDecimated(decimation, format, ...) \
do \
{ \
static int comm_decim##__COUNTER__ = 0; \
if(comm_decim##__COUNTER__++ % decimation == 0) \
{ \
comm_SendDebugMessage(format, ##__VA_ARGS__); \
} \
} while(0)
/**
* @}
*/
#endif
diff --git a/Firmware/src/controller.c b/Firmware/src/controller.c
index 320603d..272e9d4 100644
--- a/Firmware/src/controller.c
+++ b/Firmware/src/controller.c
@@ -1,131 +1,162 @@
#include "controller.h"
#include "communication.h"
#include "drivers/adc.h"
#include "drivers/dac.h"
#include "drivers/incr_encoder.h"
#include "drivers/hall.h"
#include "drivers/h_bridge.h"
#include "drivers/callback_timers.h"
#include "lib/basic_filter.h"
#include "lib/pid.h"
#include "lib/utils.h"
extern uint32_t statusReg;
volatile uint32_t ctrl_timestamp = 0; // [us].
+volatile float32_t ctrl_motorPosHall;
volatile float32_t ctrl_motorPosCod;
volatile float32_t ctrl_motorPosCod_c;
volatile float32_t ctrl_motorPosCodFilt = 0.0f;
volatile float32_t ctrl_motorTorque_Nm_c = 0.0f;
volatile uint32_t ctrl_currentFilterType = FILTER_TYPE_NONE;
volatile pid_Pid ctrl_currentPid, ctrl_positionPid;
volatile bfilt_BasicFilter ctrl_encoderFilter;
+bool ctrl_regulateCurrent;
+
void ctrl_RegulateCurrent(void);
void ctrl_RegulatePosition(void);
/**
* @brief Initialize the position and current controllers.
*/
void ctrl_Init(void)
{
+ // By default the current regulator is off, to allow the calibration of the
+ // current sensor.
+ ctrl_regulateCurrent = false;
+
// Setup the PIDs.
pid_Init((pid_Pid*)&ctrl_currentPid, KP_CURRENT_DEFAULT_VAL, KI_CURRENT_DEFAULT_VAL, 0.0f, CURRENT_INTEGRATOR_SAT, FF_CURRENT_DEFAULT_VAL);
#ifdef REGULATION_TYPE_SYMMETRIC
pid_Init((pid_Pid*)&ctrl_positionPid, KP_POSITION_DEFAULT_VAL, KI_POSITION_DEFAULT_VAL, KD_POSITION_DEFAULT_VAL, POSITION_INTEGRATOR_SAT_DEFAULT_VAL, 0.0f);
#else
pid_Init((pid_Pid*)&ctrl_positionPid, KP_POSITION_DEFAULT_VAL, 0.0f, KD_POSITION_DEFAULT_VAL, 0.0f, 0.0f);
#endif
// Setup the increment encoder filter.
bfilt_Init((bfilt_BasicFilter*)&ctrl_encoderFilter, ENCODER_FILT_TAU_DEFAULT_VAL, 0.0f);
// Make the timers call the regulation functions periodically.
cbt_SetCurrentLoopTimer(ctrl_RegulateCurrent, TE_CURRENT_LOOP_DEFAULT_VAL);
cbt_SetPositionLoopTimer(ctrl_RegulatePosition, TE_CONTROL_LOOP_DEFAULT_VAL);
+
+ // Share the key variables with the computer.
+ comm_monitorFloat("current [A]", (float32_t*)&ctrl_currentPid.current, READONLY);
+ comm_monitorFloat("motor_torque [N.m]", (float32_t*)&ctrl_motorTorque_Nm_c, READWRITE);
+ comm_monitorFloat("encoder_pos [deg]", (float32_t*)&ctrl_motorPosCod, READONLY);
+ comm_monitorFloat("hall_raw_pos [deg]", (float32_t*)&ctrl_motorPosHall, READONLY);
+}
+
+/**
+ * @brief Start the current regulation.
+ */
+void ctrl_StartCurrentLoop(void)
+{
+ ctrl_regulateCurrent = true;
}
/**
* @brief Current regulation "loop" function.
*/
void ctrl_RegulateCurrent()
{
float32_t motorVoltage, // Motor command voltage [V].
pwmNormalizedDutyCycle, // Motor normalized PWM duty (-1 ot 1).
targetCurrent; // [A].
// Get the current.
float32_t motorCurrrentCurrent = adc_GetCurrent();
// Convert the target torque to current.
targetCurrent = ctrl_motorTorque_Nm_c / MOTOR_TORQUE_CONST;
// Regulate.
motorVoltage = -pid_Step((pid_Pid*)&ctrl_currentPid,
motorCurrrentCurrent,
targetCurrent,
(float32_t)cbt_GetCurrentLoopPeriod()*MICROSECOND_TO_SECOND);
// Normalize to get a signed PWM duty (between -1 and 1).
pwmNormalizedDutyCycle = motorVoltage / H_BRIDGE_SUPPLY_VOLTAGE;
utils_SaturateF(&pwmNormalizedDutyCycle, -CURRENT_LOOP_PWM_MAX_DUTY_CYCLE, CURRENT_LOOP_PWM_MAX_DUTY_CYCLE);
- // Apply the computed PWM duty.
- hb_SetPWM(pwmNormalizedDutyCycle);
+ // Apply the computed PWM duty, if the enabled.
+ if(ctrl_regulateCurrent)
+ hb_SetPWM(pwmNormalizedDutyCycle);
+ else
+ hb_SetPWM(0.0f);
}
/**
* @brief Position regulation "loop" function.
* @note As an example, a basic position regulator that uses the encoder is
* implemented. Feel free to remove all the content of the function to add your
* own code.
*/
void ctrl_RegulatePosition()
{
// Increment the timestamp.
ctrl_timestamp += cbt_GetPositionLoopPeriod();
-/*
+
+ // Get the Hall sensor position.
+ ctrl_motorPosHall = hall_Get();
+
// Get the encoder position.
ctrl_motorPosCod = enc_GetPosition();
-
+/*
// Filter the position given by the encoder.
if(ctrl_currentFilterType == FILTER_TYPE_FIRST_ORDER)
{
ctrl_motorPosCodFilt = bfilt_Step((bfilt_BasicFilter*)&ctrl_encoderFilter, ctrl_motorPosCod);
}
else if(ctrl_currentFilterType == FILTER_TYPE_RUNNING_MEAN)
{
// TODO.
ctrl_motorPosCodFilt = ctrl_motorPosCod;
}
else // No filter.
ctrl_motorPosCodFilt = ctrl_motorPosCod;
// Regulate.
#ifdef REGULATION_TYPE_SYMMETRIC // No wall, just regulation on a point.
ctrl_motorTorque_Nm_c = -pid_Step((pid_Pid*)&ctrl_positionPid,
ctrl_motorPosCodFilt,
ctrl_motorPosCod_c,
(float32_t)(cbt_GetPositionLoopPeriod())*MICROSECOND_TO_SECOND);
#else // Wall, regulate only if not touching the wall.
if(ctrl_motorPosCodFilt < ctrl_motorPosCod_c) // Touching the wall.
{
ctrl_motorTorque_Nm_c = -pid_Step((pid_Pid*)&ctrl_positionPid,
ctrl_motorPosCodFilt,
(float32_t)ctrl_motorPosCod_c,
(float32_t)(cbt_GetPositionLoopPeriod())*MICROSECOND_TO_SECOND);
}
else
ctrl_motorTorque_Nm_c = 0.0f;
-#endif*/
+#endif
- ctrl_motorTorque_Nm_c = 0.0f;
+ ctrl_motorTorque_Nm_c = 0.0f;*/
+
+ // Saturate the torque to the motor nominal.
+ utils_SaturateF((float32_t*)&ctrl_motorTorque_Nm_c, -MOTOR_NOMINAL_TORQUE,
+ MOTOR_NOMINAL_TORQUE);
}
diff --git a/Firmware/src/controller.h b/Firmware/src/controller.h
index 3d56639..2085d06 100644
--- a/Firmware/src/controller.h
+++ b/Firmware/src/controller.h
@@ -1,62 +1,63 @@
#ifndef __CONTROLLER_H
#define __CONTROLLER_H
#include "main.h"
// Type of regulation.
// Comment out to get the "wall" non-symetric behavior.
#define REGULATION_TYPE_SYMMETRIC
// Loops periode values
#define TE_LOOP_MIN_VALUE 50 // Minimum period for any loop [us].
#define TE_LOOP_MAX_VALUE 65534 // Maximum period for any loop [us].
#define TE_CURRENT_LOOP_DEFAULT_VAL 50 // Current control loop period [us] (max 2^16-1) (default value at reset).
#define TE_CONTROL_LOOP_DEFAULT_VAL 350 // Main control loop period [us] (max 2^16-1) (default value at reset).
-#define TE_DATA_LOOP_DEFAULT_VAL 8000 // Data loop period [us] (max 2^16-1) (default value at reset).
+#define TE_DATA_LOOP_DEFAULT_VAL 1000 // Data loop period [us] (max 2^16-1) (default value at reset).
// Current loop parameters
#define KP_CURRENT_DEFAULT_VAL 5.0f
#define KI_CURRENT_DEFAULT_VAL 0.0f
#define FF_CURRENT_DEFAULT_VAL MOTOR_RESISTANCE
#define CURRENT_INTEGRATOR_SAT 2.0f
#define CURRENT_LOOP_PWM_MAX_DUTY_CYCLE 0.98f // Max PWM value (normalized) duty cycle (less than 1 to ensure bootstrap capacitor charging)
// Main control loop parameters
#define KP_POSITION_DEFAULT_VAL 0.01f
#define KD_POSITION_DEFAULT_VAL 0.0001f
#define KI_POSITION_DEFAULT_VAL 0.0f
#define POSITION_INTEGRATOR_SAT_DEFAULT_VAL 0.01f
// Increment encoder filter default coef.
#define ENCODER_FILT_TAU_DEFAULT_VAL 0.999f
// Filter types
#define FILTER_TYPE_NONE 0
#define FILTER_TYPE_FIRST_ORDER 1
#define FILTER_TYPE_RUNNING_MEAN 2
/** @defgroup Controller Main / Controllers
* @brief Control the paddle
*
* This module is the high-level controller of the board. It runs two "loops"
* (in fact, functions called periodically by the interrupt of a timer), one to
* regulate the motor current (ctrl_RegulateCurrent(), high frequency) and
* another to control the rest (ctrl_RegulatePosition(), lower frequency).
* The content of ctrl_RegulatePosition() is typically what the user of the
* board will modify, depending on the selected sensors and control algorithms.
*
* Call ctrl_Init() to setup this module. Its interrupt functions will be called
* automatically periodically.
*
* @addtogroup Controller
* @{
*/
void ctrl_Init(void);
+void ctrl_StartCurrentLoop(void);
/**
* @}
*/
#endif
diff --git a/Firmware/src/definitions.h b/Firmware/src/definitions.h
index 9f263b2..c780aa5 100644
--- a/Firmware/src/definitions.h
+++ b/Firmware/src/definitions.h
@@ -1,50 +1,48 @@
#ifndef DEF_DEFINITIONS_H
#define DEF_DEFINITIONS_H
+#include
+
// Message IDs sent by host (PC) to the device (STM).
-#define PC_MESSAGE_DO_NOTHING 0 // Do nothing.
-#define PC_MESSAGE_TOGGLE_LED 1 // Toggle the LED (debug).
-#define PC_MESSAGE_SEND_STATUS 2 // Request the device to send the main status register.
-#define PC_MESSAGE_START_STREAMING 3 // Request the device to start streaming continuously.
-#define PC_MESSAGE_STOP_STREAMING 4 // Request the device to stop streaming continuously.
-#define PC_MESSAGE_GET_VAR 5 // Request the device to send the selected value.
-#define PC_MESSAGE_SET_VAR 6 // Set the selected variable.
-#define PC_MESSAGE_SET_STREAMED_VAR 7 // Set the variables to be streamed.
+typedef enum
+{
+ PC_MESSAGE_DO_NOTHING = 0, ///< Do nothing.
+ PC_MESSAGE_PING, ///< Request the board an answer, to check the connection status.
+ PC_MESSAGE_GET_VARS_LIST, ///< Request the SyncVars list.
+ PC_MESSAGE_SET_STREAMED_VAR, ///< Set the variables to be streamed continuously.
+ PC_MESSAGE_GET_VAR, ///< Request the device to send the selected value.
+ PC_MESSAGE_SET_VAR ///< Set the selected variable.
+} comm_PcMessage;
// Message IDs sent by device (STM) to the device (PC).
-#define STM_MESSAGE_STATUS 0 // Main status register.
-#define STM_MESSAGE_VAR 1 // Variable state.
-#define STM_MESSAGE_STREAMING_PACKET 2 // Streaming packet.
-#define STM_MESSAGE_DEBUG_TEXT 3 // Debug text message.
-
-// Variables IDs.
-#define VAR_TIMESTAMP 0 // Timestamp. [us]
-#define VAR_POS_REG_PID_P 1 // Position regulator - proportionnal coefficient. [1]
-#define VAR_POS_REG_PID_I 2 // Position regulator - integral coefficient. [1]
-#define VAR_POS_REG_PID_D 3 // Position regulator - derivative coefficient. [1]
-#define VAR_POS_REG_PID_ARW 4 // Position regulator - ARW (integrator max value). [1]
-#define VAR_POS_REG_PID_INTEGRATOR 5 // Position regulator - integrator state. [1]
-#define VAR_POS_REG_PID_OUTPUT 6 // Position regulator - output. [mN.m]
-#define VAR_POS_REG_SAMPLING_PERIOD 7 // Position regulator - sampling period [us]
-#define VAR_WALL_POSITION 8 // Position regulator - target (wall position). [pulses]
-#define VAR_MEASURED_ANGLE_RAW 9 // Measured output angle, unfiltered. [deg]
-#define VAR_MEASURED_ANGLE_FILTERED 10 // Measured output angle, filtered. [deg]
-#define VAR_COMM_TX_PERIOD 11 // Communication loop - sending period. [us]
-#define VAR_ACTIVE_FILTER 12 // Active filter type (0: none, 1: first-order, 2: running mean). [None]
-#define VAR_1ST_FILT_TAU 13 // First order filter - time constant. [1]
-#define VAR_DAC1_VOLTAGE 14 // DAC 1 channel voltage. [V]
-#define VAR_DAC2_VOLTAGE 15 // DAC 2 channel voltage. [V]
-#define VAR_ADC1_VOLTAGE 16 // ACDC 1 channel voltage. [V]
-#define VAR_ADC2_VOLTAGE 17 // ACDC 2 channel voltage. [V]
-#define VAR_HALL_ANGLE 18 // Angle measured by the Hall sensor. [deg]
-#define VAR_TACHO_SPEED 19 // Angular speed measured by the tachometer. [deg/s]
-#define VAR_STRAIN_GAUGE_FORCE 20 // Force measured by the strain gauge. [?]
-#define VAR_USER_VAR_1 21 // User Variable 1 []
-#define VAR_USER_VAR_2 22 // User Variable 2 []
-#define VAR_USER_VAR_3 23 // User Variable 3 []
-#define VAR_USER_VAR_4 24 // User Variable 4 []
-#define VAR_USER_VAR_5 25 // User Variable 5 []
-
-#define N_VARIABLES 26 // Number of variables, used for enumeration of all variables.
+typedef enum
+{
+ STM_MESSAGE_PINGBACK = 0, ///< Response to a ping request.
+ STM_MESSAGE_VAR, ///< Variable state.
+ STM_MESSAGE_STREAMING_PACKET, ///< Streaming packet.
+ STM_MESSAGE_DEBUG_TEXT, ///< Debug text message.
+ STM_MESSAGE_VARS_LIST, ///< Monitored variables list.
+ STM_MESSAGE_START_INFO ///< Notification that the board has (re)started.
+} comm_StmMessage;
+
+// SyncVar.
+#define SYNCVAR_NAME_SIZE 20 // Max size of a SyncVar name, including the '\0' trailing character.
+
+typedef enum
+{
+ READONLY = 0,
+ WRITEONLY,
+ READWRITE
+} comm_VarAccess;
+
+typedef enum
+{
+ BOOL = 0,
+ UINT8, INT8, UINT16, INT16, UINT32, INT32, UINT64, INT64,
+ FLOAT32, FLOAT64
+} comm_VarType;
+
+// UART baudrate [b/s].
+#define UART_BAUDRATE 1843200
#endif
diff --git a/Firmware/src/drivers/callback_timers.c b/Firmware/src/drivers/callback_timers.c
index 1e4d267..697cf50 100644
--- a/Firmware/src/drivers/callback_timers.c
+++ b/Firmware/src/drivers/callback_timers.c
@@ -1,227 +1,227 @@
#include "callback_timers.h"
#include "../controller.h"
#include "../lib/utils.h"
cbt_PeriodicTaskFunc cbt_tim1Task, cbt_tim6Task, cbt_tim7Task;
volatile float32_t cbt_ucLoad; // Processor load (%).
void tim1InitFunc(void);
void tim67InitFunc(void);
/**
* @brief Initialize the timers to call an interrupt routine periodically.
*/
void cbt_Init(void)
{
// Initialize the timers.
tim1InitFunc();
tim67InitFunc();
// Initialize the function pointers to NULL, in order to be able to test
// if they are already affected or not.
cbt_tim1Task = NULL;
cbt_tim6Task = NULL;
cbt_tim7Task = NULL;
}
/**
* @brief Set the function to call periodically by the timer 1.
* @param f: the function to call periodically.
* @param period: the period between each call of f [us].
*/
void cbt_SetCurrentLoopTimer(cbt_PeriodicTaskFunc f, uint32_t period)
{
cbt_tim1Task = f;
utils_SaturateU(&period, TE_LOOP_MIN_VALUE, TE_LOOP_MAX_VALUE);
TIM1->ARR = (uint16_t) period;
}
/**
* @brief Set the function to call periodically by the timer 6.
* @param f: the function to call periodically.
* @param period: the period between each call of f [us].
*/
void cbt_SetPositionLoopTimer(cbt_PeriodicTaskFunc f, uint32_t period)
{
cbt_tim6Task = f;
utils_SaturateU(&period, TE_LOOP_MIN_VALUE, TE_LOOP_MAX_VALUE);
TIM6->ARR = (uint16_t) period;
}
/**
* @brief Set the function to call periodically by the timer 7.
* @param f: the function to call periodically.
* @param period: the period between each call of f [us].
*/
void cbt_SetCommLoopTimer(cbt_PeriodicTaskFunc f, uint32_t period)
{
cbt_tim7Task = f;
utils_SaturateU(&period, TE_LOOP_MIN_VALUE, TE_LOOP_MAX_VALUE);
TIM7->ARR = (uint16_t) period;
}
/**
* @brief Initialize TIM1 used for timing the current loop (resolution 1us).
*/
void tim1InitFunc(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStruct;
//TIM_OCInitTypeDef TIM_OCInitStruct;
NVIC_InitTypeDef NVIC_InitStruct;
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
NVIC_InitStruct.NVIC_IRQChannel = TIM1_UP_TIM10_IRQn;
NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = CURRENT_LOOP_IRQ_PRIORITY;
NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStruct);
TIM_TimeBaseStruct.TIM_Period = (uint16_t)(TE_CURRENT_LOOP_DEFAULT_VAL-1);
TIM_TimeBaseStruct.TIM_Prescaler = TIM1_PRESCALER;
TIM_TimeBaseStruct.TIM_ClockDivision = 0; // TIM_CKD_DIV2;
TIM_TimeBaseStruct.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStruct);
TIM_ITConfig(TIM1, TIM_IT_Update, ENABLE);
TIM_Cmd(TIM1, ENABLE);
}
/**
* @brief Initialize TIM6, for timing the main control loop (resolution 1us)
* Initialize TIM7, for timing the data transmission loop (resolution 1us)
*/
void tim67InitFunc(void)
{
TIM_TimeBaseInitTypeDef TIM_TimeBaseStruct;
NVIC_InitTypeDef NVIC_InitStruct;
RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM6|RCC_APB1Periph_TIM7, ENABLE);
NVIC_InitStruct.NVIC_IRQChannel = TIM6_DAC_IRQn;
NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = CONTROL_LOOP_IRQ_PRIORITY;
NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStruct);
NVIC_InitStruct.NVIC_IRQChannel = TIM7_IRQn;
NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = DATA_LOOP_IRQ_PRIORITY;
NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0;
NVIC_Init(&NVIC_InitStruct);
TIM_TimeBaseStructInit(&TIM_TimeBaseStruct);
TIM_TimeBaseStruct.TIM_Prescaler = TIM6_PRESCALER;
TIM_TimeBaseStruct.TIM_Period = (uint16_t)(TE_CONTROL_LOOP_DEFAULT_VAL-1);
TIM_TimeBaseStruct.TIM_ClockDivision = 0;
TIM_TimeBaseStruct.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM6, &TIM_TimeBaseStruct);
TIM_TimeBaseStructInit(&TIM_TimeBaseStruct);
TIM_TimeBaseStruct.TIM_Prescaler = TIM7_PRESCALER;
TIM_TimeBaseStruct.TIM_Period = (uint16_t)(TE_DATA_LOOP_DEFAULT_VAL-1);
TIM_TimeBaseStruct.TIM_ClockDivision = 0;
TIM_TimeBaseStruct.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM7, &TIM_TimeBaseStruct);
TIM_ITConfig(TIM6, TIM_IT_Update, ENABLE);
TIM_ITConfig(TIM7, TIM_IT_Update, ENABLE);
TIM_Cmd(TIM6, ENABLE);
- TIM_Cmd(TIM7, DISABLE);
+ TIM_Cmd(TIM7, ENABLE);
}
/**
* @brief Interrupt from current control loop timer (TIM1)
*/
void TIM1_UP_TIM10_IRQHandler(void)
{
if(TIM_GetITStatus(TIM1, TIM_IT_Update) != RESET)
{
if(cbt_tim1Task != NULL)
cbt_tim1Task();
TIM_ClearITPendingBit(TIM1, TIM_IT_Update);
}
}
/**
* @brief Interrupt from main control loop timer (TIM6)
*/
void TIM6_DAC_IRQHandler(void)
{
if(TIM_GetITStatus(TIM6, TIM_IT_Update) != RESET)
{
if(cbt_tim6Task != NULL)
cbt_tim6Task();
// Percentage of time consumed by the control task 0..100%.
cbt_ucLoad = (((float32_t)(TIM6->CNT))*100)/((float32_t)(TIM6->ARR));
TIM_ClearITPendingBit(TIM6, TIM_IT_Update);
}
}
/**
* @brief Interrupt from data transmission loop timer (TIM7) (data loop)
*/
void TIM7_IRQHandler(void)
{
if(TIM_GetITStatus(TIM7, TIM_IT_Update) != RESET)
{
if(cbt_tim7Task != NULL)
cbt_tim7Task();
TIM_ClearITPendingBit(TIM7, TIM_IT_Update);
}
}
/**
* @brief Set the period of the position loop.
* @param period: the new period of the position loop [us].
*/
void cbt_SetPositionLoopPeriod(uint32_t period)
{
utils_SaturateU(&period, TE_LOOP_MIN_VALUE, TE_LOOP_MAX_VALUE);
TIM6->ARR = period;
}
/**
* @brief Set the period of the communication loop.
* @param period: the new period of the communication loop [us].
*/
void cbt_SetCommLoopPeriod(uint32_t period)
{
utils_SaturateU(&period, TE_LOOP_MIN_VALUE, TE_LOOP_MAX_VALUE);
TIM7->ARR = period;
}
/**
* @brief Get the period of the current loop.
* @return the period of the current loop [us].
*/
uint32_t cbt_GetCurrentLoopPeriod(void)
{
return TIM1->ARR;
}
/**
* @brief Get the period of the position loop.
* @return the period of the position loop [us].
*/
uint32_t cbt_GetPositionLoopPeriod(void)
{
return TIM6->ARR;
}
/**
* @brief Get the period of the communication loop.
* @return the period of the communication loop [us].
*/
uint32_t cbt_GetCommLoopPeriod(void)
{
return TIM7->ARR;
}
diff --git a/Firmware/src/drivers/h_bridge.c b/Firmware/src/drivers/h_bridge.c
index 299d634..7b2342c 100644
--- a/Firmware/src/drivers/h_bridge.c
+++ b/Firmware/src/drivers/h_bridge.c
@@ -1,125 +1,123 @@
#include "h_bridge.h"
extern uint32_t statusReg;
void hb_Tim8Init(void);
/**
* @brief Initialize the pins and the PWM timer to control the H-bridge.
*/
void hb_Init(void)
{
// Init the GPIO pins.
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.GPIO_Pin = nFAULT_Pin;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IN;
GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_Init(nFAULT_Port, &GPIO_InitStruct);
GPIO_InitStruct.GPIO_Pin = nSLEEP_Pin;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_OUT;
GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL;
GPIO_Init(nSLEEP_Port, &GPIO_InitStruct);
GPIO_WriteBit(nSLEEP_Port, nSLEEP_Pin, Bit_SET);
GPIO_InitStruct.GPIO_Pin = (uint16_t)(1<>1)); //50% duty cycle
TIM_OCInitStruct.TIM_OCPolarity = TIM_OCPolarity_Low;
TIM_OCInitStruct.TIM_OCIdleState = TIM_OCIdleState_Set;
TIM_OC1Init(TIM8, &TIM_OCInitStruct);
TIM_OCInitStruct.TIM_OCIdleState = TIM_OCIdleState_Reset;
TIM_OCInitStruct.TIM_OCPolarity = TIM_OCPolarity_High;
TIM_OC2Init(TIM8, &TIM_OCInitStruct);
TIM_OC1PreloadConfig(TIM8, TIM_OCPreload_Enable);
TIM_OC2PreloadConfig(TIM8, TIM_OCPreload_Enable);
// TIM8 TRGO selection.
TIM_SelectOutputTrigger(TIM8, TIM_TRGOSource_Update);
TIM_Cmd(TIM8, ENABLE);
TIM_CtrlPWMOutputs(TIM8, ENABLE);
}
/**
* @brief Enable the motor driver.
*/
void hb_Enable()
{
GPIO_WriteBit(ENB1_Port, (uint16_t)(1<ARR)>>1))));
- else // 50% fixed duty cycle if motorDrv enabled but RUN_CURRENT_LOOP=false
- pwmDutyCycle = (float32_t)((TIM8->ARR)>>1);
+ uint16_t pwmDutyCycle;
+
+ pwmDutyCycle = ((uint16_t)((ratio+1)*((float32_t)((TIM8->ARR)>>1))));
TIM8->CCR1 = pwmDutyCycle; // The same value is affected to both timers, because they have
TIM8->CCR2 = pwmDutyCycle; // an opposite polarity.
}
diff --git a/Firmware/src/drivers/uart.c b/Firmware/src/drivers/uart.c
index 8030485..48b5fa7 100644
--- a/Firmware/src/drivers/uart.c
+++ b/Firmware/src/drivers/uart.c
@@ -1,281 +1,281 @@
#include "uart.h"
#include "../lib/utils.h"
#include "../communication.h" // TODO: REMOVE. DEBUG ONLY.
#define UART_RX_DMA DMA1_Stream5
#define UART_RX_DMA_CHANNEL DMA_Channel_4
#define UART_TX_DMA DMA1_Stream6
#define UART_TX_DMA_CHANNEL DMA_Channel_4
#define UART_TX_BUFFER_SIZE 512
#define UART_RX_BUFFER_SIZE 512
#define UART_USER_RX_QUEUE_SIZE 512
uint8_t uart_txBuffer[2][UART_TX_BUFFER_SIZE];
uint8_t uart_currentTxBufferToWriteTo;
uint16_t uart_txBufferIndex;
uint16_t uart_nBytesToTransferDma;
uint8_t uart_rxBuffer[UART_RX_BUFFER_SIZE];
uint8_t const * uart_rxBuffTail;
uint8_t uart_userRxQueue[UART_USER_RX_QUEUE_SIZE];
cb_CircularBuffer uart_rxQueue;
#define UART_DMA_TX_IS_BUSY (DMA_GetCmdStatus(UART_TX_DMA) == ENABLE && DMA_GetCurrDataCounter(UART_TX_DMA) > 0)
/**
* @brief Initializes the UART module.
*/
void uart_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct;
USART_InitTypeDef USART_InitStruct;
DMA_InitTypeDef DMA_InitStruct;
// Enable UART and DMA peripherals clocks.
RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE);
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_DMA1, ENABLE);
// Setup GPIOs as UART pins.
GPIO_InitStruct.GPIO_Pin = USART_TX_Pin;
GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF;
GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP;
GPIO_Init(USART_TX_Port, &GPIO_InitStruct);
GPIO_PinAFConfig(USART_TX_Port, USART_TX_PinSource, GPIO_AF_USART2);
GPIO_InitStruct.GPIO_Pin = USART_RX_Pin;
GPIO_Init(USART_RX_Port, &GPIO_InitStruct);
GPIO_PinAFConfig(USART_RX_Port, USART_RX_PinSource, GPIO_AF_USART2);
// Setup the UART peripheral.
- USART_InitStruct.USART_BaudRate = USART_BAUDRATE;
+ USART_InitStruct.USART_BaudRate = UART_BAUDRATE;
USART_InitStruct.USART_WordLength = USART_WordLength_8b;
USART_InitStruct.USART_StopBits = USART_StopBits_1;
USART_InitStruct.USART_Parity = USART_Parity_No;
USART_InitStruct.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
USART_InitStruct.USART_Mode = USART_Mode_Tx | USART_Mode_Rx;
USART_Init(USART_PC_COMM, &USART_InitStruct);
USART_Cmd(USART_PC_COMM, ENABLE);
// Setup the DMA for RX.
DMA_DeInit(UART_RX_DMA);
DMA_InitStruct.DMA_Channel = UART_RX_DMA_CHANNEL;
DMA_InitStruct.DMA_PeripheralBaseAddr = (uint32_t)&USART2->DR;
DMA_InitStruct.DMA_Memory0BaseAddr = (uint32_t)&uart_rxBuffer[0];
DMA_InitStruct.DMA_DIR = DMA_DIR_PeripheralToMemory;
DMA_InitStruct.DMA_BufferSize = UART_RX_BUFFER_SIZE;
DMA_InitStruct.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
DMA_InitStruct.DMA_MemoryInc = DMA_MemoryInc_Enable;
DMA_InitStruct.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
DMA_InitStruct.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
DMA_InitStruct.DMA_Mode = DMA_Mode_Circular;
DMA_InitStruct.DMA_Priority = DMA_Priority_Medium;
DMA_InitStruct.DMA_FIFOMode = DMA_FIFOMode_Disable;
DMA_InitStruct.DMA_MemoryBurst = DMA_MemoryBurst_Single;
DMA_InitStruct.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
DMA_Init(UART_RX_DMA, &DMA_InitStruct);
USART_DMACmd(USART_PC_COMM, USART_DMAReq_Rx | USART_DMAReq_Tx, ENABLE);
DMA_ITConfig(UART_RX_DMA, DMA_IT_TC, ENABLE);
DMA_Cmd(UART_RX_DMA, ENABLE);
uart_rxBuffTail = &uart_rxBuffer[0];
// Setup the DMA for TX.
DMA_DeInit(UART_TX_DMA);
DMA_InitStruct.DMA_Channel = UART_TX_DMA_CHANNEL;
DMA_InitStruct.DMA_PeripheralBaseAddr = (uint32_t)&USART2->DR;
DMA_InitStruct.DMA_Memory0BaseAddr = (uint32_t)&uart_txBuffer[0][0];
DMA_InitStruct.DMA_DIR = DMA_DIR_MemoryToPeripheral;
DMA_InitStruct.DMA_BufferSize = UART_TX_BUFFER_SIZE;
DMA_InitStruct.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
DMA_InitStruct.DMA_MemoryInc = DMA_MemoryInc_Enable;
DMA_InitStruct.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
DMA_InitStruct.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
DMA_InitStruct.DMA_Mode = DMA_Mode_Normal;
DMA_InitStruct.DMA_Priority = DMA_Priority_Medium;
DMA_InitStruct.DMA_FIFOMode = DMA_FIFOMode_Disable;
DMA_InitStruct.DMA_MemoryBurst = DMA_MemoryBurst_Single;
DMA_InitStruct.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
DMA_Init(UART_TX_DMA, &DMA_InitStruct);
USART_DMACmd(USART_PC_COMM, USART_DMAReq_Rx | USART_DMAReq_Tx, ENABLE);
DMA_ITConfig(UART_TX_DMA, DMA_IT_TC, ENABLE);
//DMA_Cmd(UART_TX_DMA, ENABLE);
// Initialize the RX circular buffer.
cb_Init(&uart_rxQueue, uart_userRxQueue, UART_USER_RX_QUEUE_SIZE);
// Initialize the variables for the UART TX.
uart_currentTxBufferToWriteTo = 0;
uart_txBufferIndex = 0;
uart_nBytesToTransferDma = 0;
}
/**
* @brief Copies the received bytes into the user-accessible queue.
* Reads all the available bytes in the DMA RX buffer, and copies them to the
* user-accessible queue.
* @remark This function must called often, otherwise the DMA RX buffer may be
* full.
*/
void uart_Step(void)
{
// Get the location of the location currently pointed by the DMA.
uint8_t const * head = uart_rxBuffer + UART_RX_BUFFER_SIZE
- DMA_GetCurrDataCounter(UART_RX_DMA);
// Even if the STM32F4 reference manual (RM0090) states that the NDTR
// register is decremented after the transfer, this is not the case.
// So we wait a few cycles to be sure that the DMA actually performed the
// transfer.
utils_DelayUs(1);
// RX: add the received bytes into the user queue.
while(uart_rxBuffTail != head)
{
uint8_t b = *uart_rxBuffTail;
cb_Push(&uart_rxQueue, b);
uart_rxBuffTail++;
if(uart_rxBuffTail >= uart_rxBuffer + UART_RX_BUFFER_SIZE)
uart_rxBuffTail -= UART_RX_BUFFER_SIZE;
}
}
/**
* @brief Gets the user-accessible queue of the received bytes.
* @return the address of the queue.
*/
cb_CircularBuffer* uart_GetRxQueue(void)
{
return &uart_rxQueue;
}
/**
* @brief Starts the DMA transfer to send bytes to UART peripheral.
*/
void uart_StartDma(void)
{
DMA_InitTypeDef DMA_InitStruct;
//
uart_nBytesToTransferDma = uart_txBufferIndex;
uart_currentTxBufferToWriteTo = !uart_currentTxBufferToWriteTo;
uart_txBufferIndex = 0;
// Start the DMA transfer.
DMA_DeInit(UART_TX_DMA);
DMA_InitStruct.DMA_Channel = UART_TX_DMA_CHANNEL;
DMA_InitStruct.DMA_PeripheralBaseAddr = (uint32_t)&USART2->DR;
DMA_InitStruct.DMA_Memory0BaseAddr = (uint32_t)&uart_txBuffer[!uart_currentTxBufferToWriteTo][0];
DMA_InitStruct.DMA_DIR = DMA_DIR_MemoryToPeripheral;
DMA_InitStruct.DMA_BufferSize = uart_nBytesToTransferDma;
DMA_InitStruct.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
DMA_InitStruct.DMA_MemoryInc = DMA_MemoryInc_Enable;
DMA_InitStruct.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
DMA_InitStruct.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
DMA_InitStruct.DMA_Mode = DMA_Mode_Normal;
DMA_InitStruct.DMA_Priority = DMA_Priority_Medium;
DMA_InitStruct.DMA_FIFOMode = DMA_FIFOMode_Disable;
DMA_InitStruct.DMA_MemoryBurst = DMA_MemoryBurst_Single;
DMA_InitStruct.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
DMA_Init(UART_TX_DMA, &DMA_InitStruct);
DMA_Cmd(UART_TX_DMA, ENABLE);
}
/**
* @brief Asynchronously sends the given byte through the UART bus.
* @param data the data byte to send.
*/
void uart_SendByteAsync(uint8_t data)
{
// Check that it is possible to write in the current buffer.
if(uart_txBufferIndex >= UART_TX_BUFFER_SIZE)
{
// If the DMA is idle, switch to the other buffer.
if(UART_DMA_TX_IS_BUSY)
{
utils_TrapCpu(); // Error, can't write anywhere!
return;
}
else
uart_StartDma();
}
// Write the byte in the buffer.
uart_txBuffer[uart_currentTxBufferToWriteTo][uart_txBufferIndex] = data;
uart_txBufferIndex++;
// If the buffer is full, start the DMA transfer.
if(uart_txBufferIndex == UART_TX_BUFFER_SIZE)
uart_StartDma();
}
/**
* @brief Asynchronously sends the given bytes through the UART bus.
* @param data pointer to the data bytes array to send.
* @param length number of bytes to send (array size).
* @remark The bytes may not be send immediately, they are stored temporarily in
* an intermediate buffer that the DMA will copy to UART peripheral, when it is
* full. Call uart_FlushTx() to start the DMA transfer immediately.
*/
void uart_SendBytesAsync(uint8_t *data, int length)
{
while(length > 0)
{
uint16_t nBytesToWriteInBuf;
// Check that it is possible to write in the current buffer.
if(uart_txBufferIndex >= UART_TX_BUFFER_SIZE)
{
// If the DMA is idle, switch to the other buffer.
if(UART_DMA_TX_IS_BUSY)
{
utils_TrapCpu(); // Error, can't write anywhere!
return;
}
else
uart_StartDma();
}
// Write as many bytes as possible in the current buffer.
nBytesToWriteInBuf = UART_TX_BUFFER_SIZE - uart_txBufferIndex;
if(nBytesToWriteInBuf > length)
nBytesToWriteInBuf = length;
memcpy(&uart_txBuffer[uart_currentTxBufferToWriteTo][uart_txBufferIndex],
data, nBytesToWriteInBuf);
uart_txBufferIndex += nBytesToWriteInBuf;
data += nBytesToWriteInBuf;
length -= nBytesToWriteInBuf;
// If the buffer is full, start the DMA transfer.
if(uart_txBufferIndex == UART_TX_BUFFER_SIZE)
uart_StartDma();
}
}
/**
* @brief Start the DMA to send the bytes waiting in the intermediate buffer.
*/
void uart_FlushTx(void)
{
if(uart_txBufferIndex > 0 && !UART_DMA_TX_IS_BUSY)
uart_StartDma();
}
diff --git a/Firmware/src/drivers/uart.h b/Firmware/src/drivers/uart.h
index 8a83d89..b01f29b 100644
--- a/Firmware/src/drivers/uart.h
+++ b/Firmware/src/drivers/uart.h
@@ -1,52 +1,51 @@
#ifndef __UART_H
#define __UART_H
#include "../main.h"
#include "../lib/circular_buffer.h"
#define USART_RX_Pin GPIO_Pin_5
#define USART_RX_PinSource GPIO_PinSource5
#define USART_RX_Port GPIOD
#define USART_TX_Pin GPIO_Pin_6
#define USART_TX_PinSource GPIO_PinSource6
#define USART_TX_Port GPIOD
#define USART_PC_COMM USART2 // UART peripheral used for the comm with the PC.
-#define USART_BAUDRATE 1843200 //921600 // USART baudrate [b/s].
/** @defgroup UART Driver / UART
* @brief Driver for the UART serial communication peripheral.
*
* This driver controls the UART peripheral of the STM32, connected to the
* USB-to-serial chip.
*
* Call uart_Init() first in the initialization code, specifiying the function
* to call when a byte arrives (sent from the computer). To send data, call
* uart_SendByte() or uart_SendBytes().
*
* Note that this module should not be used directly. It is a better option to
* use it through the Communication module, to benefit from the already
* implemented communication protocol between the board and MATLAB.
*
* @addtogroup UART
* @{
*/
/**
* Typedef for a pointer to a function to call automatically when a byte
* arrives from the computer.
*/
typedef void (*uart_rxByteHandlerFunc)(uint8_t rxByte);
void uart_Init(void);
void uart_Step(void);
cb_CircularBuffer* uart_GetRxQueue(void);
void uart_SendByteAsync(uint8_t data);
void uart_SendBytesAsync(uint8_t *data, int length);
void uart_FlushTx(void);
/**
* @}
*/
#endif
diff --git a/Firmware/src/main.c b/Firmware/src/main.c
index 3472032..b98c26f 100644
--- a/Firmware/src/main.c
+++ b/Firmware/src/main.c
@@ -1,62 +1,89 @@
#include "main.h"
#include "communication.h"
#include "controller.h"
#include "drivers/adc.h"
#include "drivers/callback_timers.h"
#include "drivers/dac.h"
#include "drivers/h_bridge.h"
#include "drivers/hall.h"
#include "drivers/incr_encoder.h"
#include "drivers/strain_gauge.h"
#include "drivers/tachometer.h"
#include "lib/utils.h"
+extern volatile uint32_t ctrl_timestamp;
+
volatile uint32_t statusReg = 0x00000000; // Main state register.
/**
* @brief Main function, setups all the drivers and controllers.
*/
int main(void)
{
+ float demoSineOutput, demoCosineOutput, demoSineAmplitude;
+ int16_t testInt;
+
// Setup the GPIOs and the interrupts.
RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA | RCC_AHB1Periph_GPIOB |
RCC_AHB1Periph_GPIOC | RCC_AHB1Periph_GPIOD |
RCC_AHB1Periph_GPIOE, ENABLE);
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_4);
//
cbt_Init(); // Setup the timers that will call the loops functions.
adc_Init(); // Setup the ADC.
comm_Init(); // Setup the communication module.
ctrl_Init(); // Setup the controllers.
- enc_Init(); // Incremental encoders.
- hb_Init(); // H-bridge.
- hb_Enable();
+ enc_Init(); // Setup the incremental encoders.
+
+ hb_Init(); // Setup the H-bridge.
+ hb_Enable(); //
+
+ dac_Init(); // Setup the DAC.
// Delay to let the power electronics stabilize before calibrating the
// current sensor.
utils_DelayMs(200);
adc_CalibrateCurrentSens();
- statusReg |= RUN_CURRENT_LOOP;
- dac_Init();
+ // Start the current loop.
+ ctrl_StartCurrentLoop();
- // Init external sensors drivers, connected to the ADC.
+ // Init the external sensors drivers, connected to the ADC.
hall_Init(ANIN1);
//sg_Init(ANIN2);
//tac_Init(ANIN2);
-
- comm_SendDebugMessage("Initialization finished.");
+
+ // SyncVar demo with a configurable sine wave.
+ testInt = 0.0f;
+ demoSineAmplitude = 1.0f;
+ comm_monitorFloat("demo_sine_output", &demoSineOutput, READONLY);
+ comm_monitorFloat("demo_cosine_output", &demoCosineOutput, READONLY);
+ comm_monitorFloat("demo_sine_amplitude", &demoSineAmplitude, READWRITE);
+ comm_monitorInt16("demo_test_int", &testInt, READWRITE);
+
+ // End of the initialization. Lock the SyncVar list, and notify the PC that
+ // the board has (re)started.
+ comm_LockSyncVarsList();
+ comm_NotifyReady();
// Endless loop. The low priority functions are called here.
while(1)
{
// Update the communication.
comm_Step();
+
+ // SyncVar demo with a configurable sine wave.
+ demoSineOutput = demoSineAmplitude *
+ sinf(2.0f*3.14f*((float)ctrl_timestamp) / 1000000.0f
+ * 0.2f);
+ demoCosineOutput = demoSineAmplitude *
+ cosf(2.0f*3.14f*((float)ctrl_timestamp) / 1000000.0f
+ * 0.2f);
}
}
diff --git a/Firmware/src/main.h b/Firmware/src/main.h
index 5b48018..34e6d49 100644
--- a/Firmware/src/main.h
+++ b/Firmware/src/main.h
@@ -1,98 +1,62 @@
/** \mainpage HRI board firmware documentation
* \section intro_sec Introduction
* This is the documentation of the HRI board firmware. You will find a
* description of the provided library and drivers. As all the hardware and
* software is very new, please do not hesitate to suggest improvements to the
* course assistants (Romain Baud, Philipp Hörler and Laurent Jenni).
*
* \section code_struct_sec Code structure
* The code is divided into 3 parts:
* - main: this is where the controllers and the communication protocol are
* implemented. The call of all the initialization functions is also made here.
* - library: all the algorithms, data structures that could be used at many
* places, are put into the library.
* - drivers: there is one driver per peripheral. All the hardware-specific
* code is wrapped into these files, so that the user does not have to use a
*
* One module (controller, communication, library or driver) is always splitted
* in two files, .h and .c, like in C++.
* @image html soft_architecture.png
*
* \section lib_how_to How to use the provided library
* For most of the driver/library modules, call x_Init() only once, when the
* program starts (typically in the main()). Then, you can call
* x_Step()/x_Set()/x_Get() everytime as needed, typically in the control
* functions called periodically (ctrl_RegulatePosition()...).
*
* See the "Modules" section of this documentation to see how they work.
*/
#ifndef __MAIN_H
#define __MAIN_H
#include "stm32f4_discovery.h"
#include
#include
#include "arm_math.h"
// Clocks configuration.
#define APB1_PRESCALER 4
#define APB2_PRESCALER 2
#define TIM_MULTIPLIER 2
// Interupt priority.
#define CURRENT_LOOP_IRQ_PRIORITY 1 // High freq loop, should not be interrupted.
#define CONTROL_LOOP_IRQ_PRIORITY 2
#define CODER_INDEX_IRQ_PRIORITY 2 // Useless, remove?
#define UART_RX_IRQ_PRIORIY 3
#define DATA_LOOP_IRQ_PRIORITY 4 // Streaming packets, lowest priority.
// Electrical parameters.
#define STM_SUPPLY_VOLTAGE 3.3f // [V].
#define H_BRIDGE_SUPPLY_VOLTAGE 24.0f // [V].
#define CURRENT_SHUNT_RESISTANCE 0.01f // [ohm].
#define CURRENT_SHUNT_AMPLIFIER_GAIN 50 // [].
#define MOTOR_RESISTANCE 10.6f + 5.0f // 10.6 ohm according to the datasheet, actually more, depends on the motor [ohm].
// Mechanical parameters.
#define REDUCTION_RATIO 16.5f
#define MOTOR_TORQUE_CONST 0.0538f // [N.m/A].
-
-// Status register bit list.
-#define STREG_0 ((uint32_t)0x00000001)
-#define STREG_1 ((uint32_t)0x00000002)
-#define REF_POS_INIT ((uint32_t)0x00000004) // 1 -> Ref position succesfully found
-#define CALIBRE ((uint32_t)0x00000008) // 1 -> Calibration done
-#define RUN_CURRENT_LOOP ((uint32_t)0x00000010) // 1 -> Current loop active (if motorDrv eneable but RUN_CURRENT_LOOP=false -> fixed 50% PWM on the motor)
-#define UN_CONTROL_LOOP ((uint32_t)0x00000020) // 1 -> Control loop active
-#define STREG_6 ((uint32_t)0x00000040)
-#define STREG_7 ((uint32_t)0x00000080)
-
-#define STREG_8 ((uint32_t)0x00000100)
-#define DATA_UPSTREAM_ENB ((uint32_t)0x00000200) // 1 -> data upstream active
-#define STREG_10 ((uint32_t)0x00000400)
-#define RX_COMPLETED ((uint32_t)0x00000800) // 1 -> packet recieved
-#define RX_ERROR ((uint32_t)0x00001000) // 1 -> last received packet incorrect
-#define PROCESSING_REQUEST ((uint32_t)0x00002000) // 1 -> processing last received packet in progress (can not receive new data)
-#define LAST_REQUEST_IGNORED ((uint32_t)0x00004000)
-#define STREG_15 ((uint32_t)0x00008000)
-
-#define STREG_16 ((uint32_t)0x00010000)
-#define STREG_17 ((uint32_t)0x00020000)
-#define STREG_18 ((uint32_t)0x00040000)
-#define STREG_19 ((uint32_t)0x00080000)
-#define STREG_20 ((uint32_t)0x00100000)
-#define STREG_21 ((uint32_t)0x00200000)
-#define STREG_22 ((uint32_t)0x00400000)
-#define STREG_23 ((uint32_t)0x00800000)
-
-#define INDEX_ERROR ((uint32_t)0x01000000) // 1 -> wrong step count over 1 rotation
-#define STREG_25 ((uint32_t)0x02000000)
-#define STREG_26 ((uint32_t)0x04000000)
-#define STREG_27 ((uint32_t)0x08000000)
-#define STREG_28 ((uint32_t)0x10000000)
-#define STREG_29 ((uint32_t)0x20000000)
-#define STREG_30 ((uint32_t)0x40000000)
-#define STREG_31 ((uint32_t)0x80000000)
+#define MOTOR_NOMINAL_TORQUE 0.0323f // [N.m].
#endif
diff --git a/MATLAB/Data/info.txt b/MATLAB/Data/info.txt
deleted file mode 100644
index b1a0eed..0000000
--- a/MATLAB/Data/info.txt
+++ /dev/null
@@ -1 +0,0 @@
-This folder will contain all the logfiles recorded by the MATLAB interface.
\ No newline at end of file
diff --git a/MATLAB/Functions/getStatusReg.m b/MATLAB/Functions/getStatusReg.m
deleted file mode 100644
index 90d2ad0..0000000
--- a/MATLAB/Functions/getStatusReg.m
+++ /dev/null
@@ -1,47 +0,0 @@
-function [StatReg,ERR] = getStatusReg(COMOPT,s)
-% Demande le registe de status de la carte (stand alone function)
-%
-% Output :
-% StatReg -> Renvoie le registre sous forme d'un string
-% ERR -> 0 : ok
-% 1 : TimeOut
-%
-
-ERR = 0;
-TimeOut = COMOPT.USART_TIMEOUT_SHORT;
-PacketSize = 8; % uint32 is sent on 8 (half) bytes
-
-flushinput(s); %Vide le buffer
-[~] = usartSendPacket(COMOPT.PC_MESSAGE_SEND_STATUS, [], COMOPT, s); %Demande le registre
-
-
-tmp = 0;
-while(TimeOut>0)
- BA = get(s,{'BytesAvailable'});
- if(BA{1}= COMOPT.StartFrame) && (tmp-COMOPT.StartFrame == COMOPT.STM_MESSAGE_STATUS))
- tmp = fread(s,8);
- TimeOut = 0;
- else
- TimeOut = TimeOut-0.01; %trés aproximatif
- end
- end
-end
-
-if(max(size(tmp))~=8)
- StatReg=[];
- ERR=1;
- return;
-end
-
-StatReg = dec2bin(2^28*tmp(7) + 2^24*tmp(8) + 2^20*tmp(5) + 2^16*tmp(6)+...
- 2^12*tmp(3) + 2^8*tmp(4) + 2^4*tmp(1) + tmp(2),32);
-%78 56 34 12 because this corresponds to the data bytes 3 2 1 0.
-%StatReg = tmp;
-end
-
-
diff --git a/MATLAB/Functions/getVar.m b/MATLAB/Functions/getVar.m
deleted file mode 100644
index 91f1e2f..0000000
--- a/MATLAB/Functions/getVar.m
+++ /dev/null
@@ -1,86 +0,0 @@
-function [Var,ERR] = getVar(Var_id,handles)
-% Demande l'etat d'une variable (stand alone function)
-%
-% Output :
-% Var -> returns the asked variable
-% ERR -> 0 : ok
-% 1 : TimeOut (nothing received)
-% 2 : The packet has not the right size
-% 3 : The received variable is not the one asked
-
-ERR = 0;
-TimeOut = handles.COMOPT.USART_TIMEOUT_SHORT;
-PacketSize = 11; % header, var_id on 2 (half) bytes, variable on 8 (half) bytes
-if handles.streaming_enable
- usartSendPacket(handles.COMOPT.PC_MESSAGE_STOP_STREAMING,[],handles.COMOPT,handles.s);
- stop(handles.UsartTimer);
-end
-flushinput(handles.s); %Vide le buffer
-
-tmp0 = dec2hex(Var_id,2);
-packet = [handles.COMOPT.StartFrame + handles.COMOPT.PC_MESSAGE_GET_VAR, hex2dec(tmp0(1)), hex2dec(tmp0(2))];
-fwrite(handles.s,packet);
-
-tmp = 0;
-while(TimeOut>0)
- BA = get(handles.s,{'BytesAvailable'});
- if(BA{1}= handles.COMOPT.StartFrame) && (tmp-handles.COMOPT.StartFrame == handles.COMOPT.STM_MESSAGE_VAR))
- tmp0 = fread(handles.s,2);
- tmp1 = fread(handles.s,8);
- TimeOut = 0;
- else
- TimeOut = TimeOut-0.01; %trés aproximatif
- if TimeOut == 0
- Var=[];
- ERR=1;
- return;
- end
- end
- end
-end
-
-if(max(size(tmp1))~=8)
- Var=[];
- ERR=2;
- return;
-end
-
-Var_id_recu = 2^4*tmp0(1) + tmp0(2);
-if Var_id_recu ~= Var_id
- Var=[];
- ERR=3;
- return;
-end
-
-Var = uint32(2^28*tmp1(7) + 2^24*tmp1(8) + 2^20*tmp1(5) + 2^16*tmp1(6)+...
- 2^12*tmp1(3) + 2^8*tmp1(4) + 2^4*tmp1(1) + tmp1(2));
-%78 56 34 12 because this corresponds to the data bytes 3 2 1 0.
-%StatReg = tmp;
-
-if sum(Var_id == [1:6,8:10,13:26]) %Var is a float
- Var = typecast(Var,'single');
-end
-
-%{
-if sum(Var_id == []) %Var is a int
- Var = typecast(Var,'int32');
-end
-%}
-
-if handles.streaming_enable
- usartSendPacket(handles.COMOPT.PC_MESSAGE_START_STREAMING,[],handles.COMOPT,handles.s);
- start(handles.UsartTimer);
-end
-end
-
-
diff --git a/MATLAB/Functions/grabStreamPackets.m b/MATLAB/Functions/grabStreamPackets.m
deleted file mode 100644
index 492561c..0000000
--- a/MATLAB/Functions/grabStreamPackets.m
+++ /dev/null
@@ -1,63 +0,0 @@
-function [Data,ERR] = grabStreamPackets(STREAMING_VARIABLES,COMOPT,s)
-% Reads all the packets in the USART buffer of Matlab
-%
-% ERR = 1: There was no complete packet in the buffer
-% ERR = 2: There were not only streaming packets
-
-StreamSize = sum(dec2bin(STREAMING_VARIABLES,32)-'0'); %number of '1' in binary STREAMING_VARIABLES
-ERR = 0;
-BA = get(s,{'BytesAvailable'});
-if(BA{1}= COMOPT.StartFrame); % ID de tout les débuts de packet
-
-CompletPacketNum = max(size(ID))-1;
-
-if(CompletPacketNum<2) return; end
-
-Vars_index = 33-find(dec2bin(STREAMING_VARIABLES,32)-'0'); %index of the streamed variables
-Data = nan(CompletPacketNum,COMOPT.NumberVariables);
-
-% All Streamed variables are put in 'Data' as if they were uint32
-for i=1:CompletPacketNum
- packet = tmp( ID(i)+1 : ID(i+1)-1);
-
- if size(packet,1) == StreamSize*8 %a packet should contain 8 half bytes per variable
- if tmp(ID(i))-COMOPT.StartFrame == COMOPT.STM_MESSAGE_STREAMING_PACKET
- j = StreamSize + 1;
- for k = Vars_index
- j = j - 1;
- if sum(k-1 == [1:6,8:10,13:26]) %Var is a float
- Data(i,k) = typecast([2^4*packet((j-1)*8 + 1) + packet((j-1)*8 + 2), ...
- 2^4*packet((j-1)*8 + 3) + packet((j-1)*8 + 4), ...
- 2^4*packet((j-1)*8 + 5) + packet((j-1)*8 + 6), ...
- 2^4*packet((j-1)*8 + 7) + packet((j-1)*8 + 8)],'single');
- %{
- elseif sum(k-1 == []) %Var is a int32
- Data(i,k) = typecast([2^4*packet((j-1)*8 + 1) + packet((j-1)*8 + 2), ...
- 2^4*packet((j-1)*8 + 3) + packet((j-1)*8 + 4), ...
- 2^4*packet((j-1)*8 + 5) + packet((j-1)*8 + 6), ...
- 2^4*packet((j-1)*8 + 7) + packet((j-1)*8 + 8)],'int32');
- %}
- else %Var is a uint32
- Data(i,k) = typecast([2^4*packet((j-1)*8 + 1) + packet((j-1)*8 + 2), ...
- 2^4*packet((j-1)*8 + 3) + packet((j-1)*8 + 4), ...
- 2^4*packet((j-1)*8 + 5) + packet((j-1)*8 + 6), ...
- 2^4*packet((j-1)*8 + 7) + packet((j-1)*8 + 8)],'uint32');
- end
- end
- else
- ERR = 2;
- end
- else
- disp('some packets were not complete');
- end
-
-end
-
diff --git a/MATLAB/Functions/setStreamedVars.m b/MATLAB/Functions/setStreamedVars.m
deleted file mode 100644
index 1884c81..0000000
--- a/MATLAB/Functions/setStreamedVars.m
+++ /dev/null
@@ -1,14 +0,0 @@
-function setStreamedVars(Vars,COMOPT,s)
-% Envoie les variables à streamer (stand alone function)
-
-
-tmp = dec2hex(Vars,8);
-packet = [COMOPT.StartFrame + COMOPT.PC_MESSAGE_SET_STREAMED_VAR,... %Header
- hex2dec(tmp(7)),hex2dec(tmp(8)),hex2dec(tmp(5)),hex2dec(tmp(6)),... %4 bytes with the Vars
- hex2dec(tmp(3)),hex2dec(tmp(4)),hex2dec(tmp(1)),hex2dec(tmp(2))];
- %78 56 34 12 because this corresponds to the data bytes 3 2 1 0.
-fwrite(s,packet);
-
-end
-
-
diff --git a/MATLAB/Functions/setVar.m b/MATLAB/Functions/setVar.m
deleted file mode 100644
index 1c860f9..0000000
--- a/MATLAB/Functions/setVar.m
+++ /dev/null
@@ -1,25 +0,0 @@
-function setVar(Var_id,Value,COMOPT,s)
-% Envoie la valeur pour une variable (stand alone function)
-
-if sum(Var_id == [1:6,8:10,13:26]) %Var is a float
- Value = typecast(Value,'uint32');
-end
-
-%{
-if sum(Var_id == [8,9]) %Var is a int
- Value = typecast(Value,'uint32');
-end
-%}
-
-tmp0 = dec2hex(Var_id,2);
-tmp1 = dec2hex(Value,8);
-packet = [COMOPT.StartFrame + COMOPT.PC_MESSAGE_SET_VAR,... %Header
- hex2dec(tmp0(1)), hex2dec(tmp0(2)),... %1 byte with the Var_id
- hex2dec(tmp1(7)),hex2dec(tmp1(8)),hex2dec(tmp1(5)),hex2dec(tmp1(6)),...%4 bytes with the value
- hex2dec(tmp1(3)),hex2dec(tmp1(4)),hex2dec(tmp1(1)),hex2dec(tmp1(2))];
- %78 56 34 12 because this corresponds to the data bytes 3 2 1 0.
-fwrite(s,packet);
-
-end
-
-
diff --git a/MATLAB/Functions/usartSendPacket.m b/MATLAB/Functions/usartSendPacket.m
deleted file mode 100644
index e4d6524..0000000
--- a/MATLAB/Functions/usartSendPacket.m
+++ /dev/null
@@ -1,53 +0,0 @@
-function [ERR] = usartSendPacket(Request, PayLoad, COMOPT, s)
-% Transmition d'une trame de données via port COM. Deux formats de trame
-% disponible :
-%
-% Trame sans données -> |.START.|
-% Trame avec données -> |.START.|.DATA1 ... DATAn.|
-%
-% Arguments :
-% - Request -> Une des requêtes listées dans "HostRequestList.m"
-% - PayLoad -> Les données à envoyer sous forme d'un vecteur [data1,data2 ... dataN]
-% avec N plus petit ou égal à "COMOPT.MaxPayloadSize".
-% Si pas de PayLoad laisser vide ([])
-%
-% Erreur1 : Si la requète ne doit pas être accompagrnée de donnée et que
-% PayLoad != []
-%
-% Erreur2 : Si la taille de la PayLoad >= COMOPT.MaxPayloadSize
-%
-
-PayLoadSize = max(size(PayLoad)); %En nb. de int32
-ERR = 0;
-
-if(PayLoadSize>COMOPT.MaxPayloadSize)
- disp('PayLoad trop grande pour être envoyée en 1 fois');
- ERR = 1;
- return;
-end
-if((Request<5) && (PayLoadSize>0))
- disp('Cette requète ne peut pas être accompagnée de donnée');
- ERR = 2;
- return;
-end
-
-
-
-if(PayLoadSize) %Si Payload existante
- %Crée un packet vide (entête mais sans payload)
- packet = [ COMOPT.StartFrame + Request,...
- zeros(1,2*PayLoadSize*4)];
- %Ajout de la payload sous forme de segments de 2*4 bits
- for i=1:PayLoadSize
- tmp0 = dec2hex(PayLoad(i),8);
- packet(2+(i-1)*8:9+(i-1)*8) = [hex2dec(tmp0(7)),hex2dec(tmp0(8)),hex2dec(tmp0(5)),hex2dec(tmp0(6)),...
- hex2dec(tmp0(3)),hex2dec(tmp0(4)),hex2dec(tmp0(1)),hex2dec(tmp0(2))];
- %78 56 34 12 because this corresponds to the data bytes 3 2 1 0.
- end
-else %Si pas de Payload
- packet = COMOPT.StartFrame + Request;
-end
-
-% Envoit le packet
-fwrite(s,packet);
-
diff --git a/MATLAB/GUI.fig b/MATLAB/GUI.fig
deleted file mode 100644
index fe0d079..0000000
Binary files a/MATLAB/GUI.fig and /dev/null differ
diff --git a/MATLAB/GUI.m b/MATLAB/GUI.m
deleted file mode 100644
index 64eda1c..0000000
--- a/MATLAB/GUI.m
+++ /dev/null
@@ -1,1836 +0,0 @@
-% #########################################################################
-% File : GUI.m
-% Auteurs : Laurent Jenni, Philipp Hörler
-% Laboratoire de Systèmes Robotiques, EPFL
-%
-% #########################################################################
-
-function varargout = GUI(varargin)
-% GUI MATLAB code for GUI.fig-+
-% GUI, by itself, creates a new GUI or raises the existing
-% singleton*.
-%
-% H = GUI returns the handle to a new GUI or the handle to
-% the existing singleton*.
-%
-% GUI('CALLBACK',hObject,eventData,handles,...) calls the local
-% function named CALLBACK in GUI.M with the given input arguments.
-%
-% GUI('Property','Value',...) creates a new GUI or raises the
-% existing singleton*. Starting from the left, property value pairs are
-% applied to the GUI before GUI_OpeningFcn gets called. An
-% unrecognized property name or invalid value makes property application
-% stop. All inputs are passed to GUI_OpeningFcn via varargin.
-%
-% *See GUI Options on GUIDE's Tools menu. Choose "GUI allows only one
-% instance to run (singleton)".
-%
-% See also: GUIDE, GUIDATA, GUIHANDLES
-
-% Edit the above text to modify the response to help GUI
-
-% Last Modified by GUIDE v2.5 08-Apr-2015 13:59:59
-
-% Begin initialization code - DO NOT EDIT
-gui_Singleton = 1;
-gui_State = struct('gui_Name', mfilename, ...
- 'gui_Singleton', gui_Singleton, ...
- 'gui_OpeningFcn', @GUI_OpeningFcn, ...
- 'gui_OutputFcn', @GUI_OutputFcn, ...
- 'gui_LayoutFcn', [] , ...
- 'gui_Callback', []);
-if nargin && ischar(varargin{1})
- gui_State.gui_Callback = str2func(varargin{1});
-end
-
-if nargout
- [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
-else
- gui_mainfcn(gui_State, varargin{:});
-end
-end
-% End initialization code - DO NOT EDIT
-
-% --- Executes just before GUI is made visible.
-function GUI_OpeningFcn(hObject, eventdata, handles, varargin)
-% This function has no output args, see OutputFcn.
-% hObject handle to figure
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-% varargin command line arguments to GUI (see VARARGIN)
-
-% Choose default command line output for GUI
-handles.output = hObject;
-
-%% Options
-clc;
-delete(timerfind);
-addpath(strcat(pwd,'\Functions'));
-addpath(strcat(pwd,'\ParamFiles'));
-run('ComProtocolOpt.m');
-run('HostRequestList.m');
-run('StatusRegBitsList.m');
-run('UserVars.m');
-
-handles.COMOPT = COMOPT;
-handles.USER_VARS = USER_VARS;
-
-%% Initialize USART
-if exist('handles.s')
- fclose(handles.s);
- delete(handles.s);
- clear handles.s;
-end
-delete(instrfind);
-% Check the available COM ports. If there are more than one ask which one.
-% The MATLAB question dialogue is limited to 3 answers, so if there are 4
-% or more COM ports
-HWinfo = instrhwinfo('serial');
-if size(HWinfo.AvailableSerialPorts,1) == 0
- errordlg('There is no COM port available','COM Port Error');
- close all
- return
-elseif size(HWinfo.AvailableSerialPorts,1) == 1
- handles.COMOPT.USART_PORT = HWinfo.AvailableSerialPorts{1};
-elseif size(HWinfo.AvailableSerialPorts,1) >= 2
- [sel,ok] = listdlg('PromptString','There are more than one COM ports available. Which one should be used?',...
- 'SelectionMode','single',...
- 'ListSize',[400 100],...
- 'ListString',HWinfo.AvailableSerialPorts);
- if ok
- handles.COMOPT.USART_PORT = HWinfo.AvailableSerialPorts{sel};
- else
- close all
- return
- end
-end
-handles.s = serial(handles.COMOPT.USART_PORT);
-set(handles.s,'BaudRate', handles.COMOPT.USART_BAUDRATE, ...
- 'Timeout', handles.COMOPT.USART_TIMEOUT , ...
- 'InputBufferSize',handles.COMOPT.USART_INPUT_BUFFER_SIZE);
-fopen(handles.s);
-if(~strcmp(handles.s.Status,'open'))
- disp(strcat('Unable to open :',handles.COMOPT.USART_PORT));
- disp(strcat('Execution stopped'));
- return;
-end
-
-% Stop Streaming and verify Status
-usartSendPacket(handles.COMOPT.PC_MESSAGE_STOP_STREAMING,[],handles.COMOPT,handles.s);
-pause(0.1);
-
-[StatReg,ERR] = getStatusReg(handles.COMOPT,handles.s);
-if(ERR)
- errordlg('The microcontroller doesn''t answer. It probably isn''t powered or crashed. Verify the power supply or reset the microcontroller','Communication problem');
- close all
- return
-end
-
-%Set Timestamp to 0
-setVar(0,0,handles.COMOPT,handles.s);
-
-% Initialize data reception
-handles.streaming_vars = bin2dec('00000000000000000000000000000001');
-setStreamedVars(handles.streaming_vars,handles.COMOPT,handles.s);
-handles.streaming_enable = 0;
-handles.index = 1; %index
-handles.DisplayBufferSize = 20000;
-handles.dataBuffer = nan(handles.DisplayBufferSize, COMOPT.NumberVariables);
-period = 0.5;
-handles.UsartTimer = timer('BusyMode','drop',...
- 'ExecutionMode','fixedrate',...
- 'Period',round(period*1000)/1000,...
- 'TimerFcn',{@Receive_usart, hObject},...
- 'ErrorFcn',@(mTimer,event)disp(event.Data.message));
-
-%% Initialize Axes
-handles.PlotLines = plot(handles.axes1,0,zeros(1,COMOPT.NumberVariables),'visible','off'); %26 plotlines
-xlabel(handles.axes1,'Time [s]');
-ylabel(handles.axes1,'Data');
-
-set(handles.SaveCheckbox,'Value',false);
-% Update handles structure
-guidata(hObject, handles);
-
-end
-
-% --- Outputs from this function are returned to the command line.
-function varargout = GUI_OutputFcn(hObject, eventdata, handles)
-% varargout cell array for returning output args (see VARARGOUT);
-% hObject handle to figure
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-
-% Get default command line output from handles structure
-varargout{1} = handles;
-end
-
-% --- USART Timer function ---
-function Receive_usart(~, ~, hObject)
-
-if ~exist('handles')
- handles = guidata(hObject);
-end
-
-if(handles.streaming_enable == 1)
-
- [Data,ERR] = grabStreamPackets(handles.streaming_vars,handles.COMOPT,handles.s);
- if(~ERR)
-
-
- handles.dataBuffer(mod(handles.index-1:handles.index+size(Data,1)-1,handles.DisplayBufferSize)+1,:) = [Data; nan(1,handles.COMOPT.NumberVariables)];
- handles.index = handles.index + size(Data,1);
- handles.index = mod(handles.index,handles.DisplayBufferSize);
- if get(handles.SaveCheckbox,'Value')
- fwrite(handles.savefile,Data','double');
- end
-
- %Update data for the streamed plotlines
- Vars_index = 33-find(dec2bin(handles.streaming_vars,32)-'0');
- for i = Vars_index(1:end-1)
- set(handles.PlotLines(i),...
- 'xdata',handles.dataBuffer(:,1) / 1e6,...
- 'ydata',handles.dataBuffer(:,i));
- end
-
- else
- disp(['Error while grabStreamPackets: ' num2str(ERR)])
- end
-
-end
-% Update handles structure
-guidata(hObject, handles);
-end
-
-
-% --- Executes on button press in StartStreamingbutton.
-function StartStreamingbutton_Callback(hObject, eventdata, handles)
-% hObject handle to StartStreamingbutton (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-if handles.streaming_enable == 0
- usartSendPacket(handles.COMOPT.PC_MESSAGE_START_STREAMING,[],handles.COMOPT,handles.s);
- handles.streaming_enable = 1;
- start(handles.UsartTimer);
-end
-% Update handles structure
-guidata(hObject, handles);
-end
-
-% --- Executes on button press in StopStreamingbutton.
-function StopStreamingbutton_Callback(hObject, eventdata, handles)
-% hObject handle to StopStreamingbutton (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if handles.streaming_enable == 1
- stop(handles.UsartTimer);
- usartSendPacket(handles.COMOPT.PC_MESSAGE_STOP_STREAMING,[],handles.COMOPT,handles.s);
- pause(0.1); % to be sure that the card stopped streaming
- flushinput(handles.s);
- handles.streaming_enable = 0;
-
- handles.index = 1;
- handles.dataBuffer = handles.dataBuffer*0;
-
-end
-% Update handles structure
-guidata(hObject, handles);
-end
-
-
-% --- Executes on button press in ClearScreenButton.
-function ClearScreenButton_Callback(hObject, eventdata, handles)
-% hObject handle to ClearScreenButton (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-handles.index = 1;
-handles.dataBuffer(:) = nan;
-
-% Update handles structure
-guidata(hObject, handles);
-end
-
-
-% --------------------------------------------------------------------
-function FileMenu_Callback(hObject, eventdata, handles)
-% hObject handle to FileMenu (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-end
-
-% --------------------------------------------------------------------
-function OpenMenuItem_Callback(hObject, eventdata, handles)
-% hObject handle to OpenMenuItem (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-file = uigetfile('*.fig');
-if ~isequal(file, 0)
- open(file);
-end
-end
-
-% --------------------------------------------------------------------
-function PrintMenuItem_Callback(hObject, eventdata, handles)
-% hObject handle to PrintMenuItem (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-printdlg(handles.figure1)
-end
-
-% --------------------------------------------------------------------
-function CloseMenuItem_Callback(hObject, eventdata, handles)
-% hObject handle to CloseMenuItem (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-
-delete(handles.figure1)
-end
-
-
-% --- Executes during object deletion, before destroying properties.
-function figure1_DeleteFcn(hObject, eventdata, handles)
-% hObject handle to figure1 (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-
-close_function(hObject,eventdata,handles);
-end
-
-
-function close_function(hObject,eventdata,handles)
-% Close function to be executed when the GUI is closed
- stop(handles.UsartTimer);
- usartSendPacket(handles.COMOPT.PC_MESSAGE_STOP_STREAMING,[],handles.COMOPT,handles.s);
- pause(0.1); % to be sure that the card stopped streaming
- flushinput(handles.s);
- delete(handles.figure1);
- delete(handles.UsartTimer);
- fclose('all');
-end
-
-
-% --- Executes on button press in SaveCheckbox.
-function SaveCheckbox_Callback(hObject, eventdata, handles)
-% hObject handle to SaveCheckbox (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-
-% Hint: get(hObject,'Value') returns toggle state of SaveCheckbox
-if get(handles.SaveCheckbox,'Value')
- handles.savefile = fopen(['Data\Data_' datestr(now,'yyyy.mm.dd_HH.MM.SS') '.bin'],'a');
-else
- fclose(handles.savefile);
-end
-% Update handles structure
-guidata(hObject, handles);
-end
-
-% --- Executes on button press in KpChkbox.
-function KpChkbox_Callback(hObject, eventdata, handles)
-% hObject handle to KpChkbox (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-usartSendPacket(handles.COMOPT.PC_MESSAGE_STOP_STREAMING,[],handles.COMOPT,handles.s);
-if get(hObject,'Value')
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_POS_REG_PID_P+1,1);
- set(handles.PlotLines(handles.USER_VARS.VAR_POS_REG_PID_P+1),'visible','on');
-else
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_POS_REG_PID_P+1,0);
- set(handles.PlotLines(handles.USER_VARS.VAR_POS_REG_PID_P+1),'visible','off');
-end
-setStreamedVars(handles.streaming_vars,handles.COMOPT,handles.s);
-flushinput(handles.s);
-usartSendPacket(handles.COMOPT.PC_MESSAGE_START_STREAMING,[],handles.COMOPT,handles.s);
-% Update handles structure
-guidata(hObject, handles);
-end
-
-% --- Executes during object creation, after setting all properties.
-function KpEdit_CreateFcn(hObject, eventdata, handles)
-% hObject handle to KpEdit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles empty - handles not created until after all CreateFcns called
-
-% Hint: edit controls usually have a white background on Windows.
-% See ISPC and COMPUTER.
-if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
- set(hObject,'BackgroundColor','white');
-end
-end
-
-% --- Executes on button press in KpSet.
-function KpSet_Callback(hObject, eventdata, handles)
-% hObject handle to KpSet (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = str2num(get(handles.KpEdit,'String'));
-setVar(handles.USER_VARS.VAR_POS_REG_PID_P,single(Value),handles.COMOPT,handles.s);
-end
-
-% --- Executes on button press in KpGet.
-function KpGet_Callback(hObject, eventdata, handles)
-% hObject handle to KpGet (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = getVar(handles.USER_VARS.VAR_POS_REG_PID_P,handles);
-set(handles.KpText,'String',Value);
-end
-
-% --- Executes on button press in KiChkbox.
-function KiChkbox_Callback(hObject, eventdata, handles)
-% hObject handle to KiChkbox (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-usartSendPacket(handles.COMOPT.PC_MESSAGE_STOP_STREAMING,[],handles.COMOPT,handles.s);
-if get(hObject,'Value')
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_POS_REG_PID_I+1,1);
- set(handles.PlotLines(handles.USER_VARS.VAR_POS_REG_PID_I+1),'visible','on');
-else
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_POS_REG_PID_I+1,0);
- set(handles.PlotLines(handles.USER_VARS.VAR_POS_REG_PID_I+1),'visible','off');
-end
-setStreamedVars(handles.streaming_vars,handles.COMOPT,handles.s);
-flushinput(handles.s);
-usartSendPacket(handles.COMOPT.PC_MESSAGE_START_STREAMING,[],handles.COMOPT,handles.s);
-% Update handles structure
-guidata(hObject, handles);
-end
-
-% --- Executes during object creation, after setting all properties.
-function KiEdit_CreateFcn(hObject, eventdata, handles)
-% hObject handle to KiEdit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles empty - handles not created until after all CreateFcns called
-
-% Hint: edit controls usually have a white background on Windows.
-% See ISPC and COMPUTER.
-if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
- set(hObject,'BackgroundColor','white');
-end
-end
-
-% --- Executes on button press in KiSet.
-function KiSet_Callback(hObject, eventdata, handles)
-% hObject handle to KiSet (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = str2num(get(handles.KiEdit,'String'));
-setVar(handles.USER_VARS.VAR_POS_REG_PID_I,single(Value),handles.COMOPT,handles.s);
-end
-
-% --- Executes on button press in KiGet.
-function KiGet_Callback(hObject, eventdata, handles)
-% hObject handle to KiGet (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = getVar(handles.USER_VARS.VAR_POS_REG_PID_I,handles);
-set(handles.KiText,'String',Value);
-end
-
-
-% --- Executes on button press in KdChkbox.
-function KdChkbox_Callback(hObject, eventdata, handles)
-% hObject handle to KdChkbox (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-usartSendPacket(handles.COMOPT.PC_MESSAGE_STOP_STREAMING,[],handles.COMOPT,handles.s);
-if get(hObject,'Value')
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_POS_REG_PID_D+1,1);
- set(handles.PlotLines(handles.USER_VARS.VAR_POS_REG_PID_D+1),'visible','on');
-else
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_POS_REG_PID_D+1,0);
- set(handles.PlotLines(handles.USER_VARS.VAR_POS_REG_PID_D+1),'visible','off');
-end
-setStreamedVars(handles.streaming_vars,handles.COMOPT,handles.s);
-flushinput(handles.s);
-usartSendPacket(handles.COMOPT.PC_MESSAGE_START_STREAMING,[],handles.COMOPT,handles.s);
-% Update handles structure
-guidata(hObject, handles);
-end
-
-
-function KdEdit_Callback(hObject, eventdata, handles)
-% hObject handle to KdEdit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-
-% Hints: get(hObject,'String') returns contents of KdEdit as text
-% str2double(get(hObject,'String')) returns contents of KdEdit as a double
-end
-
-% --- Executes during object creation, after setting all properties.
-function KdEdit_CreateFcn(hObject, eventdata, handles)
-% hObject handle to KdEdit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles empty - handles not created until after all CreateFcns called
-
-% Hint: edit controls usually have a white background on Windows.
-% See ISPC and COMPUTER.
-if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
- set(hObject,'BackgroundColor','white');
-end
-end
-
-% --- Executes on button press in KdSet.
-function KdSet_Callback(hObject, eventdata, handles)
-% hObject handle to KdSet (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = str2num(get(handles.KdEdit,'String'));
-setVar(handles.USER_VARS.VAR_POS_REG_PID_D,single(Value),handles.COMOPT,handles.s);
-end
-
-% --- Executes on button press in KdGet.
-function KdGet_Callback(hObject, eventdata, handles)
-% hObject handle to KdGet (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = getVar(handles.USER_VARS.VAR_POS_REG_PID_D,handles);
-set(handles.KdText,'String',Value);
-end
-
-
-% --- Executes on button press in ARWChkbox.
-function ARWChkbox_Callback(hObject, eventdata, handles)
-% hObject handle to ARWChkbox (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-usartSendPacket(handles.COMOPT.PC_MESSAGE_STOP_STREAMING,[],handles.COMOPT,handles.s);
-if get(hObject,'Value')
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_POS_REG_PID_ARW+1,1);
- set(handles.PlotLines(handles.USER_VARS.VAR_POS_REG_PID_ARW+1),'visible','on');
-else
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_POS_REG_PID_ARW+1,0);
- set(handles.PlotLines(handles.USER_VARS.VAR_POS_REG_PID_ARW+1),'visible','off');
-end
-setStreamedVars(handles.streaming_vars,handles.COMOPT,handles.s);
-flushinput(handles.s);
-usartSendPacket(handles.COMOPT.PC_MESSAGE_START_STREAMING,[],handles.COMOPT,handles.s);
-% Update handles structure
-guidata(hObject, handles);
-end
-
-
-function ARWEdit_Callback(hObject, eventdata, handles)
-% hObject handle to ARWEdit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-
-% Hints: get(hObject,'String') returns contents of ARWEdit as text
-% str2double(get(hObject,'String')) returns contents of ARWEdit as a double
-end
-
-% --- Executes during object creation, after setting all properties.
-function ARWEdit_CreateFcn(hObject, eventdata, handles)
-% hObject handle to ARWEdit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles empty - handles not created until after all CreateFcns called
-
-% Hint: edit controls usually have a white background on Windows.
-% See ISPC and COMPUTER.
-if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
- set(hObject,'BackgroundColor','white');
-end
-end
-
-% --- Executes on button press in ARWSet.
-function ARWSet_Callback(hObject, eventdata, handles)
-% hObject handle to ARWSet (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = str2num(get(handles.ARWEdit,'String'));
-setVar(handles.USER_VARS.VAR_POS_REG_PID_ARW,single(Value),handles.COMOPT,handles.s);
-end
-
-% --- Executes on button press in ARWGet.
-function ARWGet_Callback(hObject, eventdata, handles)
-% hObject handle to ARWGet (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = getVar(handles.USER_VARS.VAR_POS_REG_PID_ARW,handles);
-set(handles.ARWText,'String',Value);
-end
-
-% --- Executes on button press in IntegratorChkbox.
-function IntegratorChkbox_Callback(hObject, eventdata, handles)
-% hObject handle to IntegratorChkbox (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-usartSendPacket(handles.COMOPT.PC_MESSAGE_STOP_STREAMING,[],handles.COMOPT,handles.s);
-if get(hObject,'Value')
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_POS_REG_PID_INTEGRATOR+1,1);
- set(handles.PlotLines(handles.USER_VARS.VAR_POS_REG_PID_INTEGRATOR+1),'visible','on');
-else
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_POS_REG_PID_INTEGRATOR+1,0);
- set(handles.PlotLines(handles.USER_VARS.VAR_POS_REG_PID_INTEGRATOR+1),'visible','off');
-end
-setStreamedVars(handles.streaming_vars,handles.COMOPT,handles.s);
-flushinput(handles.s);
-usartSendPacket(handles.COMOPT.PC_MESSAGE_START_STREAMING,[],handles.COMOPT,handles.s);
-% Update handles structure
-guidata(hObject, handles);
-end
-
-
-function IntegratorEdit_Callback(hObject, eventdata, handles)
-% hObject handle to IntegratorEdit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-
-% Hints: get(hObject,'String') returns contents of IntegratorEdit as text
-% str2double(get(hObject,'String')) returns contents of IntegratorEdit as a double
-end
-
-% --- Executes during object creation, after setting all properties.
-function IntegratorEdit_CreateFcn(hObject, eventdata, handles)
-% hObject handle to IntegratorEdit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles empty - handles not created until after all CreateFcns called
-
-% Hint: edit controls usually have a white background on Windows.
-% See ISPC and COMPUTER.
-if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
- set(hObject,'BackgroundColor','white');
-end
-end
-
-% --- Executes on button press in IntegratorSet.
-function IntegratorSet_Callback(hObject, eventdata, handles)
-% hObject handle to IntegratorSet (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = str2num(get(handles.IntegratorEdit,'String'));
-setVar(handles.USER_VARS.VAR_POS_REG_PID_INTEGRATOR,single(Value),handles.COMOPT,handles.s);
-end
-
-% --- Executes on button press in IntegratorGet.
-function IntegratorGet_Callback(hObject, eventdata, handles)
-% hObject handle to IntegratorGet (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = getVar(handles.USER_VARS.VAR_POS_REG_PID_INTEGRATOR,handles);
-set(handles.IntegratorText,'String',Value);
-end
-
-% --- Executes on button press in RegOutChkbox.
-function RegOutChkbox_Callback(hObject, eventdata, handles)
-% hObject handle to RegOutChkbox (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-usartSendPacket(handles.COMOPT.PC_MESSAGE_STOP_STREAMING,[],handles.COMOPT,handles.s);
-if get(hObject,'Value')
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_POS_REG_PID_OUTPUT+1,1);
- set(handles.PlotLines(handles.USER_VARS.VAR_POS_REG_PID_OUTPUT+1),'visible','on');
-else
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_POS_REG_PID_OUTPUT+1,0);
- set(handles.PlotLines(handles.USER_VARS.VAR_POS_REG_PID_OUTPUT+1),'visible','off');
-end
-setStreamedVars(handles.streaming_vars,handles.COMOPT,handles.s);
-flushinput(handles.s);
-usartSendPacket(handles.COMOPT.PC_MESSAGE_START_STREAMING,[],handles.COMOPT,handles.s);
-% Update handles structure
-guidata(hObject, handles);
-end
-
-% --- Executes on button press in RegOutGet.
-function RegOutGet_Callback(hObject, eventdata, handles)
-% hObject handle to RegOutGet (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = getVar(handles.USER_VARS.VAR_POS_REG_PID_OUTPUT,handles);
-set(handles.RegOutText,'String',Value);
-end
-
-% --- Executes on button press in RegPeriodChckbox.
-function RegPeriodChckbox_Callback(hObject, eventdata, handles)
-% hObject handle to RegPeriodChckbox (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-usartSendPacket(handles.COMOPT.PC_MESSAGE_STOP_STREAMING,[],handles.COMOPT,handles.s);
-if get(hObject,'Value')
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_POS_REG_SAMPLING_PERIOD+1,1);
- set(handles.PlotLines(handles.USER_VARS.VAR_POS_REG_SAMPLING_PERIOD+1),'visible','on');
-else
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_POS_REG_SAMPLING_PERIOD+1,0);
- set(handles.PlotLines(handles.USER_VARS.VAR_POS_REG_SAMPLING_PERIOD+1),'visible','off');
-end
-setStreamedVars(handles.streaming_vars,handles.COMOPT,handles.s);
-flushinput(handles.s);
-usartSendPacket(handles.COMOPT.PC_MESSAGE_START_STREAMING,[],handles.COMOPT,handles.s);
-% Update handles structure
-guidata(hObject, handles);
-end
-
-
-function RegPeriodEdit_Callback(hObject, eventdata, handles)
-% hObject handle to RegPeriodEdit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-
-% Hints: get(hObject,'String') returns contents of RegPeriodEdit as text
-% str2double(get(hObject,'String')) returns contents of RegPeriodEdit as a double
-end
-
-% --- Executes during object creation, after setting all properties.
-function RegPeriodEdit_CreateFcn(hObject, eventdata, handles)
-% hObject handle to RegPeriodEdit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles empty - handles not created until after all CreateFcns called
-
-% Hint: edit controls usually have a white background on Windows.
-% See ISPC and COMPUTER.
-if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
- set(hObject,'BackgroundColor','white');
-end
-end
-
-% --- Executes on button press in RegPeriodSet.
-function RegPeriodSet_Callback(hObject, eventdata, handles)
-% hObject handle to RegPeriodSet (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = str2num(get(handles.RegPeriodEdit,'String'));
-setVar(handles.USER_VARS.VAR_POS_REG_SAMPLING_PERIOD,single(Value),handles.COMOPT,handles.s);
-end
-
-% --- Executes on button press in RegPeriodGet.
-function RegPeriodGet_Callback(hObject, eventdata, handles)
-% hObject handle to RegPeriodGet (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = getVar(handles.USER_VARS.VAR_POS_REG_SAMPLING_PERIOD,handles);
-set(handles.RegPeriodText,'String',Value);
-end
-
-% --- Executes on button press in WallPosChkbox.
-function WallPosChkbox_Callback(hObject, eventdata, handles)
-% hObject handle to WallPosChkbox (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-usartSendPacket(handles.COMOPT.PC_MESSAGE_STOP_STREAMING,[],handles.COMOPT,handles.s);
-if get(hObject,'Value')
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_WALL_POSITION+1,1);
- set(handles.PlotLines(handles.USER_VARS.VAR_WALL_POSITION+1),'visible','on');
-else
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_WALL_POSITION+1,0);
- set(handles.PlotLines(handles.USER_VARS.VAR_WALL_POSITION+1),'visible','off');
-end
-setStreamedVars(handles.streaming_vars,handles.COMOPT,handles.s);
-flushinput(handles.s);
-usartSendPacket(handles.COMOPT.PC_MESSAGE_START_STREAMING,[],handles.COMOPT,handles.s);
-% Update handles structure
-guidata(hObject, handles);
-end
-
-
-function WallPosEdit_Callback(hObject, eventdata, handles)
-% hObject handle to WallPosEdit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-
-% Hints: get(hObject,'String') returns contents of WallPosEdit as text
-% str2double(get(hObject,'String')) returns contents of WallPosEdit as a double
-end
-
-% --- Executes during object creation, after setting all properties.
-function WallPosEdit_CreateFcn(hObject, eventdata, handles)
-% hObject handle to WallPosEdit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles empty - handles not created until after all CreateFcns called
-
-% Hint: edit controls usually have a white background on Windows.
-% See ISPC and COMPUTER.
-if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
- set(hObject,'BackgroundColor','white');
-end
-end
-
-% --- Executes on button press in WallPosSet.
-function WallPosSet_Callback(hObject, eventdata, handles)
-% hObject handle to WallPosSet (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-Value = str2num(get(handles.WallPosEdit,'String'));
-setVar(handles.USER_VARS.VAR_WALL_POSITION,single(Value),handles.COMOPT,handles.s);
-end
-
-% --- Executes on button press in WallPosGet.
-function WallPosGet_Callback(hObject, eventdata, handles)
-% hObject handle to WallPosGet (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = getVar(handles.USER_VARS.VAR_WALL_POSITION,handles);
-set(handles.WallPosText,'String',Value);
-end
-
-% --- Executes on button press in MeasuredAngleRawChkbox.
-function MeasuredAngleRawChkbox_Callback(hObject, eventdata, handles)
-% hObject handle to MeasuredAngleRawChkbox (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-usartSendPacket(handles.COMOPT.PC_MESSAGE_STOP_STREAMING,[],handles.COMOPT,handles.s);
-if get(hObject,'Value')
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_MEASURED_ANGLE_RAW+1,1);
- set(handles.PlotLines(handles.USER_VARS.VAR_MEASURED_ANGLE_RAW+1),'visible','on');
-else
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_MEASURED_ANGLE_RAW+1,0);
- set(handles.PlotLines(handles.USER_VARS.VAR_MEASURED_ANGLE_RAW+1),'visible','off');
-end
-setStreamedVars(handles.streaming_vars,handles.COMOPT,handles.s);
-flushinput(handles.s);
-usartSendPacket(handles.COMOPT.PC_MESSAGE_START_STREAMING,[],handles.COMOPT,handles.s);
-% Update handles structure
-guidata(hObject, handles);
-end
-
-function MeasuredAngleRawEdit_Callback(hObject, eventdata, handles)
-% hObject handle to MeasuredAngleRawEdit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-
-% Hints: get(hObject,'String') returns contents of MeasuredAngleRawEdit as text
-% str2double(get(hObject,'String')) returns contents of MeasuredAngleRawEdit as a double
-end
-
-% --- Executes during object creation, after setting all properties.
-function MeasuredAngleRawEdit_CreateFcn(hObject, eventdata, handles)
-% hObject handle to MeasuredAngleRawEdit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles empty - handles not created until after all CreateFcns called
-
-% Hint: edit controls usually have a white background on Windows.
-% See ISPC and COMPUTER.
-if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
- set(hObject,'BackgroundColor','white');
-end
-end
-
-% --- Executes on button press in MeasuredAngleRawSet.
-function MeasuredAngleRawSet_Callback(hObject, eventdata, handles)
-% hObject handle to MeasuredAngleRawSet (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-Value = str2num(get(handles.MeasuredAngleRawEdit,'String'));
-setVar(handles.USER_VARS.VAR_MEASURED_ANGLE_RAW,single(Value),handles.COMOPT,handles.s);
-end
-
-% --- Executes on button press in MeasuredAngleRawGet.
-function MeasuredAngleRawGet_Callback(hObject, eventdata, handles)
-% hObject handle to MeasuredAngleRawGet (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = getVar(handles.USER_VARS.VAR_MEASURED_ANGLE_RAW,handles);
-set(handles.MeasuredAngleRawText,'String',Value);
-end
-
-
-% --- Executes on button press in MeasuredAngleFilteredChkbox.
-function MeasuredAngleFilteredChkbox_Callback(hObject, eventdata, handles)
-% hObject handle to MeasuredAngleFilteredChkbox (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-usartSendPacket(handles.COMOPT.PC_MESSAGE_STOP_STREAMING,[],handles.COMOPT,handles.s);
-if get(hObject,'Value')
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_MEASURED_ANGLE_FILTERED+1,1);
- set(handles.PlotLines(handles.USER_VARS.VAR_MEASURED_ANGLE_FILTERED+1),'visible','on');
-else
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_MEASURED_ANGLE_FILTERED+1,0);
- set(handles.PlotLines(handles.USER_VARS.VAR_MEASURED_ANGLE_FILTERED+1),'visible','off');
-end
-setStreamedVars(handles.streaming_vars,handles.COMOPT,handles.s);
-flushinput(handles.s);
-usartSendPacket(handles.COMOPT.PC_MESSAGE_START_STREAMING,[],handles.COMOPT,handles.s);
-% Update handles structure
-guidata(hObject, handles);
-end
-
-% --- Executes on button press in MeasuredAngleFilteredGet.
-function MeasuredAngleFilteredGet_Callback(hObject, eventdata, handles)
-% hObject handle to MeasuredAngleFilteredGet (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = getVar(handles.USER_VARS.VAR_MEASURED_ANGLE_FILTERED,handles);
-set(handles.MeasuredAngleFilteredText,'String',Value);
-end
-
-% --- Executes on button press in CommunicationPeriodChkbox.
-function CommunicationPeriodChkbox_Callback(hObject, eventdata, handles)
-% hObject handle to CommunicationPeriodChkbox (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-usartSendPacket(handles.COMOPT.PC_MESSAGE_STOP_STREAMING,[],handles.COMOPT,handles.s);
-if get(hObject,'Value')
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_COMM_TX_PERIOD+1,1);
- set(handles.PlotLines(handles.USER_VARS.VAR_COMM_TX_PERIOD+1),'visible','on');
-else
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_COMM_TX_PERIOD+1,0);
- set(handles.PlotLines(handles.USER_VARS.VAR_COMM_TX_PERIOD+1),'visible','off');
-end
-setStreamedVars(handles.streaming_vars,handles.COMOPT,handles.s);
-flushinput(handles.s);
-usartSendPacket(handles.COMOPT.PC_MESSAGE_START_STREAMING,[],handles.COMOPT,handles.s);
-% Update handles structure
-guidata(hObject, handles);
-end
-
-
-function CommunicationPeriodEdit_Callback(hObject, eventdata, handles)
-% hObject handle to CommunicationPeriodEdit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-
-% Hints: get(hObject,'String') returns contents of CommunicationPeriodEdit as text
-% str2double(get(hObject,'String')) returns contents of CommunicationPeriodEdit as a double
-end
-
-% --- Executes during object creation, after setting all properties.
-function CommunicationPeriodEdit_CreateFcn(hObject, eventdata, handles)
-% hObject handle to CommunicationPeriodEdit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles empty - handles not created until after all CreateFcns called
-
-% Hint: edit controls usually have a white background on Windows.
-% See ISPC and COMPUTER.
-if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
- set(hObject,'BackgroundColor','white');
-end
-end
-
-% --- Executes on button press in CommunicationPeriodSet.
-function CommunicationPeriodSet_Callback(hObject, eventdata, handles)
-% hObject handle to CommunicationPeriodSet (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = str2num(get(handles.CommunicationPeriodEdit,'String'));
-setVar(handles.USER_VARS.VAR_COMM_TX_PERIOD,single(Value),handles.COMOPT,handles.s);
-end
-
-% --- Executes on button press in CommunicationPeriodGet.
-function CommunicationPeriodGet_Callback(hObject, eventdata, handles)
-% hObject handle to CommunicationPeriodGet (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = getVar(handles.USER_VARS.VAR_COMM_TX_PERIOD,handles);
-set(handles.CommunicationPeriodText,'String',Value);
-end
-
-% --- Executes on button press in ActiveFilterChkbox.
-function ActiveFilterChkbox_Callback(hObject, eventdata, handles)
-% hObject handle to ActiveFilterChkbox (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-usartSendPacket(handles.COMOPT.PC_MESSAGE_STOP_STREAMING,[],handles.COMOPT,handles.s);
-if get(hObject,'Value')
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_ACTIVE_FILTER+1,1);
- set(handles.PlotLines(handles.USER_VARS.VAR_ACTIVE_FILTER+1),'visible','on');
-else
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_ACTIVE_FILTER+1,0);
- set(handles.PlotLines(handles.USER_VARS.VAR_ACTIVE_FILTER+1),'visible','off');
-end
-setStreamedVars(handles.streaming_vars,handles.COMOPT,handles.s);
-flushinput(handles.s);
-usartSendPacket(handles.COMOPT.PC_MESSAGE_START_STREAMING,[],handles.COMOPT,handles.s);
-% Update handles structure
-guidata(hObject, handles);
-end
-
-
-function ActiveFilterEdit_Callback(hObject, eventdata, handles)
-% hObject handle to ActiveFilterEdit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-
-% Hints: get(hObject,'String') returns contents of ActiveFilterEdit as text
-% str2double(get(hObject,'String')) returns contents of ActiveFilterEdit as a double
-end
-
-% --- Executes during object creation, after setting all properties.
-function ActiveFilterEdit_CreateFcn(hObject, eventdata, handles)
-% hObject handle to ActiveFilterEdit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles empty - handles not created until after all CreateFcns called
-
-% Hint: edit controls usually have a white background on Windows.
-% See ISPC and COMPUTER.
-if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
- set(hObject,'BackgroundColor','white');
-end
-end
-
-% --- Executes on button press in ActiveFilterSet.
-function ActiveFilterSet_Callback(hObject, eventdata, handles)
-% hObject handle to ActiveFilterSet (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = str2num(get(handles.ActiveFilterEdit,'String'));
-setVar(handles.USER_VARS.VAR_ACTIVE_FILTER,single(Value),handles.COMOPT,handles.s);
-end
-
-% --- Executes on button press in ActiveFilterGet.
-function ActiveFilterGet_Callback(hObject, eventdata, handles)
-% hObject handle to ActiveFilterGet (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = getVar(handles.USER_VARS.VAR_ACTIVE_FILTER,handles);
-set(handles.ActiveFilterText,'String',Value);
-end
-
-% --- Executes on button press in FirstOrderFilterTauChkbox.
-function FirstOrderFilterTauChkbox_Callback(hObject, eventdata, handles)
-% hObject handle to FirstOrderFilterTauChkbox (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-usartSendPacket(handles.COMOPT.PC_MESSAGE_STOP_STREAMING,[],handles.COMOPT,handles.s);
-if get(hObject,'Value')
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_1ST_FILT_TAU+1,1);
- set(handles.PlotLines(handles.USER_VARS.VAR_1ST_FILT_TAU+1),'visible','on');
-else
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_1ST_FILT_TAU+1,0);
- set(handles.PlotLines(handles.USER_VARS.VAR_1ST_FILT_TAU+1),'visible','off');
-end
-setStreamedVars(handles.streaming_vars,handles.COMOPT,handles.s);
-flushinput(handles.s);
-usartSendPacket(handles.COMOPT.PC_MESSAGE_START_STREAMING,[],handles.COMOPT,handles.s);
-% Update handles structure
-guidata(hObject, handles);
-end
-
-function FirstOrderFilterTauEdit_Callback(hObject, eventdata, handles)
-% hObject handle to FirstOrderFilterTauEdit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-
-% Hints: get(hObject,'String') returns contents of FirstOrderFilterTauEdit as text
-% str2double(get(hObject,'String')) returns contents of FirstOrderFilterTauEdit as a double
-end
-
-% --- Executes during object creation, after setting all properties.
-function FirstOrderFilterTauEdit_CreateFcn(hObject, eventdata, handles)
-% hObject handle to FirstOrderFilterTauEdit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles empty - handles not created until after all CreateFcns called
-
-% Hint: edit controls usually have a white background on Windows.
-% See ISPC and COMPUTER.
-if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
- set(hObject,'BackgroundColor','white');
-end
-end
-
-% --- Executes on button press in FirstOrderFilterTauSet.
-function FirstOrderFilterTauSet_Callback(hObject, eventdata, handles)
-% hObject handle to FirstOrderFilterTauSet (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = str2num(get(handles.FirstOrderFilterTauEdit,'String'));
-setVar(handles.USER_VARS.VAR_1ST_FILT_TAU,single(Value),handles.COMOPT,handles.s);
-end
-
-% --- Executes on button press in FirstOrderFilterTauGet.
-function FirstOrderFilterTauGet_Callback(hObject, eventdata, handles)
-% hObject handle to FirstOrderFilterTauGet (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = getVar(handles.USER_VARS.VAR_1ST_FILT_TAU,handles);
-set(handles.FirstOrderFilterTauText,'String',Value);
-end
-
-
-function KpEdit_Callback(hObject, eventdata, handles)
-% hObject handle to KpEdit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-
-% Hints: get(hObject,'String') returns contents of KpEdit as text
-% str2double(get(hObject,'String')) returns contents of KpEdit as a double
-end
-function KiEdit_Callback(hObject, eventdata, handles)
-% hObject handle to KiEdit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-
-% Hints: get(hObject,'String') returns contents of KiEdit as text
-% str2double(get(hObject,'String')) returns contents of KiEdit as a double
-end
-
-
-% --- Executes on button press in Dac1Chkbox.
-function Dac1Chkbox_Callback(hObject, eventdata, handles)
-% hObject handle to Dac1Chkbox (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-usartSendPacket(handles.COMOPT.PC_MESSAGE_STOP_STREAMING,[],handles.COMOPT,handles.s);
-if get(hObject,'Value')
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_DAC1_VOLTAGE+1,1);
- set(handles.PlotLines(handles.USER_VARS.VAR_DAC1_VOLTAGE+1),'visible','on');
-else
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_DAC1_VOLTAGE+1,0);
- set(handles.PlotLines(handles.USER_VARS.VAR_DAC1_VOLTAGE+1),'visible','off');
-end
-setStreamedVars(handles.streaming_vars,handles.COMOPT,handles.s);
-flushinput(handles.s);
-usartSendPacket(handles.COMOPT.PC_MESSAGE_START_STREAMING,[],handles.COMOPT,handles.s);
-% Update handles structure
-guidata(hObject, handles);
-end
-
-
-function Dac1Edit_Callback(hObject, eventdata, handles)
-% hObject handle to Dac1Edit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-
-% Hints: get(hObject,'String') returns contents of Dac1Edit as text
-% str2double(get(hObject,'String')) returns contents of Dac1Edit as a double
-end
-
-% --- Executes during object creation, after setting all properties.
-function Dac1Edit_CreateFcn(hObject, eventdata, handles)
-% hObject handle to Dac1Edit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles empty - handles not created until after all CreateFcns called
-
-% Hint: edit controls usually have a white background on Windows.
-% See ISPC and COMPUTER.
-if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
- set(hObject,'BackgroundColor','white');
-end
-end
-
-% --- Executes on button press in Dac1Set.
-function Dac1Set_Callback(hObject, eventdata, handles)
-% hObject handle to Dac1Set (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = str2num(get(handles.Dac1Edit,'String'));
-setVar(handles.USER_VARS.VAR_DAC1_VOLTAGE,single(Value),handles.COMOPT,handles.s);
-end
-
-% --- Executes on button press in Dac1Get.
-function Dac1Get_Callback(hObject, eventdata, handles)
-% hObject handle to Dac1Get (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = getVar(handles.USER_VARS.VAR_DAC1_VOLTAGE,handles);
-set(handles.Dac1Text,'String',Value);
-end
-
-% --- Executes on button press in Dac2Chkbox.
-function Dac2Chkbox_Callback(hObject, eventdata, handles)
-% hObject handle to Dac2Chkbox (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-usartSendPacket(handles.COMOPT.PC_MESSAGE_STOP_STREAMING,[],handles.COMOPT,handles.s);
-if get(hObject,'Value')
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_DAC2_VOLTAGE+1,1);
- set(handles.PlotLines(handles.USER_VARS.VAR_DAC2_VOLTAGE+1),'visible','on');
-else
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_DAC2_VOLTAGE+1,0);
- set(handles.PlotLines(handles.USER_VARS.VAR_DAC2_VOLTAGE+1),'visible','off');
-end
-setStreamedVars(handles.streaming_vars,handles.COMOPT,handles.s);
-flushinput(handles.s);
-usartSendPacket(handles.COMOPT.PC_MESSAGE_START_STREAMING,[],handles.COMOPT,handles.s);
-% Update handles structure
-guidata(hObject, handles);
-end
-
-
-function Dac2Edit_Callback(hObject, eventdata, handles)
-% hObject handle to Dac2Edit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-
-% Hints: get(hObject,'String') returns contents of Dac2Edit as text
-% str2double(get(hObject,'String')) returns contents of Dac2Edit as a double
-end
-
-% --- Executes during object creation, after setting all properties.
-function Dac2Edit_CreateFcn(hObject, eventdata, handles)
-% hObject handle to Dac2Edit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles empty - handles not created until after all CreateFcns called
-
-% Hint: edit controls usually have a white background on Windows.
-% See ISPC and COMPUTER.
-if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
- set(hObject,'BackgroundColor','white');
-end
-end
-
-
-% --- Executes on button press in Dac2Set.
-function Dac2Set_Callback(hObject, eventdata, handles)
-% hObject handle to Dac2Set (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = str2num(get(handles.Dac2Edit,'String'));
-setVar(handles.USER_VARS.VAR_DAC2_VOLTAGE,single(Value),handles.COMOPT,handles.s);
-end
-
-% --- Executes on button press in Dac2Get.
-function Dac2Get_Callback(hObject, eventdata, handles)
-% hObject handle to Dac2Get (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = getVar(handles.USER_VARS.VAR_DAC2_VOLTAGE,handles);
-set(handles.Dac2Text,'String',Value);
-end
-
-% --- Executes on button press in Adc1Chkbox.
-function Adc1Chkbox_Callback(hObject, eventdata, handles)
-% hObject handle to Adc1Chkbox (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-usartSendPacket(handles.COMOPT.PC_MESSAGE_STOP_STREAMING,[],handles.COMOPT,handles.s);
-if get(hObject,'Value')
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_ADC1_VOLTAGE+1,1);
- set(handles.PlotLines(handles.USER_VARS.VAR_ADC1_VOLTAGE+1),'visible','on');
-else
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_ADC1_VOLTAGE+1,0);
- set(handles.PlotLines(handles.USER_VARS.VAR_ADC1_VOLTAGE+1),'visible','off');
-end
-setStreamedVars(handles.streaming_vars,handles.COMOPT,handles.s);
-flushinput(handles.s);
-usartSendPacket(handles.COMOPT.PC_MESSAGE_START_STREAMING,[],handles.COMOPT,handles.s);
-% Update handles structure
-guidata(hObject, handles);
-end
-
-% --- Executes on button press in Adc1Get.
-function Adc1Get_Callback(hObject, eventdata, handles)
-% hObject handle to Adc1Get (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = getVar(handles.USER_VARS.VAR_ADC1_VOLTAGE,handles);
-set(handles.Adc1Text,'String',Value);
-end
-
-% --- Executes on button press in Adc2Chkbox.
-function Adc2Chkbox_Callback(hObject, eventdata, handles)
-% hObject handle to Adc2Chkbox (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-usartSendPacket(handles.COMOPT.PC_MESSAGE_STOP_STREAMING,[],handles.COMOPT,handles.s);
-if get(hObject,'Value')
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_ADC2_VOLTAGE+1,1);
- set(handles.PlotLines(handles.USER_VARS.VAR_ADC2_VOLTAGE+1),'visible','on');
-else
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_ADC2_VOLTAGE+1,0);
- set(handles.PlotLines(handles.USER_VARS.VAR_ADC2_VOLTAGE+1),'visible','off');
-end
-setStreamedVars(handles.streaming_vars,handles.COMOPT,handles.s);
-flushinput(handles.s);
-usartSendPacket(handles.COMOPT.PC_MESSAGE_START_STREAMING,[],handles.COMOPT,handles.s);
-% Update handles structure
-guidata(hObject, handles);
-end
-
-% --- Executes on button press in Adc2Get.
-function Adc2Get_Callback(hObject, eventdata, handles)
-% hObject handle to Adc2Get (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = getVar(handles.USER_VARS.VAR_ADC2_VOLTAGE,handles);
-set(handles.Adc2Text,'String',Value);
-end
-
-
-% --- Executes on button press in HallSensorChkbox.
-function HallSensorChkbox_Callback(hObject, eventdata, handles)
-% hObject handle to HallSensorChkbox (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-usartSendPacket(handles.COMOPT.PC_MESSAGE_STOP_STREAMING,[],handles.COMOPT,handles.s);
-if get(hObject,'Value')
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_HALL_ANGLE+1,1);
- set(handles.PlotLines(handles.USER_VARS.VAR_HALL_ANGLE+1),'visible','on');
-else
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_HALL_ANGLE+1,0);
- set(handles.PlotLines(handles.USER_VARS.VAR_HALL_ANGLE+1),'visible','off');
-end
-setStreamedVars(handles.streaming_vars,handles.COMOPT,handles.s);
-flushinput(handles.s);
-usartSendPacket(handles.COMOPT.PC_MESSAGE_START_STREAMING,[],handles.COMOPT,handles.s);
-% Update handles structure
-guidata(hObject, handles);
-end
-
-% --- Executes on button press in HallSensorGet.
-function HallSensorGet_Callback(hObject, eventdata, handles)
-% hObject handle to HallSensorGet (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = getVar(handles.USER_VARS.VAR_HALL_ANGLE,handles);
-set(handles.HallSensorText,'String',Value);
-end
-
-
-% --- Executes on button press in TachoSpeedChkbox.
-function TachoSpeedChkbox_Callback(hObject, eventdata, handles)
-% hObject handle to TachoSpeedChkbox (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-usartSendPacket(handles.COMOPT.PC_MESSAGE_STOP_STREAMING,[],handles.COMOPT,handles.s);
-if get(hObject,'Value')
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_TACHO_SPEED+1,1);
- set(handles.PlotLines(handles.USER_VARS.VAR_TACHO_SPEED+1),'visible','on');
-else
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_TACHO_SPEED+1,0);
- set(handles.PlotLines(handles.USER_VARS.VAR_TACHO_SPEED+1),'visible','off');
-end
-setStreamedVars(handles.streaming_vars,handles.COMOPT,handles.s);
-flushinput(handles.s);
-usartSendPacket(handles.COMOPT.PC_MESSAGE_START_STREAMING,[],handles.COMOPT,handles.s);
-% Update handles structure
-guidata(hObject, handles);
-end
-
-
-% --- Executes on button press in TachoSpeedGet.
-function TachoSpeedGet_Callback(hObject, eventdata, handles)
-% hObject handle to TachoSpeedGet (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = getVar(handles.USER_VARS.VAR_TACHO_SPEED,handles);
-set(handles.TachoSpeedText,'String',Value);
-end
-
-
-% --- Executes on button press in StrainGauge.
-function StrainGauge_Callback(hObject, eventdata, handles)
-% hObject handle to StrainGauge (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-usartSendPacket(handles.COMOPT.PC_MESSAGE_STOP_STREAMING,[],handles.COMOPT,handles.s);
-if get(hObject,'Value')
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_STRAIN_GAUGE_FORCE+1,1);
- set(handles.PlotLines(handles.USER_VARS.VAR_STRAIN_GAUGE_FORCE+1),'visible','on');
-else
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_STRAIN_GAUGE_FORCE+1,0);
- set(handles.PlotLines(handles.USER_VARS.VAR_STRAIN_GAUGE_FORCE+1),'visible','off');
-end
-setStreamedVars(handles.streaming_vars,handles.COMOPT,handles.s);
-flushinput(handles.s);
-usartSendPacket(handles.COMOPT.PC_MESSAGE_START_STREAMING,[],handles.COMOPT,handles.s);
-% Update handles structure
-guidata(hObject, handles);
-end
-
-
-% --- Executes on button press in StrainGaugeGet.
-function StrainGaugeGet_Callback(hObject, eventdata, handles)
-% hObject handle to StrainGaugeGet (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = getVar(handles.USER_VARS.VAR_STRAIN_GAUGE_FORCE,handles);
-set(handles.StrainGaugeText,'String',Value);
-end
-
-
-% --- Executes on button press in UserVar1Chkbox.
-function UserVar1Chkbox_Callback(hObject, eventdata, handles)
-% hObject handle to UserVar1Chkbox (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-usartSendPacket(handles.COMOPT.PC_MESSAGE_STOP_STREAMING,[],handles.COMOPT,handles.s);
-if get(hObject,'Value')
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_USER_VAR_1+1,1);
- set(handles.PlotLines(handles.USER_VARS.VAR_USER_VAR_1+1),'visible','on');
-else
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_USER_VAR_1+1,0);
- set(handles.PlotLines(handles.USER_VARS.VAR_USER_VAR_1+1),'visible','off');
-end
-setStreamedVars(handles.streaming_vars,handles.COMOPT,handles.s);
-flushinput(handles.s);
-usartSendPacket(handles.COMOPT.PC_MESSAGE_START_STREAMING,[],handles.COMOPT,handles.s);
-% Update handles structure
-guidata(hObject, handles);
-end
-
-
-function UserVar1Edit_Callback(hObject, eventdata, handles)
-% hObject handle to UserVar1Edit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-
-% Hints: get(hObject,'String') returns contents of UserVar1Edit as text
-% str2double(get(hObject,'String')) returns contents of UserVar1Edit as a double
-end
-
-% --- Executes during object creation, after setting all properties.
-function UserVar1Edit_CreateFcn(hObject, eventdata, handles)
-% hObject handle to UserVar1Edit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles empty - handles not created until after all CreateFcns called
-
-% Hint: edit controls usually have a white background on Windows.
-% See ISPC and COMPUTER.
-if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
- set(hObject,'BackgroundColor','white');
-end
-end
-
-% --- Executes on button press in UserVar1Set.
-function UserVar1Set_Callback(hObject, eventdata, handles)
-% hObject handle to UserVar1Set (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = str2num(get(handles.UserVar1Edit,'String'));
-setVar(handles.USER_VARS.VAR_USER_VAR_1,single(Value),handles.COMOPT,handles.s);
-end
-
-% --- Executes on button press in UserVar1Get.
-function UserVar1Get_Callback(hObject, eventdata, handles)
-% hObject handle to UserVar1Get (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = getVar(handles.USER_VARS.VAR_USER_VAR_1,handles);
-set(handles.UserVar1Text,'String',Value);
-end
-
-% --- Executes on button press in UserVar2Chkbox.
-function UserVar2Chkbox_Callback(hObject, eventdata, handles)
-% hObject handle to UserVar2Chkbox (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-usartSendPacket(handles.COMOPT.PC_MESSAGE_STOP_STREAMING,[],handles.COMOPT,handles.s);
-if get(hObject,'Value')
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_USER_VAR_2+1,1);
- set(handles.PlotLines(handles.USER_VARS.VAR_USER_VAR_2+1),'visible','on');
-else
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_USER_VAR_2+1,0);
- set(handles.PlotLines(handles.USER_VARS.VAR_USER_VAR_2+1),'visible','off');
-end
-setStreamedVars(handles.streaming_vars,handles.COMOPT,handles.s);
-flushinput(handles.s);
-usartSendPacket(handles.COMOPT.PC_MESSAGE_START_STREAMING,[],handles.COMOPT,handles.s);
-% Update handles structure
-guidata(hObject, handles);
-end
-
-
-function UserVar2Edit_Callback(hObject, eventdata, handles)
-% hObject handle to UserVar2Edit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-
-% Hints: get(hObject,'String') returns contents of UserVar2Edit as text
-% str2double(get(hObject,'String')) returns contents of UserVar2Edit as a double
-end
-
-% --- Executes during object creation, after setting all properties.
-function UserVar2Edit_CreateFcn(hObject, eventdata, handles)
-% hObject handle to UserVar2Edit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles empty - handles not created until after all CreateFcns called
-
-% Hint: edit controls usually have a white background on Windows.
-% See ISPC and COMPUTER.
-if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
- set(hObject,'BackgroundColor','white');
-end
-end
-
-% --- Executes on button press in UserVar2Set.
-function UserVar2Set_Callback(hObject, eventdata, handles)
-% hObject handle to UserVar2Set (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = str2num(get(handles.UserVar2Edit,'String'));
-setVar(handles.USER_VARS.VAR_USER_VAR_2,single(Value),handles.COMOPT,handles.s);
-end
-
-% --- Executes on button press in UserVar2Get.
-function UserVar2Get_Callback(hObject, eventdata, handles)
-% hObject handle to UserVar2Get (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = getVar(handles.USER_VARS.VAR_USER_VAR_2,handles);
-set(handles.UserVar2Text,'String',Value);
-end
-
-% --- Executes on button press in UserVar3Chkbox.
-function UserVar3Chkbox_Callback(hObject, eventdata, handles)
-% hObject handle to UserVar3Chkbox (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-usartSendPacket(handles.COMOPT.PC_MESSAGE_STOP_STREAMING,[],handles.COMOPT,handles.s);
-if get(hObject,'Value')
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_USER_VAR_3+1,1);
- set(handles.PlotLines(handles.USER_VARS.VAR_USER_VAR_3+1),'visible','on');
-else
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_USER_VAR_3+1,0);
- set(handles.PlotLines(handles.USER_VARS.VAR_USER_VAR_3+1),'visible','off');
-end
-setStreamedVars(handles.streaming_vars,handles.COMOPT,handles.s);
-flushinput(handles.s);
-usartSendPacket(handles.COMOPT.PC_MESSAGE_START_STREAMING,[],handles.COMOPT,handles.s);
-% Update handles structure
-guidata(hObject, handles);
-end
-
-
-function UserVar3Edit_Callback(hObject, eventdata, handles)
-% hObject handle to UserVar3Edit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-
-% Hints: get(hObject,'String') returns contents of UserVar3Edit as text
-% str2double(get(hObject,'String')) returns contents of UserVar3Edit as a double
-end
-
-% --- Executes during object creation, after setting all properties.
-function UserVar3Edit_CreateFcn(hObject, eventdata, handles)
-% hObject handle to UserVar3Edit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles empty - handles not created until after all CreateFcns called
-
-% Hint: edit controls usually have a white background on Windows.
-% See ISPC and COMPUTER.
-if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
- set(hObject,'BackgroundColor','white');
-end
-end
-
-% --- Executes on button press in UserVar3Set.
-function UserVar3Set_Callback(hObject, eventdata, handles)
-% hObject handle to UserVar3Set (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = str2num(get(handles.UserVar3Edit,'String'));
-setVar(handles.USER_VARS.VAR_USER_VAR_3,single(Value),handles.COMOPT,handles.s);
-end
-
-% --- Executes on button press in UserVar3Get.
-function UserVar3Get_Callback(hObject, eventdata, handles)
-% hObject handle to UserVar3Get (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = getVar(handles.USER_VARS.VAR_USER_VAR_3,handles);
-set(handles.UserVar3Text,'String',Value);
-end
-
-% --- Executes on button press in UserVar4Chkbox.
-function UserVar4Chkbox_Callback(hObject, eventdata, handles)
-% hObject handle to UserVar4Chkbox (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-usartSendPacket(handles.COMOPT.PC_MESSAGE_STOP_STREAMING,[],handles.COMOPT,handles.s);
-if get(hObject,'Value')
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_USER_VAR_4+1,1);
- set(handles.PlotLines(handles.USER_VARS.VAR_USER_VAR_4+1),'visible','on');
-else
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_USER_VAR_4+1,0);
- set(handles.PlotLines(handles.USER_VARS.VAR_USER_VAR_4+1),'visible','off');
-end
-setStreamedVars(handles.streaming_vars,handles.COMOPT,handles.s);
-flushinput(handles.s);
-usartSendPacket(handles.COMOPT.PC_MESSAGE_START_STREAMING,[],handles.COMOPT,handles.s);
-% Update handles structure
-guidata(hObject, handles);
-end
-
-
-function UserVar4Edit_Callback(hObject, eventdata, handles)
-% hObject handle to UserVar4Edit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-
-% Hints: get(hObject,'String') returns contents of UserVar4Edit as text
-% str2double(get(hObject,'String')) returns contents of UserVar4Edit as a double
-end
-
-% --- Executes during object creation, after setting all properties.
-function UserVar4Edit_CreateFcn(hObject, eventdata, handles)
-% hObject handle to UserVar4Edit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles empty - handles not created until after all CreateFcns called
-
-% Hint: edit controls usually have a white background on Windows.
-% See ISPC and COMPUTER.
-if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
- set(hObject,'BackgroundColor','white');
-end
-end
-
-% --- Executes on button press in UserVar4Set.
-function UserVar4Set_Callback(hObject, eventdata, handles)
-% hObject handle to UserVar4Set (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = str2num(get(handles.UserVar4Edit,'String'));
-setVar(handles.USER_VARS.VAR_USER_VAR_4,single(Value),handles.COMOPT,handles.s);
-end
-
-% --- Executes on button press in UserVar4Get.
-function UserVar4Get_Callback(hObject, eventdata, handles)
-% hObject handle to UserVar4Get (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = getVar(handles.USER_VARS.VAR_USER_VAR_4,handles);
-set(handles.UserVar4Text,'String',Value);
-end
-
-
-% --- Executes on button press in UserVar5Chkbox.
-function UserVar5Chkbox_Callback(hObject, eventdata, handles)
-% hObject handle to UserVar5Chkbox (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-usartSendPacket(handles.COMOPT.PC_MESSAGE_STOP_STREAMING,[],handles.COMOPT,handles.s);
-if get(hObject,'Value')
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_USER_VAR_5+1,1);
- set(handles.PlotLines(handles.USER_VARS.VAR_USER_VAR_5+1),'visible','on');
-else
- handles.streaming_vars = bitset(handles.streaming_vars,handles.USER_VARS.VAR_USER_VAR_5+1,0);
- set(handles.PlotLines(handles.USER_VARS.VAR_USER_VAR_5+1),'visible','off');
-end
-setStreamedVars(handles.streaming_vars,handles.COMOPT,handles.s);
-flushinput(handles.s);
-usartSendPacket(handles.COMOPT.PC_MESSAGE_START_STREAMING,[],handles.COMOPT,handles.s);
-% Update handles structure
-guidata(hObject, handles);
-end
-
-
-function UserVar5Edit_Callback(hObject, eventdata, handles)
-% hObject handle to UserVar5Edit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-
-% Hints: get(hObject,'String') returns contents of UserVar5Edit as text
-% str2double(get(hObject,'String')) returns contents of UserVar5Edit as a double
-end
-
-% --- Executes during object creation, after setting all properties.
-function UserVar5Edit_CreateFcn(hObject, eventdata, handles)
-% hObject handle to UserVar5Edit (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles empty - handles not created until after all CreateFcns called
-
-% Hint: edit controls usually have a white background on Windows.
-% See ISPC and COMPUTER.
-if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor'))
- set(hObject,'BackgroundColor','white');
-end
-end
-
-% --- Executes on button press in UserVar5Set.
-function UserVar5Set_Callback(hObject, eventdata, handles)
-% hObject handle to UserVar5Set (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = str2num(get(handles.UserVar5Edit,'String'));
-setVar(handles.USER_VARS.VAR_USER_VAR_5,single(Value),handles.COMOPT,handles.s);
-end
-
-% --- Executes on button press in UserVar5Get.
-function UserVar5Get_Callback(hObject, eventdata, handles)
-% hObject handle to UserVar5Get (see GCBO)
-% eventdata reserved - to be defined in a future version of MATLAB
-% handles structure with handles and user data (see GUIDATA)
-if ~exist('handles')
- handles = guidata(hObject);
-end
-Value = getVar(handles.USER_VARS.VAR_USER_VAR_5,handles);
-set(handles.UserVar5Text,'String',Value);
-end
diff --git a/MATLAB/ParamFiles/ComProtocolOpt.m b/MATLAB/ParamFiles/ComProtocolOpt.m
deleted file mode 100644
index 45e1083..0000000
--- a/MATLAB/ParamFiles/ComProtocolOpt.m
+++ /dev/null
@@ -1,14 +0,0 @@
-% Contient les options pour la communication entre l'hôte (PC) et le device
-
-COMOPT.USART_PORT = 'COM7';
-COMOPT.USART_BAUDRATE = 1843200;%921600;%
-COMOPT.USART_TIMEOUT = 4; %Secondes
-COMOPT.USART_TIMEOUT_SHORT = 0.1; %Secondes
-COMOPT.USART_INPUT_BUFFER_SIZE = 5000;
-
-COMOPT.MinPacketSize = 1; %En octet
-COMOPT.StartFrame = bin2dec('10000000');
-COMOPT.MaxPayloadSize = 32; %Taille maximum des payloads de chaques trames de données (en nb. de int32)
-COMOPT.MaxPacketSize = 1+... %COMOPT.StartFrame(1*uint8) + Payload Size(1*uint8)
- 4*COMOPT.MaxPayloadSize; %Payload
-COMOPT.NumberVariables = 26;
\ No newline at end of file
diff --git a/MATLAB/ParamFiles/HostRequestList.m b/MATLAB/ParamFiles/HostRequestList.m
deleted file mode 100644
index 31af243..0000000
--- a/MATLAB/ParamFiles/HostRequestList.m
+++ /dev/null
@@ -1,20 +0,0 @@
-% Contient la liste des requètes que l'hôte (le PC) peut envoyer à la carte
-% et inversement
-%
-% Les requètes pouvant être envoyées à la carte sont codées sur 8 bits.
-% Les 7 LSB correspondent au code de la requète elle-même,
-% Le 1 MSB est le header
-
-COMOPT.PC_MESSAGE_DO_NOTHING = 0; % Do nothing.
-COMOPT.PC_MESSAGE_TOGGLE_LED = 1; % Toggle the LED (debug).
-COMOPT.PC_MESSAGE_SEND_STATUS = 2; % Request the device to send the main status register.
-COMOPT.PC_MESSAGE_START_STREAMING = 3; % Request the device to start streaming continuously.
-COMOPT.PC_MESSAGE_STOP_STREAMING = 4; % Request the device to stop streaming continuously.
-COMOPT.PC_MESSAGE_GET_VAR = 5; % Request the device to send the selected value.
-COMOPT.PC_MESSAGE_SET_VAR = 6; % Set the selected variable.
-COMOPT.PC_MESSAGE_SET_STREAMED_VAR = 7; % Set the variables to be streamed.
-
-
-COMOPT.STM_MESSAGE_STATUS = 0; % Main status register.
-COMOPT.STM_MESSAGE_VAR = 1; % Variable state.
-COMOPT.STM_MESSAGE_STREAMING_PACKET = 2; % Streaming packet.
diff --git a/MATLAB/ParamFiles/StatusRegBitsList.m b/MATLAB/ParamFiles/StatusRegBitsList.m
deleted file mode 100644
index 94da062..0000000
--- a/MATLAB/ParamFiles/StatusRegBitsList.m
+++ /dev/null
@@ -1,35 +0,0 @@
-% Contient la liste des bits du registe de status principale du devices
-
-
-STREG.STREG_0 = 0 + 1;
-STREG.STREG_1 = 1 + 1;
-STREG.STREG_2 = 2 + 1;
-STREG.STREG_3 = 3 + 1;
-STREG.STREG_4 = 4 + 1;
-STREG.STREG_5 = 5 + 1;
-STREG.STREG_6 = 6 + 1;
-STREG.INDEX_ERROR = 7 + 1;
-STREG.STREG_8 = 8 + 1;
-STREG.STREG_9 = 9 + 1;
-STREG.STREG_10 = 10 + 1;
-STREG.STREG_11 = 11 + 1;
-STREG.STREG_12 = 12 + 1;
-STREG.STREG_13 = 13 + 1;
-STREG.STREG_14 = 14 + 1;
-STREG.STREG_15 = 15 + 1;
-STREG.STREG_16 = 16 + 1;
-STREG.LAST_REQUEST_IGNORED = 17 + 1;
-STREG.PROCESSING_REQUEST = 18 + 1;
-STREG.RX_ERROR = 19 + 1;
-STREG.RX_COMPLETED = 20 + 1;
-STREG.STREG_21 = 21 + 1;
-STREG.DATA_UPSTREAM_ENB = 22 + 1;
-STREG.STREG_23 = 23 + 1;
-STREG.STREG_24 = 24 + 1;
-STREG.STREG_25 = 25 + 1;
-STREG.RUN_CONTROL_LOOP = 26 + 1;
-STREG.RUN_CURRENT_LOOP = 27 + 1;
-STREG.CALIBRE = 28 + 1;
-STREG.REF_POS_INIT = 29 + 1;
-STREG.STREG_30 = 30 + 1;
-STREG.STREG_31 = 31 + 1;
diff --git a/MATLAB/ParamFiles/UserVars.m b/MATLAB/ParamFiles/UserVars.m
deleted file mode 100644
index 43868c7..0000000
--- a/MATLAB/ParamFiles/UserVars.m
+++ /dev/null
@@ -1,29 +0,0 @@
-% Variables which can be streamed and plotted
-
-USER_VARS.VAR_TIMESTAMP = 0; % Timestamp. [us]
-USER_VARS.VAR_POS_REG_PID_P = 1; % Position regulator - proportionnal coefficient. [1]
-USER_VARS.VAR_POS_REG_PID_I = 2; % Position regulator - integral coefficient. [1]
-USER_VARS.VAR_POS_REG_PID_D = 3; % Position regulator - derivative coefficient. [1]
-USER_VARS.VAR_POS_REG_PID_ARW = 4; % Position regulator - ARW (integrator max value). [1]
-USER_VARS.VAR_POS_REG_PID_INTEGRATOR = 5; % Position regulator - integrator state. [1]
-USER_VARS.VAR_POS_REG_PID_OUTPUT = 6; % Position regulator - output. [mN.m]
-USER_VARS.VAR_POS_REG_SAMPLING_PERIOD = 7; % Position regulator - sampling period [us]
-USER_VARS.VAR_WALL_POSITION = 8; % Position regulator - target (wall position). [degrees]
-USER_VARS.VAR_MEASURED_ANGLE_RAW = 9; % Measured output angle, unfiltered. [degrees]
-USER_VARS.VAR_MEASURED_ANGLE_FILTERED = 10; % Measured output angle, filtered. [degrees]
-USER_VARS.VAR_COMM_TX_PERIOD = 11; % Communication loop - sending period. [us]
-USER_VARS.VAR_ACTIVE_FILTER = 12; % Active filter type (0: none, 1: first-order, 2: running mean). [None]
-USER_VARS.VAR_1ST_FILT_TAU = 13; % First order filter - time constant. [1]
-USER_VARS.VAR_DAC1_VOLTAGE = 14; % DAC 1 channel voltage. [V]
-USER_VARS.VAR_DAC2_VOLTAGE = 15; % DAC 2 channel voltage. [V]
-USER_VARS.VAR_ADC1_VOLTAGE = 16; % ACDC 1 channel voltage. [V]
-USER_VARS.VAR_ADC2_VOLTAGE = 17; % ACDC 2 channel voltage. [V]
-USER_VARS.VAR_HALL_ANGLE = 18; % Angle measured by the Hall sensor. [deg]
-USER_VARS.VAR_TACHO_SPEED = 19; % Angular speed measured by the tachometer. [deg/s]
-USER_VARS.VAR_STRAIN_GAUGE_FORCE = 20; % Force measured by the strain gauge. [?]
-USER_VARS.VAR_USER_VAR_1 = 21; % User Variable 1 []
-USER_VARS.VAR_USER_VAR_2 = 22; % User Variable 2 []
-USER_VARS.VAR_USER_VAR_3 = 23; % User Variable 3 []
-USER_VARS.VAR_USER_VAR_4 = 24; % User Variable 4 []
-USER_VARS.VAR_USER_VAR_5 = 25; % User Variable 5 []
-
diff --git a/MATLAB/ReadDataFile.m b/MATLAB/ReadDataFile.m
deleted file mode 100644
index 6c2e1e4..0000000
--- a/MATLAB/ReadDataFile.m
+++ /dev/null
@@ -1,9 +0,0 @@
-clear all
-
-files = dir('Data');
-filename = ['Data\' files(end).name];
-fid = fopen(filename,'r');
-Data = fread(fid,[26,inf],'double')';
-fclose(fid);
-
-plot(Data(:,1),Data(:,2:end));
diff --git a/MATLAB/hri_comm.m b/MATLAB/hri_comm.m
new file mode 100644
index 0000000..62deb9c
--- /dev/null
+++ b/MATLAB/hri_comm.m
@@ -0,0 +1,458 @@
+classdef hri_comm < handle
+ % HHRIBOARD HHRI board class.
+ % Manages the communication between MATLAB on PC and the HHRI board,
+ % and allows remote operation.
+
+ properties(Access = private)
+ hSerial
+ packetBuffer
+ rxCurrentMessageType
+ rxBytesCount
+ firstHalfByte
+ rxTimer
+ pendingPingback
+ pendingGetVarsList
+ pendingGetVar
+ logfile
+ varsList
+ streamId
+ varsIdsToStream
+ streamPacketSize
+ streamBuffer
+ currentStreamBufferIndex
+ gotVarsListCallbackFunc
+ boardOfflineCallbackFunc
+ plotDecimCounter
+ end
+
+ properties(Hidden = true, Access = private, Constant = true)
+ %% Messages sent by MATLAB to the board.
+ PC_MESSAGE_DO_NOTHING = 0; % Do nothing.
+ PC_MESSAGE_PING = 1;
+ PC_MESSAGE_GET_VARS_LIST = 2; % Request the device to send the variables list.
+ PC_MESSAGE_SET_STREAMED_VAR = 3; % Set the variables to be streamed.
+ PC_MESSAGE_GET_VAR = 4; % Request the device to send the selected value.
+ PC_MESSAGE_SET_VAR = 5; % Set the selected variable.
+
+ %% Messages sent by the board to MATLAB.
+ STM_MESSAGE_PINGBACK = 0; % Response to a ping request.
+ STM_MESSAGE_VAR = 1; % Variable state.
+ STM_MESSAGE_STREAMING_PACKET = 2; % Streaming packet.
+ STM_MESSAGE_DEBUG_TEXT = 3; % Debug text message.
+ STM_MESSAGE_VARS_LIST = 4; % Monitored variables list.
+ STM_MESSAGE_START_INFO = 5; % Notification that the board has (re)started.
+
+ %% Other constants.
+ SYNCVAR_NAME_SIZE = 20; % Max size of a SyncVar name, including the '\0' trailing character.
+ STREAMING_BUFF_LEN = 1000; % [bytes].
+ READ_BYTES_PERIOD = 0.100; % [s].
+ VAR_ACCESSES = {'readonly', 'writeonly', 'read+write'};
+ VAR_TYPES = {'bool', 'uint8', 'int8', 'uint16', 'int16', ...
+ 'uint32', 'int32', 'uint64', 'int64', 'single', ...
+ 'double'};
+ end
+
+ methods
+ function hbh = hri_comm(comPortName, gotVarsListCallbackFunc)
+ % HHRIBOARD Class constructor.
+ hbh.hSerial = serial(comPortName, ...
+ 'BaudRate', 1843200, ...
+ 'Parity', 'none', ...
+ 'StopBits', 1, ...
+ 'FlowControl', 'none', ...
+ 'Timeout', 0.5, ...
+ 'InputBufferSize', 100000, ...
+ 'ReadAsyncMode', 'continuous');
+ hbh.packetBuffer = zeros(1, 1000, 'uint8');
+ hbh.rxCurrentMessageType = 0;
+ hbh.rxBytesCount = 0;
+ hbh.pendingPingback = 0;
+ hbh.logfile = 0;
+ hbh.rxTimer = 0;
+ hbh.streamId = 0;
+ hbh.gotVarsListCallbackFunc = gotVarsListCallbackFunc;
+ hbh.plotDecimCounter = 1;
+ end
+
+ function open(hbh)
+ % OPEN Open the serial link, and start receiving data.
+
+ % Open the serial link.
+ fopen(hbh.hSerial);
+ flushinput(hbh.hSerial);
+
+ % Setup the callback function, called periodically to read the
+ % bytes sent by the board.
+ hbh.rxTimer = timer('Period', hbh.READ_BYTES_PERIOD, ...
+ 'TimerFcn', @(~,~)readBytes(hbh), ...
+ 'ExecutionMode', 'fixedRate', ...
+ 'BusyMode', 'drop');
+ start(hbh.rxTimer);
+
+ % Stop the streaming, if it was running.
+ setStreamedVars(hbh, []);
+
+ % Get the remote variables list, if the board is OK.
+ if ping(hbh)
+ requestVarsList(hbh);
+ end
+ end
+
+ function close(hbh)
+ % CLOSE Close the serial link.
+ stop(hbh.rxTimer);
+
+ if strcmp(hbh.hSerial.Status, 'open')
+ fclose(hbh.hSerial);
+ delete(hbh);
+ end
+ end
+
+ function delete(hbh)
+ % DELETE Class destructor.
+ close(hbh);
+ end
+
+ function ok = ping(hbh)
+ % PING Check if the board is responding to a ping request.
+ hbh.pendingPingback = 1;
+ sendPacket(hbh, hbh.PC_MESSAGE_PING, []);
+
+ startTime = tic;
+
+ while hbh.pendingPingback && toc(startTime) < 1.0
+ pause(0.1);
+ end
+
+ ok = (hbh.pendingPingback == 0);
+ end
+
+% function ok = getLinkStatus(hbh)
+%
+% end
+
+ function startLogging(hbh)
+ % STARTLOGGING Start the logging to a text file.
+ % wmh WalkiMotorboard object to set.
+
+ % Open the logfile to write.
+ hbh.logfile = fopen(['logs/log_' ...
+ datestr(now, 'yyyy-mm-dd_HH_MM_SS') ...
+ '.csv'], 'w');
+ % Build the header.
+ header = 'timestamp [us];';
+ for i=1:length(hbh.varsList(hbh.varsIdsToStream))
+ header = [header hbh.varsList(i).name ';']; %#ok
+ end
+ header(end) = [];
+
+ fprintf(hbh.logfile, [header '\n']);
+ end
+
+ function stopLogging(hbh)
+ % STOPLOGGING Stop the logging to a text file.
+ % wmh WalkiMotorboard object to set.
+
+ if hbh.logfile ~= 0
+ fclose(hbh.logfile);
+ hbh.logfile = 0;
+ end
+ end
+
+ function requestVarsList(hbh)
+ sendPacket(hbh, hbh.PC_MESSAGE_GET_VARS_LIST, []);
+ end
+
+ function varsList = getVarsList(hbh)
+ sendPacket(hbh, hbh.PC_MESSAGE_GET_VARS_LIST, []);
+
+ hbh.pendingGetVarsList = 1;
+
+ while hbh.pendingGetVarsList
+ pause(0.1);
+ end
+
+ varsList = hbh.varsList;
+ end
+
+ function setVar(hbh, varId, newValue)
+ % SETVAR Set a board variable to the desired value.
+ % wmh WalkiMotorboard object to set.
+ % varId the selected variable to set.
+ % newValue the new desired value of the selected variable. The
+ % given value will be cast to the correct type.
+
+ % Send a packet to set the value of the variable.
+ if strcmp(hbh.varsList(varId).type, 'bool')
+ newValue = cast(newValue, 'uint8');
+ else
+ newValue = cast(newValue, hbh.varsList(varId).type);
+ end
+ data = [varId-1 typecast(newValue, 'uint8')];
+ sendPacket(hbh, hbh.PC_MESSAGE_SET_VAR, data);
+ end
+
+ function varValue = getVar(hbh, varId)
+ % REQUESTVAR Get the variable value from the board.
+ % This function blocks until the variable has been received.
+ % wmh WalkiMotorboard object to set.
+ % varId the ID of the selected variable to get.
+
+ hbh.pendingGetVar = 1;
+
+ sendPacket(hbh, hbh.PC_MESSAGE_GET_VAR, varId-1);
+
+ while(hbh.pendingGetVar)
+ pause(0.01);
+ end
+
+ varValue = hbh.varsList(varId).value{:};
+ end
+
+ function dispVarsList(hbh)
+ for i=1:length(hbh.varsList)
+ fprintf('#%i %s %s (size %i) %s\n', i, ...
+ hbh.varsList(i).name, ...
+ hbh.varsList(i).type, ...
+ hbh.varsList(i).size, ...
+ hbh.varsList(i).access);
+ end
+ end
+
+ function setStreamedVars(hbh, varsIdsToStream)
+ % Allocate the buffer.
+ hbh.streamBuffer = zeros(1000, length(varsIdsToStream));
+ hbh.currentStreamBufferIndex = 1;
+
+ % Build the communication packet.
+ hbh.streamId = hbh.streamId + 1;
+ hbh.streamPacketSize = 1 + 4; % ID + timestamp.
+ hbh.varsIdsToStream = varsIdsToStream;
+
+ data = zeros(1, 2 + length(varsIdsToStream));
+
+ data(1) = uint8(length(varsIdsToStream));
+ data(2) = uint8(hbh.streamId);
+
+ for i=1:length(varsIdsToStream)
+ data(2+i) = uint8(varsIdsToStream(i)-1);
+ hbh.streamPacketSize = hbh.streamPacketSize + ...
+ hbh.varsList(varsIdsToStream(i)).size;
+ end
+
+ sendPacket(hbh, hbh.PC_MESSAGE_SET_STREAMED_VAR, data);
+
+ % TODO: remove.
+ % This avoids a bug when removing a variable while streaming.
+ pause(0.1);
+ end
+
+ function vars = getStreaming(hbh)
+ vars = hbh.streamBuffer(1:hbh.currentStreamBufferIndex-1, :);
+ hbh.currentStreamBufferIndex = 1;
+ end
+ end
+
+ methods(Static)
+ function boardComPort = getHriComPort()
+ hwinfo = instrhwinfo('Serial');
+ ports = hwinfo.AvailableSerialPorts;
+
+ for i=1:length(ports)
+ port = ports{i};
+ h = hri_comm(port, 0);
+ open(h);
+
+ if ping(h)
+ boardComPort = port;
+ delete(h);
+ return;
+ end
+
+ delete(h);
+ clear h;
+ end
+
+ boardComPort = '';
+ end
+ end
+
+ methods(Access = private)
+ function sendPacket(wmh, type, data)
+ % Build the packet.
+ txBuffer = zeros(1, 1 + length(data) * 2, 'uint8');
+ txBuffer(1) = bitset(type, 8); % Set bit 8 to 1 ("start byte").
+
+ for i = 1:length(data)
+ b = data(i);
+ txBuffer(i*2+0) = bitshift(b, -4); % MSB.
+ txBuffer(i*2+1) = bitshift(bitshift(b, 4), -4); % LSB.
+ end
+
+ % Send the packet through the serial link.
+ fwrite(wmh.hSerial, txBuffer); % TODO: set to 'async'?
+ end
+
+ function readBytes(hbh)
+ try
+ hri_constants;
+
+ % Read the available bytes from the serial link.
+ nAvailableBytes = hbh.hSerial.BytesAvailable;
+
+ if nAvailableBytes == 0
+ return;
+ end
+
+ rxBytes = fread(hbh.hSerial, nAvailableBytes, 'uint8');
+
+ % Cache the most accessed members.
+ rxBytesCountC = hbh.rxBytesCount;
+ packetBufferC = hbh.packetBuffer;
+
+ % Process each byte, one by one.
+ for rxByte = rxBytes'
+
+ if bitget(rxByte, 8) == 1 % Start byte.
+ hbh.rxCurrentMessageType = bitset(rxByte, 8, 0);
+ rxBytesCountC = 0;
+ else % "Normal" byte.
+ rxBytesCountC = rxBytesCountC + 1;
+ end
+
+ if rem(rxBytesCountC, 2) == 1 % Got first half byte.
+ hbh.firstHalfByte = rxByte;
+ else % Got nothing, or the second half byte.
+ dataBytesReady = floor(rxBytesCountC / 2);
+
+ % Accumulate the incoming byte in the RX packet buffer.
+ if dataBytesReady > 0
+ packetBufferC(dataBytesReady) = ...
+ bitshift(hbh.firstHalfByte, 4) + ... % MSB.
+ bitshift(bitshift(rxByte, 4), -4); % LSB.
+ end
+
+ % If enough data bytes have been received, interpret
+ % them.
+ switch hbh.rxCurrentMessageType
+ case hbh.STM_MESSAGE_PINGBACK
+ hbh.pendingPingback = 0;
+ case hbh.STM_MESSAGE_VAR
+ if dataBytesReady >= 1
+ varId = packetBufferC(1) + 1;
+ varSize = hbh.varsList(varId).size;
+
+ if dataBytesReady == 1 + varSize
+ if strcmp(hbh.varsList(varId).type, 'bool')
+ hbh.varsList(varId).value = {packetBufferC(2) ~= 0};
+ else
+ hbh.varsList(varId).value = {typecast(packetBufferC(2:1+varSize), hbh.varsList(varId).type)};
+ end
+ end
+
+ hbh.pendingGetVar = 0;
+ end
+ case hbh.STM_MESSAGE_STREAMING_PACKET
+ if dataBytesReady == hbh.streamPacketSize
+ if packetBufferC(1) == hbh.streamId
+ timestamp = cast(typecast(packetBufferC(2:5), 'uint32'), 'double');
+ p = 6;
+ for varId=hbh.varsIdsToStream
+ if strcmp(hbh.varsList(varId).type, 'bool')
+ hbh.varsList(varId).value = {packetBufferC(p) ~= 0};
+ p = p + 1;
+ else
+ varSize = hbh.varsList(varId).size;
+ hbh.varsList(varId).value = {typecast(packetBufferC(p:p+varSize-1), hbh.varsList(varId).type)};
+ p = p + varSize;
+ end
+ end
+
+ % Add the values into the streaming buffer.
+ streamBufferLine = cellfun(@(x)cast(x{:}, 'double'), {hbh.varsList(hbh.varsIdsToStream).value});
+
+ if hbh.plotDecimCounter >= PLOT_DOWNSAMPLING
+ hbh.plotDecimCounter = 1;
+ hbh.streamBuffer(hbh.currentStreamBufferIndex,:) = streamBufferLine;
+ hbh.currentStreamBufferIndex = hbh.currentStreamBufferIndex + 1;
+
+ % If the streaming buffer is full, restart.
+ if hbh.currentStreamBufferIndex >= hbh.STREAMING_BUFF_LEN
+ hbh.currentStreamBufferIndex = 1;
+ warning('The streaming buffer was full, and has been reseted.');
+ end
+ else
+ hbh.plotDecimCounter = hbh.plotDecimCounter + 1;
+ end
+
+ % Log to file, if enabled.
+ if hbh.logfile ~= 0
+ fprintf(hbh.logfile, '%f;', [timestamp streamBufferLine]);
+ fprintf(hbh.logfile, '\n');
+ end
+ end
+ end
+ case hbh.STM_MESSAGE_DEBUG_TEXT
+ if dataBytesReady > 0 && ...
+ packetBufferC(dataBytesReady) == 0
+ message = char(...
+ packetBufferC(...
+ 1:find(packetBufferC == 0)-1));
+ message(message == 13) = [];
+ fprintf([message '\n']);
+ end
+ case hbh.STM_MESSAGE_VARS_LIST
+ if dataBytesReady > 0
+ nVars = double(packetBufferC(1));
+
+ hbh.varsList = [];
+
+ if dataBytesReady == 1 + nVars * (hbh.SYNCVAR_NAME_SIZE + 3)
+ j = 2;
+ for i=1:nVars
+ name = packetBufferC(j:j+hbh.SYNCVAR_NAME_SIZE);
+ hbh.varsList(i).name = char(name(1:find(name == 0, 1, 'first')-1));
+ j = j + hbh.SYNCVAR_NAME_SIZE;
+ hbh.varsList(i).type = hbh.VAR_TYPES{packetBufferC(j)+1};
+ j = j + 1;
+ hbh.varsList(i).access = hbh.VAR_ACCESSES{packetBufferC(j)+1};
+ j = j + 1;
+ hbh.varsList(i).size = double(packetBufferC(j));
+ j = j + 1;
+
+ if strcmp(hbh.varsList(i).type, 'bool')
+ hbh.varsList(i).value = 0;
+ else
+ hbh.varsList(i).value = {cast(0, hbh.varsList(i).type)};
+ end
+ end
+
+ hbh.pendingGetVarsList = 0;
+
+ if isa(hbh.gotVarsListCallbackFunc, 'function_handle')
+ hbh.gotVarsListCallbackFunc(hbh.varsList);
+ end
+ end
+ end
+ case hbh.STM_MESSAGE_START_INFO
+ if dataBytesReady == 0
+ disp('The board has (re)started.');
+ requestVarsList(hbh);
+ end
+ end
+ end
+ end
+
+ % De-cache the cached variables.
+ hbh.rxBytesCount = rxBytesCountC;
+ hbh.packetBuffer = packetBufferC;
+ catch e
+ % Having this variable in global allows accessing it from
+ % the main workspace.
+ global err; %#ok
+ err = e;
+ rethrow(e);
+ end
+ end
+ end
+end
\ No newline at end of file
diff --git a/MATLAB/hri_constants.m b/MATLAB/hri_constants.m
new file mode 100644
index 0000000..9289653
--- /dev/null
+++ b/MATLAB/hri_constants.m
@@ -0,0 +1,9 @@
+%% HRI_GUI CONSTANTS
+
+%% GUI-related constants.
+N_VARS_MAX = 25;
+PLOT_SAMPLING_FREQS = [2 10];
+
+%% Plot-related constants.
+PLOT_N_SAMPLES = 1000;
+PLOT_DOWNSAMPLING = 10;
\ No newline at end of file
diff --git a/MATLAB/hri_gui.fig b/MATLAB/hri_gui.fig
new file mode 100644
index 0000000..48487dd
Binary files /dev/null and b/MATLAB/hri_gui.fig differ
diff --git a/MATLAB/hri_gui.m b/MATLAB/hri_gui.m
new file mode 100644
index 0000000..24ee588
--- /dev/null
+++ b/MATLAB/hri_gui.m
@@ -0,0 +1,395 @@
+function varargout = hri_gui(varargin)
+% HRI_GUI MATLAB code for hri_gui.fig
+% HRI_GUI, by itself, creates a new HRI_GUI or raises the existing
+% singleton*.
+%
+% H = HRI_GUI returns the handle to a new HRI_GUI or the handle to
+% the existing singleton*.
+%
+% HRI_GUI('CALLBACK',hObject,eventData,handles,...) calls the local
+% function named CALLBACK in HRI_GUI.M with the given input arguments.
+%
+% HRI_GUI('Property','Value',...) creates a new HRI_GUI or raises the
+% existing singleton*. Starting from the left, property value pairs are
+% applied to the GUI before hri_gui_OpeningFcn gets called. An
+% unrecognized property name or invalid value makes property application
+% stop. All inputs are passed to hri_gui_OpeningFcn via varargin.
+%
+% *See GUI Options on GUIDE's Tools menu. Choose "GUI allows only one
+% instance to run (singleton)".
+%
+% See also: GUIDE, GUIDATA, GUIHANDLES
+
+% Edit the above text to modify the response to help hri_gui
+
+% Last Modified by GUIDE v2.5 26-Dec-2015 20:20:49
+
+% Begin initialization code - DO NOT EDIT
+gui_Singleton = 1;
+gui_State = struct('gui_Name', mfilename, ...
+ 'gui_Singleton', gui_Singleton, ...
+ 'gui_OpeningFcn', @hri_gui_OpeningFcn, ...
+ 'gui_OutputFcn', @hri_gui_OutputFcn, ...
+ 'gui_LayoutFcn', [] , ...
+ 'gui_Callback', []);
+if nargin && ischar(varargin{1})
+ gui_State.gui_Callback = str2func(varargin{1});
+end
+
+if nargout
+ [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:});
+else
+ gui_mainfcn(gui_State, varargin{:});
+end
+% End initialization code - DO NOT EDIT
+
+
+% --- Executes just before hri_gui is made visible.
+function hri_gui_OpeningFcn(hObject, eventdata, handles, varargin) %#ok
+% This function has no output args, see OutputFcn.
+% hObject handle to figure
+% eventdata reserved - to be defined in a future version of MATLAB
+% handles structure with handles and user data (see GUIDATA)
+% varargin command line arguments to hri_gui (see VARARGIN)
+
+% Choose default command line output for hri_gui
+handles.output = hObject;
+
+% UIWAIT makes hri_gui wait for user response (see UIRESUME)
+% uiwait(handles.main_window);
+
+hri_constants;
+
+handles.streamedVars = [];
+handles.varsWidgets = [];
+handles.plotDataCurrentIndex = 1;
+handles.plotData = [];
+
+handles.plotFig = findobj('Tag', 'plot_figure');
+handles.dataStreamingCheckbox = findobj('Tag', 'data_streaming_checkbox');
+
+% Create the variables widgets.
+lineHeight = 1 / N_VARS_MAX;
+y = 1 - lineHeight;
+
+for i=1:N_VARS_MAX
+ % Create the UI controls at the right location.
+ handles.varsWidgets(i).checkbox = uicontrol('Parent', handles.vars_panel, ...
+ 'Style', 'checkbox', ...
+ 'Units', 'normalized', ...
+ 'HandleVisibility', 'callback', ...
+ 'Position', [0 y 3/6 lineHeight], ...
+ 'Visible', 'off');
+
+ handles.varsWidgets(i).get = uicontrol('Parent', handles.vars_panel, ...
+ 'Units', 'normalized', ...
+ 'HandleVisibility', 'callback', ...
+ 'Position', [3/6 y 1/6 lineHeight], ...
+ 'String', 'Get', ...
+ 'Visible', 'off');
+
+ handles.varsWidgets(i).edit = uicontrol('Parent', handles.vars_panel, ...
+ 'Style', 'edit', ...
+ 'Units', 'normalized', ...
+ 'HandleVisibility', 'callback', ...
+ 'Position', [4/6 y 1/6 lineHeight], ...
+ 'String', '', ...
+ 'Visible', 'off');
+
+ handles.varsWidgets(i).set = uicontrol('Parent', handles.vars_panel, ...
+ 'Units', 'normalized', ...
+ 'HandleVisibility', 'callback', ...
+ 'Position', [5/6 y 1/6 lineHeight], ...
+ 'String', 'Set', ...
+ 'Visible', 'off');
+
+ y = y - lineHeight;
+end
+
+if isempty(varargin)
+ % Find which serial port corresponds to the board.
+ comPort = hri_comm.getHriComPort();
+
+ if isempty(comPort)
+ warning('The board is not detected.');
+ handles.hbh = [];
+ guidata(hObject, handles);
+ return;
+ else
+ fprintf('The board was detected on %s.\n', comPort);
+ end
+else
+ comPort = varargin{1};
+end
+
+% Open the serial link with the board.
+handles.hbh = hri_comm(comPort, @(varsList) hri_gui_UpdateVarsList(hObject, varsList));
+open(handles.hbh);
+
+% Set the plot figure update timer.
+handles.plotFreqPopup = findobj('Tag', 'plot_freq_popup');
+plotFreq = PLOT_SAMPLING_FREQS(handles.plotFreqPopup.Value);
+
+handles.plotTimer = timer('Period', 1.0 / plotFreq, ...
+ 'TimerFcn', @(~,~)updatePlot(hObject), ...
+ 'ExecutionMode', 'fixedRate', ...
+ 'BusyMode', 'drop');
+start(handles.plotTimer);
+
+%
+guidata(hObject, handles);
+
+function hri_gui_UpdateVarsList(hObject, varsList)
+
+hri_constants;
+handles = guidata(hObject);
+handles.varsList = varsList;
+
+% Stop data streaming and logging, if running.
+handles.streamedVars = [];
+set(handles.data_streaming_checkbox, 'Value', 0);
+set(handles.log_to_file_checkbox, 'Value', 0);
+
+%
+for i=1:length(handles.varsList)
+ % Modify the widgets to they match the associated variable.
+ set(handles.varsWidgets(i).checkbox, 'String', handles.varsList(i).name);
+ set(handles.varsWidgets(i).checkbox, 'Value', 0);
+ set(handles.varsWidgets(i).checkbox, 'Visible', 'on');
+ set(handles.varsWidgets(i).checkbox, 'ForegroundColor', 'black');
+
+ if ~strcmp(varsList(i).access, 'writeonly')
+ set(handles.varsWidgets(i).get, 'Visible', 'on');
+ end
+
+ set(handles.varsWidgets(i).edit, 'Visible', 'on');
+ set(handles.varsWidgets(i).edit, 'String', '');
+
+ if ~strcmp(varsList(i).access, 'readonly')
+ set(handles.varsWidgets(i).set, 'Visible', 'on');
+ end
+
+ % Set the callbacks.
+ handles.varsWidgets(i).checkbox.Callback = @(h,~)onToggleVarCheckbox(h,i);
+
+ if handles.varsWidgets(i).get ~= 0
+ handles.varsWidgets(i).get.Callback = @(h,~)onGetVar(h,i);
+ end
+
+ if handles.varsWidgets(i).set ~= 0
+ handles.varsWidgets(i).set.Callback = @(h,~)onSetVar(h,i);
+ end
+end
+
+% Hide the other widgets.
+for i=length(handles.varsList)+1:N_VARS_MAX
+ set(handles.varsWidgets(i).checkbox, 'Visible', 'off');
+ set(handles.varsWidgets(i).get, 'Visible', 'off');
+ set(handles.varsWidgets(i).edit, 'Visible', 'off');
+ set(handles.varsWidgets(i).set, 'Visible', 'off');
+end
+
+% Update handles structure.
+guidata(hObject, handles);
+
+
+function onToggleVarCheckbox(hObject, varID, ~)
+% Get handles structure.
+handles = guidata(hObject);
+
+% Add or remove the variable ID associated to the checkbox.
+if handles.varsWidgets(varID).checkbox.Value == 0
+ handles.streamedVars(handles.streamedVars == varID) = [];
+else
+ handles.streamedVars = sort([handles.streamedVars varID]);
+end
+
+% Change the streaming of the board, if it currently running.
+guidata(hObject, handles);
+
+if handles.dataStreamingCheckbox.Value ~= 0
+ setupStreaming(hObject, handles.streamedVars);
+end
+
+
+function setupStreaming(hObject, streamedVars)
+hri_constants;
+handles = guidata(hObject);
+
+% Stop the streaming, if running.
+handles.log_to_file_checkbox.Value = 0;
+
+% Request the board to change the streamed vars.
+setStreamedVars(handles.hbh, streamedVars);
+
+% Pre-allocate the plot points buffer.
+% TODO: keep data for the variable that remain the same.
+if ~isempty(streamedVars)
+ handles.plotData = nan(PLOT_N_SAMPLES, length(handles.streamedVars));
+ if ~isempty(handles.plotData)
+ handles.plotFig = plot(1:PLOT_N_SAMPLES, handles.plotData);
+ end
+end
+
+% Color the variables labels to match the lines colors.
+% The standard MATLAB legend is too slow for live plots.
+colors = get(handles.plot_figure, 'ColorOrder');
+
+for i = 1:N_VARS_MAX
+ streamVarIndex = find(i == streamedVars, 1);
+
+ if ~isempty(streamVarIndex)
+ set(handles.varsWidgets(i).checkbox, ...
+ 'ForegroundColor', colors(rem(streamVarIndex-1,7)+1, :));
+ else
+ set(handles.varsWidgets(i).checkbox, ...
+ 'ForegroundColor', 'black');
+ end
+end
+
+guidata(hObject, handles);
+
+
+function onGetVar(hObject, varID)
+% Get handles structure.
+handles = guidata(hObject);
+
+% Get the variable value from the board.
+varValue = getVar(handles.hbh, varID);
+
+% Display the value on the associated edit box widget.
+set(handles.varsWidgets(varID).edit, 'String', varValue);
+
+
+function onSetVar(hObject, varID)
+% Get handles structure.
+handles = guidata(hObject);
+
+% Get the desired variable value from the associated edit box widget.
+varValue = str2double(handles.varsWidgets(varID).edit.String);
+
+% Set the variable value on the board.
+setVar(handles.hbh, varID, varValue);
+
+
+function updatePlot(hObject)
+hri_constants;
+
+try
+ handles = guidata(hObject);
+catch
+ return;
+end
+
+if ~isfield(handles, 'plotData') || isempty(handles.plotData)
+ return;
+end
+
+vars = getStreaming(handles.hbh);
+
+if isempty(vars) || (size(vars, 2) ~= size(handles.plotData, 2))
+ return;
+end
+
+% If the plot buffer is full, restart.
+if handles.plotDataCurrentIndex + size(vars,1) > PLOT_N_SAMPLES - 1
+ handles.plotDataCurrentIndex = 1;
+end
+
+range = handles.plotDataCurrentIndex:handles.plotDataCurrentIndex+size(vars,1)-1;
+handles.plotData(range, :) = vars;
+handles.plotData(handles.plotDataCurrentIndex+size(vars,1), :) = nan;
+handles.plotDataCurrentIndex = handles.plotDataCurrentIndex + size(vars,1);
+
+for i=1:size(handles.plotData, 2)
+ set(handles.plotFig(i), 'ydata', handles.plotData(:,i));
+end
+
+guidata(hObject, handles);
+
+
+% --- Outputs from this function are returned to the command line.
+function varargout = hri_gui_OutputFcn(hObject, eventdata, handles) %#ok
+% varargout cell array for returning output args (see VARARGOUT);
+% hObject handle to figure
+% eventdata reserved - to be defined in a future version of MATLAB
+% handles structure with handles and user data (see GUIDATA)
+
+% Get default command line output from handles structure
+varargout{1} = handles.output;
+
+
+% --- Executes on button press in data_streaming_checkbox.
+function data_streaming_checkbox_Callback(hObject, ~, handles) %#ok
+% hObject handle to data_streaming_checkbox (see GCBO)
+% eventdata reserved - to be defined in a future version of MATLAB
+% handles structure with handles and user data (see GUIDATA)
+
+if get(hObject, 'Value') == 0
+ setupStreaming(hObject, []);
+else
+ setupStreaming(hObject, handles.streamedVars);
+end
+
+
+% --- Executes on button press in clear_plot_button.
+function clear_plot_button_Callback(hObject, ~, handles) %#ok
+% hObject handle to clear_plot_button (see GCBO)
+% eventdata reserved - to be defined in a future version of MATLAB
+% handles structure with handles and user data (see GUIDATA)
+handles.plotData = nan(size(handles.plotData));
+handles.plotDataCurrentIndex = 1;
+
+for i=1:size(handles.plotData, 2)
+ set(handles.plotFig(i), 'ydata', handles.plotData(:,i));
+end
+
+guidata(hObject, handles);
+
+
+% --- Executes on button press in log_to_file_checkbox.
+function log_to_file_checkbox_Callback(hObject, ~, handles) %#ok
+% hObject handle to log_to_file_checkbox (see GCBO)
+% eventdata reserved - to be defined in a future version of MATLAB
+% handles structure with handles and user data (see GUIDATA)
+if get(hObject,'Value') == 0
+ stopLogging(handles.hbh);
+else
+ startLogging(handles.hbh);
+end
+
+
+function main_window_CloseRequestFcn(hObject, ~, ~) %#ok
+
+handles = guidata(hObject);
+
+if isfield(handles, 'plotTimer')
+ stop(handles.plotTimer);
+ delete(handles.plotTimer);
+ handles = rmfield(handles, 'plotTimer');
+end
+
+if isfield(handles, 'hbh')
+ delete(handles.hbh);
+ handles = rmfield(handles, 'hbh');
+end
+
+guidata(hObject, handles);
+
+delete(hObject);
+close(gcf);
+
+
+% --- Executes on selection change in plot_freq_popup.
+function plot_freq_popup_Callback(hObject, ~, handles) %#ok
+% hObject handle to plot_freq_popup (see GCBO)
+% eventdata reserved - to be defined in a future version of MATLAB
+% handles structure with handles and user data (see GUIDATA)
+hri_constants; % Get the constants.
+
+currentIndex = hObject.Value;
+plotFreq = PLOT_SAMPLING_FREQS(currentIndex);
+
+stop(handles.plotTimer);
+set(handles.plotTimer, 'Period', 1.0 / plotFreq);
+start(handles.plotTimer);
diff --git a/MATLAB/hri_load_logfile.m b/MATLAB/hri_load_logfile.m
new file mode 100644
index 0000000..d6f27a7
--- /dev/null
+++ b/MATLAB/hri_load_logfile.m
@@ -0,0 +1,26 @@
+function variables = hri_load_logfile(filename)
+%HRI_LOAD_LOGFILE Loads a HRI logfile to a MATLAB structure.
+% Loads the filename with the given filename. A MATLAB structure is
+% generated, each named field is an array of the history of the variable
+% value.
+
+%% Read the header to get the variables names.
+fid = fopen(filename, 'r');
+
+line = fgetl(fid);
+
+varNames = textscan(line, '%s', 'Delimiter', ';');
+varNames = varNames{:};
+
+fclose(fid);
+
+%% Read the variables values over time.
+A = dlmread('logs/log_2015-12-27_22_40_19.csv', ';', 1, 0);
+
+%% Create a structure with a field per variable.
+for i=1:length(varNames)
+ variables.(varNames{i}) = A(:,i);
+end
+
+end
+
diff --git a/Protocol description.xlsx b/Protocol description.xlsx
deleted file mode 100644
index 5c8590f..0000000
Binary files a/Protocol description.xlsx and /dev/null differ