diff --git a/Firmware/src/communication.c b/Firmware/src/communication.c index ea6f393..0914f0a 100644 --- a/Firmware/src/communication.c +++ b/Firmware/src/communication.c @@ -1,316 +1,317 @@ #include "communication.h" #include "controller.h" #include "drivers/callback_timers.h" #include "drivers/adc.h" #include "drivers/dac.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" -extern uint32_t statusReg, ctrl_timestamp, ctrl_motorPosCod, ctrl_motorPosCod_c, ctrl_currentFilterType; +extern uint32_t statusReg, ctrl_timestamp, ctrl_currentFilterType; +extern float32_t ctrl_motorPosCod, ctrl_motorPosCod_c; extern float32_t ctrl_motorTorque_mNm_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]; 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). 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); /** * @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_mNm_c; variablesArray[VAR_POS_REG_SAMPLING_PERIOD] = NULL; variablesArray[VAR_WALL_POSITION] = (uint32_t*) &ctrl_motorPosCod_c; variablesArray[VAR_MEASURED_ANGLE_RAW] = (uint32_t*) &ctrl_motorPosCod; 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; // Setup the UART peripheral, and specify the function that will be called // each time a byte is received. uart_Init(comm_HandleByte); // Make the streaming function periodically called by the timer 7. cbt_SetCommLoopTimer(comm_Stream, TE_DATA_LOOP_DEFAULT_VAL); } /** * @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) { uint32_t i; int nDataBytesToSend = 0; for(i=0; i> 4); // Most significant bits. uart_SendByte((data[i]&0x0f)); // Least significant bits. } } /** * @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: if(dataBytesReady == 0) { statusReg |= DATA_UPSTREAM_ENB; TIM_SetCounter(TIM7, 0); TIM_Cmd(TIM7, ENABLE); } break; case PC_MESSAGE_STOP_STREAMING: if(dataBytesReady == 0) { TIM_Cmd(TIM7, DISABLE); statusReg &= ~DATA_UPSTREAM_ENB; } break; case PC_MESSAGE_GET_VAR: if(dataBytesReady == 1) { uint32_t variableValue; // 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 >= N_VARIABLES) return; // Prepare the message to be sent to the PC. // First byte: variable index. txBuffer[0] = variableIndex; // Four following bytes: actual value of the variable. variableValue = comm_GetVar(variableIndex); memcpy(&txBuffer[1], &variableValue, 4); // Send the message to the PC. comm_SendPacket(STM_MESSAGE_VAR, txBuffer, 5); } break; case PC_MESSAGE_SET_VAR: if(dataBytesReady == 5) { // 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); } break; case PC_MESSAGE_SET_STREAMED_VAR: if(dataBytesReady == 4) { uint8_t *newSelectedVars = &rxDataBytesBuffer[0]; memcpy(&selectedVariablesToStream, newSelectedVars, 4); } break; default: // No data bytes for the other message types. break; } } } diff --git a/Firmware/src/communication.h b/Firmware/src/communication.h index 2ef4250..92ee12f 100644 --- a/Firmware/src/communication.h +++ b/Firmware/src/communication.h @@ -1,77 +1,77 @@ #ifndef __COMMUNICATION_H #define __COMMUNICATION_H #include "main.h" // 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. // 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. // 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. [pulses] -#define VAR_MEASURED_ANGLE_FILTERED 10 // Measured output angle, filtered. [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. /** @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 * @{ */ void comm_Init(void); /** * @} */ #endif diff --git a/Firmware/src/controller.c b/Firmware/src/controller.c index 6753368..415e9c6 100644 --- a/Firmware/src/controller.c +++ b/Firmware/src/controller.c @@ -1,128 +1,128 @@ #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 int32_t ctrl_motorPosCod; -volatile int32_t ctrl_motorPosCod_c; +volatile float32_t ctrl_motorPosCod; +volatile float32_t ctrl_motorPosCod_c; volatile float32_t ctrl_motorPosCodFilt = 0.0f; volatile float32_t ctrl_motorTorque_mNm_c = 0.0f; volatile uint32_t ctrl_currentFilterType = FILTER_TYPE_NONE; volatile pid_Pid ctrl_currentPid, ctrl_positionPid; volatile bfilt_BasicFilter ctrl_encoderFilter; volatile float32_t motorTorque_mNm_dbg; void ctrl_RegulateCurrent(void); void ctrl_RegulatePosition(void); /** * @brief Initialize the position and current controllers. */ void ctrl_Init(void) { // Setup the PIDs. pid_Init((pid_Pid*)&ctrl_currentPid, KP_CURRENT_DEFAULT_VAL, KI_CURRENT_DEFAULT_VAL, 0.0f, CURRENT_INTEGRATOR_SAT); #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); #else pid_Init((pid_Pid*)&ctrl_positionPid, KP_POSITION_DEFAULT_VAL, 0.0f, KD_POSITION_DEFAULT_VAL, 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); } /** * @brief Current regulation "loop" function. */ void ctrl_RegulateCurrent() { float32_t pwmNormalizedDutyCycle, motorTorque_mNm; // Get the current. float32_t motorCurrent_mA = adc_GetCurrent(); // Convert to torque. motorTorque_mNm = motorCurrent_mA * TORQUE_CONST; motorTorque_mNm_dbg = motorTorque_mNm; // Regulate. pwmNormalizedDutyCycle = -pid_Step((pid_Pid*)&ctrl_currentPid, motorTorque_mNm, ctrl_motorTorque_mNm_c, (float32_t)cbt_GetCurrentLoopPeriod()*MICROSECOND_TO_SECOND); utils_SaturateF(&pwmNormalizedDutyCycle, -CURRENT_LOOP_PWM_MAX_DUTY_CYCLE, CURRENT_LOOP_PWM_MAX_DUTY_CYCLE); // Apply the computed PWM duty. hb_SetPWM(pwmNormalizedDutyCycle); } /** * @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 encoder position. - /*ctrl_motorPosCod = enc_GetPosition(); + 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, (float32_t)ctrl_motorPosCod); + ctrl_motorPosCodFilt = bfilt_Step((bfilt_BasicFilter*)&ctrl_encoderFilter, ctrl_motorPosCod); } else if(ctrl_currentFilterType == FILTER_TYPE_RUNNING_MEAN) { // TODO. - ctrl_motorPosCodFilt = (float32_t)ctrl_motorPosCod; + ctrl_motorPosCodFilt = ctrl_motorPosCod; } else // No filter. - ctrl_motorPosCodFilt = (float32_t)ctrl_motorPosCod; + ctrl_motorPosCodFilt = ctrl_motorPosCod; // Regulate. #ifdef REGULATION_TYPE_SYMMETRIC // No wall, just regulation on a point. ctrl_motorTorque_mNm_c = -pid_Step((pid_Pid*)&ctrl_positionPid, ctrl_motorPosCodFilt, - (float32_t)ctrl_motorPosCod_c, + 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_mNm_c = -pid_Step((pid_Pid*)&ctrl_positionPid, ctrl_motorPosCodFilt, (float32_t)ctrl_motorPosCod_c, (float32_t)(cbt_GetPositionLoopPeriod())*MICROSECOND_TO_SECOND); } else ctrl_motorTorque_mNm_c = 0.0f; -#endif*/ +#endif } diff --git a/Firmware/src/drivers/incr_encoder.c b/Firmware/src/drivers/incr_encoder.c index 5cbb4d5..319a4a8 100644 --- a/Firmware/src/drivers/incr_encoder.c +++ b/Firmware/src/drivers/incr_encoder.c @@ -1,126 +1,129 @@ #include "incr_encoder.h" void tim5InitFunc(void); void extitInitFunc(void); /** * @brief Initialize the incremental encoder driver. */ void enc_Init(void) { tim5InitFunc(); // Setup the timer 5 to be incremented with the quadrature signal of the encoder. //extitInitFunc(); // Setup interrupt for the index line of the encoder. } /** * @brief Returns the current position measured by the encoder counter. * @retval The current position of the motor shaft [encoder increments]. */ -int32_t enc_GetPosition(void) +float32_t enc_GetPosition(void) { - return (int32_t)TIM_GetCounter(TIM5); // Coder position (max +/-2147483648) + // Convert the counter value (coder increments) to the paddle angle in degrees. + int32_t rawCounter = (int32_t)TIM_GetCounter(TIM5); // Coder position (max +/- 2^31). + float32_t outputAngle = ((float32_t)rawCounter) / ((float32_t)CODER_RESOLUTION) * 360.0f / REDUCTION_RATIO; + return outputAngle; } /** * @brief Initialize TIM5 as a counter for quadrature signals. */ void tim5InitFunc(void) { TIM_TimeBaseInitTypeDef TIM_TimeBaseStruct; GPIO_InitTypeDef GPIO_InitStruct; // Setup the two pins wired to the encoder, to be used as the "clock" of the timer. RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM5, ENABLE); GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF; GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz; GPIO_InitStruct.GPIO_OType = GPIO_OType_PP; GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP; GPIO_InitStruct.GPIO_Pin = CODER_A_Pin; GPIO_Init(CODER_A_Port, &GPIO_InitStruct); GPIO_PinAFConfig(CODER_A_Port, CODER_A_PinSource, GPIO_AF_TIM5); GPIO_InitStruct.GPIO_Pin = CODER_B_Pin; GPIO_Init(CODER_B_Port, &GPIO_InitStruct); GPIO_PinAFConfig(CODER_B_Port, CODER_B_PinSource, GPIO_AF_TIM5); TIM_TimeBaseStruct.TIM_Period = 0xFFFFFFFF; TIM_TimeBaseStruct.TIM_Prescaler = (uint16_t)0; TIM_TimeBaseStruct.TIM_ClockDivision = 0; TIM_TimeBaseStruct.TIM_CounterMode = TIM_CounterMode_Up; TIM_TimeBaseInit(TIM5, &TIM_TimeBaseStruct); TIM_EncoderInterfaceConfig(TIM5, TIM_EncoderMode_TI12, TIM_ICPolarity_Rising, TIM_ICPolarity_Rising); TIM_SetCounter(TIM5, 0); TIM_Cmd(TIM5, ENABLE); } /** * @brief Initialize interrupt from Index line of the coder */ void extitInitFunc(void) { NVIC_InitTypeDef NVIC_InitStruct; EXTI_InitTypeDef EXTI_InitStruct; GPIO_InitTypeDef GPIO_InitStruct; RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE); GPIO_InitStruct.GPIO_Mode = GPIO_Mode_IN; GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_NOPULL; GPIO_InitStruct.GPIO_Pin = CODER_I_Pin; GPIO_Init(CODER_I_Port, &GPIO_InitStruct); SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOB, EXTI_PinSource8); EXTI_InitStruct.EXTI_Mode = EXTI_Mode_Interrupt; EXTI_InitStruct.EXTI_Trigger = EXTI_Trigger_Rising; EXTI_InitStruct.EXTI_LineCmd = ENABLE; EXTI_InitStruct.EXTI_Line = EXTI_Line8; EXTI_Init(&EXTI_InitStruct); NVIC_InitStruct.NVIC_IRQChannel = EXTI9_5_IRQn; NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = CODER_INDEX_IRQ_PRIORITY; NVIC_InitStruct.NVIC_IRQChannelSubPriority = 0x00; NVIC_InitStruct.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStruct); } /** * @brief Interrupt from Index line of the coder (check step loss) * @note This function currently does nothing. */ void EXTI9_5_IRQHandler(void){ // static int32_t motorPosTurn_old = 0; // static int32_t motorPosTurn_old_tmp = 0; // int16_t localVar; if(EXTI_GetITStatus(EXTI_Line8) != RESET) { // localVar = (int16_t)TIM_GetCounter(TIM4); // debugVar02 = localVar; // if(localVar>1000){ // motorPosTurn_old = motorPosTurn; // motorPosTurn++; // }else if(localVar<(-1000)){ // motorPosTurn_old = motorPosTurn; // motorPosTurn--; // }else{ // motorPosTurn_old_tmp = motorPosTurn; // motorPosTurn = motorPosTurn_old; // motorPosTurn_old = motorPosTurn_old_tmp; // } // // TIM_SetCounter(TIM4,(uint32_t)CODER_INDEX_POS); // if(~statusReg & REF_POS_INIT){ // First time index is detected. // statusReg |= REF_POS_INIT; // motorPosTurn = 0; // //TIM_SetCounter(TIM4,(uint32_t)6000); // } EXTI_ClearITPendingBit(EXTI_Line8); } } diff --git a/Firmware/src/drivers/incr_encoder.h b/Firmware/src/drivers/incr_encoder.h index 8652b8f..3521aa3 100644 --- a/Firmware/src/drivers/incr_encoder.h +++ b/Firmware/src/drivers/incr_encoder.h @@ -1,36 +1,36 @@ #ifndef __INCR_ENCODERS_H #define __INCR_ENCODERS_H #include "../main.h" #define CODER_A_Pin GPIO_Pin_0 #define CODER_A_Port GPIOA #define CODER_A_PinSource GPIO_PinSource0 #define CODER_B_Pin GPIO_Pin_1 #define CODER_B_Port GPIOA #define CODER_B_PinSource GPIO_PinSource1 #define CODER_I_Pin GPIO_Pin_1 #define CODER_I_Port GPIOE #define CODER_RESOLUTION ((uint32_t)2000) // Number of increments per turn. /** @defgroup Encoder Driver / Incremental encoder * @brief Driver for an incremental encoder. * * This driver uses a timer of the STM32, configured as a quadrature decoder. * * Call enc_Init() first in the initialization code. Then, call enc_GetPosition() to * get the decoded value. * * @addtogroup Encoder * @{ */ void enc_Init(void); -int32_t enc_GetPosition(void); +float32_t enc_GetPosition(void); /** * @} */ #endif diff --git a/Firmware/src/main.h b/Firmware/src/main.h index 442a6ce..4dc174c 100644 --- a/Firmware/src/main.h +++ b/Firmware/src/main.h @@ -1,89 +1,92 @@ /** \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 "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]. +// Mechanical parameters. +#define REDUCTION_RATIO 15.0f + // 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) #endif diff --git a/Protocol description.xlsx b/Protocol description.xlsx new file mode 100644 index 0000000..603e62f Binary files /dev/null and b/Protocol description.xlsx differ