diff --git a/.gitignore b/.gitignore index fd24600..ff0ec26 100644 --- a/.gitignore +++ b/.gitignore @@ -7,6 +7,8 @@ *.cppcheck Debug/ Release/ +__pycache__/ +*.pyc # Prerequisites *.d diff --git a/00-Doc/02-Papers/Leopold_SEPULCHRE.pdf b/00-Doc/02-Papers/Leopold_SEPULCHRE.pdf new file mode 100644 index 0000000..de8d337 Binary files /dev/null and b/00-Doc/02-Papers/Leopold_SEPULCHRE.pdf differ diff --git a/00-Doc/02-Papers/PED4_1038C.pdf b/00-Doc/02-Papers/PED4_1038C.pdf new file mode 100644 index 0000000..b4d6e10 Binary files /dev/null and b/00-Doc/02-Papers/PED4_1038C.pdf differ diff --git a/00-Doc/02-Papers/STM2G4_265522.pdf b/00-Doc/02-Papers/STM2G4_265522.pdf new file mode 100644 index 0000000..e8d6226 Binary files /dev/null and b/00-Doc/02-Papers/STM2G4_265522.pdf differ diff --git a/00-Doc/02-Papers/cd00298474-stm32f-pmsm-singledual-foc-sdk-v43-stmicroelectronics.pdf b/00-Doc/02-Papers/cd00298474-stm32f-pmsm-singledual-foc-sdk-v43-stmicroelectronics.pdf new file mode 100644 index 0000000..08874a8 Binary files /dev/null and b/00-Doc/02-Papers/cd00298474-stm32f-pmsm-singledual-foc-sdk-v43-stmicroelectronics.pdf differ diff --git a/00-Doc/02-Papers/en.STM32G4-System-Nested_Vectored_Interrupt_Control_NVIC.pdf b/00-Doc/02-Papers/en.STM32G4-System-Nested_Vectored_Interrupt_Control_NVIC.pdf new file mode 100644 index 0000000..0e918b2 Binary files /dev/null and b/00-Doc/02-Papers/en.STM32G4-System-Nested_Vectored_Interrupt_Control_NVIC.pdf differ diff --git a/00-Doc/02-Papers/energies-12-04526.pdf b/00-Doc/02-Papers/energies-12-04526.pdf new file mode 100644 index 0000000..de176cb Binary files /dev/null and b/00-Doc/02-Papers/energies-12-04526.pdf differ diff --git a/00-Doc/02-Papers/spraby9.pdf b/00-Doc/02-Papers/spraby9.pdf new file mode 100644 index 0000000..14ea4eb Binary files /dev/null and b/00-Doc/02-Papers/spraby9.pdf differ diff --git a/00-Doc/02-Papers/vf.pdf b/00-Doc/02-Papers/vf.pdf new file mode 100644 index 0000000..b2d8dc5 Binary files /dev/null and b/00-Doc/02-Papers/vf.pdf differ diff --git a/01-Firmware/.project b/01-Firmware/.project new file mode 100644 index 0000000..12332c0 --- /dev/null +++ b/01-Firmware/.project @@ -0,0 +1,11 @@ + + + 01-Firmware + + + + + + + + diff --git a/01-Firmware/AS5048_DS000298_4-00.pdf b/01-Firmware/AS5048_DS000298_4-00.pdf new file mode 100644 index 0000000..ddc80cc Binary files /dev/null and b/01-Firmware/AS5048_DS000298_4-00.pdf differ diff --git a/01-Firmware/B-G431B-ESC1-v2/.project b/01-Firmware/B-G431B-ESC1-v2/.project index 3f524ab..be72fd9 100644 --- a/01-Firmware/B-G431B-ESC1-v2/.project +++ b/01-Firmware/B-G431B-ESC1-v2/.project @@ -24,7 +24,6 @@ org.eclipse.cdt.core.cnature com.st.stm32cube.ide.mcu.MCUCubeIdeServicesRevAev2ProjectNature com.st.stm32cube.ide.mcu.MCUAdvancedStructureProjectNature - com.st.stm32cube.ide.mcu.MCUEndUserDisabledTrustZoneProjectNature com.st.stm32cube.ide.mcu.MCUSingleCpuProjectNature com.st.stm32cube.ide.mcu.MCURootProjectNature org.eclipse.cdt.managedbuilder.core.managedBuildNature diff --git a/01-Firmware/B-G431B-ESC1-v2/B-G431B-ESC1-v2.ioc b/01-Firmware/B-G431B-ESC1-v2/B-G431B-ESC1-v2.ioc index 4e80ea3..5bfcd10 100644 --- a/01-Firmware/B-G431B-ESC1-v2/B-G431B-ESC1-v2.ioc +++ b/01-Firmware/B-G431B-ESC1-v2/B-G431B-ESC1-v2.ioc @@ -99,7 +99,7 @@ Dma.ADC2.3.PeriphDataAlignment=DMA_PDATAALIGN_HALFWORD ADC1.Rank-0\#ChannelRegularConversion=1 Mcu.PinsNb=38 PC11.Locked=true -ADC1.IPParameters=Rank-0\#ChannelRegularConversion,Channel-0\#ChannelRegularConversion,SamplingTime-0\#ChannelRegularConversion,OffsetNumber-0\#ChannelRegularConversion,NbrOfConversionFlag,Rank-1\#ChannelRegularConversion,Channel-1\#ChannelRegularConversion,SamplingTime-1\#ChannelRegularConversion,OffsetNumber-1\#ChannelRegularConversion,NbrOfConversion,EOCSelection,ExternalTrigConv,Overrun,Rank-2\#ChannelRegularConversion,Channel-2\#ChannelRegularConversion,SamplingTime-2\#ChannelRegularConversion,OffsetNumber-2\#ChannelRegularConversion,Rank-3\#ChannelRegularConversion,Channel-3\#ChannelRegularConversion,SamplingTime-3\#ChannelRegularConversion,OffsetNumber-3\#ChannelRegularConversion,master,Rank-4\#ChannelRegularConversion,Channel-4\#ChannelRegularConversion,SamplingTime-4\#ChannelRegularConversion,OffsetNumber-4\#ChannelRegularConversion +ADC1.IPParameters=Rank-0\#ChannelRegularConversion,Channel-0\#ChannelRegularConversion,SamplingTime-0\#ChannelRegularConversion,OffsetNumber-0\#ChannelRegularConversion,NbrOfConversionFlag,Rank-1\#ChannelRegularConversion,Channel-1\#ChannelRegularConversion,SamplingTime-1\#ChannelRegularConversion,OffsetNumber-1\#ChannelRegularConversion,NbrOfConversion,EOCSelection,ExternalTrigConv,Overrun,Rank-2\#ChannelRegularConversion,Channel-2\#ChannelRegularConversion,SamplingTime-2\#ChannelRegularConversion,OffsetNumber-2\#ChannelRegularConversion,Rank-3\#ChannelRegularConversion,Channel-3\#ChannelRegularConversion,SamplingTime-3\#ChannelRegularConversion,OffsetNumber-3\#ChannelRegularConversion,master,Rank-4\#ChannelRegularConversion,Channel-4\#ChannelRegularConversion,SamplingTime-4\#ChannelRegularConversion,OffsetNumber-4\#ChannelRegularConversion,DMAContinuousRequests PC13.Locked=true TIM1.CounterMode=TIM_COUNTERMODE_CENTERALIGNED1 ADC1.OffsetNumber-0\#ChannelRegularConversion=ADC_OFFSET_NONE @@ -121,7 +121,7 @@ PA11.Locked=true Dma.ADC1.2.MemInc=DMA_MINC_ENABLE TIM4.IPParameters=Prescaler,ICPolarity_1 Dma.ADC1.2.SyncSignalID=NONE -ADC2.IPParameters=Rank-0\#ChannelRegularConversion,Channel-0\#ChannelRegularConversion,SamplingTime-0\#ChannelRegularConversion,OffsetNumber-0\#ChannelRegularConversion,NbrOfConversionFlag,EOCSelection,ExternalTrigConv,Rank-1\#ChannelRegularConversion,Channel-1\#ChannelRegularConversion,SamplingTime-1\#ChannelRegularConversion,OffsetNumber-1\#ChannelRegularConversion,NbrOfConversion,Overrun,Rank-2\#ChannelRegularConversion,Channel-2\#ChannelRegularConversion,SamplingTime-2\#ChannelRegularConversion,OffsetNumber-2\#ChannelRegularConversion +ADC2.IPParameters=Rank-0\#ChannelRegularConversion,Channel-0\#ChannelRegularConversion,SamplingTime-0\#ChannelRegularConversion,OffsetNumber-0\#ChannelRegularConversion,NbrOfConversionFlag,EOCSelection,ExternalTrigConv,Rank-1\#ChannelRegularConversion,Channel-1\#ChannelRegularConversion,SamplingTime-1\#ChannelRegularConversion,OffsetNumber-1\#ChannelRegularConversion,NbrOfConversion,Overrun,Rank-2\#ChannelRegularConversion,Channel-2\#ChannelRegularConversion,SamplingTime-2\#ChannelRegularConversion,OffsetNumber-2\#ChannelRegularConversion,DMAContinuousRequests PC6.Locked=true PA9.Signal=S_TIM1_CH2 VP_TIM1_VS_ClockSourceINT.Signal=TIM1_VS_ClockSourceINT @@ -155,6 +155,7 @@ Mcu.Pin32=VP_ADC2_OPAMP_INTERNAL3 PA9.Locked=true NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:false PC10.GPIOParameters=GPIO_Label +ADC1.DMAContinuousRequests=ENABLE PB7.GPIO_PuPdOD=GPIO_NOPULL PA13.Mode=Serial_Wire ProjectManager.FreePins=false @@ -209,7 +210,7 @@ PF0-OSC_IN.Signal=RCC_OSC_IN Dma.USART2_TX.1.MemDataAlignment=DMA_MDATAALIGN_BYTE RCC.I2C2Freq_Value=160000000 PB0.Signal=OPAMP3_VINP -TIM1.PeriodNoDither=4999 +TIM1.PeriodNoDither=1999 Dma.USART2_TX.1.SyncSignalID=NONE Mcu.Family=STM32G4 ProjectManager.MainLocation=Core/Src @@ -248,12 +249,13 @@ Mcu.IP1=ADC2 PA12.Signal=TIM1_CH2N TIM1.Prescaler=0 Mcu.UserConstants= +ADC2.DMAContinuousRequests=ENABLE Dma.USART2_RX.0.Priority=DMA_PRIORITY_LOW Mcu.ThirdPartyNb=0 ADC1.Channel-3\#ChannelRegularConversion=ADC_CHANNEL_1 RCC.HCLKFreq_Value=160000000 Mcu.IPNb=16 -ProjectManager.PreviousToolchain= +ProjectManager.PreviousToolchain=STM32CubeIDE Dma.ADC2.3.PeriphInc=DMA_PINC_DISABLE Mcu.Pin6=PA3 Mcu.Pin7=PA5 @@ -325,12 +327,13 @@ PC14-OSC32_IN.GPIO_Label=CAN_TERM ADC2.Rank-0\#ChannelRegularConversion=1 PF1-OSC_OUT.Mode=HSE-External-Oscillator Dma.Request0=USART2_RX -NVIC.CORDIC_IRQn=true\:0\:0\:false\:false\:true\:true\:true +NVIC.CORDIC_IRQn=true\:2\:0\:true\:false\:true\:true\:true PC14-OSC32_IN.Locked=true PB4.Signal=USART2_RX PB3.Locked=true PB15.Locked=true PA3.Signal=OPAMP1_VINM0 +NVIC.FDCAN1_IT0_IRQn=true\:3\:0\:true\:false\:true\:true\:true PB7.GPIO_Speed=GPIO_SPEED_FREQ_VERY_HIGH RCC.PLLSourceVirtual=RCC_PLLSOURCE_HSE RCC.I2SFreq_Value=160000000 @@ -353,7 +356,7 @@ Dma.ADC2.3.Instance=DMA1_Channel4 FDCAN1.IPParameters=AutoRetransmission,TransmitPause,NominalSyncJumpWidth,ClockDivider,NominalTimeSeg1,NominalTimeSeg2,DataSyncJumpWidth,DataTimeSeg1,DataTimeSeg2,StdFiltersNbr Dma.USART2_TX.1.PeriphDataAlignment=DMA_PDATAALIGN_BYTE Dma.USART2_TX.1.RequestNumber=1 -NVIC.USART2_IRQn=true\:0\:0\:false\:false\:true\:true\:true +NVIC.USART2_IRQn=true\:4\:0\:true\:false\:true\:true\:true ADC1.SamplingTime-2\#ChannelRegularConversion=ADC_SAMPLETIME_2CYCLES_5 Dma.USART2_TX.1.SignalID=NONE RCC.FamilyName=M diff --git a/01-Firmware/B-G431B-ESC1-v2/CaptureBugPositionEstimation.PNG b/01-Firmware/B-G431B-ESC1-v2/CaptureBugPositionEstimation.PNG new file mode 100644 index 0000000..145d27e Binary files /dev/null and b/01-Firmware/B-G431B-ESC1-v2/CaptureBugPositionEstimation.PNG differ diff --git a/01-Firmware/B-G431B-ESC1-v2/CaptureBugPositionEstimation2.PNG b/01-Firmware/B-G431B-ESC1-v2/CaptureBugPositionEstimation2.PNG new file mode 100644 index 0000000..a0871da Binary files /dev/null and b/01-Firmware/B-G431B-ESC1-v2/CaptureBugPositionEstimation2.PNG differ diff --git a/01-Firmware/B-G431B-ESC1-v2/CaptureBugPositionEstimation3.PNG b/01-Firmware/B-G431B-ESC1-v2/CaptureBugPositionEstimation3.PNG new file mode 100644 index 0000000..b14c218 Binary files /dev/null and b/01-Firmware/B-G431B-ESC1-v2/CaptureBugPositionEstimation3.PNG differ diff --git a/01-Firmware/B-G431B-ESC1-v2/ClasseurPositionEstimation.xlsx b/01-Firmware/B-G431B-ESC1-v2/ClasseurPositionEstimation.xlsx new file mode 100644 index 0000000..7094caf Binary files /dev/null and b/01-Firmware/B-G431B-ESC1-v2/ClasseurPositionEstimation.xlsx differ diff --git a/01-Firmware/B-G431B-ESC1-v2/Core/Inc/as5048a.h b/01-Firmware/B-G431B-ESC1-v2/Core/Inc/as5048a.h index af26ee1..d13d472 100644 --- a/01-Firmware/B-G431B-ESC1-v2/Core/Inc/as5048a.h +++ b/01-Firmware/B-G431B-ESC1-v2/Core/Inc/as5048a.h @@ -17,14 +17,15 @@ extern "C" { // Support AS5048A PWM interface only void API_AS5048A_Position_Sensor_Init(TIM_HandleTypeDef * htim); -void API_AS5048A_Position_Sensor_It(TIM_HandleTypeDef *htim); +void API_AS5048A_Position_Sensor_It(TIM_HandleTypeDef *htim) __attribute__((section (".ccmram"))); float API_AS5048A_Position_Sensor_Get_Radians(); -float API_AS5048A_Position_Sensor_Get_Radians_Estimation(uint16_t time_us); +float API_AS5048A_Position_Sensor_Get_Radians_Estimation(uint16_t time_us) __attribute__((section (".ccmram"))); float API_AS5048A_Position_Sensor_Get_Multiturn_Radians(); float API_AS5048A_Position_Sensor_Get_RPS(); float API_AS5048A_Position_Sensor_Get_DPS(); uint16_t API_AS5048A_Position_Sensor_Get_Timestamp(); uint16_t API_AS5048A_Position_Sensor_Get_DeltaTimestamp(); +int16_t API_AS5048A_Position_Sensor_Get_DeltaTimeEstimation(); uint32_t API_AS5048A_Position_Sensor_Error(); uint32_t API_AS5048A_Position_Sensor_Error_Counter(); float API_AS5048A_Position_Sensor_Get_DeltaRad(); diff --git a/01-Firmware/B-G431B-ESC1-v2/Core/Inc/control_table.h b/01-Firmware/B-G431B-ESC1-v2/Core/Inc/control_table.h index daf0777..4682a81 100644 --- a/01-Firmware/B-G431B-ESC1-v2/Core/Inc/control_table.h +++ b/01-Firmware/B-G431B-ESC1-v2/Core/Inc/control_table.h @@ -85,23 +85,23 @@ extern "C" { #define REG_PID_TORQUE_CURRENT_KFF_L 0x44 //68 // RESERVED #define REG_PID_TORQUE_CURRENT_KFF_H 0x45 //69 // RESERVED -#define REG_CAL_PHASE1_CURRENT_SENSE_MA_L 0x46 //70 -#define REG_CAL_PHASE1_CURRENT_SENSE_MA_H 0x47 //71 -#define REG_CAL_PHASE1_CURRENT_SENSE_OFFSET_L 0x48 //72 -#define REG_CAL_PHASE1_CURRENT_SENSE_OFFSET_H 0x49 //73 +#define REG_CAL_PHASE1_CURRENT_SENSE_MA_L 0x46 //70 // RESERVED +#define REG_CAL_PHASE1_CURRENT_SENSE_MA_H 0x47 //71 // RESERVED +#define REG_CAL_PHASE1_CURRENT_SENSE_OFFSET_L 0x48 //72 // RESERVED +#define REG_CAL_PHASE1_CURRENT_SENSE_OFFSET_H 0x49 //73 // RESERVED -#define REG_CAL_PHASE2_CURRENT_SENSE_MA_L 0x4A //74 -#define REG_CAL_PHASE2_CURRENT_SENSE_MA_H 0x4B //75 -#define REG_CAL_PHASE2_CURRENT_SENSE_OFFSET_L 0x4C //76 -#define REG_CAL_PHASE2_CURRENT_SENSE_OFFSET_H 0x4D //77 +#define REG_CAL_PHASE2_CURRENT_SENSE_MA_L 0x4A //74 // RESERVED +#define REG_CAL_PHASE2_CURRENT_SENSE_MA_H 0x4B //75 // RESERVED +#define REG_CAL_PHASE2_CURRENT_SENSE_OFFSET_L 0x4C //76 // RESERVED +#define REG_CAL_PHASE2_CURRENT_SENSE_OFFSET_H 0x4D //77 // RESERVED -#define REG_CAL_PHASE3_CURRENT_SENSE_MA_L 0x4E //78 -#define REG_CAL_PHASE3_CURRENT_SENSE_MA_H 0x4F //79 -#define REG_CAL_PHASE3_CURRENT_SENSE_OFFSET_L 0x50 //80 -#define REG_CAL_PHASE3_CURRENT_SENSE_OFFSET_H 0x51 //81 +#define REG_CAL_PHASE3_CURRENT_SENSE_MA_L 0x4E //78 // RESERVED +#define REG_CAL_PHASE3_CURRENT_SENSE_MA_H 0x4F //79 // RESERVED +#define REG_CAL_PHASE3_CURRENT_SENSE_OFFSET_L 0x50 //80 // RESERVED +#define REG_CAL_PHASE3_CURRENT_SENSE_OFFSET_H 0x51 //81 // RESERVED -#define REG_CAL_VOLTAGE_SENSOR_L 0x52 //82 -#define REG_CAL_VOLTAGE_SENSOR_H 0x53 //83 +#define REG_CAL_VOLTAGE_SENSOR_L 0x52 //82 // RESERVED +#define REG_CAL_VOLTAGE_SENSOR_H 0x53 //83 // RESERVED #define REG_EWMA_ENCODER 0x54 //84 @@ -117,11 +117,11 @@ extern "C" { #define REG_GOAL_TORQUE_CURRENT_MA_H 0x88 //8 #define REG_GOAL_FLUX_CURRENT_MA_L 0x89 //9 #define REG_GOAL_FLUX_CURRENT_MA_H 0x8A //10 -#define REG_GOAL_KP 0x8B //11 -#define REG_GOAL_KD 0x8C //12 -#define REG_GOAL_SYNCHRO_OFFSET_L 0x8D //13 -#define REG_GOAL_SYNCHRO_OFFSET_H 0x8E //14 -#define REG_GOAL_CLOSED_LOOP 0x8F //15 +#define REG_GOAL_POS_KP 0x8B //11 +#define REG_GOAL_POS_KD 0x8C //12 +#define REG_GOAL_VEL_KP 0x8D //13 +#define REG_GOAL_SYNCHRO_OFFSET_L 0x8E //14 +#define REG_GOAL_SYNCHRO_OFFSET_H 0x8F //15 #define REG_PRESENT_POSITION_DEG_L 0x90 //16 #define REG_PRESENT_POSITION_DEG_H 0x91 //17 @@ -148,6 +148,7 @@ extern "C" { #define REG_PROCESSING_TIME 0xAA //42 #define REG_FOC_FREQUENCY 0xAB //43 #define REG_PID_FREQUENCY 0xAC //44 +#define REG_MLP_FREQUENCY 0xAD //45 // SW & HW ERROR #define REG_PROTOCOL_CRC_FAIL 0xB0 //48 @@ -158,26 +159,26 @@ extern "C" { // REGISTER FACTORY DEFAULT VALUES //////////////////////////////////////////// -#define REG_MODEL_NUMBER_VALUE 92 -#define REG_VERSION_VALUE 0 -#define REG_ID_VALUE 1 +#define REG_MODEL_NUMBER_VALUE 92 // +#define REG_VERSION_VALUE 30 // FOC V3.0 +#define REG_ID_VALUE 1 // Default ID #define REG_BAUD_RATE_VALUE 3 // 1: 1Mbps #define REG_RETURN_DELAY_VALUE 0 // 0: zero delay -#define REG_MIN_POSITION_DEG_VALUE 0 // deg -#define REG_MAX_POSITION_DEG_VALUE 1000 // deg -#define REG_MAX_VELOCITY_DPS_VALUE 10000 // dps -#define REG_MAX_ACCELERATION_DPSS_VALUE 32000 // dpss -#define REG_MAX_CURRENT_MA_VALUE 3500 // mA +#define REG_MIN_POSITION_DEG_VALUE -800 // deg +#define REG_MAX_POSITION_DEG_VALUE 800 // deg +#define REG_MAX_VELOCITY_DPS_VALUE 16000 // dps +#define REG_MAX_ACCELERATION_DPSS_VALUE 0 // dpss +#define REG_MAX_CURRENT_MA_VALUE 26000 // mA -#define REG_TEMPERATURE_LIMIT_VALUE 60 // °C -#define REG_LOW_VOLTAGE_LIMIT_VALUE 6 // V +#define REG_TEMPERATURE_LIMIT_VALUE 70 // °C +#define REG_LOW_VOLTAGE_LIMIT_VALUE 10 // V #define REG_HIGH_VOLTAGE_LIMIT_VALUE 28 // V #define REG_MOVING_THRESHOLD_DPS_VALUE 5 // dps -#define REG_STATUS_RETURN_LVL_VALUE 2 // TODO : fill comment -#define REG_ALARM_LED_VALUE 36 // TODO : fill comment -#define REG_ALARM_SHUTDOWN_VALUE 36 // TODO : fill comment +#define REG_STATUS_RETURN_LVL_VALUE 0 // TODO : fill comment +#define REG_ALARM_LED_VALUE 0 // TODO : fill comment +#define REG_ALARM_SHUTDOWN_VALUE 0 // TODO : fill comment #define REG_ENCODER_BITS_VALUE 14 // #define REG_MOTOR_POLE_PAIRS_VALUE 14 // @@ -185,51 +186,44 @@ extern "C" { #define REG_INV_PHASE_VALUE 0 // 0: NORMAL 1: INV #define REG_FIELD_WEAKENING_K_VALUE 255 // (1/)255 is minimum value, recommended around (1/)5 -#define REG_PID_POSITION_KP_VALUE 30 // k x1 -#define REG_PID_POSITION_KI_VALUE 0 // k x100 -#define REG_PID_POSITION_KD_VALUE 100 // k x1 +#define REG_PID_POSITION_KP_VALUE 0 // k x1 +#define REG_PID_POSITION_KI_VALUE 0 // k x100 +#define REG_PID_POSITION_KD_VALUE 0 // k x1 -#define REG_PID_VELOCITY_KP_VALUE 0 // k x1000 -#define REG_PID_VELOCITY_KI_VALUE 0 // k x100000 -#define REG_PID_VELOCITY_KD_VALUE 0 // k x1000 -#define REG_PID_VELOCITY_KFF_VALUE 0 // k x1000 +#define REG_PID_VELOCITY_KP_VALUE 0 // k x1000 +#define REG_PID_VELOCITY_KI_VALUE 0 // k x100000 +#define REG_PID_VELOCITY_KD_VALUE 0 // k x1000 +#define REG_PID_VELOCITY_KFF_VALUE 0 // k x1000 #define REG_PID_ACCELERATION_KFF_VALUE 0 // k x1000 -#define REG_PID_FLUX_CURRENT_KP_VALUE 2000 // k x100000 -#define REG_PID_FLUX_CURRENT_KI_VALUE 0 // k x10000000 +#define REG_PID_FLUX_CURRENT_KP_VALUE 10 // k x100000 +#define REG_PID_FLUX_CURRENT_KI_VALUE 10 // k x10000000 #define REG_PID_FLUX_CURRENT_KFF_VALUE 0 // k x100000 -#define REG_PID_TORQUE_CURRENT_KP_VALUE 2000 // k x100000 -#define REG_PID_TORQUE_CURRENT_KI_VALUE 0 // k x10000000 +#define REG_PID_TORQUE_CURRENT_KP_VALUE 10 // k x100000 +#define REG_PID_TORQUE_CURRENT_KI_VALUE 10 // k x10000000 #define REG_PID_TORQUE_CURRENT_KFF_VALUE 0 // k x100000 -#define REG_CAL_PHASE1_CURRENT_SENSE_MA_VALUE 280 // mA x1000 -#define REG_CAL_PHASE2_CURRENT_SENSE_MA_VALUE 280 // mA x1000 -#define REG_CAL_PHASE3_CURRENT_SENSE_MA_VALUE 280 // mA x1000 +#define REG_CAL_PHASE1_CURRENT_SENSE_MA_VALUE 29 // mA x1000 +#define REG_CAL_PHASE2_CURRENT_SENSE_MA_VALUE 29 // mA x1000 +#define REG_CAL_PHASE3_CURRENT_SENSE_MA_VALUE 29 // mA x1000 -#define REG_CAL_PHASE1_CURRENT_SENSE_OFFSET_VALUE 2464 // ADC -#define REG_CAL_PHASE2_CURRENT_SENSE_OFFSET_VALUE 2482 // ADC -#define REG_CAL_PHASE3_CURRENT_SENSE_OFFSET_VALUE 2485 // ADC +#define REG_CAL_PHASE1_CURRENT_SENSE_OFFSET_VALUE 2500 // ADC +#define REG_CAL_PHASE2_CURRENT_SENSE_OFFSET_VALUE 2500 // ADC +#define REG_CAL_PHASE3_CURRENT_SENSE_OFFSET_VALUE 2500 // ADC #define REG_CAL_VOLTAGE_SENSOR_VALUE 1000 // V #define REG_EWMA_ENCODER_VALUE 255 -// REGISTER CONTROL MODE VALUES ////////////////////////////////////////////////////// - -#define REG_CONTROL_MODE_IDLE 0 -#define REG_CONTROL_MODE_POSITION_VELOCITY_TORQUE 1 -// #define REG_CONTROL_MODE_POSITION_VELOCITY_TORQUE_VELOCITY_PROFIL 1 // Deprecated -// #define REG_CONTROL_MODE_VELOCITY_TORQUE 2 // Deprecated -// #define REG_CONTROL_MODE_TORQUE 3 // Deprecated -// #define REG_CONTROL_MODE_VELOCITY_TORQUE_OPEN_LOOP 4 // Deprecated - // REGISTER ERROR VALUES ////////////////////////////////////////////////////// #define HW_ERROR_BIT_VOLTAGE 0 #define HW_ERROR_BIT_POSITION_SENSOR_STATUS_ERROR 1 #define HW_ERROR_BIT_POSITION_SENSOR_NOT_RESPONDING 2 +#define HW_ERROR_BIT_POSITION_SENSOR_TIMESTAMP 3 +#define HW_ERROR_BIT_FOC_TIMEOUT 4 #define HW_ERROR_BIT_OVERLOAD 5 #define HW_ERROR_BIT_OVERHEATING 6 diff --git a/01-Firmware/B-G431B-ESC1-v2/Core/Inc/cordic.h b/01-Firmware/B-G431B-ESC1-v2/Core/Inc/cordic.h index 372c84d..ef33fa8 100644 --- a/01-Firmware/B-G431B-ESC1-v2/Core/Inc/cordic.h +++ b/01-Firmware/B-G431B-ESC1-v2/Core/Inc/cordic.h @@ -13,27 +13,35 @@ extern "C" { #endif #include "stm32g4xx_hal.h" +#include "math_tool.h" + #include -#define TWO_TO_POW_31 2147483648.0f -#define Q31_TO_FLOAT(x) ( (float)(x)/(TWO_TO_POW_31)) -#define FLOAT_TO_Q31(x) ( (int)( (float)(x)*(float)0x7FFFFFFF ) ) +// CORDIC +extern CORDIC_HandleTypeDef hcordic; + +// source : MJBot https://github.com/mjbots/moteus/blob/main/fw/math.h +int32_t RadiansToQ31(float) __attribute__((always_inline)); -float WRAP_TO_PI(float angle_radians) +inline int32_t RadiansToQ31(float x) { - if (angle_radians>=0) - return fmodf(angle_radians+M_PI, 2.0f*M_PI)-M_PI; - else - return fmodf(angle_radians-M_PI, 2.0f*M_PI)+M_PI; + // First we scale, then wrap, and finally convert out. + const float scaled = x / M_2PI; + // Now we wrap to be from 0 to 1. + const int32_t i = (int32_t)(scaled); + float mod = scaled - i; + if (mod < 0) { mod += 1.0f; } + + return (int32_t)(((mod > 0.5f) ? (mod - 1.0f) : mod) * 4294967296.0f); } -int32_t FLOAT_RADIANS_TO_Q31(float angle_radians) // Q31 have a scaled input with the range [-1 1] mapping to [-pi pi). +float Q31ToRadians(int32_t) __attribute__((always_inline)); + +inline float Q31ToRadians(int32_t x) { - return FLOAT_TO_Q31(WRAP_TO_PI(angle_radians)/M_PI); + return (float)(x)/2147483648.0f; } -extern CORDIC_HandleTypeDef hcordic; - HAL_StatusTypeDef API_CORDIC_Processor_Init() { CORDIC_ConfigTypeDef config = { @@ -43,25 +51,30 @@ HAL_StatusTypeDef API_CORDIC_Processor_Init() CORDIC_OUTSIZE_32BITS, // q31 CORDIC_NBWRITE_1, // ARG2 is 1 default CORDIC_NBREAD_2, // read cosine and sine - CORDIC_PRECISION_6CYCLES // better than 10-3 + CORDIC_PRECISION_5CYCLES // better than 10-3 }; return HAL_CORDIC_Configure(&hcordic, &config); } -HAL_StatusTypeDef API_CORDIC_Processor_Update(float theta, float * c, float * s); // __attribute__((section (".ccmram"))); +HAL_StatusTypeDef API_CORDIC_Processor_Update(float theta_rad, float * c, float * s) __attribute__((always_inline)); -HAL_StatusTypeDef API_CORDIC_Processor_Update(float theta, float * c, float * s) +inline HAL_StatusTypeDef API_CORDIC_Processor_Update(float theta_rad, float * c, float * s) { - static int32_t InBuff[1] = {0}; - static int32_t OutBuff[2] = {0,0}; - InBuff[0] = FLOAT_RADIANS_TO_Q31(theta); - HAL_StatusTypeDef result = HAL_CORDIC_Calculate(&hcordic,InBuff,OutBuff,1,10); + int32_t InBuff = RadiansToQ31(theta_rad); + int32_t OutBuff[2] = {0,0}; + HAL_StatusTypeDef result = HAL_CORDIC_Calculate(&hcordic,&InBuff,OutBuff,1,0); if(HAL_OK==result) { - if(c!=0) - *c = Q31_TO_FLOAT(OutBuff[0]); - if(s!=0) - *s = Q31_TO_FLOAT(OutBuff[1]); + *c = Q31ToRadians(OutBuff[0]); + *s = Q31ToRadians(OutBuff[1]); + } + else + { + //regs[REG_PROTOCOL_CRC_FAIL]++; // DEBUG + // TODO hardware error ! + // TODO hardware error ! + // TODO hardware error ! + // TODO hardware error ! } return result; } diff --git a/01-Firmware/B-G431B-ESC1-v2/Core/Inc/foc.h b/01-Firmware/B-G431B-ESC1-v2/Core/Inc/foc.h index 34d8355..c5ab735 100644 --- a/01-Firmware/B-G431B-ESC1-v2/Core/Inc/foc.h +++ b/01-Firmware/B-G431B-ESC1-v2/Core/Inc/foc.h @@ -18,30 +18,13 @@ void API_FOC_Init(); int API_FOC_Calibrate(); -// low priority low frequency process -void API_FOC_Service_Update(); - -// high priority high frequency process -void API_FOC_Torque_Update( - uint16_t present_time_us, - float setpoint_torque_current_mA, - float setpoint_flux_current_mA, - float phase_synchro_offset_rad, // for manual adjustment during FOC execution - uint32_t closed_loop, // 0:open loop 1:closed loop - float setpoint_velocity_dps -); // __attribute__((section (".ccmram"))); - -void API_FOC_Set_Flux_Angle( - float setpoint_electrical_angle_rad, - float setpoint_flux_voltage_V -); - -void API_FOC_Set_Flux_Velocity( - uint16_t present_time_us, - float setpoint_electrical_velocity_dps, - float setpoint_flux_voltage_V -); +void API_FOC_Torque_Enable(); +void API_FOC_Torque_Disable(); +void API_FOC_Set_Torque_Flux_Currents_mA(float Iq_mA, float Id_mA); // Setpoint Iq & Id + +uint32_t API_FOC_Get_Timestamp_ms(); +uint16_t API_FOC_Get_Timestamp_us(); float API_FOC_Get_Present_Torque_Current(); float API_FOC_Get_Present_Flux_Current(); float API_FOC_Get_Present_Voltage(); @@ -49,7 +32,9 @@ float API_FOC_Get_Present_Temp(); float API_FOC_Get_Processing_Time(); float API_FOC_Get_Processing_Frequency(); -void API_FOC_It(ADC_HandleTypeDef *hadc);// __attribute__((section (".ccmram"))); +// low priority low frequency process, called by main loop() +void API_FOC_Service_Update() __attribute__((section (".ccmram"))); + #ifdef __cplusplus } diff --git a/01-Firmware/B-G431B-ESC1-v2/Core/Inc/foc_utils.h b/01-Firmware/B-G431B-ESC1-v2/Core/Inc/foc_utils.h new file mode 100644 index 0000000..d4add46 --- /dev/null +++ b/01-Firmware/B-G431B-ESC1-v2/Core/Inc/foc_utils.h @@ -0,0 +1,93 @@ +/* + * foc_utils.h + * + * Created on: 13 janv. 2022 + * Author: Patrick + */ + +#ifndef INC_FOC_UTILS_H_ +#define INC_FOC_UTILS_H_ + +#include "math_tool.h" +#include "control_table.h" + +// hard-coded settings +#define MAX_PWM_DUTY_CYCLE 0.94f // % +#define MIN_PWM_DUTY_CYCLE 0.06f // % + // with a PWM frequency (20Khz and more), deadtime must be taken in account. + // CUBEMX configuration : deadtime = 128 at f=160MHz ==> deadtime = 800ns + // MIN/MAX DUTY CYCLE is set in order to allow current sense when TIM1 update event triggered (800ns is about 2% PWM at 20KHz) + // MIN/MAX DUTY CYCLE is set in order to allow current sense when TIM1 update event triggered (800ns is about 4% PWM at 40KHz) + +// peripherals +extern TIM_HandleTypeDef htim1; + +#ifdef __cplusplus +extern "C" { +#endif + +// low level function +// this function checks REG_HARDWARE_ERROR_STATUS register and enforce BRAKE if register is not zero +// this function checks REG_CONTROL_MODE register and enforce BRAKE if control mode is zero +void LL_FOC_set_phase_voltage( float Vd, float Vq, float cosine_theta, float sine_theta, float present_voltage_V ) __attribute__((always_inline)); + +inline void LL_FOC_set_phase_voltage( float Vd, float Vq, float cosine_theta, float sine_theta, float present_voltage_V ) +{ + // Inverse Park Transformation + float const Valpha = Vd * cosine_theta - Vq * sine_theta; + float const Vbeta = Vq * cosine_theta + Vd * sine_theta; + // Inverse Clarke Transformation + float const Va = Valpha; + float const Vb = ( -Valpha + SQRT3 * Vbeta ) * 0.5f; + float const Vc = ( -Valpha - SQRT3 * Vbeta ) * 0.5f; + // apply CSVPWM to (Va,Vb,Vc) + float const Vneutral = 0.5f*(fmaxf(fmaxf(Va,Vb),Vc)+fminf(fminf(Va,Vb),Vc)); + // convert (Va,Vb,Vc) [-max_voltage_V/2,max_voltage_V/2] to PWM duty cycles % [MIN_PWM_DUTY_CYCLE MAX_PWM_DUTY_CYCLE] + float const duty_cycle_PWMa = fconstrain((Va-Vneutral)/present_voltage_V+0.5f,MIN_PWM_DUTY_CYCLE,MAX_PWM_DUTY_CYCLE); + float const duty_cycle_PWMb = fconstrain((Vb-Vneutral)/present_voltage_V+0.5f,MIN_PWM_DUTY_CYCLE,MAX_PWM_DUTY_CYCLE); + float const duty_cycle_PWMc = fconstrain((Vc-Vneutral)/present_voltage_V+0.5f,MIN_PWM_DUTY_CYCLE,MAX_PWM_DUTY_CYCLE); + // update TIMER CCR registers and apply BRAKE in case of hardware failure or torque disable + if( regs[REG_HARDWARE_ERROR_STATUS] != 0 ) // fail-safe + { + // compute a valid BRAKE value + uint16_t const CCRx = (uint16_t)(0.5f*(float)(__HAL_TIM_GET_AUTORELOAD(&htim1)+1))-1; + __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,CCRx); + __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2,CCRx); + __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_3,CCRx); + } + else + { + // convert PWM duty cycles % to TIMER1 CCR register values + // fTIM = 160MHz + // in PWM centered mode, for the finest possible resolution : + // fPWM = 22KHz ==> ARR = fTIM/(2 * fPWM) -1 => ARR = 3635 + // fPWM = 25KHz ==> ARR = fTIM/(2 * fPWM) -1 => ARR = 3199 + // fPWM = 32KHz ==> ARR = fTIM/(2 * fPWM) -1 => ARR = 2499 + // fPWM = 40KHz ==> ARR = fTIM/(2 * fPWM) -1 => ARR = 1999 + uint16_t const CCRa = (uint16_t)(duty_cycle_PWMa*(float)(__HAL_TIM_GET_AUTORELOAD(&htim1)+1))-1; + uint16_t const CCRb = (uint16_t)(duty_cycle_PWMb*(float)(__HAL_TIM_GET_AUTORELOAD(&htim1)+1))-1; + uint16_t const CCRc = (uint16_t)(duty_cycle_PWMc*(float)(__HAL_TIM_GET_AUTORELOAD(&htim1)+1))-1; + __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,CCRa); + __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2,CCRb); + __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_3,CCRc); + } +} + +void LL_FOC_brake() __attribute__((always_inline)); + +inline void LL_FOC_brake() +{ + // compute a valid BRAKE value + uint16_t const CCRx = (uint16_t)(0.5f*(float)(__HAL_TIM_GET_AUTORELOAD(&htim1)+1))-1; + __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,CCRx); + __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2,CCRx); + __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_3,CCRx); +} + + +#ifdef __cplusplus +} +#endif + + +#endif /* INC_FOC_UTILS_H_ */ diff --git a/01-Firmware/B-G431B-ESC1-v2/Core/Inc/main.h b/01-Firmware/B-G431B-ESC1-v2/Core/Inc/main.h index ebd4ebd..829382e 100644 --- a/01-Firmware/B-G431B-ESC1-v2/Core/Inc/main.h +++ b/01-Firmware/B-G431B-ESC1-v2/Core/Inc/main.h @@ -7,7 +7,7 @@ ****************************************************************************** * @attention * - *

© Copyright (c) 2021 STMicroelectronics. + *

© Copyright (c) 2022 STMicroelectronics. * All rights reserved.

* * This software component is licensed by ST under BSD 3-Clause license, diff --git a/01-Firmware/B-G431B-ESC1-v2/Core/Inc/math_tool.h b/01-Firmware/B-G431B-ESC1-v2/Core/Inc/math_tool.h index 95f16d3..63cf75c 100644 --- a/01-Firmware/B-G431B-ESC1-v2/Core/Inc/math_tool.h +++ b/01-Firmware/B-G431B-ESC1-v2/Core/Inc/math_tool.h @@ -14,21 +14,23 @@ extern "C" { #endif -#include +#include -int32_t constrain(int32_t x, int32_t min, int32_t max); -float fconstrain(float x, float min, float max); -float fconstrain_both(float x, float abs); // __attribute__((section (".ccmram"))); -uint32_t map(uint32_t x, uint32_t in_min, uint32_t in_max, uint32_t out_min, uint32_t out_max); -float fmap(float x, float in_min, float in_max, float out_min, float out_max); +#define M_2PI (6.283185307179586f) +#define M_3PI_2 (4.7123889803846f) -#define M_2PI (2.0f*M_PI) -#define M_3PI_2 (3.0f*M_PI_2) -#define RADIANS_TO_DEGREES(rad) ((rad)*180.0f/M_PI) -#define DEGREES_TO_RADIANS(deg) ((deg)*M_PI/180.0f) +#define INV_SQRT3 (0.5773502691896257f) +#define SQRT3 (1.7320508075688772f) + +float fconstrain(float x, float min, float max) __attribute__((section (".ccmram"))); +float mfmod(float x,float y) __attribute__((section (".ccmram"))); + +#define RADIANS_TO_DEGREES(rad) ((rad)*57.2957795130823208767981548f) +#define DEGREES_TO_RADIANS(deg) ((deg)*0.01745329251994329576923690f) // normalizing radian angle to [0,2PI] -float normalize_angle(float angle_rad); +float normalize_angle(float angle_rad) __attribute__((section (".ccmram"))); +float difference_angle(float a, float b) __attribute__((section (".ccmram"))); #ifdef __cplusplus } diff --git a/01-Firmware/B-G431B-ESC1-v2/Core/Inc/pid.h b/01-Firmware/B-G431B-ESC1-v2/Core/Inc/pid.h new file mode 100644 index 0000000..407afd5 --- /dev/null +++ b/01-Firmware/B-G431B-ESC1-v2/Core/Inc/pid.h @@ -0,0 +1,75 @@ +/* + * pid.h + * + * Created on: 16 nov. 2020 + * Author: Patrick + */ + +#ifndef INC_PID_H_ +#define INC_PID_H_ + +#include "stm32g4xx_hal.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define PID_INTEGRAL_DEPTH 100 + +typedef struct{ + float err_last_one; + float err_integral; + float derivative_filtered; + +} pid_context_t; + +void pid_reset( pid_context_t * ctx ); + +// PI faster than PID+FF ==> gain ~1µs +float pi_process_antiwindup_clamp( + pid_context_t * ctx, + float error, + float kp, + float ki, + float output_limit +) __attribute__((section (".ccmram"))); + +float pid_process_antiwindup_clamp_with_ff( + pid_context_t * ctx, + float error, + float kp, + float ki, + float kd, + float output_limit, + float alpha_derivative, + float feed_forward +) __attribute__((section (".ccmram"))); + + +float pid_process_antiwindup_clamp( + pid_context_t * ctx, + float error, + float kp, + float ki, + float kd, + float output_limit, + float alpha_derivative +); + +float pid_process_antiwindup_back_calculation( + pid_context_t * ctx, + float error, + float kp, + float ki, + float kd, + float output_limit, + float kt, + float alpha_derivative +); + + +#ifdef __cplusplus +} +#endif + +#endif /* INC_PID_H_ */ diff --git a/01-Firmware/B-G431B-ESC1-v2/Core/Inc/position_sensor.h b/01-Firmware/B-G431B-ESC1-v2/Core/Inc/position_sensor.h index 97b0e02..c7ce3d7 100644 --- a/01-Firmware/B-G431B-ESC1-v2/Core/Inc/position_sensor.h +++ b/01-Firmware/B-G431B-ESC1-v2/Core/Inc/position_sensor.h @@ -18,7 +18,7 @@ typedef enum } e_sensor_type; int positionSensor_init(e_sensor_type sensor_type); -float positionSensor_getRadiansEstimation(uint16_t time_us); +float positionSensor_getRadiansEstimation(uint16_t time_us) __attribute__((section (".ccmram")));; void positionSensor_update(void); float positionSensor_getRadians(void); float positionSensor_getRadiansMultiturn(void); @@ -26,5 +26,7 @@ float positionSensor_getDegree(void); float positionSensor_getDegreeMultiturn(void); float positionSensor_getVelocityDegree(void); e_sensor_type positionSensor_getType(void); +uint16_t positionSensor_getDeltaTimestamp(); +int16_t positionSensor_getDeltaTimeEstimation(); #endif /* INC_POSITION_SENSOR_H_ */ diff --git a/01-Firmware/B-G431B-ESC1-v2/Core/Inc/stm32g4xx_it.h b/01-Firmware/B-G431B-ESC1-v2/Core/Inc/stm32g4xx_it.h index a671ec0..461e247 100644 --- a/01-Firmware/B-G431B-ESC1-v2/Core/Inc/stm32g4xx_it.h +++ b/01-Firmware/B-G431B-ESC1-v2/Core/Inc/stm32g4xx_it.h @@ -6,7 +6,7 @@ ****************************************************************************** * @attention * - *

© Copyright (c) 2021 STMicroelectronics. + *

© Copyright (c) 2022 STMicroelectronics. * All rights reserved.

* * This software component is licensed by ST under BSD 3-Clause license, @@ -61,6 +61,7 @@ void DMA1_Channel2_IRQHandler(void); void DMA1_Channel3_IRQHandler(void); void DMA1_Channel4_IRQHandler(void); void ADC1_2_IRQHandler(void); +void FDCAN1_IT0_IRQHandler(void); void TIM4_IRQHandler(void); void USART2_IRQHandler(void); void CORDIC_IRQHandler(void); diff --git a/01-Firmware/B-G431B-ESC1-v2/Core/Src/as5048a.c b/01-Firmware/B-G431B-ESC1-v2/Core/Src/as5048a.c index 640ca84..fb187cf 100644 --- a/01-Firmware/B-G431B-ESC1-v2/Core/Src/as5048a.c +++ b/01-Firmware/B-G431B-ESC1-v2/Core/Src/as5048a.c @@ -11,25 +11,29 @@ #include "control_table.h" #include "serial.h" + // serial communication (UART2) for TRACEs // TODO : use STM32 CUBE MONITOR extern HAL_Serial_Handler serial; // µs TIMER extern TIM_HandleTypeDef htim6; + // PWM IC TIMER static TIM_HandleTypeDef * position_sensor_htim = 0; static uint32_t calls = 0; + // position state static uint32_t position_sensor_error = 0; static uint32_t position_sensor_error_counter = 0; static uint16_t present_time_us = 0; static float present_position_rad = 0.0f; +static int16_t delta_t_us = 0; static float delta_position_rad = 0.0f; static float const bit_to_radians_ratio = M_2PI/4096.0f; static float const max_radians = M_2PI/4096.0f*4095.0f; // velocity state -static uint16_t position_delta_time_us = 0; +static int16_t position_delta_time_us = 0; static uint16_t last_position_time_us = 0; static float last_position_rad = 0.0f; static float present_velocity_rad = 0.0f; @@ -37,7 +41,7 @@ static float present_velocity_rad = 0.0f; static int32_t present_revolution = 0; static float present_position_multi_rad = 0.0f; -#define ALPHA_VELOCITY 0.01f // 0.1f default +#define ALPHA_VELOCITY 0.25f // 0.25f default void API_AS5048A_Position_Sensor_Init(TIM_HandleTypeDef * htim) { @@ -62,7 +66,7 @@ void API_AS5048A_Position_Sensor_It(TIM_HandleTypeDef *htim) // when position is 0°, length is 16 bits // when position is MAX = 2*PI*(1-1/4096)°, length is 16+4095 bits // compute PWM width / PWM period * 4119bits that gives the number of 1 bits - // @150MHz, CHANNEL1 = period = 45500 with PSC=3 + // @160MHz, CHANNEL1 = period = 53333 with PSC=2(+1) float const init_error_data_bits = 4119.0f*(float)__HAL_TIM_GET_COMPARE(position_sensor_htim,TIM_CHANNEL_2)/(float)__HAL_TIM_GET_COMPARE(position_sensor_htim,TIM_CHANNEL_1); // if data < 0 bits ==> must be an error if(init_error_data_bits<(16.0f-0.8f)) // add a 0.8 margin due to IC TIMER PRECISION and PWM precision @@ -82,11 +86,40 @@ void API_AS5048A_Position_Sensor_It(TIM_HandleTypeDef *htim) // reset error position_sensor_error = 0; // compute new position in radians and constrain it to [0..2pi[ - present_position_rad = roundf(init_error_data_bits-16.0f)*bit_to_radians_ratio; + present_position_rad = ((init_error_data_bits-16.0f))*bit_to_radians_ratio; + // limit [0,2PI[ if(present_position_rad<0.0f) present_position_rad=0.0f; if(present_position_rad>max_radians) present_position_rad=max_radians; + // delay since the last position + position_delta_time_us = (int16_t)(present_time_us-last_position_time_us); + + // AS5048A specific + // AS5048A specific + // AS5048A specific + // there is a zero crossing problem with AS5048A at high speed + // we have to filter the actual position from an arbitrary base velocity ~ 1 RPM + float const threshold_velocity_rds = M_2PI*1.0f; // Radians/s + + // compute the expected position according last position, the current velocity and the actual rate of position (~1ms) + float const expected_position_rad = normalize_angle(last_position_rad+present_velocity_rad*position_delta_time_us/1000000.0f); + + // actual velocity is > threshold velocity ==> apply filter on actual position + if(fabsf(present_velocity_rad)>threshold_velocity_rds) + { + // if actual position is near ZERO, use the expected position, ignore the actual position + if( present_position_rad < 0.12f ) + { + present_position_rad = expected_position_rad+0.25*difference_angle(present_position_rad,expected_position_rad); + } + // else ignore expected position, actual position is precise far from ZERO + } + + // AS5048A specific + // AS5048A specific + // AS5048A specific + // compute multi-turn position and velocity in radians delta_position_rad = present_position_rad-last_position_rad; if(delta_position_rad>M_PI) @@ -100,36 +133,38 @@ void API_AS5048A_Position_Sensor_It(TIM_HandleTypeDef *htim) delta_position_rad+=M_2PI; } present_position_multi_rad = present_position_rad+(float)present_revolution*M_2PI; + // compute velocity - position_delta_time_us = present_time_us-last_position_time_us; - float const alpha_vel = (float)(regs[REG_EWMA_ENCODER]+1)/2560.0f; // 255 => B=0.1, 1 => beta = 0.0004 - present_velocity_rad = - alpha_vel * (delta_position_rad / (float)position_delta_time_us * 1000000.0f) - + (1.0f-alpha_vel) * present_velocity_rad; + present_velocity_rad = ALPHA_VELOCITY * (delta_position_rad / (float)position_delta_time_us * 1000000.0f) + (1.0f-ALPHA_VELOCITY) * present_velocity_rad; + + // save last position last_position_time_us = present_time_us; last_position_rad = present_position_rad; } + } } float API_AS5048A_Position_Sensor_Get_Radians_Estimation(uint16_t time_us) { - uint16_t delta_t_us = time_us-present_time_us; + float result = 0.0f; + delta_t_us = (int16_t)(time_us-present_time_us); // position has been received during FOC algorithm execution - if(delta_t_us>65500) + if(delta_t_us<0) // should never happend because of NVIC priority (TIM4 priority lower than ADC DMA priority) { - // return current position - return present_position_rad; + // set encoder error + regs[REG_HARDWARE_ERROR_STATUS] |= 1UL << HW_ERROR_BIT_POSITION_SENSOR_TIMESTAMP; + // return error + result = 0.0f; // force ZERO } // check old sample error - else if(delta_t_us>2000) //2ms + else if(delta_t_us>1200) //1.2ms { // set encoder error regs[REG_HARDWARE_ERROR_STATUS] |= 1UL << HW_ERROR_BIT_POSITION_SENSOR_NOT_RESPONDING; - //HAL_Serial_Print(&serial,"%d %d (%d)\n",(int)time_us,(int)present_time_us, (int)delta_t_us); - // return current position (what ever) - return present_position_rad; + // return error + result = 0.0f; // force ZERO } // normal else @@ -137,9 +172,9 @@ float API_AS5048A_Position_Sensor_Get_Radians_Estimation(uint16_t time_us) // clear encoder error regs[REG_HARDWARE_ERROR_STATUS] &= ~(1UL << HW_ERROR_BIT_POSITION_SENSOR_NOT_RESPONDING); // compute estimation - return present_position_rad + present_velocity_rad*(float)(delta_t_us)/1000000.0f; + result = present_position_rad + present_velocity_rad*((float)delta_t_us+(float)position_delta_time_us)/1000000.0f; } - + return result; } float API_AS5048A_Position_Sensor_Get_Radians() @@ -172,6 +207,11 @@ uint16_t API_AS5048A_Position_Sensor_Get_DeltaTimestamp() return position_delta_time_us; } +int16_t API_AS5048A_Position_Sensor_Get_DeltaTimeEstimation() +{ + return delta_t_us; +} + uint32_t API_AS5048A_Position_Sensor_Error() { return position_sensor_error; diff --git a/01-Firmware/B-G431B-ESC1-v2/Core/Src/control_table.c b/01-Firmware/B-G431B-ESC1-v2/Core/Src/control_table.c index 0a7d289..09e9f93 100644 --- a/01-Firmware/B-G431B-ESC1-v2/Core/Src/control_table.c +++ b/01-Firmware/B-G431B-ESC1-v2/Core/Src/control_table.c @@ -119,6 +119,5 @@ void store_eeprom_regs() void reset_ram_regs() { memset(®s[REG_TORQUE_ENABLE],0,REG_MAX-REG_TORQUE_ENABLE); - regs[REG_GOAL_CLOSED_LOOP] = 1; // CLOSED LOOP } diff --git a/01-Firmware/B-G431B-ESC1-v2/Core/Src/cordic.c b/01-Firmware/B-G431B-ESC1-v2/Core/Src/cordic.c new file mode 100644 index 0000000..fdc8d46 --- /dev/null +++ b/01-Firmware/B-G431B-ESC1-v2/Core/Src/cordic.c @@ -0,0 +1,9 @@ +/* + * cordic.c + * + * Created on: 14 janv. 2022 + * Author: Patrick + */ + + + diff --git a/01-Firmware/B-G431B-ESC1-v2/Core/Src/foc.c b/01-Firmware/B-G431B-ESC1-v2/Core/Src/foc.c index 86a82e3..43d5d97 100644 --- a/01-Firmware/B-G431B-ESC1-v2/Core/Src/foc.c +++ b/01-Firmware/B-G431B-ESC1-v2/Core/Src/foc.c @@ -9,25 +9,22 @@ /// DOC SVM https://www.embedded.com/painless-mcu-implementation-of-space-vector-modulation-for-electric-motor-systems/ #include "foc.h" +#include "foc_utils.h" #include "cordic.h" #include "serial.h" #include "position_sensor.h" #include "math_tool.h" #include "control_table.h" #include "binary_tool.h" +#include "pid.h" #include #include // hard-coded settings -#define ALPHA_CURRENT_DQ 0.05f // low pass filter for present Id and presetn Iq estimation #define ALPHA_CURRENT_SENSE_OFFSET 0.001f // low pass filter for calibrating the phase current ADC offset (automatically) -#define MAX_PWM_DUTY_CYCLE 0.98f // % -#define MIN_PWM_DUTY_CYCLE 0.02f // % -#define CSVPWM // uncomment to use CSVPWM (conventional space vector pulse width modulation), - // if commented default SPWM is used -// peripherals +// FOC peripherals extern TIM_HandleTypeDef htim1; extern TIM_HandleTypeDef htim4; extern TIM_HandleTypeDef htim6; @@ -41,40 +38,98 @@ extern OPAMP_HandleTypeDef hopamp3; // TODO : use STM32 CUBE MONITOR extern HAL_Serial_Handler serial; -// FOC period at PWM output = 16kHz (check TIMER1 ARR value = 4999 and timer frequency =160MHz) -static uint32_t const current_sample_drop_rate = 0; -// 3:250us cycle -// 2:187us cycle -// 1:125us cycle <- default (conservative, allows debbuging) -// 0: 62us cycle <- best possible (one FOC iteration takes about ~45us of processing time) +// high priority high interupt +// TIM1 => Update Event Trigger => CAN (x2) ==> DMA (x2) ==> FOC IT +void API_FOC_It(ADC_HandleTypeDef *hadc) __attribute__((section (".ccmram"))); -// FOC private variables +// high priority high frequency process called by IT +void API_FOC_Torque_Update() __attribute__((section (".ccmram"))); + +// ADC IT for motor current sense, and votlage/temperature monitoring +void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef *hadc) __attribute__((section (".ccmram"))); + +void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef *hadc) +{ + API_FOC_It(hadc); +} + +// FOC state variable +#define FOC_STATE_IDLE 0 // brake +#define FOC_STATE_TORQUE_CONTROL 1 // normal operation +#define FOC_STATE_FLUX_CONTROL 10 // calibration +static uint32_t foc_state = FOC_STATE_IDLE; +static uint16_t foc_timestamp_us = 0; +static uint32_t foc_timestamp_ms = 0; + +// FOC setpoints variables +static float setpoint_torque_current_mA = 0.0f; +static float setpoint_flux_current_mA = 0.0f; +static float setpoint_electrical_angle_rad = 0.0f; +static float setpoint_flux_voltage_V = 0.0f; + +// FOC variables +static float present_Ids_mA = 0.0; +static float present_Iqs_mA = 0.0f; +static pid_context_t flux_pi; +static pid_context_t torque_pi; +float theta_rad = 0.0f; // public // DEBUG +float absolute_position_rad = 0.0f; // public // DEBUG + +// FOC current sense +static float motor_current_mA[3] = {0.0f,0.0f,0.0f}; +static float motor_current_input_adc_offset[3] = {2464.0f,2482.0f,2485.0f}; +static float const motor_current_input_adc_KmA = -29.41f; // V/mA // note : the (-) sign here +// process phase current +// Note : when current flows inward phase, shunt voltage is negative +// Note : when current flows outward phase, shunt voltage is positive +// Note : The current sign is positive when flowing in to a phase +// Note : The current sign is negative when flowing out from a phase + +// FOC analog measure static int32_t current_samples = 0; volatile uint16_t ADC1_DMA[5] = { 0,0,0,0,0 }; // Dummy conversion (ST workaround for -x), volatile uint16_t ADC2_DMA[3] = { 0,0,0 }; // Dummy conversion (ST workaround for -x) -static uint16_t motor_current_input_adc[3] = {0.0f,0.0f,0.0f}; -static uint16_t motor_current_sample_adc[3] = {0.0f,0.0f,0.0f}; -static float motor_current_input_adc_offset[3] = {2464.0f,2482.0f,2485.0f}; // Patrick -//static float motor_current_input_adc_offset[3] = {2498.0f,2479.0f,2472.0f}; // KAI -static float motor_current_input_adc_mA[3] = {0.28f,0.28f,0.28f}; // 0.28f -static float motor_current_mA[3] = {0.0f,0.0f,0.0f}; -static float present_Id_filtered = 0.0f; -static float present_Iq_filtered = 0.0f; -// foc feedback -static float absolute_position_rad = 0.0f; -// foc analog measure -static float potentiometer_input_adc = 0.0f; +static float motor_current_input_adc[3] = {0.0f,0.0f,0.0f}; +float potentiometer_input_adc = 0.0f; // public static float vbus_input_adc = 0.0f; static float temperature_input_adc = 0.0f; static float present_voltage_V = 0.0f; static float present_temperature_C = 0.0f; -// foc performance monitoring (public) + +// FOC performance monitoring static float average_processing_time_us = 0.0f; static uint32_t foc_counter = 0; -void LL_FOC_Update_Temperature();// __attribute__((section (".ccmram"))); -void LL_FOC_Update_Voltage();// __attribute__((section (".ccmram"))); -void LL_FOC_Inverse_Clarke_Park_PWM_Generation( float Vd, float Vq, float cosine_theta, float sine_theta );// __attribute__((section (".ccmram"))); +void API_FOC_Torque_Enable() +{ + foc_state = FOC_STATE_TORQUE_CONTROL; + + setpoint_torque_current_mA = 0.0f; + setpoint_flux_current_mA = 0.0f; + setpoint_electrical_angle_rad = 0.0f; + setpoint_flux_voltage_V = 0.0f; + + present_Ids_mA = 0.0f; + present_Iqs_mA = 0.0f; + + pid_reset(&flux_pi); + pid_reset(&torque_pi); +} + +void API_FOC_Torque_Disable() +{ + foc_state = FOC_STATE_IDLE; + HAL_Delay(1); + + // enforce brake + LL_FOC_brake(); +} + +void API_FOC_Set_Torque_Flux_Currents_mA(float Iq_mA, float Id_mA) +{ + setpoint_torque_current_mA = Iq_mA; + setpoint_flux_current_mA = Id_mA; +} // user API function // this function reset state of FOC @@ -99,8 +154,12 @@ void API_FOC_Init() HAL_ADC_Start_DMA(&hadc2,(uint32_t*)ADC2_DMA,3); // CORDIC init API_CORDIC_Processor_Init(); + // disable FOC + API_FOC_Torque_Disable(); } +void LL_FOC_Update_Temperature() __attribute__((section (".ccmram"))); + // low level function // this function update present_temperature_C // this function update REG_HARDWARE_ERROR_STATUS register (set/reset HW_ERROR_BIT_OVERHEATING bit) @@ -127,13 +186,23 @@ void LL_FOC_Update_Temperature() regs[REG_HARDWARE_ERROR_STATUS] |= 1UL << HW_ERROR_BIT_OVERHEATING; //HAL_Serial_Print(&serial,"h"); } + else if( ((regs[REG_HARDWARE_ERROR_STATUS]&(1UL << HW_ERROR_BIT_OVERHEATING))!=0) ) // hard-coded hysteresis 12°C + { + if(present_temperature_C ARR = 4999 - uint16_t const CCRa = (uint16_t)(duty_cycle_PWMa*(float)(__HAL_TIM_GET_AUTORELOAD(&htim1)+1))-1; - uint16_t const CCRb = (uint16_t)(duty_cycle_PWMb*(float)(__HAL_TIM_GET_AUTORELOAD(&htim1)+1))-1; - uint16_t const CCRc = (uint16_t)(duty_cycle_PWMc*(float)(__HAL_TIM_GET_AUTORELOAD(&htim1)+1))-1; - - // update TIMER CCR registers - // and apply BRAKE if error - if(regs[REG_HARDWARE_ERROR_STATUS] != 0 ) - { - // compute a valid BRAKE value - uint16_t const CCRx = (uint16_t)(0.5f*(float)(__HAL_TIM_GET_AUTORELOAD(&htim1)+1))-1; // note : 0 is OK too - __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,CCRx); - __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2,CCRx); - __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_3,CCRx); - } - else - { - __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,CCRa); - __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2,CCRb); // switch b and c phases - __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_3,CCRc); // switch b and c phases - } -} - -// user API function -// this function process an open-loop FOC from electrical angle and voltage setpoints -void API_FOC_Set_Flux_Angle( - float setpoint_electrical_angle_rad, - float setpoint_flux_voltage_V -) -{ - // check temperature and voltage - LL_FOC_Update_Temperature(); - LL_FOC_Update_Voltage(); - - // compute theta - float const theta_rad = normalize_angle(setpoint_electrical_angle_rad); - - // compute cosine and sine - static float cosine_theta = 0.0f; - static float sine_theta = 0.0f; - API_CORDIC_Processor_Update(theta_rad,&cosine_theta,&sine_theta); - - // compute (Vd,Vq) [-max_voltage_V,max_voltage_V] - float const Vd = fconstrain(setpoint_flux_voltage_V,-regs[REG_HIGH_VOLTAGE_LIMIT_VALUE],regs[REG_HIGH_VOLTAGE_LIMIT_VALUE]); // torque setpoint open loop - float const Vq = 0.0f; // no torque - - // do inverse clarke and park transformation and update TIMER1 register (3-phase PWM generation) - LL_FOC_Inverse_Clarke_Park_PWM_Generation(Vd,Vq,cosine_theta,sine_theta); - // this function checks REG_HARDWARE_ERROR_STATUS register and enforce BRAKE is register not null - // this function use the present_voltage_V state variable to adjust PWM duty cycle according power supply voltage -} - -// user API function -// this function process an open-loop FOC from electrical velocity and voltage setpoints -void API_FOC_Set_Flux_Velocity( - uint16_t present_time_us, - float setpoint_electrical_velocity_dps, - float setpoint_flux_voltage_V -) -{ - // check temperature and voltage - LL_FOC_Update_Temperature(); - LL_FOC_Update_Voltage(); - - // compute theta - static float theta_rad = 0.0f; - static float last_time_us = 0.0f; - uint16_t delta_t_us = last_time_us-present_time_us; - last_time_us = present_time_us; - theta_rad += DEGREES_TO_RADIANS(setpoint_electrical_velocity_dps) * (float)delta_t_us/1000000.0f; - - // compute cosine and sine - static float cosine_theta = 0.0f; - static float sine_theta = 0.0f; - API_CORDIC_Processor_Update(theta_rad,&cosine_theta,&sine_theta); - - // compute (Vd,Vq) [-max_voltage_V,max_voltage_V] - float const Vd = fconstrain(setpoint_flux_voltage_V,-regs[REG_HIGH_VOLTAGE_LIMIT_VALUE],regs[REG_HIGH_VOLTAGE_LIMIT_VALUE]); // torque setpoint open loop - float const Vq = 0.0f; // no torque - - // do inverse clarke and park transformation and update TIMER1 register (3-phase PWM generation) - LL_FOC_Inverse_Clarke_Park_PWM_Generation(Vd,Vq,cosine_theta,sine_theta); - // this function checks REG_HARDWARE_ERROR_STATUS register and enforce BRAKE is register not null - // this function use the present_voltage_V state variable to adjust PWM duty cycle according power supply voltage -} // user API function // this function synchronize physical and electrical angles, set motor normal/reverse rotation, and check pole pairs // this function uses REG_MOTOR_POLE_PAIRS register int API_FOC_Calibrate() { + // change mode + foc_state = FOC_STATE_IDLE; + HAL_Delay(200); + + // reset setpoints + setpoint_electrical_angle_rad = 0.0f; + setpoint_flux_voltage_V = 0.0f; + // reset settings regs[REG_INV_PHASE_MOTOR] = 0; regs[REG_MOTOR_SYNCHRO_L] = 0; regs[REG_MOTOR_SYNCHRO_H] = 0; + regs[REG_MOTOR_SYNCHRO_H] = 0; + + // change mode + foc_state = FOC_STATE_FLUX_CONTROL; // find natural direction // set electrical angle - float setpoint_electrical_angle_rad = M_3PI_2; - float setpoint_flux_voltage_V = 1.0f; // hard-coded V setpoint - API_FOC_Set_Flux_Angle(setpoint_electrical_angle_rad,setpoint_flux_voltage_V); + setpoint_electrical_angle_rad = M_3PI_2; + setpoint_flux_voltage_V = 1.0f; // hard-coded V setpoint HAL_Delay(100); // move one electrical revolution forward for (int i = 0; i <=500; ++i ) { setpoint_electrical_angle_rad = M_3PI_2 + M_2PI * i / 500.0f; - API_FOC_Set_Flux_Angle(setpoint_electrical_angle_rad,setpoint_flux_voltage_V); HAL_Delay(2); } HAL_Delay(200); @@ -317,7 +279,6 @@ int API_FOC_Calibrate() for (int i = 500; i >=0; --i ) { setpoint_electrical_angle_rad = M_3PI_2 + M_2PI * i / 500.0f; - API_FOC_Set_Flux_Angle(setpoint_electrical_angle_rad,setpoint_flux_voltage_V); HAL_Delay(2); } HAL_Delay(200); @@ -326,7 +287,11 @@ int API_FOC_Calibrate() float const end_angle = positionSensor_getRadians(); // release motor - API_FOC_Set_Flux_Angle(0.0f,0.0f); + setpoint_electrical_angle_rad = 0.0f; + setpoint_flux_voltage_V = 0.0f; + + // change mode + foc_state = FOC_STATE_IDLE; // determine the direction the sensor moved float const delta_angle = mid_angle-end_angle; @@ -357,7 +322,11 @@ int API_FOC_Calibrate() // set electrical angle setpoint_electrical_angle_rad = 0.0f; setpoint_flux_voltage_V = 1.0f; // hard-coded V setpoint - API_FOC_Set_Flux_Angle(setpoint_electrical_angle_rad,setpoint_flux_voltage_V); + + // change mode + foc_state = FOC_STATE_FLUX_CONTROL; + + // wait HAL_Delay(1000); positionSensor_update(); float const reverse = regs[REG_INV_PHASE_MOTOR] == 0 ? 1.0f : -1.0f; @@ -366,8 +335,12 @@ int API_FOC_Calibrate() regs[REG_MOTOR_SYNCHRO_L] = LOW_BYTE((int)RADIANS_TO_DEGREES(phase_synchro_offset_rad)); regs[REG_MOTOR_SYNCHRO_H] = HIGH_BYTE((int)RADIANS_TO_DEGREES(phase_synchro_offset_rad)); - // release motor - API_FOC_Set_Flux_Angle(0.0f,0.0f); + // release motor + setpoint_electrical_angle_rad = 0.0f; + setpoint_flux_voltage_V = 0.0f; + + // change mode + foc_state = FOC_STATE_IDLE; // store calibration into EEPROM store_eeprom_regs(); @@ -388,151 +361,147 @@ void API_FOC_Service_Update() // this function allow on-the-go synchronization angle adjustment // the open loop mode means that the present Id and Iq are forced to 0 // this may require adjustment of the Kp and Ki of both flux and torque PI regulator -// note : with a 5008 motor, there is no need for Ki and Kff in both flux and torque PI -void API_FOC_Torque_Update( - uint16_t present_time_us, - float setpoint_torque_current_mA, - float setpoint_flux_current_mA, - float phase_synchro_offset_rad, - uint32_t closed_loop, - float setpoint_velocity_dps -) +void API_FOC_Torque_Update() { - // note : absolute position increases when turning CCW (encoder) - // note : FOC period is less than motor PWM period - // drop phase current samples a few times between each FOC iteration - if(current_samples>current_sample_drop_rate) + // timestamp + foc_timestamp_ms = HAL_GetTick(); + foc_timestamp_us = __HAL_TIM_GET_COUNTER(&htim6); + + float Vds = 0.0f; + float Vqs = 0.0f; + float cosine_theta = 0.0f; + float sine_theta = 1.0f; + + // synch with registers + float const phase_offset_rad = DEGREES_TO_RADIANS((int16_t)(MAKE_SHORT(regs[REG_MOTOR_SYNCHRO_L],regs[REG_MOTOR_SYNCHRO_H]))); + float const phase_synchro_offset_rad = DEGREES_TO_RADIANS((float)(MAKE_SHORT(regs[REG_GOAL_SYNCHRO_OFFSET_L],regs[REG_GOAL_SYNCHRO_OFFSET_H]))); // manual synchro triming + float const reg_pole_pairs = regs[REG_MOTOR_POLE_PAIRS]; + float const reverse = regs[REG_INV_PHASE_MOTOR] == 0 ? 1.0f : -1.0f; + float const flux_Kp = (float)((int16_t)(MAKE_SHORT(regs[REG_PID_FLUX_CURRENT_KP_L],regs[REG_PID_FLUX_CURRENT_KP_H])))/100000.0f; + float const flux_Ki = (float)((int16_t)(MAKE_SHORT(regs[REG_PID_FLUX_CURRENT_KI_L],regs[REG_PID_FLUX_CURRENT_KI_H])))/100000000.0f; + float const torque_Kp = (float)((int16_t)(MAKE_SHORT(regs[REG_PID_TORQUE_CURRENT_KP_L],regs[REG_PID_TORQUE_CURRENT_KP_H])))/100000.0f; + float const torque_Ki = (float)((int16_t)(MAKE_SHORT(regs[REG_PID_TORQUE_CURRENT_KI_L],regs[REG_PID_TORQUE_CURRENT_KI_H])))/100000000.0f; + + // check control mode + switch(foc_state) { - current_samples-=(current_sample_drop_rate+1); + case FOC_STATE_IDLE: + { + // [Theta] + theta_rad = normalize_angle(positionSensor_getRadiansEstimation(foc_timestamp_us)*reg_pole_pairs*reverse+ phase_offset_rad + phase_synchro_offset_rad); - // backup 3-phase currents as soon as possible - memcpy(motor_current_sample_adc,motor_current_input_adc,sizeof(uint16_t)*3); + // [Cosine] + API_CORDIC_Processor_Update(theta_rad,&cosine_theta,&sine_theta); - // performance monitoring - uint16_t const t_begin = __HAL_TIM_GET_COUNTER(&htim6); + // [Clarke Transformation] + float const present_Ialpha = ( 2.0f * motor_current_mA[0] - motor_current_mA[1] - motor_current_mA[2] ) / 3.0f; + float const present_Ibeta = INV_SQRT3 * ( motor_current_mA[1] - motor_current_mA[2] ); - // process absolute position, and compute theta ahead using average processing time and velocity - absolute_position_rad = positionSensor_getRadiansEstimation(t_begin); + // [Park Transformation] + present_Ids_mA = present_Ialpha * cosine_theta + present_Ibeta * sine_theta; + present_Iqs_mA = -present_Ialpha * sine_theta + present_Ibeta * cosine_theta; - // if ALARM then zeroize currents setpoints - if(regs[REG_HARDWARE_ERROR_STATUS] != 0 ) - { - setpoint_torque_current_mA = 0.0f; - setpoint_flux_current_mA = 0.0f; + // do brake + LL_FOC_brake(); } - - // process phase current - // Note : when current flows inward phase, shunt voltage is negative - // Note : when current flows outward phase, shunt voltage is positive - // Note : The current sign is positive when flowing in to a phase - // Note : The current sign is negative when flowing out from a phase - for(size_t index=0;index<3;++index) + break; + case FOC_STATE_TORQUE_CONTROL: { - motor_current_mA[index]= -((float)motor_current_sample_adc[index]-motor_current_input_adc_offset[index])/motor_current_input_adc_mA[index]; // note : the (-) sign here - } + // computation ~7µs (-02) + + // [Theta] + absolute_position_rad = positionSensor_getRadiansEstimation(foc_timestamp_us); + theta_rad = normalize_angle(absolute_position_rad*reg_pole_pairs*reverse+ phase_offset_rad + phase_synchro_offset_rad); + + // [Cosine] + API_CORDIC_Processor_Update(theta_rad,&cosine_theta,&sine_theta); + + // [Clarke Transformation] + float const present_Ialpha = ( 2.0f * motor_current_mA[0] - motor_current_mA[1] - motor_current_mA[2] ) / 3.0f; + float const present_Ibeta = INV_SQRT3 * ( motor_current_mA[1] - motor_current_mA[2] ); + + // [Park Transformation] + present_Ids_mA = ( present_Ialpha * cosine_theta + present_Ibeta * sine_theta ); + present_Iqs_mA = (-present_Ialpha * sine_theta + present_Ibeta * cosine_theta ); + + // [PI] + Vds = pi_process_antiwindup_clamp( + &flux_pi, + setpoint_flux_current_mA - present_Ids_mA, + flux_Kp, + flux_Ki, + present_voltage_V // output_limit + ); + Vqs = pi_process_antiwindup_clamp( + &torque_pi, + setpoint_torque_current_mA - present_Iqs_mA, + torque_Kp, + torque_Ki, + present_voltage_V // output_limit + ); - // process theta for Park and Clarke Transformation and compute cosine(theta) and sine(theta) - float const phase_offset_rad = DEGREES_TO_RADIANS((int16_t)(MAKE_SHORT(regs[REG_MOTOR_SYNCHRO_L],regs[REG_MOTOR_SYNCHRO_H]))); - float const reg_pole_pairs = regs[REG_MOTOR_POLE_PAIRS]; - float const reverse = regs[REG_INV_PHASE_MOTOR] == 0 ? 1.0f : -1.0f; - float const theta_rad = fmodf(absolute_position_rad*reg_pole_pairs*reverse,M_2PI) + phase_offset_rad + phase_synchro_offset_rad; // theta - static float cosine_theta = 0.0f; - static float sine_theta = 0.0f; - API_CORDIC_Processor_Update(theta_rad,&cosine_theta,&sine_theta); - - // phase current (Ia,Ib,Ic) [0..xxxmA] to (Ialpha,Ibeta) [0..xxxmA] [Clarke Transformation] - static float const sqrt3 = sqrtf(3.0f); - float const present_Ialpha = 2.0f/3.0f*motor_current_mA[0]-1.0f/3.0f*(motor_current_mA[1]+motor_current_mA[2]); - float const present_Ibeta = 1.0f/sqrt3*(motor_current_mA[1]-motor_current_mA[2]); - // Note Ialpha synchone de Ia et de même phase/signe - // Note Ibeta suit Iaplha de 90° - - // (Ialpha,Ibeta) [0..xxxmA] to (Id,Iq) [0..xxxmA] [Park Transformation] - float present_Id = present_Ialpha*cosine_theta+present_Ibeta*sine_theta; - float present_Iq = -present_Ialpha*sine_theta+present_Ibeta*cosine_theta; - - // (Id,Iq) filtering - present_Id_filtered = ALPHA_CURRENT_DQ*present_Id+(1.0f-ALPHA_CURRENT_DQ)*present_Id_filtered; - present_Iq_filtered = ALPHA_CURRENT_DQ*present_Iq+(1.0f-ALPHA_CURRENT_DQ)*present_Iq_filtered; - - // flux controller (PI+FF) ==> Vd [-max_voltage_V,max_voltage_V] - float const setpoint_Id = setpoint_flux_current_mA; - float const Flux_Kp = (float)((int16_t)(MAKE_SHORT(regs[REG_PID_FLUX_CURRENT_KP_L],regs[REG_PID_FLUX_CURRENT_KP_H])))/100000.0f; - //float const Flux_Ki = (float)((int16_t)(MAKE_SHORT(regs[REG_PID_FLUX_CURRENT_KI_L],regs[REG_PID_FLUX_CURRENT_KI_H])))/10000000.0f; - //float const Flux_Kff = (float)((int16_t)(MAKE_SHORT(regs[REG_PID_FLUX_CURRENT_KFF_L],regs[REG_PID_FLUX_CURRENT_KFF_H])))/100000.0f; - float const error_Id = setpoint_Id-( closed_loop == 1 ? present_Id_filtered : 0.0f); - float Vd = error_Id*Flux_Kp; //+Flux_Kff*setpoint_Id; - - // torque controller (PI+FF) ==> Vq [-max_voltage_V,max_voltage_V] - float const setpoint_Iq = setpoint_torque_current_mA; - float const Torque_Kp = (float)((int16_t)(MAKE_SHORT(regs[REG_PID_TORQUE_CURRENT_KP_L],regs[REG_PID_TORQUE_CURRENT_KP_H])))/100000.0f; - //float const Torque_Ki = (float)((int16_t)(MAKE_SHORT(regs[REG_PID_TORQUE_CURRENT_KI_L],regs[REG_PID_TORQUE_CURRENT_KI_H])))/10000000.0f; - //float const Torque_Kff = (float)((int16_t)(MAKE_SHORT(regs[REG_PID_TORQUE_CURRENT_KFF_L],regs[REG_PID_TORQUE_CURRENT_KFF_H])))/100000.0f; - float const error_Iq = setpoint_Iq-( closed_loop == 1 ? present_Iq_filtered : 0.0f); - float Vq = error_Iq*Torque_Kp; //+Torque_Kff*setpoint_Iq; - - // VdVq should not exceed present voltage - if(present_voltage_V>0) // avoid divide by zero, never true. - { -#ifdef CSVPWM - float const Vmax_sq = present_voltage_V*present_voltage_V*1.15f; // over modulation -#else - float const Vmax_sq = present_voltage_V*present_voltage_V; -#endif - float const Vnorm = Vd*Vd+Vq*Vq; - if(Vnorm>Vmax_sq) + // voltage norm saturation Umax = Udc/sqrt(3) + float const Vmax = present_voltage_V*INV_SQRT3; + float const Vnorm = sqrtf(Vds*Vds+Vqs*Vqs); + if(Vnorm>Vmax) { - float const k = sqrtf(fabsf(Vnorm/Vmax_sq)); - Vq *= k; - Vd *= k; + float const k = fabsf(Vmax/Vnorm); + Vqs *= k; + Vds *= k; } + + // do inverse clarke and park transformation and update 3-phase PWM generation + LL_FOC_set_phase_voltage(Vds,Vqs,cosine_theta,sine_theta,present_voltage_V); } + break; + case FOC_STATE_FLUX_CONTROL: + { + // cannot estimate phase current + present_Ids_mA = 0.0f; + present_Iqs_mA = 0.0f; - // do inverse clarke and park transformation and update TIMER1 register (3-phase PWM generation) - LL_FOC_Inverse_Clarke_Park_PWM_Generation(Vd,Vq,cosine_theta,sine_theta); + // compute theta + float const theta_rad = normalize_angle(setpoint_electrical_angle_rad); - // performance monitoring - uint16_t const t_end = __HAL_TIM_GET_COUNTER(&htim6); - uint16_t const t_tp = t_end-t_begin; - static const float alpha_performance_monitoring = 0.001f; - average_processing_time_us = (1.0f-alpha_performance_monitoring)*average_processing_time_us+alpha_performance_monitoring*(float)t_tp; - ++foc_counter; + // compute cosine and sine + API_CORDIC_Processor_Update(theta_rad,&cosine_theta,&sine_theta); - // TRACE/DEBUG - // TODO : use STM32 CUBE MONITOR - /*static uint32_t count = 0; - if(++count%4==0) - { - HAL_Serial_Print(&serial,"%d %d %d %d %d %d %d %d %d %d %d %d %d %d %d\n", - (int)RADIANS_TO_DEGREES(theta_rad), - (int)(motor_current_mA[0]), - (int)(motor_current_mA[1]), - (int)(motor_current_mA[2]), - (int)present_Ialpha, - (int)present_Ibeta, - (int)present_Id_filtered, - (int)present_Iq_filtered, - (int)(Vd*100.0f), - (int)(Vq*100.0f), - (int)(Valpha*100.0f), - (int)(Vbeta*100.0f), - (int)(duty_cycle_PWMa*100.0f), - (int)(duty_cycle_PWMb*100.0f), - (int)(duty_cycle_PWMc*100.0f) - ); - }*/ + // compute (Vd,Vq) + Vds = setpoint_flux_voltage_V; // torque setpoint open loop + Vqs = 0.0f; // no torque + + // do inverse clarke and park transformation and update 3-phase PWM generation + LL_FOC_set_phase_voltage(Vds,Vqs,cosine_theta,sine_theta,present_voltage_V); + } + break; } + + // performance monitoring + uint16_t const t_end = __HAL_TIM_GET_COUNTER(&htim6); + uint16_t const t_tp = t_end-foc_timestamp_us; + static const float alpha_performance_monitoring = 0.001f; + average_processing_time_us = (1.0f-alpha_performance_monitoring)*average_processing_time_us+alpha_performance_monitoring*(float)t_tp; + ++foc_counter; +} + +uint32_t API_FOC_Get_Timestamp_ms() +{ + return foc_timestamp_ms; } +uint16_t API_FOC_Get_Timestamp_us() +{ + return foc_timestamp_us; +} float API_FOC_Get_Present_Torque_Current() { - return present_Iq_filtered; + return present_Iqs_mA; } float API_FOC_Get_Present_Flux_Current() { - return present_Id_filtered; + return present_Ids_mA; } float API_FOC_Get_Present_Voltage() @@ -548,7 +517,6 @@ float API_FOC_Get_Present_Temp() float API_FOC_Get_Processing_Time() { return average_processing_time_us; - } float API_FOC_Get_Processing_Frequency() @@ -562,8 +530,11 @@ void API_FOC_It(ADC_HandleTypeDef *hadc) { if(__HAL_TIM_IS_TIM_COUNTING_DOWN(&htim1)) { - // Filter (EWMA) position and voltage ADC samples + // phase current motor_current_input_adc[0] = ADC1_DMA[1]; + motor_current_mA[0]= ((float)motor_current_input_adc[0]-motor_current_input_adc_offset[0])*motor_current_input_adc_KmA; + ++current_samples; + // aux potentiometer_input_adc = ADC1_DMA[2]; vbus_input_adc = ADC1_DMA[3]; temperature_input_adc = ADC1_DMA[4]; @@ -572,25 +543,29 @@ void API_FOC_It(ADC_HandleTypeDef *hadc) { motor_current_input_adc_offset[0] = ALPHA_CURRENT_SENSE_OFFSET*(float)(ADC1_DMA[1]) + (1.0f-ALPHA_CURRENT_SENSE_OFFSET)*motor_current_input_adc_offset[0]; } - // restart ADC - HAL_ADC_Start_DMA(&hadc1,(uint32_t*)ADC1_DMA,5); } if(hadc==&hadc2) { if(__HAL_TIM_IS_TIM_COUNTING_DOWN(&htim1)) { - // Filter (EWMA) position and voltage ADC samples + // phase current motor_current_input_adc[1] = ADC2_DMA[1]; motor_current_input_adc[2] = ADC2_DMA[2]; - ++current_samples; + motor_current_mA[1]= ((float)motor_current_input_adc[1]-motor_current_input_adc_offset[1])*motor_current_input_adc_KmA; + motor_current_mA[2]= ((float)motor_current_input_adc[2]-motor_current_input_adc_offset[2])*motor_current_input_adc_KmA; + current_samples+=2; } else { motor_current_input_adc_offset[1] = ALPHA_CURRENT_SENSE_OFFSET*(float)(ADC2_DMA[1]) + (1.0f-ALPHA_CURRENT_SENSE_OFFSET)*motor_current_input_adc_offset[1]; motor_current_input_adc_offset[2] = ALPHA_CURRENT_SENSE_OFFSET*(float)(ADC2_DMA[2]) + (1.0f-ALPHA_CURRENT_SENSE_OFFSET)*motor_current_input_adc_offset[2]; } - // restart ADC - HAL_ADC_Start_DMA(&hadc2,(uint32_t*)ADC2_DMA,3); + } + // once the 3 phase current are acquired, call for FOC + if(current_samples>=3) + { + current_samples=0; + API_FOC_Torque_Update(); } } diff --git a/01-Firmware/B-G431B-ESC1-v2/Core/Src/foc_utils.c b/01-Firmware/B-G431B-ESC1-v2/Core/Src/foc_utils.c new file mode 100644 index 0000000..05ce335 --- /dev/null +++ b/01-Firmware/B-G431B-ESC1-v2/Core/Src/foc_utils.c @@ -0,0 +1,8 @@ +/* + * foc_utils.c + * + * Created on: 13 janv. 2022 + * Author: Patrick + */ + + diff --git a/01-Firmware/B-G431B-ESC1-v2/Core/Src/main.c b/01-Firmware/B-G431B-ESC1-v2/Core/Src/main.c index 1d7225d..67db92b 100644 --- a/01-Firmware/B-G431B-ESC1-v2/Core/Src/main.c +++ b/01-Firmware/B-G431B-ESC1-v2/Core/Src/main.c @@ -25,6 +25,7 @@ #include "serial.h" #include "position_sensor.h" #include "foc.h" +#include "pid.h" #include "math_tool.h" #include "eeprom.h" #include "protocol.h" @@ -41,16 +42,24 @@ /* Private define ------------------------------------------------------------*/ /* USER CODE BEGIN PD */ +// CAN Bus termination +#define CAN_BUS_TERMINATION_OFF +#define CAN_BUS_TERMINATION_ON + // Position sensor type : // "AS5600_I2C" // "AS5048A_PWM" #define SENSOR_TYPE AS5048A_PWM -// PID Loop period in µs +// PID loop period in µs // normal setting is 1000us (1KHz) // performance setting is 250 (4KHz) #define PID_LOOP_PERIOD 250 +// FOC service loop period in µs +// normal setting is 10000us (100Hz) +#define SERVICE_LOOP_PERIOD 10000 + // Autocalibration at startup // uncomment this line for calibrating the ESC/MOTOR at startup // comment this line to avoid wearing EEPROM @@ -58,7 +67,6 @@ // Advanced settings (do not change) #define ALPHA_VELOCITY 0.24f // (default:0.24) F = 1000Hz ==> Fc (-3dB) = 20Hz -#define ALPHA_CURRENT_SETPOINT 0.48f // (default:0.48) F = 1000Hz ==> Fc (-3dB) = 20Hz /* USER CODE END PD */ @@ -93,11 +101,21 @@ DMA_HandleTypeDef hdma_usart2_rx; DMA_HandleTypeDef hdma_usart2_tx; /* USER CODE BEGIN PV */ + +//float setpoint_torque_current_mA = 0.0f; +//float setpoint_flux_current_mA = 0.0f; +//extern float potentiometer_input_adc; + +// serial communication HAL_Serial_Handler serial; + +// CAN communication, state and fail-safe static FDCAN_RxHeaderTypeDef RxHeader; static uint8_t RxData[8]; static FDCAN_TxHeaderTypeDef TxHeader; static uint8_t TxData[8]; +bool can_armed = false; +uint32_t can_last_time = 0; /* USER CODE END PV */ @@ -123,6 +141,10 @@ static void MX_I2C1_Init(void); /* Private user code ---------------------------------------------------------*/ /* USER CODE BEGIN 0 */ + +// PWM input capture IT for AS5048A position sensor +void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim) __attribute__((section (".ccmram"))); + void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim) { if (positionSensor_getType() == AS5048A_PWM) @@ -131,18 +153,7 @@ void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim) } } - -// current sense -void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef *hadc) -{ - API_FOC_It(hadc); -} - -/** - * @brief Configures the FDCAN. - * @param None - * @retval None - */ +// CAN configuration (filters) static void FDCAN_Config(void) { FDCAN_FilterTypeDef sFilterConfig; @@ -168,7 +179,11 @@ static void FDCAN_Config(void) } HAL_GPIO_WritePin(CAN_SHDN_GPIO_Port, CAN_SHDN_Pin, GPIO_PIN_RESET); - HAL_GPIO_WritePin(CAN_TERM_GPIO_Port, CAN_TERM_Pin, GPIO_PIN_RESET); // SET means activating R120 +#ifdef CAN_BUS_TERMINATION_ON + HAL_GPIO_WritePin(CAN_TERM_GPIO_Port, CAN_TERM_Pin, GPIO_PIN_SET); // SET means activating R120 for CAN bus termination +#else + HAL_GPIO_WritePin(CAN_TERM_GPIO_Port, CAN_TERM_Pin, GPIO_PIN_RESET); // RESET means no bus termination +#endif /* Start the FDCAN module */ if (HAL_FDCAN_Start(&hfdcan1) != HAL_OK) @@ -192,6 +207,110 @@ static void FDCAN_Config(void) TxHeader.TxEventFifoControl = FDCAN_NO_TX_EVENTS; TxHeader.MessageMarker = 0; } + +// CAN IT on message receive +void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs) +{ + if((RxFifo0ITs & FDCAN_IT_RX_FIFO0_NEW_MESSAGE) != 0) + { + // Handle CAN communication + while( HAL_FDCAN_GetRxFifoFillLevel(&hfdcan1,FDCAN_RX_FIFO0)!=0) + + /* Retrieve Rx messages from RX FIFO0 */ + if (HAL_FDCAN_GetRxMessage(hfdcan, FDCAN_RX_FIFO0, &RxHeader, RxData) == HAL_OK) + { + // decode message ID=0x000+ID + if(RxHeader.Identifier==regs[REG_ID]) // message from host controller + { + // can watchdog re-arm + if( can_armed ) + { + can_last_time = HAL_GetTick(); + } + uint32_t payload_length = RxHeader.DataLength>>16U; + // check payload size = 8 + if(can_armed && payload_length==2) // Feed Forward Torque only + { + // TODO Frist BYTE is CODE OP : 0:Torque, 1:VEL 2:POS N:Is Write M:is Read FF is start of enable + + // decode payload filed + regs[REG_GOAL_TORQUE_CURRENT_MA_L] = RxData[0]; + regs[REG_GOAL_TORQUE_CURRENT_MA_H] = RxData[1]; + //HAL_Serial_Print(&serial,"CAN (2)\n"); + } + else if(can_armed && payload_length==3) // Speed, VEL Kp + { + // decode payload filed + regs[REG_GOAL_VELOCITY_DPS_L] = RxData[0]; + regs[REG_GOAL_VELOCITY_DPS_H] = RxData[1]; + regs[REG_GOAL_VEL_KP] = RxData[4]; + //HAL_Serial_Print(&serial,"CAN (3)\n"); + } + else if(can_armed && payload_length==5) // Position, POS Kp and Kd, VEL Kp + { + // decode payload filed + regs[REG_GOAL_POSITION_DEG_L] = RxData[0]; + regs[REG_GOAL_POSITION_DEG_H] = RxData[1]; + regs[REG_GOAL_POS_KP] = RxData[3]; + regs[REG_GOAL_POS_KD] = RxData[4]; + regs[REG_GOAL_VEL_KP] = RxData[5]; + //HAL_Serial_Print(&serial,"CAN (5)\n"); + } + else if(payload_length==8) // position, speed, and torque feed forward, Kp/kd update + { + if( (RxData[0]==0xFF) && (RxData[1]==0xFF) && (RxData[2]==0xFF) && (RxData[3]==0xFF) && + (RxData[4]==0xFF) && (RxData[5]==0xFF) && (RxData[6]==0xFF) && (RxData[7]==0xFF) ) + { + // init watch dog + can_armed = true; + can_last_time = HAL_GetTick(); + regs[REG_TORQUE_ENABLE] = 1; + regs[REG_GOAL_POSITION_DEG_L] = 0; + regs[REG_GOAL_POSITION_DEG_H] = 0; + regs[REG_GOAL_VELOCITY_DPS_L] = 0; + regs[REG_GOAL_VELOCITY_DPS_H] = 0; + regs[REG_GOAL_TORQUE_CURRENT_MA_L] = 0; + regs[REG_GOAL_TORQUE_CURRENT_MA_H] = 0; + regs[REG_GOAL_POS_KP] = 0; + regs[REG_GOAL_POS_KD] = 0; + regs[REG_GOAL_VEL_KP] = 0; + //HAL_Serial_Print(&serial,"CAN request ARM\n"); + } + else if(can_armed) + { + // replace by access read/write to register ! + + + // decode payload filed + regs[REG_GOAL_POSITION_DEG_L] = RxData[0]; + regs[REG_GOAL_POSITION_DEG_H] = RxData[1]; + regs[REG_GOAL_VELOCITY_DPS_L] = RxData[2]; + regs[REG_GOAL_VELOCITY_DPS_H] = RxData[3]; + regs[REG_GOAL_TORQUE_CURRENT_MA_L] = RxData[4]; + regs[REG_GOAL_TORQUE_CURRENT_MA_H] = RxData[5]; + regs[REG_GOAL_POS_KP] = RxData[6]; + regs[REG_GOAL_POS_KD] = RxData[7]; + //HAL_Serial_Print(&serial,"CAN (8)\n"); + } + } + + // then reply by a status frame (shortened) + TxHeader.Identifier = 0x10+regs[REG_ID]; // each ESC replies with a message identifier = it is own ID + TxHeader.DataLength = FDCAN_DLC_BYTES_4; + TxData[0] = regs[REG_PRESENT_POSITION_DEG_L]; + TxData[1] = regs[REG_PRESENT_POSITION_DEG_H]; + TxData[2] = regs[REG_PRESENT_TORQUE_CURRENT_MA_L]; + TxData[3] = regs[REG_PRESENT_TORQUE_CURRENT_MA_H]; + //TxData[4] = regs[REG_HARDWARE_ERROR_STATUS]; + //TxData[5] = regs[REG_PRESENT_VOLTAGE]; + //TxData[6] = regs[REG_PRESENT_TEMPERATURE]; + //TxData[7] = .... + HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1,&TxHeader,TxData); + } + } + } +} + /* USER CODE END 0 */ /** @@ -236,12 +355,9 @@ int main(void) MX_FDCAN1_Init(); MX_I2C1_Init(); /* USER CODE BEGIN 2 */ - HAL_TIM_Base_Start(&htim6); // 1us base timer - uint16_t present_time_us = __HAL_TIM_GET_COUNTER(&htim6); - uint16_t last_time_us = present_time_us; - API_FOC_Init(); - HAL_Serial_Init(&huart2,&serial); - //HAL_Serial_Print(&serial,"RESET!\n"); + HAL_TIM_Base_Start(&htim6); // 1us base time + API_FOC_Init(); + if(eeprom_empty()) factory_reset_eeprom_regs(); load_eeprom_regs(); @@ -249,187 +365,86 @@ int main(void) FDCAN_Config(); positionSensor_init(SENSOR_TYPE); positionSensor_update(); + API_FOC_Service_Update(); #ifdef PERFORM_AUTO_CALIBRATION_AT_STARTUP - API_FOC_Calibrate(); + API_FOC_Calibrate(); #endif + HAL_Serial_Init(&huart2,&serial); +//HAL_Serial_Print(&serial,"RESET!\n"); + /* USER CODE END 2 */ /* Infinite loop */ /* USER CODE BEGIN WHILE */ + bool torque_was_enable = false; + uint16_t present_time_us = __HAL_TIM_GET_COUNTER(&htim6); + uint16_t pid_last_time_us = present_time_us; + uint16_t service_last_time_us = present_time_us; float setpoint_position_deg = 0.0f; float setpoint_velocity_dps = 0.0f; - float error_velocity_dps = 0.0f; float setpoint_torque_current_mA = 0.0f; float setpoint_flux_current_mA = 0.0f; - uint16_t last_mode = REG_CONTROL_MODE_IDLE; - bool can_armed = false; - uint32_t can_last_time = 0; + float error_velocity_dps = 0.0f; uint32_t pid_counter = 0; - while (1) - { + uint32_t mlp_counter = 0; + pid_context_t pd_position; + while (1) + { /* USER CODE END WHILE */ /* USER CODE BEGIN 3 */ - // Handle local MMI - // Led STATUS - HAL_GPIO_WritePin(STATUS_GPIO_Port,STATUS_Pin,(regs[REG_LED]>0)||(regs[REG_HARDWARE_ERROR_STATUS]>0)?GPIO_PIN_SET:GPIO_PIN_RESET); - // Pressing the button starts calibration - if(HAL_GPIO_ReadPin(BUTTON_GPIO_Port,BUTTON_Pin)==GPIO_PIN_RESET) - { - // perform calibration - API_FOC_Calibrate(); - // reset state - last_mode = REG_CONTROL_MODE_IDLE; - can_armed = false; - setpoint_position_deg = 0.0f; - setpoint_velocity_dps = 0.0f; - error_velocity_dps = 0.0f; - setpoint_torque_current_mA = 0.0f; - setpoint_flux_current_mA = 0.0f; - // update RAM - regs[REG_CONTROL_MODE] = REG_CONTROL_MODE_IDLE; - } - - // Handle CAN communication - while( HAL_FDCAN_GetRxFifoFillLevel(&hfdcan1,FDCAN_RX_FIFO0)!=0) - { - HAL_StatusTypeDef rx_result = HAL_FDCAN_GetRxMessage(&hfdcan1,FDCAN_RX_FIFO0,&RxHeader,RxData); - if(rx_result==HAL_OK) - { - // decode message ID=0x000+ID - if(RxHeader.Identifier==regs[REG_ID]) // message from host controller - { - // can watchdog re-arm - if( can_armed ) - { - can_last_time = HAL_GetTick(); - } - uint32_t payload_length = RxHeader.DataLength>>16U; - // check payload size = 8 - if(can_armed && payload_length==2) // Feed Forward Torque only - { - // decode payload filed - regs[REG_GOAL_TORQUE_CURRENT_MA_L] = RxData[0]; - regs[REG_GOAL_TORQUE_CURRENT_MA_H] = RxData[1]; - //HAL_Serial_Print(&serial,"CAN (2)\n"); - } - else if(can_armed && payload_length==4) // Position and speed, Kp/Kd unchanged - { - // decode payload filed - regs[REG_GOAL_POSITION_DEG_L] = RxData[0]; - regs[REG_GOAL_POSITION_DEG_H] = RxData[1]; - regs[REG_GOAL_VELOCITY_DPS_L] = RxData[2]; - regs[REG_GOAL_VELOCITY_DPS_H] = RxData[3]; - //HAL_Serial_Print(&serial,"CAN (4)\n"); - } - else if(can_armed && payload_length==6) // Position and speed, Kp/Kd update - { - // decode payload filed - regs[REG_GOAL_POSITION_DEG_L] = RxData[0]; - regs[REG_GOAL_POSITION_DEG_H] = RxData[1]; - regs[REG_GOAL_VELOCITY_DPS_L] = RxData[2]; - regs[REG_GOAL_VELOCITY_DPS_H] = RxData[3]; - regs[REG_GOAL_KP] = RxData[4]; - regs[REG_GOAL_KD] = RxData[5]; - //HAL_Serial_Print(&serial,"CAN (6)\n"); - } - else if(payload_length==8) // position, speed, and torque feed forward, Kp/kd update - { - if( (RxData[0]==0xFF) && (RxData[1]==0xFF) && (RxData[2]==0xFF) && (RxData[3]==0xFF) && - (RxData[4]==0xFF) && (RxData[5]==0xFF) && (RxData[6]==0xFF) && (RxData[7]==0xFF) ) - { - // init watch dog - can_armed = true; - can_last_time = HAL_GetTick(); - regs[REG_CONTROL_MODE] = REG_CONTROL_MODE_POSITION_VELOCITY_TORQUE; - regs[REG_GOAL_POSITION_DEG_L] = 0; - regs[REG_GOAL_POSITION_DEG_H] = 0; - regs[REG_GOAL_VELOCITY_DPS_L] = 0; - regs[REG_GOAL_VELOCITY_DPS_H] = 0; - regs[REG_GOAL_TORQUE_CURRENT_MA_L] = 0; - regs[REG_GOAL_TORQUE_CURRENT_MA_H] = 0; - regs[REG_GOAL_KP] = 0; - regs[REG_GOAL_KD] = 0; - //HAL_Serial_Print(&serial,"CAN request ARM\n"); - } - else if(can_armed) - { - // decode payload filed - regs[REG_GOAL_POSITION_DEG_L] = RxData[0]; - regs[REG_GOAL_POSITION_DEG_H] = RxData[1]; - regs[REG_GOAL_VELOCITY_DPS_L] = RxData[2]; - regs[REG_GOAL_VELOCITY_DPS_H] = RxData[3]; - regs[REG_GOAL_TORQUE_CURRENT_MA_L] = RxData[4]; - regs[REG_GOAL_TORQUE_CURRENT_MA_H] = RxData[5]; - regs[REG_GOAL_KP] = RxData[6]; - regs[REG_GOAL_KD] = RxData[7]; - //HAL_Serial_Print(&serial,"CAN (8)\n"); - } - } - -// // then reply by a status frame -// TxHeader.Identifier = regs[REG_ID]; // each ESC replies with a message identifier = it is own ID -// TxData[0] = regs[REG_ID]; -// TxData[1] = regs[REG_HARDWARE_ERROR_STATUS]; -// TxData[2] = regs[REG_PRESENT_POSITION_DEG_L]; -// TxData[3] = regs[REG_PRESENT_POSITION_DEG_H]; -// TxData[4] = regs[REG_PRESENT_TORQUE_CURRENT_MA_L]; -// TxData[5] = regs[REG_PRESENT_TORQUE_CURRENT_MA_H]; -// TxData[6] = regs[REG_PRESENT_VOLTAGE]; -// TxData[7] = regs[REG_PRESENT_TEMPERATURE]; -// HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1,&TxHeader,TxData); - - // then reply by a status frame (shortened) - TxHeader.Identifier = 0x10+regs[REG_ID]; // each ESC replies with a message identifier = it is own ID - TxHeader.DataLength = FDCAN_DLC_BYTES_4; - TxData[0] = regs[REG_PRESENT_POSITION_DEG_L]; - TxData[1] = regs[REG_PRESENT_POSITION_DEG_H]; - TxData[2] = regs[REG_PRESENT_TORQUE_CURRENT_MA_L]; - TxData[3] = regs[REG_PRESENT_TORQUE_CURRENT_MA_H]; - //TxData[4] = regs[REG_HARDWARE_ERROR_STATUS]; - //TxData[5] = regs[REG_PRESENT_VOLTAGE]; - //TxData[6] = regs[REG_PRESENT_TEMPERATURE]; - //TxData[7] = .... - HAL_FDCAN_AddMessageToTxFifoQ(&hfdcan1,&TxHeader,TxData); - } - } - //else - // CAN error handler - } - // CAN bus watchdog (time-out=1s) - if( (HAL_GetTick()>can_last_time+1000) && can_armed ) - { - can_armed = false; - regs[REG_CONTROL_MODE] = REG_CONTROL_MODE_IDLE; - } - - // Handle serial communication - while(HAL_Serial_Available(&serial)) - { - char c = HAL_Serial_GetChar(&serial); - packet_handler(c); - } + // hardware error clears torque enable register and disable FOC torque controller + if( regs[REG_HARDWARE_ERROR_STATUS] != 0 ) + { + // disable torque + regs[REG_TORQUE_ENABLE] = 0; + // disable FOC + API_FOC_Torque_Disable(); + // disarm CAN + can_armed = false; + } - // 1 to 4Khz low priority process - present_time_us = __HAL_TIM_GET_COUNTER(&htim6); - int16_t const delta_time_us = present_time_us-last_time_us; - if(delta_time_us>=PID_LOOP_PERIOD) - { - last_time_us+=PID_LOOP_PERIOD; - ++pid_counter; - // make alias - uint16_t const reg_control_mode = regs[REG_CONTROL_MODE]; - // process operating mode + // CAN bus time-out + // CAN bus watchdog (time-out = 1s hard-coded) + if( can_armed && (HAL_GetTick()>can_last_time+1000) ) + { + // disable torque + regs[REG_TORQUE_ENABLE] = 0; + // disable FOC + API_FOC_Torque_Disable(); + // disarm CAN + can_armed = false; + } - positionSensor_update(); + // FOC process time-out + // FOC watchdog (time-out = 10ms hard-coded) + if( HAL_GetTick() > API_FOC_Get_Timestamp_ms()+10 ) + { + // error + regs[REG_HARDWARE_ERROR_STATUS] |= (1 << HW_ERROR_BIT_FOC_TIMEOUT); + // disable torque + regs[REG_TORQUE_ENABLE] = 0; + // disable FOC + API_FOC_Torque_Disable(); + // disarm CAN + can_armed = false; + } - switch(reg_control_mode) + // 1 to 4Khz low priority process + present_time_us = __HAL_TIM_GET_COUNTER(&htim6); + uint16_t const pid_delta_time_us = present_time_us-pid_last_time_us; + if(pid_delta_time_us>=PID_LOOP_PERIOD) { - case REG_CONTROL_MODE_POSITION_VELOCITY_TORQUE: + pid_last_time_us+=PID_LOOP_PERIOD; + ++pid_counter; + + if(regs[REG_TORQUE_ENABLE]) { - if(last_mode!=REG_CONTROL_MODE_POSITION_VELOCITY_TORQUE) + // transition from torque disable to enable + if(!torque_was_enable) { + torque_was_enable = true; // set goal position to present position to avoid mechanical glicth regs[REG_GOAL_POSITION_DEG_L] = LOW_BYTE((int16_t)(10.0f*positionSensor_getDegreeMultiturn())); regs[REG_GOAL_POSITION_DEG_H] = HIGH_BYTE((int16_t)(10.0f*positionSensor_getDegreeMultiturn())); @@ -442,204 +457,198 @@ int main(void) // reset flux refenrece regs[REG_GOAL_FLUX_CURRENT_MA_L] = 0; regs[REG_GOAL_FLUX_CURRENT_MA_H] = 0; + // reset K + regs[REG_GOAL_POS_KP] = 0; + regs[REG_GOAL_POS_KD] = 0; + regs[REG_GOAL_VEL_KP] = 0; // reset setpoints setpoint_position_deg = 0.0f; setpoint_velocity_dps = 0.0f; error_velocity_dps = 0.0f; - setpoint_torque_current_mA = 0.0f; - setpoint_flux_current_mA = 0.0f; // set setpoint_position_deg to avoid glitch setpoint_position_deg = positionSensor_getDegreeMultiturn(); + // pid reset + pid_reset(&pd_position); + // foc + API_FOC_Torque_Enable(); } + + // update sensor + positionSensor_update(); + + // compute position set-point from goal and EEPROM position limits + float const goal_position_deg = (float)((int16_t)(MAKE_SHORT(regs[REG_GOAL_POSITION_DEG_L],regs[REG_GOAL_POSITION_DEG_H])))/10.0f; + float const reg_min_position_deg = (float)((int16_t)(MAKE_SHORT(regs[REG_MIN_POSITION_DEG_L],regs[REG_MIN_POSITION_DEG_H]))); + float const reg_max_position_deg = (float)((int16_t)(MAKE_SHORT(regs[REG_MAX_POSITION_DEG_L],regs[REG_MAX_POSITION_DEG_H]))); + setpoint_position_deg = fconstrain(goal_position_deg,reg_min_position_deg,reg_max_position_deg); + // compute velocity setpoint from goal and EEPROM velocity limit + float const goal_velocity_dps = (int16_t)(MAKE_SHORT(regs[REG_GOAL_VELOCITY_DPS_L],regs[REG_GOAL_VELOCITY_DPS_H])); + float const reg_max_velocity_dps = (int16_t)(MAKE_SHORT(regs[REG_MAX_VELOCITY_DPS_L],regs[REG_MAX_VELOCITY_DPS_H])); + setpoint_velocity_dps = fconstrain(goal_velocity_dps,-reg_max_velocity_dps,reg_max_velocity_dps); + // compute torque feed forward + float const torque_feed_forward_ma = (int16_t)(MAKE_SHORT(regs[REG_GOAL_TORQUE_CURRENT_MA_L],regs[REG_GOAL_TORQUE_CURRENT_MA_H])); + // compute torque setpoint + float const error_position_deg = setpoint_position_deg-positionSensor_getDegreeMultiturn(); + float const pos_kp = (float)regs[REG_GOAL_POS_KP]; + float const pos_kd = (float)regs[REG_GOAL_POS_KD]*100.0f; + float const reg_max_current_ma = (uint16_t)(MAKE_SHORT(regs[REG_MAX_CURRENT_MA_L],regs[REG_MAX_CURRENT_MA_H])); + float const vel_kp = (float)regs[REG_GOAL_VEL_KP]/10.0f; + error_velocity_dps = ALPHA_VELOCITY*(setpoint_velocity_dps-positionSensor_getVelocityDegree())+(1.0f-ALPHA_VELOCITY)*error_velocity_dps; + float const reg_reverse = regs[REG_INV_PHASE_MOTOR] == 0 ? 1.0f : -1.0f; + setpoint_torque_current_mA = reg_reverse*pid_process_antiwindup_clamp_with_ff( + &pd_position, + error_position_deg, + pos_kp, + 0.0f, + pos_kd, + reg_max_current_ma, + 0.1f, + vel_kp*error_velocity_dps+torque_feed_forward_ma + ); + //setpoint_torque_current_mA=(potentiometer_input_adc/4096)*3000.0f; // DEBUG + // set flux + float const goal_flux_current_mA = (int16_t)(MAKE_SHORT(regs[REG_GOAL_FLUX_CURRENT_MA_L],regs[REG_GOAL_FLUX_CURRENT_MA_H])); + setpoint_flux_current_mA = goal_flux_current_mA; + + // update FOC parameters + API_FOC_Set_Torque_Flux_Currents_mA(setpoint_torque_current_mA,setpoint_flux_current_mA); + } + else // torque disable + { + // transition from enable to disable + if(torque_was_enable) { - // compute position set-point from goal and EEPROM position limits - float const goal_position_deg = (float)((int16_t)(MAKE_SHORT(regs[REG_GOAL_POSITION_DEG_L],regs[REG_GOAL_POSITION_DEG_H])))/10.0f; - float const reg_min_position_deg = (float)((int16_t)(MAKE_SHORT(regs[REG_MIN_POSITION_DEG_L],regs[REG_MIN_POSITION_DEG_H]))); - float const reg_max_position_deg = (float)((int16_t)(MAKE_SHORT(regs[REG_MAX_POSITION_DEG_L],regs[REG_MAX_POSITION_DEG_H]))); - setpoint_position_deg = fconstrain(goal_position_deg,reg_min_position_deg,reg_max_position_deg); - // compute velocity setpoint from goal and EEPROM velocity limit - float const goal_velocity_dps = (int16_t)(MAKE_SHORT(regs[REG_GOAL_VELOCITY_DPS_L],regs[REG_GOAL_VELOCITY_DPS_H])); - float const reg_max_velocity_dps = (int16_t)(MAKE_SHORT(regs[REG_MAX_VELOCITY_DPS_L],regs[REG_MAX_VELOCITY_DPS_H])); - setpoint_velocity_dps = fconstrain(goal_velocity_dps,-reg_max_velocity_dps,reg_max_velocity_dps); - // compute torque feed forward - float const torque_feed_forward_ma = (int16_t)(MAKE_SHORT(regs[REG_GOAL_TORQUE_CURRENT_MA_L],regs[REG_GOAL_TORQUE_CURRENT_MA_H])); - // compute torque setpoint - float const error_position_deg = setpoint_position_deg-positionSensor_getDegreeMultiturn(); - float const kp = (float)regs[REG_GOAL_KP]; - error_velocity_dps = ALPHA_VELOCITY*(setpoint_velocity_dps-positionSensor_getVelocityDegree())+(1.0f-ALPHA_VELOCITY)*error_velocity_dps; - float const kd = (float)regs[REG_GOAL_KD]/100.0f; - float const reg_reverse = regs[REG_INV_PHASE_MOTOR] == 0 ? 1.0f : -1.0f; - setpoint_torque_current_mA = ALPHA_CURRENT_SETPOINT*reg_reverse*(kp*error_position_deg+kd*error_velocity_dps+torque_feed_forward_ma) - + (1.0f-ALPHA_CURRENT_SETPOINT)*setpoint_torque_current_mA; - // limit torque - float const reg_max_current_ma = (uint16_t)(MAKE_SHORT(regs[REG_MAX_CURRENT_MA_L],regs[REG_MAX_CURRENT_MA_H])); - setpoint_torque_current_mA = fconstrain(setpoint_torque_current_mA,-reg_max_current_ma,reg_max_current_ma); - // set flux - float const goal_flux_current_mA = (int16_t)(MAKE_SHORT(regs[REG_GOAL_FLUX_CURRENT_MA_L],regs[REG_GOAL_FLUX_CURRENT_MA_H])); - setpoint_flux_current_mA = goal_flux_current_mA; - // if target speed not reached, process field weakening : - if( - ( (setpoint_velocity_dps>1000.0f) && (setpoint_velocity_dps>positionSensor_getVelocityDegree()) ) || - ( (setpoint_velocity_dps<1000.0f) && (setpoint_velocity_dps0.0f) ? vmax : -vmax; -// // limit maximum velocity, when far from stop -// vmax = fconstrain(vmax,-max_velocity_dps,max_velocity_dps); -// // compute distance between maximun velocity and current velocity -// float delta_v = vmax - setpoint_velocity_dps; -// // now compute new velocity according acceleration -// setpoint_velocity_dps += fconstrain(delta_v, (-max_acceleration_dpss*MAIN_LOO_PERIOD_US/1000000.0f), (max_acceleration_dpss*MAIN_LOO_PERIOD_US/1000000.0f)); -// // compute new position setpoint -// setpoint_position_deg += (setpoint_velocity_dps*MAIN_LOO_PERIOD_US/1000000.0f); -// // now compute acceleration setpoint -// setpoint_acceleration_dpss = (setpoint_velocity_dps - last_setpoint_velocity_dps)*1000000.0f/MAIN_LOO_PERIOD_US; -// last_setpoint_velocity_dps = setpoint_velocity_dps; -// // compute velocity/acceleration feed forwards -// float const pid_vel_kff = (float)(MAKE_SHORT(regs[REG_PID_VELOCITY_KFF_L],regs[REG_PID_VELOCITY_KFF_H]))/1000.0f; -// float const pid_acc_kff = (float)(MAKE_SHORT(regs[REG_PID_ACCELERATION_KFF_L],regs[REG_PID_ACCELERATION_KFF_H]))/100000.0f; -// float const velocity_feed_forward = pid_vel_kff * setpoint_velocity_dps; -// float const acceleration_feed_forward = pid_acc_kff * setpoint_acceleration_dpss; -// // compute position error -// float const error_position_deg = setpoint_position_deg-RADIANS_TO_DEGREES(API_AS5048A_Position_Sensor_Get_Multiturn_Radians()); -// // compute torque current setpoint using PID position, current is limited bt goal current and EEPROM current limit -// float const goal_torque_current_mA=(int16_t)(MAKE_SHORT(regs[REG_GOAL_TORQUE_CURRENT_MA_L],regs[REG_GOAL_TORQUE_CURRENT_MA_H])); -// float const reg_max_current_ma = (uint16_t)(MAKE_SHORT(regs[REG_MAX_CURRENT_MA_L],regs[REG_MAX_CURRENT_MA_H])); -// float const reg_reverse = regs[REG_INV_PHASE_MOTOR] == 0 ? 1.0f : -1.0f; -// setpoint_torque_current_mA = ALPHA_CURRENT_SETPOINT*reg_reverse*pid_process_antiwindup_clamp_with_ff( -// &pid_position, -// error_position_deg, -// (float)((int16_t)(MAKE_SHORT(regs[REG_PID_POSITION_KP_L],regs[REG_PID_POSITION_KP_H])))/1.0f, -// (float)((int16_t)(MAKE_SHORT(regs[REG_PID_POSITION_KI_L],regs[REG_PID_POSITION_KI_H])))/100.0f, -// (float)((int16_t)(MAKE_SHORT(regs[REG_PID_POSITION_KD_L],regs[REG_PID_POSITION_KD_H])))/1.0f, -// fminf(goal_torque_current_mA,reg_max_current_ma), // limit is the lowest limit from goal and EEPROM -// 0.1f,// ALPHA D -// velocity_feed_forward+acceleration_feed_forward // FF -// ) + (1.0f-ALPHA_CURRENT_SETPOINT)*setpoint_torque_current_mA; -// // in this operating mode, flux current is forced to 0 -// setpoint_flux_current_mA=0.0f; -// } -// break; - - default: // IDLE of other unknown values - // reset unused RAM - regs[REG_GOAL_POSITION_DEG_L] = 0; - regs[REG_GOAL_POSITION_DEG_H] = 0; - regs[REG_GOAL_VELOCITY_DPS_L] = 0; - regs[REG_GOAL_VELOCITY_DPS_H] = 0; - regs[REG_GOAL_TORQUE_CURRENT_MA_L] = 0; - regs[REG_GOAL_TORQUE_CURRENT_MA_H] = 0; - regs[REG_GOAL_FLUX_CURRENT_MA_L] = 0; - regs[REG_GOAL_FLUX_CURRENT_MA_H] = 0; - regs[REG_GOAL_KP] = 0; - regs[REG_GOAL_KD] = 0; - // reset all setpoints - setpoint_position_deg = 0.0f; - setpoint_velocity_dps = 0.0f; - setpoint_torque_current_mA=0.0f; - setpoint_flux_current_mA=0.0f; - break; - } - last_mode = reg_control_mode; + + // RAM Update + regs[REG_PRESENT_POSITION_DEG_L] = LOW_BYTE((int16_t)(positionSensor_getDegreeMultiturn()*10.0f)); + regs[REG_PRESENT_POSITION_DEG_H] = HIGH_BYTE((int16_t)(positionSensor_getDegreeMultiturn()*10.0f)); + regs[REG_PRESENT_VELOCITY_DPS_L] = LOW_BYTE((int16_t)(positionSensor_getVelocityDegree()*1.0f)); + regs[REG_PRESENT_VELOCITY_DPS_H] = HIGH_BYTE((int16_t)(positionSensor_getVelocityDegree()*1.0f)); + regs[REG_PRESENT_TORQUE_CURRENT_MA_L] = LOW_BYTE((int16_t)(API_FOC_Get_Present_Torque_Current()*1.0f)); + regs[REG_PRESENT_TORQUE_CURRENT_MA_H] = HIGH_BYTE((int16_t)(API_FOC_Get_Present_Torque_Current()*1.0f)); + regs[REG_PRESENT_FLUX_CURRENT_MA_L] = LOW_BYTE((int16_t)(API_FOC_Get_Present_Flux_Current()*1.0f)); + regs[REG_PRESENT_FLUX_CURRENT_MA_H] = HIGH_BYTE((int16_t)(API_FOC_Get_Present_Flux_Current()*1.0f)); + regs[REG_PRESENT_VOLTAGE] = (uint16_t)(API_FOC_Get_Present_Voltage()); + regs[REG_PRESENT_TEMPERATURE] = (uint16_t)(API_FOC_Get_Present_Temp()); + regs[REG_MOVING] = (uint16_t)(fabsf(positionSensor_getVelocityDegree())) > (uint16_t)(regs[REG_MOVING_THRESHOLD_DPS]) ? 1 : 0; + + // DEBUG RAM Update + regs[REG_SETPOINT_POSITION_DEG_L] = LOW_BYTE((int16_t)(setpoint_position_deg*10.0f)); + regs[REG_SETPOINT_POSITION_DEG_H] = HIGH_BYTE((int16_t)(setpoint_position_deg*10.0f)); + regs[REG_SETPOINT_VELOCITY_DPS_L] = LOW_BYTE((int16_t)(setpoint_velocity_dps*1.0f)); + regs[REG_SETPOINT_VELOCITY_DPS_H] = HIGH_BYTE((int16_t)(setpoint_velocity_dps*1.0f)); + regs[REG_SETPOINT_TORQUE_CURRENT_MA_L] = LOW_BYTE((int16_t)(setpoint_torque_current_mA*1.0f)); + regs[REG_SETPOINT_TORQUE_CURRENT_MA_H] = HIGH_BYTE((int16_t)(setpoint_torque_current_mA*1.0f)); + regs[REG_SETPOINT_FLUX_CURRENT_MA_L] = LOW_BYTE((int16_t)(setpoint_flux_current_mA*1.0f)); + regs[REG_SETPOINT_FLUX_CURRENT_MA_H] = HIGH_BYTE((int16_t)(setpoint_flux_current_mA*1.0f)); + // test new FW + regs[REG_PROCESSING_TIME] = (uint8_t)(API_FOC_Get_Processing_Time()); + regs[REG_FOC_FREQUENCY] = (uint8_t)(API_FOC_Get_Processing_Frequency()/1000.0f); + regs[REG_PID_FREQUENCY] = (uint8_t)((float)pid_counter/(float)HAL_GetTick()); + regs[REG_MLP_FREQUENCY] = (uint8_t)((float)mlp_counter/(float)HAL_GetTick()); + + } // low priority process 4Khz + + // low priority low frequency + uint16_t const service_delta_time_us = present_time_us-service_last_time_us; + if(service_delta_time_us>=SERVICE_LOOP_PERIOD) + { + service_last_time_us+=SERVICE_LOOP_PERIOD; + + // FOC service update + API_FOC_Service_Update(); + + // Handle local MMI + // Led STATUS + HAL_GPIO_WritePin(STATUS_GPIO_Port,STATUS_Pin,(regs[REG_LED]>0)||(regs[REG_HARDWARE_ERROR_STATUS]>0)?GPIO_PIN_SET:GPIO_PIN_RESET); + + // DEBUG +// if(potentiometer_input_adc>200) +// regs[REG_TORQUE_ENABLE] = 1; +// else +// regs[REG_TORQUE_ENABLE] = 0; + + // Pressing the button starts calibration + if(HAL_GPIO_ReadPin(BUTTON_GPIO_Port,BUTTON_Pin)==GPIO_PIN_RESET) + { + // disable torque + regs[REG_TORQUE_ENABLE] = 0; + // disable FOC + API_FOC_Torque_Disable(); + // disarm CAN + can_armed = false; + // start FOC calibration + API_FOC_Calibrate(); + } + + // Handle serial communication + while(HAL_Serial_Available(&serial)) + { + char c = HAL_Serial_GetChar(&serial); + packet_handler(c); + } + } // low priority process very low frequency + // PERFORMANCE //uint16_t t_end = __HAL_TIM_GET_COUNTER(&htim6); - - // RAM Update - regs[REG_PRESENT_POSITION_DEG_L] = LOW_BYTE((int16_t)(positionSensor_getDegreeMultiturn()*10.0f)); - regs[REG_PRESENT_POSITION_DEG_H] = HIGH_BYTE((int16_t)(positionSensor_getDegreeMultiturn()*10.0f)); - regs[REG_PRESENT_VELOCITY_DPS_L] = LOW_BYTE((int16_t)(positionSensor_getVelocityDegree()*1.0f)); - regs[REG_PRESENT_VELOCITY_DPS_H] = HIGH_BYTE((int16_t)(positionSensor_getVelocityDegree()*1.0f)); - regs[REG_PRESENT_TORQUE_CURRENT_MA_L] = LOW_BYTE((int16_t)(API_FOC_Get_Present_Torque_Current()*1.0f)); - regs[REG_PRESENT_TORQUE_CURRENT_MA_H] = HIGH_BYTE((int16_t)(API_FOC_Get_Present_Torque_Current()*1.0f)); - regs[REG_PRESENT_FLUX_CURRENT_MA_L] = LOW_BYTE((int16_t)(API_FOC_Get_Present_Flux_Current()*1.0f)); - regs[REG_PRESENT_FLUX_CURRENT_MA_H] = HIGH_BYTE((int16_t)(API_FOC_Get_Present_Flux_Current()*1.0f)); - regs[REG_PRESENT_VOLTAGE] = (uint16_t)(API_FOC_Get_Present_Voltage()); - regs[REG_PRESENT_TEMPERATURE] = (uint16_t)(API_FOC_Get_Present_Temp()); - regs[REG_MOVING] = (uint16_t)(fabsf(positionSensor_getVelocityDegree())) > (uint16_t)(regs[REG_MOVING_THRESHOLD_DPS]) ? 1 : 0; - - // DEBUG RAM Update - regs[REG_SETPOINT_POSITION_DEG_L] = LOW_BYTE((int16_t)(setpoint_position_deg*10.0f)); - regs[REG_SETPOINT_POSITION_DEG_H] = HIGH_BYTE((int16_t)(setpoint_position_deg*10.0f)); - regs[REG_SETPOINT_VELOCITY_DPS_L] = LOW_BYTE((int16_t)(setpoint_velocity_dps*1.0f)); - regs[REG_SETPOINT_VELOCITY_DPS_H] = HIGH_BYTE((int16_t)(setpoint_velocity_dps*1.0f)); - regs[REG_SETPOINT_TORQUE_CURRENT_MA_L] = LOW_BYTE((int16_t)(setpoint_torque_current_mA*1.0f)); - regs[REG_SETPOINT_TORQUE_CURRENT_MA_H] = HIGH_BYTE((int16_t)(setpoint_torque_current_mA*1.0f)); - regs[REG_SETPOINT_FLUX_CURRENT_MA_L] = LOW_BYTE((int16_t)(setpoint_flux_current_mA*1.0f)); - regs[REG_SETPOINT_FLUX_CURRENT_MA_H] = HIGH_BYTE((int16_t)(setpoint_flux_current_mA*1.0f)); - regs[REG_PROCESSING_TIME] = (uint8_t)(API_FOC_Get_Processing_Time()); - regs[REG_FOC_FREQUENCY] = (uint8_t)(API_FOC_Get_Processing_Frequency()/1000.0f); - regs[REG_PID_FREQUENCY] = (uint8_t)((float)pid_counter/(float)HAL_GetTick()); + ++mlp_counter; // TRACE static uint32_t counter = 0; - if(((++counter)%4)==0) + if(((++counter)%100)==0) { +// HAL_Serial_Print(&serial,"%d %d\n", +// //(int)(RADIANS_TO_DEGREES(positionSensor_getRadians())*10.0f), +// (int)(init_error_data_bits), +// (int)(RADIANS_TO_DEGREES(present_position_rad)*10.0f) +// //(int)(RADIANS_TO_DEGREES(expected_position)*10.0f) +// //(int)(RADIANS_TO_DEGREES(present_velocity_rad)*0.1f) +// //(int)(RADIANS_TO_DEGREES(theta_rad)*10.0f) +// //(int)(RADIANS_TO_DEGREES(absolute_position_rad)*10.0f) +// //regs[REG_PROTOCOL_CRC_FAIL] +// //(int)(RADIANS_TO_DEGREES(API_AS5048A_Position_Sensor_Get_RPS())*10.0f) +// //(int)positionSensor_getDeltaTimeEstimation() +// ); + + // HAL_Serial_Print(&serial,"%d %d %d\n", // (int)(setpoint_torque_current_mA), // (int)(API_FOC_Get_Present_Torque_Current()), // (int)(API_FOC_Get_Present_Flux_Current()) // ); + +// HAL_Serial_Print(&serial,"%d\n", +// (int)API_AS5048A_Position_Sensor_Get_DeltaTimestamp() +// ); } - // FOC service update - API_FOC_Service_Update(); - } - // synchro adjustment - float const phase_synchro_offset_rad = DEGREES_TO_RADIANS((float)(MAKE_SHORT(regs[REG_GOAL_SYNCHRO_OFFSET_L],regs[REG_GOAL_SYNCHRO_OFFSET_H]))); - // FOC torque update - API_FOC_Torque_Update( - present_time_us, - setpoint_torque_current_mA, - setpoint_flux_current_mA, - phase_synchro_offset_rad, - regs[REG_GOAL_CLOSED_LOOP], // open loop if 0, closed loop if 1 - setpoint_velocity_dps - ); + } /* USER CODE END 3 */ } @@ -733,7 +742,7 @@ static void MX_ADC1_Init(void) hadc1.Init.DiscontinuousConvMode = DISABLE; hadc1.Init.ExternalTrigConv = ADC_EXTERNALTRIG_T1_TRGO; hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING; - hadc1.Init.DMAContinuousRequests = DISABLE; + hadc1.Init.DMAContinuousRequests = ENABLE; hadc1.Init.Overrun = ADC_OVR_DATA_OVERWRITTEN; hadc1.Init.OversamplingMode = DISABLE; if (HAL_ADC_Init(&hadc1) != HAL_OK) @@ -828,7 +837,7 @@ static void MX_ADC2_Init(void) hadc2.Init.DiscontinuousConvMode = DISABLE; hadc2.Init.ExternalTrigConv = ADC_EXTERNALTRIG_T1_TRGO; hadc2.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_RISING; - hadc2.Init.DMAContinuousRequests = DISABLE; + hadc2.Init.DMAContinuousRequests = ENABLE; hadc2.Init.Overrun = ADC_OVR_DATA_OVERWRITTEN; hadc2.Init.OversamplingMode = DISABLE; if (HAL_ADC_Init(&hadc2) != HAL_OK) @@ -1107,11 +1116,16 @@ static void MX_TIM1_Init(void) /* USER CODE BEGIN TIM1_Init 1 */ + // note : At 160MHz, + // htim1.Init.Period = 4999 gives a TIM1 frequency of 32KHz and a PWM (centered-aligned) of 16KHz + // htim1.Init.Period = 3999 gives a TIM1 frequency of 40KHz and a PWM (centered-aligned) of 20KHz + // htim1.Init.Period = 3635 gives a TIM1 frequency of 44KHz and a PWM (centered-aligned) of 22KHz + /* USER CODE END TIM1_Init 1 */ htim1.Instance = TIM1; htim1.Init.Prescaler = 0; htim1.Init.CounterMode = TIM_COUNTERMODE_CENTERALIGNED1; - htim1.Init.Period = 4999; + htim1.Init.Period = 1999; htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim1.Init.RepetitionCounter = 0; htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; diff --git a/01-Firmware/B-G431B-ESC1-v2/Core/Src/math_tool.c b/01-Firmware/B-G431B-ESC1-v2/Core/Src/math_tool.c index 345490d..6abb9a9 100644 --- a/01-Firmware/B-G431B-ESC1-v2/Core/Src/math_tool.c +++ b/01-Firmware/B-G431B-ESC1-v2/Core/Src/math_tool.c @@ -10,57 +10,61 @@ int32_t constrain(int32_t x, int32_t min, int32_t max) { - if(xmax) - return max; - else - return x; + if(xmax) return max; + return x; } float fconstrain(float x, float min, float max) { - if(xmax) - return max; - else - return x; + if(xmax) return max; + return x; } float fconstrain_both(float x, float abs) { - if(x<-abs) - return -abs; - else if(x>abs) - return abs; - else - return x; + if(x<-abs) return -abs; + if(x>abs) return abs; + return x; +} + +float mfmod(float x,float y) +{ + float a = x/y; + return (a-(int)a)*y; } uint32_t map(uint32_t x, uint32_t in_min, uint32_t in_max, uint32_t out_min, uint32_t out_max) { - if(x<=in_min) - return out_min; - else if(x>=in_max) - return out_max; - else - return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; + if(x<=in_min) return out_min; + if(x>=in_max) return out_max; + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } float fmap(float x, float in_min, float in_max, float out_min, float out_max) { - if(x<=in_min) - return out_min; - else if(x>=in_max) - return out_max; - else - return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; + if(x<=in_min) return out_min; + if(x>=in_max) return out_max; + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } float normalize_angle(float angle_rad) { - float const a = fmodf(angle_rad, M_2PI); + float const a = mfmod(angle_rad, M_2PI); return a >= 0.0f ? a : (a + M_2PI); } +float difference_angle(float a_rad, float b_rad) +{ + float delta = a_rad-b_rad; + if(delta>M_PI) + { + return delta-M_2PI; + } + if(delta<=-M_PI) + { + return delta+M_2PI; + } + return delta; +} diff --git a/01-Firmware/B-G431B-ESC1-v2/Core/Src/pic.c b/01-Firmware/B-G431B-ESC1-v2/Core/Src/pic.c new file mode 100644 index 0000000..5f20d32 --- /dev/null +++ b/01-Firmware/B-G431B-ESC1-v2/Core/Src/pic.c @@ -0,0 +1,148 @@ +/* + * pic.c + * + * Created on: 16 nov. 2020 + * Author: Patrick + */ + +#include "pid.h" +#include +#include +#include "math_tool.h" + +void pid_reset( pid_context_t * ctx ) +{ + ctx->err_last_one = 0.0f; + ctx->err_integral = 0.0f; + ctx->derivative_filtered = 0.0f; +} + +float pi_process_antiwindup_clamp( + pid_context_t * ctx, + float error, + float kp, + float ki, + float output_limit +) +{ + // PID + float const p_term = kp*error; + float const i_term = ctx->err_integral; + // compute output before saturation + float const v = p_term + i_term; + // saturation + float const u = fconstrain(v,-output_limit,output_limit); + // output saturating + bool saturating = (u!=v); + // error and output same sign + bool sign = (error*v >= 0); + // zero + bool clamp = saturating && sign; + if(!clamp) + ctx->err_integral = ctx->err_integral + ki*error; + // output + return u; +} + +float pid_process_antiwindup_clamp_with_ff( + pid_context_t * ctx, + float error, + float kp, + float ki, + float kd, + float output_limit, + float alpha_derivative, + float feed_forward +) +{ + // filter derivative + ctx->derivative_filtered = alpha_derivative*(error-ctx->err_last_one)+(1.0f-alpha_derivative)*ctx->derivative_filtered; + // update derivative state + ctx->err_last_one = error; + // PID + float const p_term = kp*error; + float const i_term = ctx->err_integral; + float const d_term = kd*ctx->derivative_filtered; + // compute output before saturation + float const v = p_term + i_term + d_term + feed_forward; + // saturation + float const u = fconstrain(v,-output_limit,output_limit); + // output saturating + bool saturating = (u!=v); + // error and output same sign + bool sign = (error*v >= 0); + // zero + bool clamp = saturating && sign; + if(!clamp) + ctx->err_integral = ctx->err_integral + ki*error; + // output + return u; +} + +float pid_process_antiwindup_clamp( + pid_context_t * ctx, + float error, + float kp, + float ki, + float kd, + float output_limit, + float alpha_derivative +) +{ + // filter derivative + ctx->derivative_filtered = alpha_derivative*(error-ctx->err_last_one)+(1.0f-alpha_derivative)*ctx->derivative_filtered; + // update derivative state + ctx->err_last_one = error; + // PID + float const p_term = kp*error; + float const i_term = ctx->err_integral; + float const d_term = kd*ctx->derivative_filtered; + // compute output before saturation + float const v = p_term + i_term + d_term; + // saturation + float const u = fconstrain(v,-output_limit,output_limit); + // output saturating + bool saturating = (u!=v); + // error and output same sign + bool sign = (error*v >= 0); + // zero + bool clamp = saturating && sign; + if(!clamp) + ctx->err_integral = ctx->err_integral + ki*error; + // output + return u; +} + +float pid_process_antiwindup_back_calculation( + pid_context_t * ctx, + float error, + float kp, + float ki, + float kd, + float output_limit, + float kt, + float alpha_derivative +) +{ + // filter derivative + ctx->derivative_filtered = alpha_derivative*(error-ctx->err_last_one)+(1.0f-alpha_derivative)*ctx->derivative_filtered; + // update derivative state + ctx->err_last_one = error; + // PID + float const p_term = kp*error; + float const i_term = ctx->err_integral; + float const d_term = kd*ctx->derivative_filtered; + // compute output before saturation + float const v = p_term + i_term + d_term; + // saturation + float const u = fconstrain(v,-output_limit,output_limit); + // compute delta saturation + float const e = u-v; + // compute feedback + float i_feedback = e*kt; + // update integral state + ctx->err_integral = ctx->err_integral + ki*error + i_feedback; + // output + return u; +} + diff --git a/01-Firmware/B-G431B-ESC1-v2/Core/Src/position_sensor.c b/01-Firmware/B-G431B-ESC1-v2/Core/Src/position_sensor.c index 2971913..b67f944 100644 --- a/01-Firmware/B-G431B-ESC1-v2/Core/Src/position_sensor.c +++ b/01-Firmware/B-G431B-ESC1-v2/Core/Src/position_sensor.c @@ -91,8 +91,8 @@ int positionSensor_init(e_sensor_type sensor_type) return status; } -float positionSensor_getRadiansEstimation(uint16_t time_us){ - +float positionSensor_getRadiansEstimation(uint16_t time_us) +{ switch(sensor->sensor_type) { case AS5600_I2C: @@ -271,3 +271,34 @@ e_sensor_type positionSensor_getType(void) { return sensor->sensor_type; } + + +uint16_t positionSensor_getDeltaTimestamp() +{ + switch(sensor->sensor_type) + { + case AS5600_I2C: + return 0; + + case AS5048A_PWM: + return API_AS5048A_Position_Sensor_Get_DeltaTimestamp(); + + default: + return 0; + } +} + +int16_t positionSensor_getDeltaTimeEstimation() +{ + switch(sensor->sensor_type) + { + case AS5600_I2C: + return 0; + + case AS5048A_PWM: + return API_AS5048A_Position_Sensor_Get_DeltaTimeEstimation(); + + default: + return 0; + } +} diff --git a/01-Firmware/B-G431B-ESC1-v2/Core/Src/stm32g4xx_hal_msp.c b/01-Firmware/B-G431B-ESC1-v2/Core/Src/stm32g4xx_hal_msp.c index b7662b9..f69784f 100644 --- a/01-Firmware/B-G431B-ESC1-v2/Core/Src/stm32g4xx_hal_msp.c +++ b/01-Firmware/B-G431B-ESC1-v2/Core/Src/stm32g4xx_hal_msp.c @@ -7,7 +7,7 @@ ****************************************************************************** * @attention * - *

© Copyright (c) 2021 STMicroelectronics. + *

© Copyright (c) 2022 STMicroelectronics. * All rights reserved.

* * This software component is licensed by ST under BSD 3-Clause license, @@ -283,7 +283,7 @@ void HAL_CORDIC_MspInit(CORDIC_HandleTypeDef* hcordic) /* Peripheral clock enable */ __HAL_RCC_CORDIC_CLK_ENABLE(); /* CORDIC interrupt Init */ - HAL_NVIC_SetPriority(CORDIC_IRQn, 0, 0); + HAL_NVIC_SetPriority(CORDIC_IRQn, 2, 0); HAL_NVIC_EnableIRQ(CORDIC_IRQn); /* USER CODE BEGIN CORDIC_MspInit 1 */ @@ -354,6 +354,9 @@ void HAL_FDCAN_MspInit(FDCAN_HandleTypeDef* hfdcan) GPIO_InitStruct.Alternate = GPIO_AF9_FDCAN1; HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + /* FDCAN1 interrupt Init */ + HAL_NVIC_SetPriority(FDCAN1_IT0_IRQn, 3, 0); + HAL_NVIC_EnableIRQ(FDCAN1_IT0_IRQn); /* USER CODE BEGIN FDCAN1_MspInit 1 */ /* USER CODE END FDCAN1_MspInit 1 */ @@ -385,6 +388,8 @@ void HAL_FDCAN_MspDeInit(FDCAN_HandleTypeDef* hfdcan) HAL_GPIO_DeInit(GPIOB, GPIO_PIN_9); + /* FDCAN1 interrupt DeInit */ + HAL_NVIC_DisableIRQ(FDCAN1_IT0_IRQn); /* USER CODE BEGIN FDCAN1_MspDeInit 1 */ /* USER CODE END FDCAN1_MspDeInit 1 */ @@ -840,7 +845,7 @@ void HAL_UART_MspInit(UART_HandleTypeDef* huart) __HAL_LINKDMA(huart,hdmatx,hdma_usart2_tx); /* USART2 interrupt Init */ - HAL_NVIC_SetPriority(USART2_IRQn, 0, 0); + HAL_NVIC_SetPriority(USART2_IRQn, 4, 0); HAL_NVIC_EnableIRQ(USART2_IRQn); /* USER CODE BEGIN USART2_MspInit 1 */ diff --git a/01-Firmware/B-G431B-ESC1-v2/Core/Src/stm32g4xx_it.c b/01-Firmware/B-G431B-ESC1-v2/Core/Src/stm32g4xx_it.c index 8935b57..f174670 100644 --- a/01-Firmware/B-G431B-ESC1-v2/Core/Src/stm32g4xx_it.c +++ b/01-Firmware/B-G431B-ESC1-v2/Core/Src/stm32g4xx_it.c @@ -6,7 +6,7 @@ ****************************************************************************** * @attention * - *

© Copyright (c) 2021 STMicroelectronics. + *

© Copyright (c) 2022 STMicroelectronics. * All rights reserved.

* * This software component is licensed by ST under BSD 3-Clause license, @@ -42,7 +42,7 @@ /* Private variables ---------------------------------------------------------*/ /* USER CODE BEGIN PV */ - +extern TIM_HandleTypeDef htim1; /* USER CODE END PV */ /* Private function prototypes -----------------------------------------------*/ @@ -61,6 +61,7 @@ extern DMA_HandleTypeDef hdma_adc2; extern ADC_HandleTypeDef hadc1; extern ADC_HandleTypeDef hadc2; extern CORDIC_HandleTypeDef hcordic; +extern FDCAN_HandleTypeDef hfdcan1; extern TIM_HandleTypeDef htim4; extern DMA_HandleTypeDef hdma_usart2_rx; extern DMA_HandleTypeDef hdma_usart2_tx; @@ -93,11 +94,20 @@ void NMI_Handler(void) void HardFault_Handler(void) { /* USER CODE BEGIN HardFault_IRQn 0 */ + // Motor PWM init and BRAKE + __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,0); + __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2,0); + __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_3,0); /* USER CODE END HardFault_IRQn 0 */ while (1) { /* USER CODE BEGIN W1_HardFault_IRQn 0 */ + + // TODO : MOTOR PHASE OFF + // TODO : MOTOR PHASE OFF + // TODO : MOTOR PHASE OFF + // TODO : MOTOR PHASE OFF /* USER CODE END W1_HardFault_IRQn 0 */ } } @@ -278,6 +288,20 @@ void ADC1_2_IRQHandler(void) /* USER CODE END ADC1_2_IRQn 1 */ } +/** + * @brief This function handles FDCAN1 interrupt 0. + */ +void FDCAN1_IT0_IRQHandler(void) +{ + /* USER CODE BEGIN FDCAN1_IT0_IRQn 0 */ + + /* USER CODE END FDCAN1_IT0_IRQn 0 */ + HAL_FDCAN_IRQHandler(&hfdcan1); + /* USER CODE BEGIN FDCAN1_IT0_IRQn 1 */ + + /* USER CODE END FDCAN1_IT0_IRQn 1 */ +} + /** * @brief This function handles TIM4 global interrupt. */ diff --git a/01-Firmware/B-G431B-ESC1-v2/Core/Src/syscalls.c b/01-Firmware/B-G431B-ESC1-v2/Core/Src/syscalls.c index 4ec9584..bc0dd6c 100644 --- a/01-Firmware/B-G431B-ESC1-v2/Core/Src/syscalls.c +++ b/01-Firmware/B-G431B-ESC1-v2/Core/Src/syscalls.c @@ -33,12 +33,9 @@ /* Variables */ -//#undef errno -extern int errno; extern int __io_putchar(int ch) __attribute__((weak)); extern int __io_getchar(void) __attribute__((weak)); -register char * stack_ptr asm("sp"); char *__env[1] = { 0 }; char **environ = __env; diff --git a/01-Firmware/B-G431B-ESC1-v2/Core/Src/system_stm32g4xx.c b/01-Firmware/B-G431B-ESC1-v2/Core/Src/system_stm32g4xx.c index 52d7236..92fc662 100644 --- a/01-Firmware/B-G431B-ESC1-v2/Core/Src/system_stm32g4xx.c +++ b/01-Firmware/B-G431B-ESC1-v2/Core/Src/system_stm32g4xx.c @@ -103,11 +103,29 @@ */ /************************* Miscellaneous Configuration ************************/ -/*!< Uncomment the following line if you need to relocate your vector Table in - Internal SRAM. */ +/* Note: Following vector table addresses must be defined in line with linker + configuration. */ +/*!< Uncomment the following line if you need to relocate the vector table + anywhere in Flash or Sram, else the vector table is kept at the automatic + remap of boot address selected */ +/* #define USER_VECT_TAB_ADDRESS */ + +#if defined(USER_VECT_TAB_ADDRESS) +/*!< Uncomment the following line if you need to relocate your vector Table + in Sram else user remap will be done in Flash. */ /* #define VECT_TAB_SRAM */ -#define VECT_TAB_OFFSET 0x00UL /*!< Vector Table base offset field. - This value must be a multiple of 0x200. */ +#if defined(VECT_TAB_SRAM) +#define VECT_TAB_BASE_ADDRESS SRAM_BASE /*!< Vector Table base address field. + This value must be a multiple of 0x200. */ +#define VECT_TAB_OFFSET 0x00000000U /*!< Vector Table base offset field. + This value must be a multiple of 0x200. */ +#else +#define VECT_TAB_BASE_ADDRESS FLASH_BASE /*!< Vector Table base address field. + This value must be a multiple of 0x200. */ +#define VECT_TAB_OFFSET 0x00000000U /*!< Vector Table base offset field. + This value must be a multiple of 0x200. */ +#endif /* VECT_TAB_SRAM */ +#endif /* USER_VECT_TAB_ADDRESS */ /******************************************************************************/ /** * @} @@ -167,11 +185,9 @@ void SystemInit(void) #endif /* Configure the Vector Table location add offset address ------------------*/ -#ifdef VECT_TAB_SRAM - SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */ -#else - SCB->VTOR = FLASH_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal FLASH */ -#endif +#if defined(USER_VECT_TAB_ADDRESS) + SCB->VTOR = VECT_TAB_BASE_ADDRESS | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */ +#endif /* USER_VECT_TAB_ADDRESS */ } /** diff --git a/01-Firmware/B-G431B-ESC1-v2/Core/Startup/startup_stm32g431cbux.s b/01-Firmware/B-G431B-ESC1-v2/Core/Startup/startup_stm32g431cbux.s index dbde35b..114b40d 100644 --- a/01-Firmware/B-G431B-ESC1-v2/Core/Startup/startup_stm32g431cbux.s +++ b/01-Firmware/B-G431B-ESC1-v2/Core/Startup/startup_stm32g431cbux.s @@ -80,6 +80,22 @@ LoopCopyDataInit: cmp r4, r1 bcc CopyDataInit + /* Copy from flash to CCMRAM */ + ldr r0, =_sccmram + ldr r1, =_eccmram + ldr r2, =_siccmram + movs r3, #0 + b LoopCopyCcmInit +CopyCcmInit: + ldr r4, [r2, r3] + str r4, [r0, r3] + adds r3, r3, #4 +LoopCopyCcmInit: + adds r4, r0, r3 + cmp r4, r1 + bcc CopyCcmInit +/* End of copy to CCMRAM */ + /* Zero fill the bss segment. */ ldr r2, =_sbss ldr r4, =_ebss @@ -94,7 +110,7 @@ LoopFillZerobss: cmp r2, r4 bcc FillZerobss -/* Call the clock system initialization function.*/ +/* Call the clock system intitialization function.*/ bl SystemInit /* Call static constructors */ bl __libc_init_array diff --git a/01-Firmware/B-G431B-ESC1-v2/STM32G431CBUX_FLASH.ld b/01-Firmware/B-G431B-ESC1-v2/STM32G431CBUX_FLASH.ld index 898d522..32ac120 100644 --- a/01-Firmware/B-G431B-ESC1-v2/STM32G431CBUX_FLASH.ld +++ b/01-Firmware/B-G431B-ESC1-v2/STM32G431CBUX_FLASH.ld @@ -51,7 +51,7 @@ SECTIONS KEEP(*(.isr_vector)) /* Startup code */ . = ALIGN(4); } >FLASH - + /*--- New CCMRAM linker section definition ---*/ _siccmram = LOADADDR(.ccmram); /* CCMRAM section */ @@ -64,7 +64,8 @@ SECTIONS . = ALIGN(4); _eccmram = .; /* define a global symbols at ccmram end */ } >CCMRAM AT> FLASH - /*--- End of CCMRAM linker section definition ---*/ + /*--- End of CCMRAM linker section definition ---*/ + /* The program code and other data into "FLASH" Rom type memory */ .text : diff --git a/01-Firmware/B-G431B-ESC1-v2/theta.txt b/01-Firmware/B-G431B-ESC1-v2/theta.txt new file mode 100644 index 0000000..65d9901 --- /dev/null +++ b/01-Firmware/B-G431B-ESC1-v2/theta.txt @@ -0,0 +1,7226 @@ +1014 879 +1014 879 +1014 879 +1014 879 +1014 879 +1014 879 +956 826 +956 826 +956 826 +956 826 +956 826 +956 826 +956 826 +900 776 +900 776 +900 776 +900 776 +900 776 +900 776 +900 776 +836 722 +836 722 +836 722 +836 722 +836 722 +836 722 +779 670 +779 670 +779 670 +779 670 +779 670 +779 670 +779 670 +722 619 +722 619 +722 619 +722 619 +722 619 +722 619 +722 619 +664 568 +664 568 +664 568 +664 568 +664 568 +664 568 +601 515 +601 515 +601 515 +601 515 +601 515 +601 515 +601 515 +543 463 +543 463 +543 463 +543 463 +543 463 +543 463 +487 412 +487 412 +487 412 +487 412 +487 412 +487 412 +487 412 +423 359 +423 359 +423 359 +423 359 +423 359 +423 359 +423 359 +365 307 +365 307 +365 307 +365 307 +365 307 +365 307 +307 255 +307 255 +307 255 +307 255 +307 255 +307 255 +307 255 +244 202 +244 202 +244 202 +244 202 +244 202 +244 202 +244 202 +184 148 +184 148 +184 148 +184 148 +184 148 +184 148 +126 96 +126 96 +126 96 +126 96 +126 96 +126 96 +126 96 +67 44 +67 44 +67 44 +67 44 +67 44 +67 44 +67 44 +4098 3592 +4098 3592 +4098 3592 +4098 3592 +4098 3592 +4098 3592 +4038 3539 +4038 3539 +4038 3539 +4038 3539 +4038 3539 +4038 3539 +4038 3539 +3979 3488 +3979 3488 +3979 3488 +3979 3488 +3979 3488 +3979 3488 +3914 3431 +3914 3431 +3914 3431 +3914 3431 +3914 3431 +3914 3431 +3914 3431 +3855 3376 +3855 3376 +3855 3376 +3855 3376 +3855 3376 +3855 3376 +3797 3323 +3797 3323 +3797 3323 +3797 3323 +3797 3323 +3797 3323 +3797 3323 +3739 3271 +3739 3271 +3739 3271 +3739 3271 +3739 3271 +3739 3271 +3675 3217 +3675 3217 +3675 3217 +3675 3217 +3675 3217 +3675 3217 +3615 3163 +3615 3163 +3615 3163 +3615 3163 +3615 3163 +3615 3163 +3615 3163 +3557 3111 +3557 3111 +3557 3111 +3557 3111 +3557 3111 +3557 3111 +3557 3111 +3492 3057 +3492 3057 +3492 3057 +3492 3057 +3492 3057 +3492 3057 +3433 3003 +3433 3003 +3433 3003 +3433 3003 +3433 3003 +3433 3003 +3433 3003 +3374 2950 +3374 2950 +3374 2950 +3374 2950 +3374 2950 +3374 2950 +3309 2896 +3309 2896 +3309 2896 +3309 2896 +3309 2896 +3309 2896 +3250 2842 +3250 2842 +3250 2842 +3250 2842 +3250 2842 +3250 2842 +3250 2842 +3190 2789 +3190 2789 +3190 2789 +3190 2789 +3190 2789 +3190 2789 +3190 2789 +3132 2737 +3132 2737 +3132 2737 +3132 2737 +3132 2737 +3132 2737 +3066 2682 +3066 2682 +3066 2682 +3066 2682 +3066 2682 +3066 2682 +3008 2629 +3008 2629 +3008 2629 +3008 2629 +3008 2629 +3008 2629 +3008 2629 +2949 2577 +2949 2577 +2949 2577 +2949 2577 +2949 2577 +2949 2577 +2886 2523 +2886 2523 +2886 2523 +2886 2523 +2886 2523 +2886 2523 +2886 2523 +2827 2470 +2827 2470 +2827 2470 +2827 2470 +2827 2470 +2827 2470 +2827 2470 +2769 2418 +2769 2418 +2769 2418 +2769 2418 +2769 2418 +2769 2418 +2706 2364 +2706 2364 +2706 2364 +2706 2364 +2706 2364 +2706 2364 +2706 2364 +2649 2313 +2649 2313 +2649 2313 +2649 2313 +2649 2313 +2649 2313 +2592 2262 +2592 2262 +2592 2262 +2592 2262 +2592 2262 +2592 2262 +2535 2211 +2535 2211 +2535 2211 +2535 2211 +2535 2211 +2535 2211 +2535 2211 +2472 2158 +2472 2158 +2472 2158 +2472 2158 +2472 2158 +2472 2158 +2415 2158 +2415 2107 +2415 2107 +2415 2107 +2415 2107 +2415 2107 +2415 2107 +2358 2056 +2358 2056 +2358 2056 +2358 2056 +2358 2056 +2358 2056 +2358 2056 +2294 2003 +2294 2003 +2294 2003 +2294 2003 +2294 2003 +2294 2003 +2237 1951 +2237 1951 +2237 1951 +2237 1951 +2237 1951 +2237 1951 +2237 1951 +2179 1900 +2179 1900 +2179 1900 +2179 1900 +2179 1900 +2179 1900 +2116 1847 +2116 1847 +2116 1847 +2116 1847 +2116 1847 +2116 1847 +2116 1847 +2058 1794 +2058 1794 +2058 1794 +2058 1794 +2058 1794 +2058 1794 +2001 1743 +2001 1743 +2001 1743 +2001 1743 +2001 1743 +2001 1743 +2001 1743 +1944 1693 +1944 1693 +1944 1693 +1944 1693 +1944 1693 +1944 1693 +1880 1639 +1880 1639 +1880 1639 +1880 1639 +1880 1639 +1880 1639 +1880 1639 +1824 1588 +1824 1588 +1824 1588 +1824 1588 +1824 1588 +1824 1588 +1766 1537 +1766 1537 +1766 1537 +1766 1537 +1766 1537 +1766 1537 +1766 1537 +1704 1484 +1704 1484 +1704 1484 +1704 1484 +1704 1484 +1704 1484 +1647 1433 +1647 1433 +1647 1433 +1647 1433 +1647 1433 +1647 1433 +1647 1433 +1591 1382 +1591 1382 +1591 1382 +1591 1382 +1591 1382 +1591 1382 +1535 1333 +1535 1333 +1535 1333 +1535 1333 +1535 1333 +1535 1333 +1535 1333 +1471 1280 +1471 1280 +1471 1280 +1471 1280 +1471 1280 +1471 1280 +1414 1228 +1414 1228 +1414 1228 +1414 1228 +1414 1228 +1414 1228 +1414 1228 +1357 1178 +1357 1178 +1357 1178 +1357 1178 +1357 1178 +1357 1178 +1294 1124 +1294 1124 +1294 1124 +1294 1124 +1294 1124 +1294 1124 +1294 1124 +1237 1073 +1237 1073 +1237 1073 +1237 1073 +1237 1073 +1237 1073 +1180 1022 +1180 1022 +1180 1022 +1180 1022 +1180 1022 +1180 1022 +1180 1022 +1116 968 +1116 968 +1116 968 +1116 968 +1116 968 +1116 968 +1060 917 +1060 917 +1060 917 +1060 917 +1060 917 +1060 917 +1060 917 +1002 866 +1002 866 +1002 866 +1002 866 +1002 866 +1002 866 +1002 866 +945 815 +945 815 +945 815 +945 815 +945 815 +945 815 +882 762 +882 762 +882 762 +882 762 +882 762 +882 762 +882 762 +824 710 +824 710 +824 710 +824 710 +824 710 +824 710 +767 659 +767 659 +767 659 +767 659 +767 659 +767 659 +767 659 +704 606 +704 606 +704 606 +704 606 +704 606 +704 606 +704 606 +647 554 +647 554 +647 554 +647 554 +647 554 +647 554 +590 503 +590 503 +590 503 +590 503 +590 503 +590 503 +590 503 +528 450 +528 450 +528 450 +528 450 +528 450 +528 450 +470 399 +470 399 +470 399 +470 399 +470 399 +470 399 +470 399 +412 347 +412 347 +412 347 +412 347 +412 347 +412 347 +355 297 +355 297 +355 297 +355 297 +355 297 +355 297 +355 297 +291 243 +291 243 +291 243 +291 243 +291 243 +291 243 +291 243 +232 191 +232 191 +232 191 +232 191 +232 191 +232 191 +173 138 +173 138 +173 138 +173 138 +173 138 +173 138 +173 138 +109 86 +109 86 +109 86 +109 86 +109 86 +109 86 +51 34 +51 34 +51 34 +51 34 +51 34 +51 34 +51 34 +4088 3582 +4088 3582 +4088 3582 +4088 3582 +4088 3582 +4088 3582 +4088 3582 +4023 3530 +4023 3530 +4023 3530 +4023 3530 +4023 3530 +4023 3530 +3964 3478 +3964 3478 +3964 3478 +3964 3478 +3964 3478 +3964 3478 +3964 3478 +3906 3422 +3906 3422 +3906 3422 +3906 3422 +3906 3422 +3906 3422 +3848 3369 +3848 3369 +3848 3369 +3848 3369 +3848 3369 +3848 3369 +3848 3369 +3785 3314 +3785 3314 +3785 3314 +3785 3314 +3785 3314 +3785 3314 +3726 3261 +3726 3261 +3726 3261 +3726 3261 +3726 3261 +3726 3261 +3726 3261 +3668 3209 +3668 3209 +3668 3209 +3668 3209 +3668 3209 +3668 3209 +3604 3155 +3604 3155 +3604 3155 +3604 3155 +3604 3155 +3604 3155 +3604 3155 +3545 3102 +3545 3102 +3545 3102 +3545 3102 +3545 3102 +3545 3102 +3545 3102 +3487 3049 +3487 3049 +3487 3049 +3487 3049 +3487 3049 +3487 3049 +3427 2998 +3427 2998 +3427 2998 +3427 2998 +3427 2998 +3427 2998 +3362 2943 +3362 2943 +3362 2943 +3362 2943 +3362 2943 +3362 2943 +3362 2943 +3304 2889 +3304 2889 +3304 2889 +3304 2889 +3304 2889 +3304 2889 +3244 2837 +3244 2837 +3244 2837 +3244 2837 +3244 2837 +3244 2837 +3244 2837 +3179 2782 +3179 2782 +3179 2782 +3179 2782 +3179 2782 +3179 2782 +3179 2782 +3120 2728 +3120 2728 +3120 2728 +3120 2728 +3120 2728 +3120 2728 +3061 2675 +3061 2675 +3061 2675 +3061 2675 +3061 2675 +3061 2675 +2996 2621 +2996 2621 +2996 2621 +2996 2621 +2996 2621 +2996 2621 +2996 2621 +2938 2568 +2938 2568 +2938 2568 +2938 2568 +2938 2568 +2938 2568 +2938 2568 +2880 2516 +2880 2516 +2880 2516 +2880 2516 +2880 2516 +2880 2516 +2822 2464 +2822 2464 +2822 2464 +2822 2464 +2822 2464 +2822 2464 +2822 2464 +2758 2411 +2758 2411 +2758 2411 +2758 2411 +2758 2411 +2758 2411 +2701 2359 +2701 2359 +2701 2359 +2701 2359 +2701 2359 +2701 2359 +2644 2307 +2644 2307 +2644 2307 +2644 2307 +2644 2307 +2644 2307 +2644 2307 +2580 2254 +2580 2254 +2580 2254 +2580 2254 +2580 2254 +2580 2254 +2580 2254 +2524 2203 +2524 2203 +2524 2203 +2524 2203 +2524 2203 +2524 2203 +2466 2151 +2466 2151 +2466 2151 +2466 2151 +2466 2151 +2466 2151 +2466 2151 +2404 2099 +2404 2099 +2404 2099 +2404 2099 +2404 2099 +2404 2099 +2347 2047 +2347 2047 +2347 2047 +2347 2047 +2347 2047 +2347 2047 +2289 1996 +2289 1996 +2289 1996 +2289 1996 +2289 1996 +2289 1996 +2289 1996 +2231 1945 +2231 1945 +2231 1945 +2231 1945 +2231 1945 +2231 1945 +2231 1945 +2167 1891 +2167 1891 +2167 1891 +2167 1891 +2167 1891 +2167 1891 +2111 1840 +2111 1840 +2111 1840 +2111 1840 +2111 1840 +2111 1840 +2111 1840 +2053 1789 +2053 1789 +2053 1789 +2053 1789 +2053 1789 +2053 1789 +1990 1736 +1990 1736 +1990 1736 +1990 1736 +1990 1736 +1990 1736 +1933 1684 +1933 1684 +1933 1684 +1933 1684 +1933 1684 +1933 1684 +1933 1684 +1876 1633 +1876 1633 +1876 1633 +1876 1633 +1876 1633 +1876 1633 +1876 1633 +1813 1580 +1813 1580 +1813 1580 +1813 1580 +1813 1580 +1813 1580 +1757 1529 +1757 1529 +1757 1529 +1757 1529 +1757 1529 +1757 1529 +1757 1529 +1699 1478 +1699 1478 +1699 1478 +1699 1478 +1699 1478 +1699 1478 +1643 1428 +1643 1428 +1643 1428 +1643 1428 +1643 1428 +1643 1428 +1581 1376 +1581 1376 +1581 1376 +1581 1376 +1581 1376 +1581 1376 +1581 1376 +1524 1324 +1524 1324 +1524 1324 +1524 1324 +1524 1324 +1524 1324 +1524 1324 +1468 1274 +1468 1274 +1468 1274 +1468 1274 +1468 1274 +1468 1274 +1405 1222 +1405 1222 +1405 1222 +1405 1222 +1405 1222 +1405 1222 +1347 1170 +1347 1170 +1347 1170 +1347 1170 +1347 1170 +1347 1170 +1347 1170 +1290 1119 +1290 1119 +1290 1119 +1290 1119 +1290 1119 +1290 1119 +1228 1066 +1228 1066 +1228 1066 +1228 1066 +1228 1066 +1228 1066 +1228 1066 +1169 1014 +1169 1014 +1169 1014 +1169 1014 +1169 1014 +1169 1014 +1113 963 +1113 963 +1113 963 +1113 963 +1113 963 +1113 963 +1113 963 +1055 912 +1055 912 +1055 912 +1055 912 +1055 912 +1055 912 +991 859 +991 859 +991 859 +991 859 +991 859 +991 859 +991 859 +934 807 +934 807 +934 807 +934 807 +934 807 +934 807 +934 807 +877 755 +877 755 +877 755 +877 755 +877 755 +877 755 +814 703 +814 703 +814 703 +814 703 +814 703 +814 703 +814 703 +757 651 +757 651 +757 651 +757 651 +757 651 +757 651 +699 599 +699 599 +699 599 +699 599 +699 599 +699 599 +699 599 +641 548 +641 548 +641 548 +641 548 +641 548 +641 548 +641 548 +579 496 +579 496 +579 496 +579 496 +579 496 +579 496 +522 444 +522 444 +522 444 +522 444 +522 444 +522 444 +522 444 +465 393 +465 393 +465 393 +465 393 +465 393 +465 393 +400 339 +400 339 +400 339 +400 339 +400 339 +400 339 +400 339 +343 287 +343 287 +343 287 +343 287 +343 287 +343 287 +285 236 +285 236 +285 236 +285 236 +285 236 +285 236 +285 236 +220 181 +220 181 +220 181 +220 181 +220 181 +220 181 +220 181 +163 129 +163 129 +163 129 +163 129 +163 129 +163 129 +104 77 +104 77 +104 77 +104 77 +104 77 +104 77 +104 77 +45 25 +45 25 +45 25 +45 25 +45 25 +45 25 +45 25 +4075 3573 +4075 3573 +4075 3573 +4075 3573 +4075 3573 +4075 3573 +4016 3521 +4016 3521 +4016 3521 +4016 3521 +4016 3521 +4016 3521 +4016 3521 +3957 3468 +3957 3468 +3957 3468 +3957 3468 +3957 3468 +3957 3468 +3892 3411 +3892 3411 +3892 3411 +3892 3411 +3892 3411 +3892 3411 +3892 3411 +3833 3357 +3833 3357 +3833 3357 +3833 3357 +3833 3357 +3833 3357 +3775 3304 +3775 3304 +3775 3304 +3775 3304 +3775 3304 +3775 3304 +3775 3304 +3711 3249 +3711 3249 +3711 3249 +3711 3249 +3711 3249 +3711 3249 +3653 3196 +3653 3196 +3653 3196 +3653 3196 +3653 3196 +3653 3196 +3653 3196 +3595 3144 +3595 3144 +3595 3144 +3595 3144 +3595 3144 +3595 3144 +3535 3092 +3535 3092 +3535 3092 +3535 3092 +3535 3092 +3535 3092 +3535 3092 +3471 3038 +3471 3038 +3471 3038 +3471 3038 +3471 3038 +3471 3038 +3471 3038 +3411 2984 +3411 2984 +3411 2984 +3411 2984 +3411 2984 +3411 2984 +3352 2932 +3352 2932 +3352 2932 +3352 2932 +3352 2932 +3352 2932 +3286 2876 +3286 2876 +3286 2876 +3286 2876 +3286 2876 +3286 2876 +3286 2876 +3227 2822 +3227 2822 +3227 2822 +3227 2822 +3227 2822 +3227 2822 +3168 2770 +3168 2770 +3168 2770 +3168 2770 +3168 2770 +3168 2770 +3168 2770 +3102 2714 +3102 2714 +3102 2714 +3102 2714 +3102 2714 +3102 2714 +3102 2714 +3044 2661 +3044 2661 +3044 2661 +3044 2661 +3044 2661 +3044 2661 +2985 2608 +2985 2608 +2985 2608 +2985 2608 +2985 2608 +2985 2608 +2926 2556 +2926 2556 +2926 2556 +2926 2556 +2926 2556 +2926 2556 +2926 2556 +2862 2502 +2862 2502 +2862 2502 +2862 2502 +2862 2502 +2862 2502 +2804 2449 +2804 2449 +2804 2449 +2804 2449 +2804 2449 +2804 2449 +2804 2449 +2745 2397 +2745 2397 +2745 2397 +2745 2397 +2745 2397 +2745 2397 +2682 2344 +2682 2344 +2682 2344 +2682 2344 +2682 2344 +2682 2344 +2682 2344 +2625 2292 +2625 2292 +2625 2292 +2625 2292 +2625 2292 +2625 2292 +2568 2240 +2568 2240 +2568 2240 +2568 2240 +2568 2240 +2568 2240 +2568 2240 +2510 2190 +2510 2190 +2510 2190 +2510 2190 +2510 2190 +2510 2190 +2510 2190 +2447 2137 +2447 2137 +2447 2137 +2447 2137 +2447 2137 +2447 2137 +2390 2085 +2390 2085 +2390 2085 +2390 2085 +2390 2085 +2390 2085 +2390 2085 +2333 2034 +2333 2034 +2333 2034 +2333 2034 +2333 2034 +2333 2034 +2269 1981 +2269 1981 +2269 1981 +2269 1981 +2269 1981 +2269 1981 +2211 1929 +2211 1929 +2211 1929 +2211 1929 +2211 1929 +2211 1929 +2211 1929 +2154 1877 +2154 1877 +2154 1877 +2154 1877 +2154 1877 +2154 1877 +2090 1824 +2090 1824 +2090 1824 +2090 1824 +2090 1824 +2090 1824 +2090 1824 +2034 1773 +2034 1773 +2034 1773 +2034 1773 +2034 1773 +2034 1773 +1976 1721 +1976 1721 +1976 1721 +1976 1721 +1976 1721 +1976 1721 +1976 1721 +1919 1671 +1919 1671 +1919 1671 +1919 1671 +1919 1671 +1919 1671 +1856 1618 +1856 1618 +1856 1618 +1856 1618 +1856 1618 +1856 1618 +1856 1618 +1798 1566 +1798 1566 +1798 1566 +1798 1566 +1798 1566 +1798 1566 +1742 1515 +1742 1515 +1742 1515 +1742 1515 +1742 1515 +1742 1515 +1742 1515 +1678 1462 +1678 1462 +1678 1462 +1678 1462 +1678 1462 +1678 1462 +1622 1411 +1622 1411 +1622 1411 +1622 1411 +1622 1411 +1622 1411 +1622 1411 +1566 1360 +1566 1360 +1566 1360 +1566 1360 +1566 1360 +1566 1360 +1503 1308 +1503 1308 +1503 1308 +1503 1308 +1503 1308 +1503 1308 +1503 1308 +1446 1256 +1446 1256 +1446 1256 +1446 1256 +1446 1256 +1446 1256 +1389 1205 +1389 1205 +1389 1205 +1389 1205 +1389 1205 +1389 1205 +1389 1205 +1332 1155 +1332 1155 +1332 1155 +1332 1155 +1332 1155 +1332 1155 +1269 1102 +1269 1102 +1269 1102 +1269 1102 +1269 1102 +1269 1102 +1269 1102 +1211 1050 +1211 1050 +1211 1050 +1211 1050 +1211 1050 +1211 1050 +1154 999 +1154 999 +1154 999 +1154 999 +1154 999 +1154 999 +1154 999 +1090 946 +1090 946 +1090 946 +1090 946 +1090 946 +1090 946 +1032 893 +1032 893 +1032 893 +1032 893 +1032 893 +1032 893 +1032 893 +975 842 +975 842 +975 842 +975 842 +975 842 +975 842 +975 842 +912 789 +912 789 +912 789 +912 789 +912 789 +912 789 +854 736 +854 736 +854 736 +854 736 +854 736 +854 736 +854 736 +797 685 +797 685 +797 685 +797 685 +797 685 +797 685 +739 634 +739 634 +739 634 +739 634 +739 634 +739 634 +739 634 +676 581 +676 581 +676 581 +676 581 +676 581 +676 581 +619 529 +619 529 +619 529 +619 529 +619 529 +619 529 +619 529 +562 478 +562 478 +562 478 +562 478 +562 478 +562 478 +562 478 +499 426 +499 426 +499 426 +499 426 +499 426 +499 426 +441 373 +441 373 +441 373 +441 373 +441 373 +441 373 +441 373 +383 322 +383 322 +383 322 +383 322 +383 322 +383 322 +383 322 +325 271 +325 271 +325 271 +325 271 +325 271 +325 271 +261 217 +261 217 +261 217 +261 217 +261 217 +261 217 +261 217 +203 164 +203 164 +203 164 +203 164 +203 164 +203 164 +144 112 +144 112 +144 112 +144 112 +144 112 +144 112 +144 112 +79 60 +79 60 +79 60 +79 60 +79 60 +79 60 +79 60 +21 8 +21 8 +21 8 +21 8 +21 8 +21 8 +21 8 +4058 3556 +4058 3556 +4058 3556 +4058 3556 +4058 3556 +4058 3556 +3993 3503 +3993 3503 +3993 3503 +3993 3503 +3993 3503 +3993 3503 +3993 3503 +3935 3452 +3935 3452 +3935 3452 +3935 3452 +3935 3452 +3935 3452 +3876 3396 +3876 3396 +3876 3396 +3876 3396 +3876 3396 +3876 3396 +3876 3396 +3818 3342 +3818 3342 +3818 3342 +3818 3342 +3818 3342 +3818 3342 +3755 3288 +3755 3288 +3755 3288 +3755 3288 +3755 3288 +3755 3288 +3755 3288 +3697 3235 +3697 3235 +3697 3235 +3697 3235 +3697 3235 +3697 3235 +3639 3183 +3639 3183 +3639 3183 +3639 3183 +3639 3183 +3639 3183 +3639 3183 +3575 3129 +3575 3129 +3575 3129 +3575 3129 +3575 3129 +3575 3129 +3516 3076 +3516 3076 +3516 3076 +3516 3076 +3516 3076 +3516 3076 +3516 3076 +3457 3024 +3457 3024 +3457 3024 +3457 3024 +3457 3024 +3457 3024 +3392 2969 +3392 2969 +3392 2969 +3392 2969 +3392 2969 +3392 2969 +3392 2969 +3334 2916 +3334 2916 +3334 2916 +3334 2916 +3334 2916 +3334 2916 +3275 2864 +3275 2864 +3275 2864 +3275 2864 +3275 2864 +3275 2864 +3275 2864 +3215 2811 +3215 2811 +3215 2811 +3215 2811 +3215 2811 +3215 2811 +3151 2756 +3151 2756 +3151 2756 +3151 2756 +3151 2756 +3151 2756 +3151 2756 +3092 2703 +3092 2703 +3092 2703 +3092 2703 +3092 2703 +3092 2703 +3033 2651 +3033 2651 +3033 2651 +3033 2651 +3033 2651 +3033 2651 +3033 2651 +2968 2596 +2968 2596 +2968 2596 +2968 2596 +2968 2596 +2968 2596 +2910 2543 +2910 2543 +2910 2543 +2910 2543 +2910 2543 +2910 2543 +2910 2543 +2852 2491 +2852 2491 +2852 2491 +2852 2491 +2852 2491 +2852 2491 +2789 2438 +2789 2438 +2789 2438 +2789 2438 +2789 2438 +2789 2438 +2789 2438 +2731 2385 +2731 2385 +2731 2385 +2731 2385 +2731 2385 +2731 2385 +2674 2334 +2674 2334 +2674 2334 +2674 2334 +2674 2334 +2674 2334 +2674 2334 +2617 2284 +2617 2284 +2617 2284 +2617 2284 +2617 2284 +2617 2284 +2554 2231 +2554 2231 +2554 2231 +2554 2231 +2554 2231 +2554 2231 +2497 2179 +2497 2179 +2497 2179 +2497 2179 +2497 2179 +2497 2179 +2497 2179 +2440 2128 +2440 2128 +2440 2128 +2440 2128 +2440 2128 +2440 2128 +2377 2076 +2377 2076 +2377 2076 +2377 2076 +2377 2076 +2377 2076 +2377 2076 +2320 2024 +2320 2024 +2320 2024 +2320 2024 +2320 2024 +2320 2024 +2320 2024 +2262 1973 +2262 1973 +2262 1973 +2262 1973 +2262 1973 +2262 1973 +2205 1922 +2205 1922 +2205 1922 +2205 1922 +2205 1922 +2205 1922 +2142 1869 +2142 1869 +2142 1869 +2142 1869 +2142 1869 +2142 1869 +2142 1869 +2084 1817 +2084 1817 +2084 1817 +2084 1817 +2084 1817 +2084 1817 +2084 1817 +2027 1766 +2027 1766 +2027 1766 +2027 1766 +2027 1766 +2027 1766 +1964 1713 +1964 1713 +1964 1713 +1964 1713 +1964 1713 +1964 1713 +1964 1713 +1908 1662 +1908 1662 +1908 1662 +1908 1662 +1908 1662 +1908 1662 +1850 1611 +1850 1611 +1850 1611 +1850 1611 +1850 1611 +1850 1611 +1850 1611 +1788 1558 +1788 1558 +1788 1558 +1788 1558 +1788 1558 +1788 1558 +1732 1507 +1732 1507 +1732 1507 +1732 1507 +1732 1507 +1732 1507 +1732 1507 +1674 1456 +1674 1456 +1674 1456 +1674 1456 +1674 1456 +1674 1456 +1618 1406 +1618 1406 +1618 1406 +1618 1406 +1618 1406 +1618 1406 +1618 1406 +1556 1354 +1556 1354 +1556 1354 +1556 1354 +1556 1354 +1556 1354 +1499 1303 +1499 1303 +1499 1303 +1499 1303 +1499 1303 +1499 1303 +1499 1303 +1442 1252 +1442 1252 +1442 1252 +1442 1252 +1442 1252 +1442 1252 +1379 1199 +1379 1199 +1379 1199 +1379 1199 +1379 1199 +1379 1199 +1379 1199 +1323 1148 +1323 1148 +1323 1148 +1323 1148 +1323 1148 +1323 1148 +1266 1097 +1266 1097 +1266 1097 +1266 1097 +1266 1097 +1266 1097 +1266 1097 +1202 1044 +1202 1044 +1202 1044 +1202 1044 +1202 1044 +1202 1044 +1202 1044 +1145 992 +1145 992 +1145 992 +1145 992 +1145 992 +1145 992 +1088 941 +1088 941 +1088 941 +1088 941 +1088 941 +1088 941 +1088 941 +1030 890 +1030 890 +1030 890 +1030 890 +1030 890 +1030 890 +967 837 +967 837 +967 837 +967 837 +967 837 +967 837 +967 837 +910 785 +910 785 +910 785 +910 785 +910 785 +910 785 +852 734 +852 734 +852 734 +852 734 +852 734 +852 734 +852 734 +789 681 +789 681 +789 681 +789 681 +789 681 +789 681 +732 629 +732 629 +732 629 +732 629 +732 629 +732 629 +732 629 +675 578 +675 578 +675 578 +675 578 +675 578 +675 578 +675 578 +613 525 +613 525 +613 525 +613 525 +613 525 +613 525 +555 473 +555 473 +555 473 +555 473 +555 473 +555 473 +555 473 +498 422 +498 422 +498 422 +498 422 +498 422 +498 422 +498 422 +440 372 +440 372 +440 372 +440 372 +440 372 +440 372 +377 318 +377 318 +377 318 +377 318 +377 318 +377 318 +377 318 +318 266 +318 266 +318 266 +318 266 +318 266 +318 266 +260 214 +260 214 +260 214 +260 214 +260 214 +260 214 +260 214 +196 160 +196 160 +196 160 +196 160 +196 160 +196 160 +137 108 +137 108 +137 108 +137 108 +137 108 +137 108 +137 108 +78 56 +78 56 +78 56 +78 56 +78 56 +78 56 +78 56 +4109 4 +4109 4 +4109 4 +4109 4 +4109 4 +4109 4 +4051 3552 +4051 3552 +4051 3552 +4051 3552 +4051 3552 +4051 3552 +4051 3552 +3992 3500 +3992 3500 +3992 3500 +3992 3500 +3992 3500 +3992 3500 +3992 3500 +3933 3447 +3933 3447 +3933 3447 +3933 3447 +3933 3447 +3933 3447 +3869 3391 +3869 3391 +3869 3391 +3869 3391 +3869 3391 +3869 3391 +3811 3337 +3811 3337 +3811 3337 +3811 3337 +3811 3337 +3811 3337 +3811 3337 +3753 3284 +3753 3284 +3753 3284 +3753 3284 +3753 3284 +3753 3284 +3688 3229 +3688 3229 +3688 3229 +3688 3229 +3688 3229 +3688 3229 +3688 3229 +3630 3176 +3630 3176 +3630 3176 +3630 3176 +3630 3176 +3630 3176 +3573 3125 +3573 3125 +3573 3125 +3573 3125 +3573 3125 +3573 3125 +3573 3125 +3513 3072 +3513 3072 +3513 3072 +3513 3072 +3513 3072 +3513 3072 +3448 3018 +3448 3018 +3448 3018 +3448 3018 +3448 3018 +3448 3018 +3448 3018 +3390 2965 +3390 2965 +3390 2965 +3390 2965 +3390 2965 +3390 2965 +3330 2912 +3330 2912 +3330 2912 +3330 2912 +3330 2912 +3330 2912 +3330 2912 +3264 2857 +3264 2857 +3264 2857 +3264 2857 +3264 2857 +3264 2857 +3206 2804 +3206 2804 +3206 2804 +3206 2804 +3206 2804 +3206 2804 +3206 2804 +3146 2751 +3146 2751 +3146 2751 +3146 2751 +3146 2751 +3146 2751 +3082 2696 +3082 2696 +3082 2696 +3082 2696 +3082 2696 +3082 2696 +3082 2696 +3023 2642 +3023 2642 +3023 2642 +3023 2642 +3023 2642 +3023 2642 +2965 2591 +2965 2591 +2965 2591 +2965 2591 +2965 2591 +2965 2591 +2965 2591 +2906 2539 +2906 2539 +2906 2539 +2906 2539 +2906 2539 +2906 2539 +2841 2484 +2841 2484 +2841 2484 +2841 2484 +2841 2484 +2841 2484 +2841 2484 +2784 2431 +2784 2431 +2784 2431 +2784 2431 +2784 2431 +2784 2431 +2726 2380 +2726 2380 +2726 2380 +2726 2380 +2726 2380 +2726 2380 +2726 2380 +2663 2327 +2663 2327 +2663 2327 +2663 2327 +2663 2327 +2663 2327 +2607 2275 +2607 2275 +2607 2275 +2607 2275 +2607 2275 +2607 2275 +2607 2275 +2549 2224 +2549 2224 +2549 2224 +2549 2224 +2549 2224 +2549 2224 +2486 2171 +2486 2171 +2486 2171 +2486 2171 +2486 2171 +2486 2171 +2486 2171 +2429 2119 +2429 2119 +2429 2119 +2429 2119 +2429 2119 +2429 2119 +2429 2119 +2371 2068 +2371 2068 +2371 2068 +2371 2068 +2371 2068 +2371 2068 +2315 2018 +2315 2018 +2315 2018 +2315 2018 +2315 2018 +2315 2018 +2251 1965 +2251 1965 +2251 1965 +2251 1965 +2251 1965 +2251 1965 +2251 1965 +2193 1913 +2193 1913 +2193 1913 +2193 1913 +2193 1913 +2193 1913 +2136 1862 +2136 1862 +2136 1862 +2136 1862 +2136 1862 +2136 1862 +2136 1862 +2073 1809 +2073 1809 +2073 1809 +2073 1809 +2073 1809 +2073 1809 +2015 1757 +2015 1757 +2015 1757 +2015 1757 +2015 1757 +2015 1757 +2015 1757 +1958 1705 +1958 1705 +1958 1705 +1958 1705 +1958 1705 +1958 1705 +1895 1652 +1895 1652 +1895 1652 +1895 1652 +1895 1652 +1895 1652 +1895 1652 +1838 1601 +1838 1601 +1838 1601 +1838 1601 +1838 1601 +1838 1601 +1782 1550 +1782 1550 +1782 1550 +1782 1550 +1782 1550 +1782 1550 +1782 1550 +1725 1500 +1725 1500 +1725 1500 +1725 1500 +1725 1500 +1725 1500 +1725 1500 +1662 1447 +1662 1447 +1662 1447 +1662 1447 +1662 1447 +1662 1447 +1606 1396 +1606 1396 +1606 1396 +1606 1396 +1606 1396 +1606 1396 +1550 1346 +1550 1346 +1550 1346 +1550 1346 +1550 1346 +1550 1346 +1550 1346 +1487 1293 +1487 1293 +1487 1293 +1487 1293 +1487 1293 +1487 1293 +1429 1242 +1429 1242 +1429 1242 +1429 1242 +1429 1242 +1429 1242 +1429 1242 +1373 1191 +1373 1191 +1373 1191 +1373 1191 +1373 1191 +1373 1191 +1316 1141 +1316 1141 +1316 1141 +1316 1141 +1316 1141 +1316 1141 +1316 1141 +1252 1087 +1252 1087 +1252 1087 +1252 1087 +1252 1087 +1252 1087 +1195 1036 +1195 1036 +1195 1036 +1195 1036 +1195 1036 +1195 1036 +1195 1036 +1137 985 +1137 985 +1137 985 +1137 985 +1137 985 +1137 985 +1074 931 +1074 931 +1074 931 +1074 931 +1074 931 +1074 931 +1074 931 +1017 880 +1017 880 +1017 880 +1017 880 +1017 880 +1017 880 +1017 880 +959 828 +959 828 +959 828 +959 828 +959 828 +959 828 +896 775 +896 775 +896 775 +896 775 +896 775 +896 775 +896 775 +839 723 +839 723 +839 723 +839 723 +839 723 +839 723 +782 672 +782 672 +782 672 +782 672 +782 672 +782 672 +782 672 +724 621 +724 621 +724 621 +724 621 +724 621 +724 621 +662 568 +662 568 +662 568 +662 568 +662 568 +662 568 +662 568 +604 516 +604 516 +604 516 +604 516 +604 516 +604 516 +604 516 +547 466 +547 466 +547 466 +547 466 +547 466 +547 466 +485 413 +485 413 +485 413 +485 413 +485 413 +485 413 +485 413 +426 360 +426 360 +426 360 +426 360 +426 360 +426 360 +369 309 +369 309 +369 309 +369 309 +369 309 +369 309 +369 309 +306 256 +306 256 +306 256 +306 256 +306 256 +306 256 +306 256 +247 203 +247 203 +247 203 +247 203 +247 203 +247 203 +189 151 +189 151 +189 151 +189 151 +189 151 +189 151 +189 151 +130 100 +130 100 +130 100 +130 100 +130 100 +130 100 +66 48 +66 48 +66 48 +66 48 +66 48 +66 48 +66 48 +4102 3596 +4102 3596 +4102 3596 +4102 3596 +4102 3596 +4102 3596 +4102 3596 +4043 3543 +4043 3543 +4043 3543 +4043 3543 +4043 3543 +4043 3543 +3979 3492 +3979 3492 +3979 3492 +3979 3492 +3979 3492 +3979 3492 +3979 3492 +3920 3440 +3920 3440 +3920 3440 +3920 3440 +3920 3440 +3920 3440 +3862 3384 +3862 3384 +3862 3384 +3862 3384 +3862 3384 +3862 3384 +3862 3384 +3798 3327 +3798 3327 +3798 3327 +3798 3327 +3798 3327 +3798 3327 +3740 3274 +3740 3274 +3740 3274 +3740 3274 +3740 3274 +3740 3274 +3740 3274 +3683 3222 +3683 3222 +3683 3222 +3683 3222 +3683 3222 +3683 3222 +3625 3170 +3625 3170 +3625 3170 +3625 3170 +3625 3170 +3625 3170 +3625 3170 +3561 3117 +3561 3117 +3561 3117 +3561 3117 +3561 3117 +3561 3117 +3502 3064 +3502 3064 +3502 3064 +3502 3064 +3502 3064 +3502 3064 +3502 3064 +3443 3011 +3443 3011 +3443 3011 +3443 3011 +3443 3011 +3443 3011 +3378 2956 +3378 2956 +3378 2956 +3378 2956 +3378 2956 +3378 2956 +3378 2956 +3319 2903 +3319 2903 +3319 2903 +3319 2903 +3319 2903 +3319 2903 +3260 2851 +3260 2851 +3260 2851 +3260 2851 +3260 2851 +3260 2851 +3260 2851 +3201 2798 +3201 2798 +3201 2798 +3201 2798 +3201 2798 +3201 2798 +3136 2743 +3136 2743 +3136 2743 +3136 2743 +3136 2743 +3136 2743 +3136 2743 +3077 2691 +3077 2691 +3077 2691 +3077 2691 +3077 2691 +3077 2691 +3019 2638 +3019 2638 +3019 2638 +3019 2638 +3019 2638 +3019 2638 +3019 2638 +2955 2584 +2955 2584 +2955 2584 +2955 2584 +2955 2584 +2955 2584 +2896 2531 +2896 2531 +2896 2531 +2896 2531 +2896 2531 +2896 2531 +2896 2531 +2838 2479 +2838 2479 +2838 2479 +2838 2479 +2838 2479 +2838 2479 +2776 2426 +2776 2426 +2776 2426 +2776 2426 +2776 2426 +2776 2426 +2776 2426 +2718 2373 +2718 2373 +2718 2373 +2718 2373 +2718 2373 +2718 2373 +2718 2373 +2661 2323 +2661 2323 +2661 2323 +2661 2323 +2661 2323 +2661 2323 +2604 2272 +2604 2272 +2604 2272 +2604 2272 +2604 2272 +2604 2272 +2541 2219 +2541 2219 +2541 2219 +2541 2219 +2541 2219 +2541 2219 +2541 2219 +2484 2168 +2484 2168 +2484 2168 +2484 2168 +2484 2168 +2484 2168 +2427 2117 +2427 2117 +2427 2117 +2427 2117 +2427 2117 +2427 2117 +2427 2117 +2364 2064 +2364 2064 +2364 2064 +2364 2064 +2364 2064 +2364 2064 +2306 2012 +2306 2012 +2306 2012 +2306 2012 +2306 2012 +2306 2012 +2306 2012 +2250 1962 +2250 1962 +2250 1962 +2250 1962 +2250 1962 +2250 1962 +2186 1908 +2186 1908 +2186 1908 +2186 1908 +2186 1908 +2186 1908 +2186 1908 +2129 1856 +2129 1856 +2129 1856 +2129 1856 +2129 1856 +2129 1856 +2129 1856 +2072 1805 +2072 1805 +2072 1805 +2072 1805 +2072 1805 +2072 1805 +2014 1755 +2014 1755 +2014 1755 +2014 1755 +2014 1755 +2014 1755 +2014 1755 +1951 1702 +1951 1702 +1951 1702 +1951 1702 +1951 1702 +1951 1702 +1894 1650 +1894 1650 +1894 1650 +1894 1650 +1894 1650 +1894 1650 +1838 1599 +1838 1599 +1838 1599 +1838 1599 +1838 1599 +1838 1599 +1838 1599 +1774 1546 +1774 1546 +1774 1546 +1774 1546 +1774 1546 +1774 1546 +1718 1495 +1718 1495 +1718 1495 +1718 1495 +1718 1495 +1718 1495 +1718 1495 +1661 1444 +1661 1444 +1661 1444 +1661 1444 +1661 1444 +1661 1444 +1661 1444 +1599 1392 +1599 1392 +1599 1392 +1599 1392 +1599 1392 +1599 1392 +1543 1341 +1543 1341 +1543 1341 +1543 1341 +1543 1341 +1543 1341 +1486 1290 +1486 1290 +1486 1290 +1486 1290 +1486 1290 +1486 1290 +1486 1290 +1429 1240 +1429 1240 +1429 1240 +1429 1240 +1429 1240 +1429 1240 +1367 1188 +1367 1188 +1367 1188 +1367 1188 +1367 1188 +1367 1188 +1367 1188 +1309 1136 +1309 1136 +1309 1136 +1309 1136 +1309 1136 +1309 1136 +1251 1085 +1251 1085 +1251 1085 +1251 1085 +1251 1085 +1251 1085 +1251 1085 +1189 1032 +1189 1032 +1189 1032 +1189 1032 +1189 1032 +1189 1032 +1131 980 +1131 980 +1131 980 +1131 980 +1131 980 +1131 980 +1131 980 +1074 929 +1074 929 +1074 929 +1074 929 +1074 929 +1074 929 +1010 875 +1010 875 +1010 875 +1010 875 +1010 875 +1010 875 +1010 875 +953 823 +953 823 +953 823 +953 823 +953 823 +953 823 +953 823 +896 772 +896 772 +896 772 +896 772 +896 772 +896 772 +839 722 +839 722 +839 722 +839 722 +839 722 +839 722 +839 722 +776 669 +776 669 +776 669 +776 669 +776 669 +776 669 +719 617 +719 617 +719 617 +719 617 +719 617 +719 617 +719 617 +662 567 +662 567 +662 567 +662 567 +662 567 +662 567 +599 513 +599 513 +599 513 +599 513 +599 513 +599 513 +599 513 +542 462 +542 462 +542 462 +542 462 +542 462 +542 462 +542 462 +484 411 +484 411 +484 411 +484 411 +484 411 +484 411 +427 360 +427 360 +427 360 +427 360 +427 360 +427 360 +427 360 +363 306 +363 306 +363 306 +363 306 +363 306 +363 306 +363 306 +305 254 +305 254 +305 254 +305 254 +305 254 +305 254 +248 203 +248 203 +248 203 +248 203 +248 203 +248 203 +248 203 +183 149 +183 149 +183 149 +183 149 +183 149 +183 149 +125 96 +125 96 +125 96 +125 96 +125 96 +125 96 +125 96 +67 45 +67 45 +67 45 +67 45 +67 45 +67 45 +67 45 +4097 3593 +4097 3593 +4097 3593 +4097 3593 +4097 3593 +4097 3593 +4038 3541 +4038 3541 +4038 3541 +4038 3541 +4038 3541 +4038 3541 +4038 3541 +3979 3488 +3979 3488 +3979 3488 +3979 3488 +3979 3488 +3979 3488 +3921 3434 +3921 3434 +3921 3434 +3921 3434 +3921 3434 +3921 3434 +3921 3434 +3856 3378 +3856 3378 +3856 3378 +3856 3378 +3856 3378 +3856 3378 +3799 3325 +3799 3325 +3799 3325 +3799 3325 +3799 3325 +3799 3325 +3799 3325 +3740 3272 +3740 3272 +3740 3272 +3740 3272 +3740 3272 +3740 3272 +3676 3218 +3676 3218 +3676 3218 +3676 3218 +3676 3218 +3676 3218 +3676 3218 +3618 3166 +3618 3166 +3618 3166 +3618 3166 +3618 3166 +3618 3166 +3559 3113 +3559 3113 +3559 3113 +3559 3113 +3559 3113 +3559 3113 +3559 3113 +3495 3059 +3495 3059 +3495 3059 +3495 3059 +3495 3059 +3495 3059 +3436 3006 +3436 3006 +3436 3006 +3436 3006 +3436 3006 +3436 3006 +3436 3006 +3376 2953 +3376 2953 +3376 2953 +3376 2953 +3376 2953 +3376 2953 +3317 2900 +3317 2900 +3317 2900 +3317 2900 +3317 2900 +3317 2900 +3317 2900 +3252 2846 +3252 2846 +3252 2846 +3252 2846 +3252 2846 +3252 2846 +3194 2793 +3194 2793 +3194 2793 +3194 2793 +3194 2793 +3194 2793 +3194 2793 +3135 2740 +3135 2740 +3135 2740 +3135 2740 +3135 2740 +3135 2740 +3069 2685 +3069 2685 +3069 2685 +3069 2685 +3069 2685 +3069 2685 +3069 2685 +3011 2632 +3011 2632 +3011 2632 +3011 2632 +3011 2632 +3011 2632 +2952 2580 +2952 2580 +2952 2580 +2952 2580 +2952 2580 +2952 2580 +2952 2580 +2888 2525 +2888 2525 +2888 2525 +2888 2525 +2888 2525 +2888 2525 +2830 2472 +2830 2472 +2830 2472 +2830 2472 +2830 2472 +2830 2472 +2830 2472 +2772 2421 +2772 2421 +2772 2421 +2772 2421 +2772 2421 +2772 2421 +2715 2370 +2715 2370 +2715 2370 +2715 2370 +2715 2370 +2715 2370 +2715 2370 +2652 2316 +2652 2316 +2652 2316 +2652 2316 +2652 2316 +2652 2316 +2594 2265 +2594 2265 +2594 2265 +2594 2265 +2594 2265 +2594 2265 +2594 2265 +2537 2214 +2537 2214 +2537 2214 +2537 2214 +2537 2214 +2537 2214 +2473 2160 +2473 2160 +2473 2160 +2473 2160 +2473 2160 +2473 2160 +2473 2160 +2417 2108 +2417 2108 +2417 2108 +2417 2108 +2417 2108 +2417 2108 +2360 2058 +2360 2058 +2360 2058 +2360 2058 +2360 2058 +2360 2058 +2360 2058 +2302 2007 +2302 2007 +2302 2007 +2302 2007 +2302 2007 +2302 2007 +2239 1954 +2239 1954 +2239 1954 +2239 1954 +2239 1954 +2239 1954 +2239 1954 +2182 1902 +2182 1902 +2182 1902 +2182 1902 +2182 1902 +2182 1902 +2124 1852 +2124 1852 +2124 1852 +2124 1852 +2124 1852 +2124 1852 +2061 1798 +2061 1798 +2061 1798 +2061 1798 +2061 1798 +2061 1798 +2061 1798 +2003 1746 +2003 1746 +2003 1746 +2003 1746 +2003 1746 +2003 1746 +1946 1695 +1946 1695 +1946 1695 +1946 1695 +1946 1695 +1946 1695 +1946 1695 +1884 1642 +1884 1642 +1884 1642 +1884 1642 +1884 1642 +1884 1642 +1884 1642 +1827 1591 +1827 1591 +1827 1591 +1827 1591 +1827 1591 +1827 1591 +1770 1540 +1770 1540 +1770 1540 +1770 1540 +1770 1540 +1770 1540 +1770 1540 +1714 1490 +1714 1490 +1714 1490 +1714 1490 +1714 1490 +1714 1490 +1651 1438 +1651 1438 +1651 1438 +1651 1438 +1651 1438 +1651 1438 +1595 1386 +1595 1386 +1595 1386 +1595 1386 +1595 1386 +1595 1386 +1595 1386 +1539 1336 +1539 1336 +1539 1336 +1539 1336 +1539 1336 +1539 1336 +1476 1284 +1476 1284 +1476 1284 +1476 1284 +1476 1284 +1476 1284 +1476 1284 +1419 1233 +1419 1233 +1419 1233 +1419 1233 +1419 1233 +1419 1233 +1362 1182 +1362 1182 +1362 1182 +1362 1182 +1362 1182 +1362 1182 +1362 1182 +1299 1129 +1299 1129 +1299 1129 +1299 1129 +1299 1129 +1299 1129 +1242 1077 +1242 1077 +1242 1077 +1242 1077 +1242 1077 +1242 1077 +1242 1077 +1184 1026 +1184 1026 +1184 1026 +1184 1026 +1184 1026 +1184 1026 +1127 975 +1127 975 +1127 975 +1127 975 +1127 975 +1127 975 +1127 975 +1064 922 +1064 922 +1064 922 +1064 922 +1064 922 +1064 922 +1064 922 +1006 870 +1006 870 +1006 870 +1006 870 +1006 870 +1006 870 +948 819 +948 819 +948 819 +948 819 +948 819 +948 819 +948 819 +884 765 +884 765 +884 765 +884 765 +884 765 +884 765 +827 713 +827 713 +827 713 +827 713 +827 713 +827 713 +827 713 +770 662 +770 662 +770 662 +770 662 +770 662 +770 662 +707 608 +707 608 +707 608 +707 608 +707 608 +707 608 +707 608 +649 556 +649 556 +649 556 +649 556 +649 556 +649 556 +649 556 +592 505 +592 505 +592 505 +592 505 +592 505 +592 505 +534 454 +534 454 +534 454 +534 454 +534 454 +534 454 +534 454 +472 401 +472 401 +472 401 +472 401 +472 401 +472 401 +413 349 +413 349 +413 349 +413 349 +413 349 +413 349 +413 349 +355 297 +355 297 +355 297 +355 297 +355 297 +355 297 +355 297 +292 244 +292 244 +292 244 +292 244 +292 244 +292 244 +233 191 +233 191 +233 191 +233 191 +233 191 +233 191 +233 191 +174 139 +174 139 +174 139 +174 139 +174 139 +174 139 +174 139 +116 87 +116 87 +116 87 +116 87 +116 87 +116 87 +51 34 +51 34 +51 34 +51 34 +51 34 +51 34 +51 34 +4088 3582 +4088 3582 +4088 3582 +4088 3582 +4088 3582 +4088 3582 +4088 3582 +4029 3530 +4029 3530 +4029 3530 +4029 3530 +4029 3530 +4029 3530 +3964 3478 +3964 3478 +3964 3478 +3964 3478 +3964 3478 +3964 3478 +3964 3478 +3905 3422 +3905 3422 +3905 3422 +3905 3422 +3905 3422 +3905 3422 +3846 3368 +3846 3368 +3846 3368 +3846 3368 +3846 3368 +3846 3368 +3846 3368 +3783 3313 +3783 3313 +3783 3313 +3783 3313 +3783 3313 +3783 3313 +3724 3259 +3724 3259 +3724 3259 +3724 3259 +3724 3259 +3724 3259 +3724 3259 +3666 3207 +3666 3207 +3666 3207 +3666 3207 +3666 3207 +3666 3207 +3608 3156 +3608 3156 +3608 3156 +3608 3156 +3608 3156 +3608 3156 +3608 3156 +3543 3101 +3543 3101 +3543 3101 +3543 3101 +3543 3101 +3543 3101 +3483 3047 +3483 3047 +3483 3047 +3483 3047 +3483 3047 +3483 3047 +3483 3047 +3425 2995 +3425 2995 +3425 2995 +3425 2995 +3425 2995 +3425 2995 +3359 2940 +3359 2940 +3359 2940 +3359 2940 +3359 2940 +3359 2940 +3359 2940 +3300 2887 +3300 2887 +3300 2887 +3300 2887 +3300 2887 +3300 2887 +3241 2834 +3241 2834 +3241 2834 +3241 2834 +3241 2834 +3241 2834 +3241 2834 +3176 2779 +3176 2779 +3176 2779 +3176 2779 +3176 2779 +3176 2779 +3116 2725 +3116 2725 +3116 2725 +3116 2725 +3116 2725 +3116 2725 +3116 2725 +3058 2672 +3058 2672 +3058 2672 +3058 2672 +3058 2672 +3058 2672 +2999 2620 +2999 2620 +2999 2620 +2999 2620 +2999 2620 +2999 2620 +2999 2620 +2935 2566 +2935 2566 +2935 2566 +2935 2566 +2935 2566 +2935 2566 +2876 2513 +2876 2513 +2876 2513 +2876 2513 +2876 2513 +2876 2513 +2818 2461 +2818 2461 +2818 2461 +2818 2461 +2818 2461 +2818 2461 +2818 2461 +2755 2407 +2755 2407 +2755 2407 +2755 2407 +2755 2407 +2755 2407 +2698 2356 +2698 2356 +2698 2356 +2698 2356 +2698 2356 +2698 2356 +2698 2356 +2640 2304 +2640 2304 +2640 2304 +2640 2304 +2640 2304 +2640 2304 +2578 2251 +2578 2251 +2578 2251 +2578 2251 +2578 2251 +2578 2251 +2578 2251 +2520 2200 +2520 2200 +2520 2200 +2520 2200 +2520 2200 +2520 2200 +2463 2149 +2463 2149 +2463 2149 +2463 2149 +2463 2149 +2463 2149 +2463 2149 +2405 2098 +2405 2098 +2405 2098 +2405 2098 +2405 2098 +2405 2098 +2405 2098 +2342 2044 +2342 2044 +2342 2044 +2342 2044 +2342 2044 +2342 2044 +2286 1994 +2286 1994 +2286 1994 +2286 1994 +2286 1994 +2286 1994 +2228 1943 +2228 1943 +2228 1943 +2228 1943 +2228 1943 +2228 1943 +2228 1943 +2165 1889 +2165 1889 +2165 1889 +2165 1889 +2165 1889 +2165 1889 +2165 1889 +2107 1837 +2107 1837 +2107 1837 +2107 1837 +2107 1837 +2107 1837 +2049 1786 +2049 1786 +2049 1786 +2049 1786 +2049 1786 +2049 1786 +1986 1732 +1986 1732 +1986 1732 +1986 1732 +1986 1732 +1986 1732 +1986 1732 +1929 1680 +1929 1680 +1929 1680 +1929 1680 +1929 1680 +1929 1680 +1872 1629 +1872 1629 +1872 1629 +1872 1629 +1872 1629 +1872 1629 +1872 1629 +1815 1579 +1815 1579 +1815 1579 +1815 1579 +1815 1579 +1815 1579 +1752 1526 +1752 1526 +1752 1526 +1752 1526 +1752 1526 +1752 1526 +1752 1526 +1696 1475 +1696 1475 +1696 1475 +1696 1475 +1696 1475 +1696 1475 +1696 1475 +1639 1425 +1639 1425 +1639 1425 +1639 1425 +1639 1425 +1639 1425 +1577 1372 +1577 1372 +1577 1372 +1577 1372 +1577 1372 +1577 1372 +1577 1372 +1520 1321 +1520 1321 +1520 1321 +1520 1321 +1520 1321 +1520 1321 +1464 1271 +1464 1271 +1464 1271 +1464 1271 +1464 1271 +1464 1271 +1464 1271 +1406 1220 +1406 1220 +1406 1220 +1406 1220 +1406 1220 +1406 1220 +1343 1167 +1343 1167 +1343 1167 +1343 1167 +1343 1167 +1343 1167 +1343 1167 +1286 1116 +1286 1116 +1286 1116 +1286 1116 +1286 1116 +1286 1116 +1229 1065 +1229 1065 +1229 1065 +1229 1065 +1229 1065 +1229 1065 +1165 1012 +1165 1012 +1165 1012 +1165 1012 +1165 1012 +1165 1012 +1165 1012 +1108 960 +1108 960 +1108 960 +1108 960 +1108 960 +1108 960 +1108 960 +1051 909 +1051 909 +1051 909 +1051 909 +1051 909 +1051 909 +988 856 +988 856 +988 856 +988 856 +988 856 +988 856 +988 856 +930 803 +930 803 +930 803 +930 803 +930 803 +930 803 +873 752 +873 752 +873 752 +873 752 +873 752 +873 752 +873 752 +816 701 +816 701 +816 701 +816 701 +816 701 +816 701 +816 701 +752 648 +752 648 +752 648 +752 648 +752 648 +752 648 +696 597 +696 597 +696 597 +696 597 +696 597 +696 597 +696 597 +638 546 +638 546 +638 546 +638 546 +638 546 +638 546 +576 493 +576 493 +576 493 +576 493 +576 493 +576 493 +576 493 +519 442 +519 442 +519 442 +519 442 +519 442 +519 442 +462 391 +462 391 +462 391 +462 391 +462 391 +462 391 +462 391 +398 337 +398 337 +398 337 +398 337 +398 337 +398 337 +398 337 +340 285 +340 285 +340 285 +340 285 +340 285 +340 285 +283 234 +283 234 +283 234 +283 234 +283 234 +283 234 +283 234 +224 182 +224 182 +224 182 +224 182 +224 182 +224 182 +224 182 +160 128 +160 128 +160 128 +160 128 +160 128 +160 128 +101 77 +101 77 +101 77 +101 77 +101 77 +101 77 +101 77 +42 24 +42 24 +42 24 +42 24 +42 24 +42 24 +4073 3572 +4073 3572 +4073 3572 +4073 3572 +4073 3572 +4073 3572 +4073 3572 +4014 3520 +4014 3520 +4014 3520 +4014 3520 +4014 3520 +4014 3520 +4014 3520 +3956 3468 +3956 3468 +3956 3468 +3956 3468 +3956 3468 +3956 3468 +3892 3411 +3892 3411 +3892 3411 +3892 3411 +3892 3411 +3892 3411 +3834 3357 +3834 3357 +3834 3357 +3834 3357 +3834 3357 +3834 3357 +3834 3357 +3775 3304 +3775 3304 +3775 3304 +3775 3304 +3775 3304 +3775 3304 +3718 3253 +3718 3253 +3718 3253 +3718 3253 +3718 3253 +3718 3253 +3718 3253 +3654 3198 +3654 3198 +3654 3198 +3654 3198 +3654 3198 +3654 3198 +3595 3145 +3595 3145 +3595 3145 +3595 3145 +3595 3145 +3595 3145 +3595 3145 +3536 3093 +3536 3093 +3536 3093 +3536 3093 +3536 3093 +3536 3093 +3471 3038 +3471 3038 +3471 3038 +3471 3038 +3471 3038 +3471 3038 +3471 3038 +3413 2985 +3413 2985 +3413 2985 +3413 2985 +3413 2985 +3413 2985 +3353 2932 +3353 2932 +3353 2932 +3353 2932 +3353 2932 +3353 2932 +3353 2932 +3294 2880 +3294 2880 +3294 2880 +3294 2880 +3294 2880 +3294 2880 +3230 2826 +3230 2826 +3230 2826 +3230 2826 +3230 2826 +3230 2826 +3230 2826 +3171 2773 +3171 2773 +3171 2773 +3171 2773 +3171 2773 +3171 2773 +3112 2720 +3112 2720 +3112 2720 +3112 2720 +3112 2720 +3112 2720 +3112 2720 +3047 2666 +3047 2666 +3047 2666 +3047 2666 +3047 2666 +3047 2666 +2989 2613 +2989 2613 +2989 2613 +2989 2613 +2989 2613 +2989 2613 +2989 2613 +2930 2560 +2930 2560 +2930 2560 +2930 2560 +2930 2560 +2930 2560 +2867 2506 +2867 2506 +2867 2506 +2867 2506 +2867 2506 +2867 2506 +2867 2506 +2808 2453 +2808 2453 +2808 2453 +2808 2453 +2808 2453 +2808 2453 +2751 2402 +2751 2402 +2751 2402 +2751 2402 +2751 2402 +2751 2402 +2751 2402 +2694 2351 +2694 2351 +2694 2351 +2694 2351 +2694 2351 +2694 2351 +2631 2298 +2631 2298 +2631 2298 +2631 2298 +2631 2298 +2631 2298 +2631 2298 +2575 2247 +2575 2247 +2575 2247 +2575 2247 +2575 2247 +2575 2247 +2517 2196 +2517 2196 +2517 2196 +2517 2196 +2517 2196 +2517 2196 +2517 2196 +2454 2143 +2454 2143 +2454 2143 +2454 2143 +2454 2143 +2454 2143 +2397 2092 +2397 2092 +2397 2092 +2397 2092 +2397 2092 +2397 2092 +2397 2092 +2340 2041 +2340 2041 +2340 2041 +2340 2041 +2340 2041 +2340 2041 +2340 2041 +2276 1987 +2276 1987 +2276 1987 +2276 1987 +2276 1987 +2276 1987 +2220 1936 +2220 1936 +2220 1936 +2220 1936 +2220 1936 +2220 1936 +2162 1885 +2162 1885 +2162 1885 +2162 1885 +2162 1885 +2162 1885 +2162 1885 +2105 1834 +2105 1834 +2105 1834 +2105 1834 +2105 1834 +2105 1834 +2041 1781 +2041 1781 +2041 1781 +2041 1781 +2041 1781 +2041 1781 +2041 1781 +1985 1730 +1985 1730 +1985 1730 +1985 1730 +1985 1730 +1985 1730 +1985 1730 +1927 1678 +1927 1678 +1927 1678 +1927 1678 +1927 1678 +1927 1678 +1864 1625 +1864 1625 +1864 1625 +1864 1625 +1864 1625 +1864 1625 +1864 1625 +1807 1573 +1807 1573 +1807 1573 +1807 1573 +1807 1573 +1807 1573 +1751 1523 +1751 1523 +1751 1523 +1751 1523 +1751 1523 +1751 1523 +1751 1523 +1689 1471 +1689 1471 +1689 1471 +1689 1471 +1689 1471 +1689 1471 +1632 1419 +1632 1419 +1632 1419 +1632 1419 +1632 1419 +1632 1419 +1632 1419 +1576 1369 +1576 1369 +1576 1369 +1576 1369 +1576 1369 +1576 1369 +1519 1319 +1519 1319 +1519 1319 +1519 1319 +1519 1319 +1519 1319 +1456 1266 +1456 1266 +1456 1266 +1456 1266 +1456 1266 +1456 1266 +1456 1266 +1399 1215 +1399 1215 +1399 1215 +1399 1215 +1399 1215 +1399 1215 +1399 1215 +1342 1164 +1342 1164 +1342 1164 +1342 1164 +1342 1164 +1342 1164 +1280 1112 +1280 1112 +1280 1112 +1280 1112 +1280 1112 +1280 1112 +1280 1112 +1222 1060 +1222 1060 +1222 1060 +1222 1060 +1222 1060 +1222 1060 +1165 1009 +1165 1009 +1165 1009 +1165 1009 +1165 1009 +1165 1009 +1165 1009 +1108 959 +1108 959 +1108 959 +1108 959 +1108 959 +1108 959 +1045 906 +1045 906 +1045 906 +1045 906 +1045 906 +1045 906 +1045 906 +987 853 +987 853 +987 853 +987 853 +987 853 +987 853 +930 802 +930 802 +930 802 +930 802 +930 802 +930 802 +930 802 +867 749 +867 749 +867 749 +867 749 +867 749 +867 749 +809 697 +809 697 +809 697 +809 697 +809 697 +809 697 +809 697 +752 646 +752 646 +752 646 +752 646 +752 646 +752 646 +752 646 +689 592 +689 592 +689 592 +689 592 +689 592 +689 592 +632 541 +632 541 +632 541 +632 541 +632 541 +632 541 +632 541 +576 491 +576 491 +576 491 +576 491 +576 491 +576 491 +517 439 +517 439 +517 439 +517 439 +517 439 +517 439 +517 439 +454 386 +454 386 +454 386 +454 386 +454 386 +454 386 +454 386 +398 335 +398 335 +398 335 +398 335 +398 335 +398 335 +339 283 +339 283 +339 283 +339 283 +339 283 +339 283 +339 283 +276 230 +276 230 +276 230 +276 230 +276 230 +276 230 +217 177 +217 177 +217 177 +217 177 +217 177 +217 177 +217 177 +159 125 +159 125 +159 125 +159 125 +159 125 +159 125 +159 125 +95 73 +95 73 +95 73 +95 73 +95 73 +95 73 +37 21 +37 21 +37 21 +37 21 +37 21 +37 21 +37 21 +4073 3569 +4073 3569 +4073 3569 +4073 3569 +4073 3569 +4073 3569 +4073 3569 +4014 3517 +4014 3517 +4014 3517 +4014 3517 +4014 3517 +4014 3517 +3949 3465 +3949 3465 +3949 3465 +3949 3465 +3949 3465 +3949 3465 +3949 3465 +3891 3409 +3891 3409 +3891 3409 +3891 3409 +3891 3409 +3891 3409 +3832 3355 +3832 3355 +3832 3355 +3832 3355 +3832 3355 +3832 3355 +3832 3355 +3769 3300 +3769 3300 +3769 3300 +3769 3300 +3769 3300 +3769 3300 +3769 3300 +3712 3248 +3712 3248 +3712 3248 +3712 3248 +3712 3248 +3712 3248 +3653 3196 +3653 3196 +3653 3196 +3653 3196 +3653 3196 +3653 3196 +3589 3142 +3589 3142 +3589 3142 +3589 3142 +3589 3142 +3589 3142 +3589 3142 +3530 3088 +3530 3088 +3530 3088 +3530 3088 +3530 3088 +3530 3088 +3471 3036 +3471 3036 +3471 3036 +3471 3036 +3471 3036 +3471 3036 +3471 3036 +3412 2984 +3412 2984 +3412 2984 +3412 2984 +3412 2984 +3412 2984 +3347 2929 +3347 2929 +3347 2929 +3347 2929 +3347 2929 +3347 2929 +3347 2929 +3288 2876 +3288 2876 +3288 2876 +3288 2876 +3288 2876 +3288 2876 +3228 2823 +3228 2823 +3228 2823 +3228 2823 +3228 2823 +3228 2823 +3228 2823 +3164 2768 +3164 2768 +3164 2768 +3164 2768 +3164 2768 +3164 2768 +3164 2768 +3105 2715 +3105 2715 +3105 2715 +3105 2715 +3105 2715 +3105 2715 +3046 2662 +3046 2662 +3046 2662 +3046 2662 +3046 2662 +3046 2662 +2987 2610 +2987 2610 +2987 2610 +2987 2610 +2987 2610 +2987 2610 +2987 2610 +2923 2556 +2923 2556 +2923 2556 +2923 2556 +2923 2556 +2923 2556 +2865 2503 +2865 2503 +2865 2503 +2865 2503 +2865 2503 +2865 2503 +2865 2503 +2807 2451 +2807 2451 +2807 2451 +2807 2451 +2807 2451 +2807 2451 +2744 2398 +2744 2398 +2744 2398 +2744 2398 +2744 2398 +2744 2398 +2744 2398 +2686 2346 +2686 2346 +2686 2346 +2686 2346 +2686 2346 +2686 2346 +2630 2295 +2630 2295 +2630 2295 +2630 2295 +2630 2295 +2630 2295 +2630 2295 +2566 2242 +2566 2242 +2566 2242 +2566 2242 +2566 2242 +2566 2242 +2511 2191 +2511 2191 +2511 2191 +2511 2191 +2511 2191 +2511 2191 +2511 2191 +2453 2140 +2453 2140 +2453 2140 +2453 2140 +2453 2140 +2453 2140 +2453 2140 +2396 2089 +2396 2089 +2396 2089 +2396 2089 +2396 2089 +2396 2089 +2333 2037 +2333 2037 +2333 2037 +2333 2037 +2333 2037 +2333 2037 +2276 1985 +2276 1985 +2276 1985 +2276 1985 +2276 1985 +2276 1985 +2276 1985 +2218 1934 +2218 1934 +2218 1934 +2218 1934 +2218 1934 +2218 1934 +2155 1881 +2155 1881 +2155 1881 +2155 1881 +2155 1881 +2155 1881 +2155 1881 +2098 1829 +2098 1829 +2098 1829 +2098 1829 +2098 1829 +2098 1829 +2041 1778 +2041 1778 +2041 1778 +2041 1778 +2041 1778 +2041 1778 +2041 1778 +1978 1725 +1978 1725 +1978 1725 +1978 1725 +1978 1725 +1978 1725 +1921 1673 +1921 1673 +1921 1673 +1921 1673 +1921 1673 +1921 1673 +1921 1673 +1864 1623 +1864 1623 +1864 1623 +1864 1623 +1864 1623 +1864 1623 +1807 1572 +1807 1572 +1807 1572 +1807 1572 +1807 1572 +1807 1572 +1807 1572 +1745 1520 +1745 1520 +1745 1520 +1745 1520 +1745 1520 +1745 1520 +1687 1468 +1687 1468 +1687 1468 +1687 1468 +1687 1468 +1687 1468 +1687 1468 +1631 1418 +1631 1418 +1631 1418 +1631 1418 +1631 1418 +1631 1418 +1569 1365 +1569 1365 +1569 1365 +1569 1365 +1569 1365 +1569 1365 +1569 1365 +1513 1314 +1513 1314 +1513 1314 +1513 1314 +1513 1314 +1513 1314 +1456 1264 +1456 1264 +1456 1264 +1456 1264 +1456 1264 +1456 1264 +1456 1264 +1393 1211 +1393 1211 +1393 1211 +1393 1211 +1393 1211 +1393 1211 +1336 1160 +1336 1160 +1336 1160 +1336 1160 +1336 1160 +1336 1160 +1336 1160 +1278 1108 +1278 1108 +1278 1108 +1278 1108 +1278 1108 +1278 1108 +1221 1058 +1221 1058 +1221 1058 +1221 1058 +1221 1058 +1221 1058 +1221 1058 +1158 1005 +1158 1005 +1158 1005 +1158 1005 +1158 1005 +1158 1005 +1101 953 +1101 953 +1101 953 +1101 953 +1101 953 +1101 953 +1101 953 +1043 902 +1043 902 +1043 902 +1043 902 +1043 902 +1043 902 +980 849 +980 849 +980 849 +980 849 +980 849 +980 849 +980 849 +922 796 +922 796 +922 796 +922 796 +922 796 +922 796 +865 745 +865 745 +865 745 +865 745 +865 745 +865 745 +865 745 +802 692 +802 692 +802 692 +802 692 +802 692 +802 692 +802 692 +745 640 +745 640 +745 640 +745 640 +745 640 +745 640 +687 589 +687 589 +687 589 +687 589 +687 589 +687 589 +687 589 +630 538 +630 538 +630 538 +630 538 +630 538 +630 538 +630 538 +568 486 +568 486 +568 486 +568 486 +568 486 +568 486 +509 433 +509 433 +509 433 +509 433 +509 433 +509 433 +509 433 +452 382 +452 382 +452 382 +452 382 +452 382 +452 382 +388 328 +388 328 +388 328 +388 328 +388 328 +388 328 +388 328 +330 276 +330 276 +330 276 +330 276 +330 276 +330 276 +272 224 +272 224 +272 224 +272 224 +272 224 +272 224 +272 224 +214 173 +214 173 +214 173 +214 173 +214 173 +214 173 +214 173 +150 119 +150 119 +150 119 +150 119 +150 119 +150 119 +91 67 +91 67 +91 67 +91 67 +91 67 +91 67 +91 67 +32 15 +32 15 +32 15 +32 15 +32 15 +32 15 +32 15 +4063 3563 +4063 3563 +4063 3563 +4063 3563 +4063 3563 +4063 3563 +4063 3563 +4004 3511 +4004 3511 +4004 3511 +4004 3511 +4004 3511 +4004 3511 +3945 3459 +3945 3459 +3945 3459 +3945 3459 +3945 3459 +3945 3459 +3945 3459 +3881 3402 +3881 3402 +3881 3402 +3881 3402 +3881 3402 +3881 3402 +3823 3347 +3823 3347 +3823 3347 +3823 3347 +3823 3347 +3823 3347 +3823 3347 +3765 3295 +3765 3295 +3765 3295 +3765 3295 +3765 3295 +3765 3295 +3707 3243 +3707 3243 +3707 3243 +3707 3243 +3707 3243 +3707 3243 +3707 3243 +3642 3188 +3642 3188 +3642 3188 +3642 3188 +3642 3188 +3642 3188 +3584 3136 +3584 3136 +3584 3136 +3584 3136 +3584 3136 +3584 3136 +3584 3136 +3525 3083 +3525 3083 +3525 3083 +3525 3083 +3525 3083 +3525 3083 +3460 3029 +3460 3029 +3460 3029 +3460 3029 +3460 3029 +3460 3029 +3460 3029 +3401 2975 +3401 2975 +3401 2975 +3401 2975 +3401 2975 +3401 2975 +3342 2923 +3342 2923 +3342 2923 +3342 2923 +3342 2923 +3342 2923 +3342 2923 +3278 2868 +3278 2868 +3278 2868 +3278 2868 +3278 2868 +3278 2868 +3219 2815 +3219 2815 +3219 2815 +3219 2815 +3219 2815 +3219 2815 +3219 2815 +3160 2762 +3160 2762 +3160 2762 +3160 2762 +3160 2762 +3160 2762 +3100 2710 +3100 2710 +3100 2710 +3100 2710 +3100 2710 +3100 2710 +3100 2710 +3036 2655 +3036 2655 +3036 2655 +3036 2655 +3036 2655 +3036 2655 +2977 2602 +2977 2602 +2977 2602 +2977 2602 +2977 2602 +2977 2602 +2977 2602 +2919 2550 +2919 2550 +2919 2550 +2919 2550 +2919 2550 +2919 2550 +2855 2496 +2855 2496 +2855 2496 +2855 2496 +2855 2496 +2855 2496 +2855 2496 +2797 2443 +2797 2443 +2797 2443 +2797 2443 +2797 2443 +2797 2443 +2739 2392 +2739 2392 +2739 2392 +2739 2392 +2739 2392 +2739 2392 +2739 2392 +2677 2339 +2677 2339 +2677 2339 +2677 2339 +2677 2339 +2677 2339 +2620 2287 +2620 2287 +2620 2287 +2620 2287 +2620 2287 +2620 2287 +2620 2287 +2563 2236 +2563 2236 +2563 2236 +2563 2236 +2563 2236 +2563 2236 +2505 2185 +2505 2185 +2505 2185 +2505 2185 +2505 2185 +2505 2185 +2505 2185 +2443 2133 +2443 2133 +2443 2133 +2443 2133 +2443 2133 +2443 2133 +2386 2082 +2386 2082 +2386 2082 +2386 2082 +2386 2082 +2386 2082 +2386 2082 +2328 2030 +2328 2030 +2328 2030 +2328 2030 +2328 2030 +2328 2030 +2266 1978 +2266 1978 +2266 1978 +2266 1978 +2266 1978 +2266 1978 +2266 1978 +2208 1926 +2208 1926 +2208 1926 +2208 1926 +2208 1926 +2208 1926 +2150 1875 +2150 1875 +2150 1875 +2150 1875 +2150 1875 +2150 1875 +2094 1824 +2094 1824 +2094 1824 +2094 1824 +2094 1824 +2094 1824 +2094 1824 +2030 1771 +2030 1771 +2030 1771 +2030 1771 +2030 1771 +2030 1771 +2030 1771 +1972 1719 +1972 1719 +1972 1719 +1972 1719 +1972 1719 +1972 1719 +1915 1668 +1915 1668 +1915 1668 +1915 1668 +1915 1668 +1915 1668 +1853 1615 +1853 1615 +1853 1615 +1853 1615 +1853 1615 +1853 1615 +1853 1615 +1796 1564 +1796 1564 +1796 1564 +1796 1564 +1796 1564 +1796 1564 +1796 1564 +1740 1513 +1740 1513 +1740 1513 +1740 1513 +1740 1513 +1740 1513 +1677 1460 +1677 1460 +1677 1460 +1677 1460 +1677 1460 +1677 1460 +1677 1460 +1621 1409 +1621 1409 +1621 1409 +1621 1409 +1621 1409 +1621 1409 +1565 1360 +1565 1360 +1565 1360 +1565 1360 +1565 1360 +1565 1360 +1565 1360 +1508 1309 +1508 1309 +1508 1309 +1508 1309 +1508 1309 +1508 1309 +1445 1257 +1445 1257 +1445 1257 +1445 1257 +1445 1257 +1445 1257 +1445 1257 +1388 1205 +1388 1205 +1388 1205 +1388 1205 +1388 1205 +1388 1205 +1332 1155 +1332 1155 +1332 1155 +1332 1155 +1332 1155 +1332 1155 +1332 1155 +1269 1102 +1269 1102 +1269 1102 +1269 1102 +1269 1102 +1269 1102 +1212 1051 +1212 1051 +1212 1051 +1212 1051 +1212 1051 +1212 1051 +1212 1051 +1154 1000 +1154 1000 +1154 1000 +1154 1000 +1154 1000 +1154 1000 +1091 946 +1091 946 +1091 946 +1091 946 +1091 946 +1091 946 +1091 946 +1034 894 +1034 894 +1034 894 +1034 894 +1034 894 +1034 894 +977 843 +977 843 +977 843 +977 843 +977 843 +977 843 +977 843 +918 792 +918 792 +918 792 +918 792 +918 792 +918 792 +856 739 +856 739 +856 739 +856 739 +856 739 +856 739 +856 739 +799 688 +799 688 +799 688 +799 688 +799 688 +799 688 +799 688 +741 636 +741 636 +741 636 +741 636 +741 636 +741 636 +679 584 +679 584 +679 584 +679 584 +679 584 +679 584 +679 584 +622 532 +622 532 +622 532 +622 532 +622 532 +622 532 +565 481 +565 481 +565 481 +565 481 +565 481 +565 481 +565 481 +502 428 +502 428 +502 428 +502 428 +502 428 +502 428 +502 428 +445 377 +445 377 +445 377 +445 377 +445 377 +445 377 +387 325 +387 325 +387 325 +387 325 +387 325 +387 325 +387 325 +330 274 +330 274 +330 274 +330 274 +330 274 +330 274 +330 274 +264 220 +264 220 +264 220 +264 220 +264 220 +264 220 +206 168 +206 168 +206 168 +206 168 +206 168 +206 168 +206 168 +148 116 +148 116 +148 116 +148 116 +148 116 +148 116 +25 63 +25 63 +25 63 +25 63 +25 63 +25 63 +25 63 +25 12 +25 12 +25 12 +25 12 +25 12 +25 12 +25 12 +4062 3560 +4062 3560 +4062 3560 +4062 3560 +4062 3560 +4062 3560 +4003 3507 +4003 3507 +4003 3507 +4003 3507 +4003 3507 +4003 3507 +4003 3507 +3939 3455 +3939 3455 +3939 3455 +3939 3455 +3939 3455 +3939 3455 +3939 3455 +3880 3400 +3880 3400 +3880 3400 +3880 3400 +3880 3400 +3880 3400 +3822 3346 +3822 3346 +3822 3346 +3822 3346 +3822 3346 +3822 3346 +3822 3346 +3758 3291 +3758 3291 +3758 3291 +3758 3291 +3758 3291 +3758 3291 +3701 3239 +3701 3239 +3701 3239 +3701 3239 +3701 3239 +3701 3239 +3643 3187 +3643 3187 +3643 3187 +3643 3187 +3643 3187 +3643 3187 +3643 3187 +3579 3133 +3579 3133 +3579 3133 +3579 3133 +3579 3133 +3579 3133 +3579 3133 +3520 3079 +3520 3079 +3520 3079 +3520 3079 +3520 3079 +3520 3079 +3461 3027 +3461 3027 +3461 3027 +3461 3027 +3461 3027 +3461 3027 +3401 2975 +3401 2975 +3401 2975 +3401 2975 +3401 2975 +3401 2975 +3401 2975 +3337 2920 +3337 2920 +3337 2920 +3337 2920 +3337 2920 +3337 2920 +3277 2866 +3277 2866 +3277 2866 +3277 2866 +3277 2866 +3277 2866 +3277 2866 +3219 2815 +3219 2815 +3219 2815 +3219 2815 +3219 2815 +3219 2815 +3154 2760 +3154 2760 +3154 2760 +3154 2760 +3154 2760 +3154 2760 +3154 2760 +3095 2706 +3095 2706 +3095 2706 +3095 2706 +3095 2706 +3095 2706 +3036 2654 +3036 2654 +3036 2654 +3036 2654 +3036 2654 +3036 2654 +3036 2654 +2972 2599 +2972 2599 +2972 2599 +2972 2599 +2972 2599 +2972 2599 +2914 2546 +2914 2546 +2914 2546 +2914 2546 +2914 2546 +2914 2546 +2914 2546 +2855 2494 +2855 2494 +2855 2494 +2855 2494 +2855 2494 +2855 2494 +2797 2443 +2797 2443 +2797 2443 +2797 2443 +2797 2443 +2797 2443 +2797 2443 +2735 2390 +2735 2390 +2735 2390 +2735 2390 +2735 2390 +2735 2390 +2676 2337 +2676 2337 +2676 2337 +2676 2337 +2676 2337 +2676 2337 +2676 2337 +2619 2286 +2619 2286 +2619 2286 +2619 2286 +2619 2286 +2619 2286 +2558 2234 +2558 2234 +2558 2234 +2558 2234 +2558 2234 +2558 2234 +2558 2234 +2500 2182 +2500 2182 +2500 2182 +2500 2182 +2500 2182 +2500 2182 +2443 2131 +2443 2131 +2443 2131 +2443 2131 +2443 2131 +2443 2131 +2443 2131 +2380 2078 +2380 2078 +2380 2078 +2380 2078 +2380 2078 +2380 2078 +2322 2026 +2322 2026 +2322 2026 +2322 2026 +2322 2026 +2322 2026 +2322 2026 +2266 1975 +2266 1975 +2266 1975 +2266 1975 +2266 1975 +2266 1975 +2208 1925 +2208 1925 +2208 1925 +2208 1925 +2208 1925 +2208 1925 +2208 1925 +2144 1872 +2144 1872 +2144 1872 +2144 1872 +2144 1872 +2144 1872 +2086 1819 +2086 1819 +2086 1819 +2086 1819 +2086 1819 +2086 1819 +2086 1819 +2030 1768 +2030 1768 +2030 1768 +2030 1768 +2030 1768 +2030 1768 +1966 1715 +1966 1715 +1966 1715 +1966 1715 +1966 1715 +1966 1715 +1966 1715 +1909 1663 +1909 1663 +1909 1663 +1909 1663 +1909 1663 +1909 1663 +1852 1612 +1852 1612 +1852 1612 +1852 1612 +1852 1612 +1852 1612 +1852 1612 +1790 1559 +1790 1559 +1790 1559 +1790 1559 +1790 1559 +1790 1559 +1733 1508 +1733 1508 +1733 1508 +1733 1508 +1733 1508 +1733 1508 +1733 1508 +1676 1458 +1676 1458 +1676 1458 +1676 1458 +1676 1458 +1676 1458 +1620 1407 +1620 1407 +1620 1407 +1620 1407 +1620 1407 +1620 1407 +1620 1407 +1558 1355 +1558 1355 +1558 1355 +1558 1355 +1558 1355 +1558 1355 +1501 1304 +1501 1304 +1501 1304 +1501 1304 +1501 1304 +1501 1304 +1501 1304 +1444 1254 +1444 1254 +1444 1254 +1444 1254 +1444 1254 +1444 1254 +1382 1201 +1382 1201 +1382 1201 +1382 1201 +1382 1201 +1382 1201 +1382 1201 +1324 1149 +1324 1149 +1324 1149 +1324 1149 +1324 1149 +1324 1149 +1267 1099 +1267 1099 +1267 1099 +1267 1099 +1267 1099 +1267 1099 +1267 1099 +1209 1048 +1209 1048 +1209 1048 +1209 1048 +1209 1048 +1209 1048 +1146 994 +1146 994 +1146 994 +1146 994 +1146 994 +1146 994 +1146 994 +1089 943 +1089 943 +1089 943 +1089 943 +1089 943 +1089 943 +1089 943 +1030 891 +1030 891 +1030 891 +1030 891 +1030 891 +1030 891 +967 837 +967 837 +967 837 +967 837 +967 837 +967 837 +967 837 +910 785 +910 785 +910 785 +910 785 +910 785 +910 785 +853 735 +853 735 +853 735 +853 735 +853 735 +853 735 +853 735 +790 681 +790 681 +790 681 +790 681 +790 681 +790 681 +733 630 +733 630 +733 630 +733 630 +733 630 +733 630 +733 630 +676 579 +676 579 +676 579 +676 579 +676 579 +676 579 +676 579 +618 528 +618 528 +618 528 +618 528 +618 528 +618 528 +555 475 +555 475 +555 475 +555 475 +555 475 +555 475 +555 475 +497 423 +497 423 +497 423 +497 423 +497 423 +497 423 +497 423 +440 371 +440 371 +440 371 +440 371 +440 371 +440 371 +376 318 +376 318 +376 318 +376 318 +376 318 +376 318 +376 318 +320 266 +320 266 +320 266 +320 266 +320 266 +320 266 +261 215 +261 215 +261 215 +261 215 +261 215 +261 215 +261 215 +196 160 +196 160 +196 160 +196 160 +196 160 +196 160 +137 109 +137 109 +137 109 +137 109 +137 109 +137 109 +137 109 +79 57 +79 57 +79 57 +79 57 +79 57 +79 57 +79 57 +20 4 +20 4 +20 4 +20 4 +20 4 +20 4 +20 4 +4051 3552 +4051 3552 +4051 3552 +4051 3552 +4051 3552 +4051 3552 +3992 3500 +3992 3500 +3992 3500 +3992 3500 +3992 3500 +3992 3500 +3933 3448 +3933 3448 +3933 3448 +3933 3448 +3933 3448 +3933 3448 +3933 3448 +3869 3391 +3869 3391 +3869 3391 +3869 3391 +3869 3391 +3869 3391 +3869 3391 +3811 3337 +3811 3337 +3811 3337 +3811 3337 +3811 3337 +3811 3337 +3753 3284 +3753 3284 +3753 3284 +3753 3284 +3753 3284 +3753 3284 +3753 3284 +3688 3229 +3688 3229 +3688 3229 +3688 3229 +3688 3229 +3688 3229 +3630 3176 +3630 3176 +3630 3176 +3630 3176 +3630 3176 +3630 3176 +3572 3124 +3572 3124 +3572 3124 +3572 3124 +3572 3124 +3572 3124 +3572 3124 +3513 3072 +3513 3072 +3513 3072 +3513 3072 +3513 3072 +3513 3072 +3513 3072 +3448 3018 +3448 3018 +3448 3018 +3448 3018 +3448 3018 +3448 3018 +3388 2964 +3388 2964 +3388 2964 +3388 2964 +3388 2964 +3388 2964 +3388 2964 +3329 2911 +3329 2911 +3329 2911 +3329 2911 +3329 2911 +3329 2911 +3264 2856 +3264 2856 +3264 2856 +3264 2856 +3264 2856 +3264 2856 +3264 2856 +3205 2803 +3205 2803 +3205 2803 +3205 2803 +3205 2803 +3205 2803 +3146 2750 +3146 2750 +3146 2750 +3146 2750 +3146 2750 +3146 2750 +3146 2750 +3086 2698 +3086 2698 +3086 2698 +3086 2698 +3086 2698 +3086 2698 +3022 2643 +3022 2643 +3022 2643 +3022 2643 +3022 2643 +3022 2643 +3022 2643 +2963 2590 +2963 2590 +2963 2590 +2963 2590 +2963 2590 +2963 2590 +2905 2538 +2905 2538 +2905 2538 +2905 2538 +2905 2538 +2905 2538 +2905 2538 +2841 2484 +2841 2484 +2841 2484 +2841 2484 +2841 2484 +2841 2484 +2783 2431 +2783 2431 +2783 2431 +2783 2431 +2783 2431 +2783 2431 +2783 2431 +2725 2379 +2725 2379 +2725 2379 +2725 2379 +2725 2379 +2725 2379 +2663 2326 +2663 2326 +2663 2326 +2663 2326 +2663 2326 +2663 2326 +2663 2326 +2606 2275 +2606 2275 +2606 2275 +2606 2275 +2606 2275 +2606 2275 +2549 2224 +2549 2224 +2549 2224 +2549 2224 +2549 2224 +2549 2224 +2549 2224 +2492 2174 +2492 2174 +2492 2174 +2492 2174 +2492 2174 +2492 2174 +2429 2121 +2429 2121 +2429 2121 +2429 2121 +2429 2121 +2429 2121 +2429 2121 +2372 2069 +2372 2069 +2372 2069 +2372 2069 +2372 2069 +2372 2069 +2315 2018 +2315 2018 +2315 2018 +2315 2018 +2315 2018 +2315 2018 +2315 2018 +2252 1966 +2252 1966 +2252 1966 +2252 1966 +2252 1966 +2252 1966 +2194 1914 +2194 1914 +2194 1914 +2194 1914 +2194 1914 +2194 1914 +2194 1914 +2137 1863 +2137 1863 +2137 1863 +2137 1863 +2137 1863 +2137 1863 +2075 1810 +2075 1810 +2075 1810 +2075 1810 +2075 1810 +2075 1810 +2075 1810 +2017 1758 +2017 1758 +2017 1758 +2017 1758 +2017 1758 +2017 1758 +1959 1707 +1959 1707 +1959 1707 +1959 1707 +1959 1707 +1959 1707 +1959 1707 +1903 1656 +1903 1656 +1903 1656 +1903 1656 +1903 1656 +1903 1656 +1903 1656 +1839 1603 +1839 1603 +1839 1603 +1839 1603 +1839 1603 +1839 1603 +1783 1552 +1783 1552 +1783 1552 +1783 1552 +1783 1552 +1783 1552 +1726 1501 +1726 1501 +1726 1501 +1726 1501 +1726 1501 +1726 1501 +1726 1501 +1664 1449 +1664 1449 +1664 1449 +1664 1449 +1664 1449 +1664 1449 +1664 1449 +1607 1398 +1607 1398 +1607 1398 +1607 1398 +1607 1398 +1607 1398 +1550 1347 +1550 1347 +1550 1347 +1550 1347 +1550 1347 +1550 1347 +1550 1347 +1488 1294 +1488 1294 +1488 1294 +1488 1294 +1488 1294 +1488 1294 +1431 1243 +1431 1243 +1431 1243 +1431 1243 +1431 1243 +1431 1243 +1374 1192 +1374 1192 +1374 1192 +1374 1192 +1374 1192 +1374 1192 +1374 1192 +1317 1142 +1317 1142 +1317 1142 +1317 1142 +1317 1142 +1317 1142 +1253 1089 +1253 1089 +1253 1089 +1253 1089 +1253 1089 +1253 1089 +1253 1089 +1195 1037 +1195 1037 +1195 1037 +1195 1037 +1195 1037 +1195 1037 +1195 1037 +1139 986 +1139 986 +1139 986 +1139 986 +1139 986 +1139 986 +1075 932 +1075 932 +1075 932 +1075 932 +1075 932 +1075 932 +1075 932 +1018 880 +1018 880 +1018 880 +1018 880 +1018 880 +1018 880 +960 829 +960 829 +960 829 +960 829 +960 829 +960 829 +960 829 +902 778 +902 778 +902 778 +902 778 +902 778 +902 778 +839 724 +839 724 +839 724 +839 724 +839 724 +839 724 +839 724 +782 673 +782 673 +782 673 +782 673 +782 673 +782 673 +725 622 +725 622 +725 622 +725 622 +725 622 +725 622 +725 622 +662 569 +662 569 +662 569 +662 569 +662 569 +662 569 +662 569 +605 517 +605 517 +605 517 +605 517 +605 517 +605 517 +548 466 +548 466 +548 466 +548 466 +548 466 +548 466 +548 466 +485 413 +485 413 +485 413 +485 413 +485 413 +485 413 +428 362 +428 362 +428 362 +428 362 +428 362 +428 362 +428 362 +370 310 +370 310 +370 310 +370 310 +370 310 +370 310 +311 259 +311 259 +311 259 +311 259 +311 259 +311 259 +311 259 +247 205 +247 205 +247 205 +247 205 +247 205 +247 205 +247 205 +189 152 +189 152 +189 152 +189 152 +189 152 +189 152 +130 100 +130 100 +130 100 +130 100 +130 100 +130 100 +130 100 +66 48 +66 48 +66 48 +66 48 +66 48 +66 48 +66 48 +4103 3596 +4103 3596 +4103 3596 +4103 3596 +4103 3596 +4103 3596 +4044 3544 +4044 3544 +4044 3544 +4044 3544 +4044 3544 +4044 3544 +4044 3544 +3979 3491 +3979 3491 +3979 3491 +3979 3491 +3979 3491 +3979 3491 +3920 3440 +3920 3440 +3920 3440 +3920 3440 +3920 3440 +3920 3440 +3920 3440 +3862 3384 +3862 3384 +3862 3384 +3862 3384 +3862 3384 +3862 3384 +3862 3384 +3804 3330 +3804 3330 +3804 3330 +3804 3330 +3804 3330 +3804 3330 +3741 3276 +3741 3276 +3741 3276 +3741 3276 +3741 3276 +3741 3276 +3741 3276 +3683 3223 +3683 3223 +3683 3223 +3683 3223 +3683 3223 +3683 3223 +3625 3171 +3625 3171 +3625 3171 +3625 3171 +3625 3171 +3625 3171 +3625 3171 +3562 3117 +3562 3117 +3562 3117 +3562 3117 +3562 3117 +3562 3117 +3503 3065 +3503 3065 +3503 3065 +3503 3065 +3503 3065 +3503 3065 +3503 3065 +3444 3012 +3444 3012 +3444 3012 +3444 3012 +3444 3012 +3444 3012 +3379 2957 +3379 2957 +3379 2957 +3379 2957 +3379 2957 +3379 2957 +3379 2957 +3319 2903 +3319 2903 +3319 2903 +3319 2903 +3319 2903 +3319 2903 +3261 2851 +3261 2851 +3261 2851 +3261 2851 +3261 2851 +3261 2851 +3261 2851 +3202 2799 +3202 2799 +3202 2799 +3202 2799 +3202 2799 +3202 2799 +3137 2744 +3137 2744 +3137 2744 +3137 2744 +3137 2744 +3137 2744 +3137 2744 +3077 2691 +3077 2691 +3077 2691 +3077 2691 +3077 2691 +3077 2691 +3019 2638 +3019 2638 +3019 2638 +3019 2638 +3019 2638 +3019 2638 +3019 2638 +2955 2584 +2955 2584 +2955 2584 +2955 2584 +2955 2584 +2955 2584 +2896 2531 +2896 2531 +2896 2531 +2896 2531 +2896 2531 +2896 2531 +2896 2531 +2839 2479 +2839 2479 +2839 2479 +2839 2479 +2839 2479 +2839 2479 +2774 2425 +2774 2425 +2774 2425 +2774 2425 +2774 2425 +2774 2425 +2774 2425 +2717 2373 +2717 2373 +2717 2373 +2717 2373 +2717 2373 +2717 2373 +2659 2321 +2659 2321 +2659 2321 +2659 2321 +2659 2321 +2659 2321 +2659 2321 +2603 2271 +2603 2271 +2603 2271 +2603 2271 +2603 2271 +2603 2271 +2540 2218 +2540 2218 +2540 2218 +2540 2218 +2540 2218 +2540 2218 +2540 2218 +2483 2167 +2483 2167 +2483 2167 +2483 2167 +2483 2167 +2483 2167 +2426 2116 +2426 2116 +2426 2116 +2426 2116 +2426 2116 +2426 2116 +2426 2116 +2363 2063 +2363 2063 +2363 2063 +2363 2063 +2363 2063 +2363 2063 +2306 2012 +2306 2012 +2306 2012 +2306 2012 +2306 2012 +2306 2012 +2306 2012 +2248 1960 +2248 1960 +2248 1960 +2248 1960 +2248 1960 +2248 1960 +2190 1909 +2190 1909 +2190 1909 +2190 1909 +2190 1909 +2190 1909 +2190 1909 +2128 1857 +2128 1857 +2128 1857 +2128 1857 +2128 1857 +2128 1857 +2070 1805 +2070 1805 +2070 1805 +2070 1805 +2070 1805 +2070 1805 +2070 1805 +2013 1753 +2013 1753 +2013 1753 +2013 1753 +2013 1753 +2013 1753 +1949 1700 +1949 1700 +1949 1700 +1949 1700 +1949 1700 +1949 1700 +1949 1700 +1894 1649 +1894 1649 +1894 1649 +1894 1649 +1894 1649 +1894 1649 +1836 1598 +1836 1598 +1836 1598 +1836 1598 +1836 1598 +1836 1598 +1836 1598 +1773 1545 +1773 1545 +1773 1545 +1773 1545 +1773 1545 +1773 1545 +1717 1494 +1717 1494 +1717 1494 +1717 1494 +1717 1494 +1717 1494 +1717 1494 +1661 1444 +1661 1444 +1661 1444 +1661 1444 +1661 1444 +1661 1444 +1604 1394 +1604 1394 +1604 1394 +1604 1394 +1604 1394 +1604 1394 +1541 1341 +1541 1341 +1541 1341 +1541 1341 +1541 1341 +1541 1341 +1541 1341 +1485 1290 +1485 1290 +1485 1290 +1485 1290 +1485 1290 +1485 1290 +1428 1240 +1428 1240 +1428 1240 +1428 1240 +1428 1240 +1428 1240 +1428 1240 +1366 1187 +1366 1187 +1366 1187 +1366 1187 +1366 1187 +1366 1187 +1366 1187 +1308 1136 +1308 1136 +1308 1136 +1308 1136 +1308 1136 +1308 1136 +1251 1085 +1251 1085 +1251 1085 +1251 1085 +1251 1085 +1251 1085 +1188 1031 +1188 1031 +1188 1031 +1188 1031 +1188 1031 +1188 1031 +1188 1031 +1130 979 +1130 979 +1130 979 +1130 979 +1130 979 +1130 979 +1130 979 +1072 928 +1072 928 +1072 928 +1072 928 +1072 928 +1072 928 +1015 877 +1015 877 +1015 877 +1015 877 +1015 877 +1015 877 +952 824 +952 824 +952 824 +952 824 +952 824 +952 824 +952 824 +894 772 +894 772 +894 772 +894 772 +894 772 +894 772 +894 772 +837 721 +837 721 +837 721 +837 721 +837 721 +837 721 +774 667 +774 667 +774 667 +774 667 +774 667 +774 667 +774 667 +717 615 +717 615 +717 615 +717 615 +717 615 +717 615 +717 615 +660 565 +660 565 +660 565 +660 565 +660 565 +660 565 +597 512 +597 512 +597 512 +597 512 +597 512 +597 512 +597 512 +540 460 +540 460 +540 460 +540 460 +540 460 +540 460 +540 460 +482 408 +482 408 +482 408 +482 408 +482 408 +482 408 +424 358 +424 358 +424 358 +424 358 +424 358 +424 358 +424 358 +361 304 +361 304 +361 304 +361 304 +361 304 +361 304 +302 252 +302 252 +302 252 +302 252 +302 252 +302 252 +302 252 +244 200 +244 200 +244 200 +244 200 +244 200 +244 200 +244 200 +180 146 +180 146 +180 146 +180 146 +180 146 +180 146 +121 94 +121 94 +121 94 +121 94 +121 94 +121 94 +121 94 +63 41 +63 41 +63 41 +63 41 +63 41 +63 41 +63 41 +4099 3590 +4099 3590 +4099 3590 +4099 3590 +4099 3590 +4099 3590 +4035 3537 +4035 3537 +4035 3537 +4035 3537 +4035 3537 +4035 3537 +4035 3537 +3975 3485 +3975 3485 +3975 3485 +3975 3485 +3975 3485 +3975 3485 +3917 3431 +3917 3431 +3917 3431 +3917 3431 +3917 3431 +3917 3431 +3917 3431 +3852 3375 +3852 3375 +3852 3375 +3852 3375 +3852 3375 +3852 3375 +3794 3321 +3794 3321 +3794 3321 +3794 3321 +3794 3321 +3794 3321 +3794 3321 +3735 3268 +3735 3268 +3735 3268 +3735 3268 +3735 3268 +3735 3268 +3672 3214 +3672 3214 +3672 3214 +3672 3214 +3672 3214 +3672 3214 +3672 3214 +3613 3161 +3613 3161 +3613 3161 +3613 3161 +3613 3161 +3613 3161 +3555 3109 +3555 3109 +3555 3109 +3555 3109 +3555 3109 +3555 3109 +3555 3109 +3495 3057 +3495 3057 +3495 3057 +3495 3057 +3495 3057 +3495 3057 +3430 3002 +3430 3002 +3430 3002 +3430 3002 +3430 3002 +3430 3002 +3430 3002 +3371 2949 +3371 2949 +3371 2949 +3371 2949 +3371 2949 +3371 2949 +3311 2896 +3311 2896 +3311 2896 +3311 2896 +3311 2896 +3311 2896 +3311 2896 +3246 2840 +3246 2840 +3246 2840 +3246 2840 +3246 2840 +3246 2840 +3187 2787 +3187 2787 +3187 2787 +3187 2787 +3187 2787 +3187 2787 +3187 2787 +3128 2734 +3128 2734 +3128 2734 +3128 2734 +3128 2734 +3128 2734 +3063 2679 +3063 2679 +3063 2679 +3063 2679 +3063 2679 +3063 2679 +3063 2679 +3004 2626 +3004 2626 +3004 2626 +3004 2626 +3004 2626 +3004 2626 +2945 2573 +2945 2573 +2945 2573 +2945 2573 +2945 2573 +2945 2573 +2945 2573 +2888 2522 +2888 2522 +2888 2522 +2888 2522 +2888 2522 +2888 2522 +2823 2468 +2823 2468 +2823 2468 +2823 2468 +2823 2468 +2823 2468 +2823 2468 +2765 2415 +2765 2415 +2765 2415 +2765 2415 +2765 2415 +2765 2415 +2707 2364 +2707 2364 +2707 2364 +2707 2364 +2707 2364 +2707 2364 +2707 2364 +2644 2310 +2644 2310 +2644 2310 +2644 2310 +2644 2310 +2644 2310 +2587 2258 +2587 2258 +2587 2258 +2587 2258 +2587 2258 +2587 2258 +2530 2207 +2530 2207 +2530 2207 +2530 2207 +2530 2207 +2530 2207 +2530 2207 +2468 2155 +2468 2155 +2468 2155 +2468 2155 +2468 2155 +2468 2155 +2410 2103 +2410 2103 +2410 2103 +2410 2103 +2410 2103 +2410 2103 +2410 2103 +2353 2052 +2353 2052 +2353 2052 +2353 2052 +2353 2052 +2353 2052 +2353 2052 +2295 2001 +2295 2001 +2295 2001 +2295 2001 +2295 2001 +2295 2001 +2232 1948 +2232 1948 +2232 1948 +2232 1948 +2232 1948 +2232 1948 +2174 1896 +2174 1896 +2174 1896 +2174 1896 +2174 1896 +2174 1896 +2174 1896 +2116 1844 +2116 1844 +2116 1844 +2116 1844 +2116 1844 +2116 1844 +2054 1792 +2054 1792 +2054 1792 +2054 1792 +2054 1792 +2054 1792 +2054 1792 +1996 1740 +1996 1740 +1996 1740 +1996 1740 +1996 1740 +1996 1740 +1939 1689 +1939 1689 +1939 1689 +1939 1689 +1939 1689 +1939 1689 +1939 1689 +1881 1638 +1881 1638 +1881 1638 +1881 1638 +1881 1638 +1881 1638 +1819 1585 +1819 1585 +1819 1585 +1819 1585 +1819 1585 +1819 1585 +1819 1585 +1762 1534 +1762 1534 +1762 1534 +1762 1534 +1762 1534 +1762 1534 +1762 1534 +1705 1483 +1705 1483 +1705 1483 +1705 1483 +1705 1483 +1705 1483 +1642 1430 +1642 1430 +1642 1430 +1642 1430 +1642 1430 +1642 1430 +1642 1430 +1586 1379 +1586 1379 +1586 1379 +1586 1379 +1586 1379 +1586 1379 +1530 1329 +1530 1329 +1530 1329 +1530 1329 +1530 1329 +1530 1329 +1467 1276 +1467 1276 +1467 1276 +1467 1276 +1467 1276 +1467 1276 +1467 1276 +1410 1225 +1410 1225 +1410 1225 +1410 1225 +1410 1225 +1410 1225 +1353 1174 +1353 1174 +1353 1174 +1353 1174 +1353 1174 +1353 1174 +1353 1174 +1295 1123 +1295 1123 +1295 1123 +1295 1123 +1295 1123 +1295 1123 +1232 1070 +1232 1070 +1232 1070 +1232 1070 +1232 1070 +1232 1070 +1232 1070 +1174 1018 +1174 1018 +1174 1018 +1174 1018 +1174 1018 +1174 1018 +1174 1018 +1117 967 +1117 967 +1117 967 +1117 967 +1117 967 +1117 967 +1054 913 +1054 913 +1054 913 +1054 913 +1054 913 +1054 913 +1054 913 +996 861 +996 861 +996 861 +996 861 +996 861 +996 861 +939 810 +939 810 +939 810 +939 810 +939 810 +939 810 +939 810 +876 757 +876 757 +876 757 +876 757 +876 757 +876 757 +818 705 +818 705 +818 705 +818 705 +818 705 +818 705 +818 705 +761 654 +761 654 +761 654 +761 654 +761 654 +761 654 +704 603 +704 603 +704 603 +704 603 +704 603 +704 603 +704 603 +641 550 +641 550 +641 550 +641 550 +641 550 +641 550 +641 550 +583 498 +583 498 +583 498 +583 498 +583 498 +583 498 +526 447 +526 447 +526 447 +526 447 +526 447 +526 447 +526 447 +464 394 +464 394 +464 394 +464 394 +464 394 +464 394 +464 394 +405 342 +405 342 +405 342 +405 342 +405 342 +405 342 +348 291 +348 291 +348 291 +348 291 +348 291 +348 291 +348 291 +283 237 +283 237 +283 237 +283 237 +283 237 +283 237 +226 184 +226 184 +226 184 +226 184 +226 184 +226 184 +226 184 +166 132 +166 132 +166 132 +166 132 +166 132 +166 132 +166 132 +108 80 +108 80 +108 80 +108 80 +108 80 +108 80 +43 28 +43 28 +43 28 +43 28 +43 28 +43 28 +43 28 +4079 3576 +4079 3576 +4079 3576 +4079 3576 +4079 3576 +4079 3576 +4079 3576 +4020 3523 +4020 3523 +4020 3523 +4020 3523 +4020 3523 +4020 3523 +3955 3472 +3955 3472 +3955 3472 +3955 3472 +3955 3472 +3955 3472 +3955 3472 +3896 3415 +3896 3415 +3896 3415 +3896 3415 +3896 3415 +3896 3415 +3838 3360 +3838 3360 +3838 3360 +3838 3360 +3838 3360 +3838 3360 +3838 3360 +3780 3308 +3780 3308 +3780 3308 +3780 3308 +3780 3308 +3780 3308 +3716 3253 +3716 3253 +3716 3253 +3716 3253 +3716 3253 +3716 3253 +3716 3253 +3659 3201 +3659 3201 +3659 3201 +3659 3201 +3659 3201 +3659 3201 +3600 3149 +3600 3149 +3600 3149 +3600 3149 +3600 3149 +3600 3149 +3600 3149 +3536 3095 +3536 3095 +3536 3095 +3536 3095 +3536 3095 +3536 3095 +3477 3042 +3477 3042 +3477 3042 +3477 3042 +3477 3042 +3477 3042 +3477 3042 +3418 2989 +3418 2989 +3418 2989 +3418 2989 +3418 2989 +3418 2989 +3353 2934 +3353 2934 +3353 2934 +3353 2934 +3353 2934 +3353 2934 +3353 2934 +3294 2881 +3294 2881 +3294 2881 +3294 2881 +3294 2881 +3294 2881 +3235 2829 +3235 2829 +3235 2829 +3235 2829 +3235 2829 +3235 2829 +3176 2776 +3176 2776 +3176 2776 +3176 2776 +3176 2776 +3176 2776 +3176 2776 +3111 2722 +3111 2722 +3111 2722 +3111 2722 +3111 2722 +3111 2722 +3052 2668 +3052 2668 +3052 2668 +3052 2668 +3052 2668 +3052 2668 +3052 2668 +2993 2616 +2993 2616 +2993 2616 +2993 2616 +2993 2616 +2993 2616 +2993 2616 +2929 2561 +2929 2561 +2929 2561 +2929 2561 +2929 2561 +2929 2561 +2872 2509 +2872 2509 +2872 2509 +2872 2509 +2872 2509 +2872 2509 +2814 2457 +2814 2457 +2814 2457 +2814 2457 +2814 2457 +2814 2457 +2814 2457 +2750 2403 +2750 2403 +2750 2403 +2750 2403 +2750 2403 +2750 2403 +2693 2351 +2693 2351 +2693 2351 +2693 2351 +2693 2351 +2693 2351 +2693 2351 +2636 2301 +2636 2301 +2636 2301 +2636 2301 +2636 2301 +2636 2301 +2579 2250 +2579 2250 +2579 2250 +2579 2250 +2579 2250 +2579 2250 +2579 2250 +2516 2197 +2516 2197 +2516 2197 +2516 2197 +2516 2197 +2516 2197 +2459 2146 +2459 2146 +2459 2146 +2459 2146 +2459 2146 +2459 2146 +2459 2146 +2402 2095 +2402 2095 +2402 2095 +2402 2095 +2402 2095 +2402 2095 +2338 2042 +2338 2042 +2338 2042 +2338 2042 +2338 2042 +2338 2042 +2338 2042 +2282 1990 +2282 1990 +2282 1990 +2282 1990 +2282 1990 +2282 1990 +2224 1939 +2224 1939 +2224 1939 +2224 1939 +2224 1939 +2224 1939 +2224 1939 +2161 1886 +2161 1886 +2161 1886 +2161 1886 +2161 1886 +2161 1886 +2104 1834 +2104 1834 +2104 1834 +2104 1834 +2104 1834 +2104 1834 +2104 1834 +2046 1783 +2046 1783 +2046 1783 +2046 1783 +2046 1783 +2046 1783 +2046 1783 +1988 1732 +1988 1732 +1988 1732 +1988 1732 +1988 1732 +1988 1732 +1925 1679 +1925 1679 +1925 1679 +1925 1679 +1925 1679 +1925 1679 +1925 1679 +1869 1627 +1869 1627 +1869 1627 +1869 1627 +1869 1627 +1869 1627 +1812 1577 +1812 1577 +1812 1577 +1812 1577 +1812 1577 +1812 1577 +1749 1524 +1749 1524 +1749 1524 +1749 1524 +1749 1524 +1749 1524 +1749 1524 +1693 1473 +1693 1473 +1693 1473 +1693 1473 +1693 1473 +1693 1473 +1636 1422 +1636 1422 +1636 1422 +1636 1422 +1636 1422 +1636 1422 +1636 1422 +1573 1369 +1573 1369 +1573 1369 +1573 1369 +1573 1369 +1573 1369 +1573 1369 +1516 1318 +1516 1318 +1516 1318 +1516 1318 +1516 1318 +1516 1318 +1460 1267 +1460 1267 +1460 1267 +1460 1267 +1460 1267 +1460 1267 +1460 1267 +1403 1217 +1403 1217 +1403 1217 +1403 1217 +1403 1217 +1403 1217 +1340 1165 +1340 1165 +1340 1165 +1340 1165 +1340 1165 +1340 1165 +1340 1165 +1283 1113 +1283 1113 +1283 1113 +1283 1113 +1283 1113 +1283 1113 +1225 1062 +1225 1062 +1225 1062 +1225 1062 +1225 1062 +1225 1062 +1225 1062 +1162 1008 +1162 1008 +1162 1008 +1162 1008 +1162 1008 +1162 1008 +1104 957 +1104 957 +1104 957 +1104 957 +1104 957 +1104 957 +1104 957 +1047 905 +1047 905 +1047 905 +1047 905 +1047 905 +1047 905 +989 854 +989 854 +989 854 +989 854 +989 854 +989 854 +989 854 +926 801 +926 801 +926 801 +926 801 +926 801 +926 801 +868 749 +868 749 +868 749 +868 749 +868 749 +868 749 +868 749 +812 698 +812 698 +812 698 +812 698 +812 698 +812 698 +812 698 +748 645 +748 645 diff --git a/01-Firmware/an4296-use-stm32f3stm32g4-ccm-sram-with-iar-embedded-workbench-keil-mdkarm-stmicroelectronics-stm32cubeide-and-other-gnubased-toolchains-stmicroelectronics.pdf b/01-Firmware/an4296-use-stm32f3stm32g4-ccm-sram-with-iar-embedded-workbench-keil-mdkarm-stmicroelectronics-stm32cubeide-and-other-gnubased-toolchains-stmicroelectronics.pdf new file mode 100644 index 0000000..25b2a02 Binary files /dev/null and b/01-Firmware/an4296-use-stm32f3stm32g4-ccm-sram-with-iar-embedded-workbench-keil-mdkarm-stmicroelectronics-stm32cubeide-and-other-gnubased-toolchains-stmicroelectronics.pdf differ diff --git a/01-Firmware/en.MB1419-G431CBU6-C01_Schematic.pdf b/01-Firmware/en.MB1419-G431CBU6-C01_Schematic.pdf new file mode 100644 index 0000000..111132c Binary files /dev/null and b/01-Firmware/en.MB1419-G431CBU6-C01_Schematic.pdf differ diff --git a/01-Firmware/um2516-electronic-speed-controller-discovery-kit-for-drones-with-stm32g431cb-stmicroelectronics (1).pdf b/01-Firmware/um2516-electronic-speed-controller-discovery-kit-for-drones-with-stm32g431cb-stmicroelectronics (1).pdf new file mode 100644 index 0000000..4c6182e Binary files /dev/null and b/01-Firmware/um2516-electronic-speed-controller-discovery-kit-for-drones-with-stm32g431cb-stmicroelectronics (1).pdf differ diff --git a/02-GUI/SBSUtility/SBSUtility.py b/02-GUI/SBSUtility/SBSUtility.py index d2890f2..12b2257 100644 --- a/02-GUI/SBSUtility/SBSUtility.py +++ b/02-GUI/SBSUtility/SBSUtility.py @@ -17,10 +17,9 @@ def main(): ## change COM port here - servo = servo_protocol2('COM3',1000000) ##eeprom baud = 3 - + servo = servo_protocol2() window = Tk() - window.title(" MyServoGUI") + window.title(" 8yServoGUI") window.geometry("1600x1020") window.minsize(1600,1020) @@ -45,4 +44,3 @@ def main(): ##cProfile.run("main()") main() - diff --git a/02-GUI/SBSUtility/__pycache__/eeprom_frame.cpython-37.pyc b/02-GUI/SBSUtility/__pycache__/eeprom_frame.cpython-37.pyc index c00c527..ea2022d 100644 Binary files a/02-GUI/SBSUtility/__pycache__/eeprom_frame.cpython-37.pyc and b/02-GUI/SBSUtility/__pycache__/eeprom_frame.cpython-37.pyc differ diff --git a/02-GUI/SBSUtility/__pycache__/id_frame.cpython-37.pyc b/02-GUI/SBSUtility/__pycache__/id_frame.cpython-37.pyc index 7978aa8..09ba45f 100644 Binary files a/02-GUI/SBSUtility/__pycache__/id_frame.cpython-37.pyc and b/02-GUI/SBSUtility/__pycache__/id_frame.cpython-37.pyc differ diff --git a/02-GUI/SBSUtility/__pycache__/protocol2.cpython-37.pyc b/02-GUI/SBSUtility/__pycache__/protocol2.cpython-37.pyc index 3e2ab53..592cf1a 100644 Binary files a/02-GUI/SBSUtility/__pycache__/protocol2.cpython-37.pyc and b/02-GUI/SBSUtility/__pycache__/protocol2.cpython-37.pyc differ diff --git a/02-GUI/SBSUtility/__pycache__/ram_frame.cpython-37.pyc b/02-GUI/SBSUtility/__pycache__/ram_frame.cpython-37.pyc index 135f408..992bfa0 100644 Binary files a/02-GUI/SBSUtility/__pycache__/ram_frame.cpython-37.pyc and b/02-GUI/SBSUtility/__pycache__/ram_frame.cpython-37.pyc differ diff --git a/02-GUI/SBSUtility/__pycache__/trace_frame.cpython-37.pyc b/02-GUI/SBSUtility/__pycache__/trace_frame.cpython-37.pyc index f79f725..0ebd5d4 100644 Binary files a/02-GUI/SBSUtility/__pycache__/trace_frame.cpython-37.pyc and b/02-GUI/SBSUtility/__pycache__/trace_frame.cpython-37.pyc differ diff --git a/02-GUI/SBSUtility/eeprom_frame.py b/02-GUI/SBSUtility/eeprom_frame.py index 295595f..4442942 100644 --- a/02-GUI/SBSUtility/eeprom_frame.py +++ b/02-GUI/SBSUtility/eeprom_frame.py @@ -38,42 +38,42 @@ def __init__(self,window,protocol,id): self.gui_entry("Min Voltage", "min_voltage", 0, True, True, True, 0x1D, 1, 1 ) self.gui_entry("Max Voltage", "max_voltage", 0, True, True, True, 0x1E, 1, 1 ) self.gui_spacer("---") - self.gui_entry("Moving Threshold", "moving_threshold", 0, True, True, True, 0x1F, 1, 1 ) + #self.gui_entry("Moving Threshold", "moving_threshold", 0, True, True, True, 0x1F, 1, 1 ) #self.gui_entry("Status Return lvl", "status_return_level", 0, True, True, True, 0x20, 1, 1 ) #self.gui_entry("Alarm Led", "alarm_led", 0, True, True, True, 0x21, 1, 1 ) #self.gui_entry("Alarm Shutdown", "alarm_shutdown", 0, True, True, True, 0x22, 1, 1 ) - self.gui_spacer("---") + #self.gui_spacer("---") self.gui_entry("Encoder Resolution Bits", "encoder_bits", 0, True, True, True, 0x23, 1, 2 ) self.gui_entry("Motor Pole Pairs", "motor_pole_pairs", 0, True, True, True, 0x24, 1, 1 ) self.gui_entry("Motor Synchro Angle", "motor_synchro", 0, True, True, True, 0x25, 1, 2 ) - self.gui_entry("Inv Phase Motor", "inv_phase_motor", 0, True, True, True, 0x28, 1, 1 ) - self.gui_entry("Field Weakening K", "field_weaknening_k", 0, True, True, True, 0x29, 1, 1 ) + self.gui_entry("Motor Reverse Phase", "inv_phase_motor", 0, True, True, True, 0x28, 1, 1 ) + #self.gui_entry("Field Weakening K", "field_weaknening_k", 0, True, True, True, 0x29, 1, 1 ) self.gui_spacer("---") - self.gui_entry("PID Position KP", "pid_position_kp", 0, True, True, True, 0x2A, 1, 2 ) + #self.gui_entry("PID Position KP", "pid_position_kp", 0, True, True, True, 0x2A, 1, 2 ) #self.gui_entry("PID Position KI", "pid_position_ki", 0, True, True, True, 0x2C, 1, 2 ) - self.gui_entry("PID Position KD", "pid_position_kd", 0, True, True, True, 0x2E, 1, 2 ) + #self.gui_entry("PID Position KD", "pid_position_kd", 0, True, True, True, 0x2E, 1, 2 ) #self.gui_entry("PID Velocity KP", "pid_velocity_kp", 0, True, True, True, 0x30, 1, 2 ) #self.gui_entry("PID Velocity KI", "pid_velocity_ki", 0, True, True, True, 0x32, 1, 2 ) #self.gui_entry("PID Velocity KD", "pid_velocity_kd", 0, True, True, True, 0x34, 1, 2 ) #self.gui_entry("PID Velocity KFF", "pid_velocity_kff", 0, True, True, True, 0x36, 1, 2 ) #self.gui_entry("PID Acceleration KFF", "pid_acceleration_kff", 0, True, True, True, 0x38, 1, 2 ) - self.gui_spacer("---") - self.gui_entry("PID Flux Current KP", "pid_flux_current_kp", 0, True, True, True, 0x3A, 1, 2 ) - #self.gui_entry("PID Flux Current KI", "pid_flux_current_ki", 0, True, True, True, 0x3C, 1, 2 ) + #self.gui_spacer("---") + self.gui_entry("PI Flux Current KP", "pid_flux_current_kp", 0, True, True, True, 0x3A, 1, 2 ) + self.gui_entry("PI Flux Current KI", "pid_flux_current_ki", 0, True, True, True, 0x3C, 1, 2 ) #self.gui_entry("PID Flux Current KFF", "pid_flux_current_kff", 0, True, True, True, 0x3E, 1, 2 ) - self.gui_spacer("---") - self.gui_entry("PID Torque Current KP", "pid_torque_current_kp", 0, True, True, True, 0x40, 1, 2 ) - #self.gui_entry("PID Torque Current KI", "pid_torque_current_ki", 0, True, True, True, 0x42, 1, 2 ) + #self.gui_spacer("---") + self.gui_entry("PI Torque Current KP", "pid_torque_current_kp", 0, True, True, True, 0x40, 1, 2 ) + self.gui_entry("PI Torque Current KI", "pid_torque_current_ki", 0, True, True, True, 0x42, 1, 2 ) #self.gui_entry("PID Torque Current KFF", "pid_torque_current_kff", 0, True, True, True, 0x44, 1, 2 ) - self.gui_spacer("---") - self.gui_entry("PH1 Current Sense MA", "cal_phase1_current_sense_ma", 0, True, True, True, 0x46, 1, 2 ) - self.gui_entry("Ph1 Current Sense Offset", "cal_phase1_current_sense_offset", 0, True, True, True, 0x48, 1, 2 ) - self.gui_entry("PH2 Current Sense MA", "cal_phase2_current_sense_ma", 0, True, True, True, 0x4A, 1, 2 ) - self.gui_entry("Ph2 Current Sense Offset", "cal_phase2_current_sense_offset", 0, True, True, True, 0x4C, 1, 2 ) - self.gui_entry("PH3 Current Sense MA", "cal_phase3_current_sense_ma", 0, True, True, True, 0x4E, 1, 2 ) - self.gui_entry("Ph3 Current Sense Offset", "cal_phase3_current_sense_offset", 0, True, True, True, 0x50, 1, 2 ) - self.gui_entry("Calibration Voltage sensor", "cal_voltage_sensor", 0, True, True, True, 0x52, 1, 2 ) - self.gui_entry("EWMA encoder", "ewma_encoder", 0, True, True, True, 0x54, 1, 1 ) + #self.gui_spacer("---") + #self.gui_entry("PH1 Current Sense MA", "cal_phase1_current_sense_ma", 0, True, True, True, 0x46, 1, 2 ) + #self.gui_entry("Ph1 Current Sense Offset", "cal_phase1_current_sense_offset", 0, True, True, True, 0x48, 1, 2 ) + #self.gui_entry("PH2 Current Sense MA", "cal_phase2_current_sense_ma", 0, True, True, True, 0x4A, 1, 2 ) + #self.gui_entry("Ph2 Current Sense Offset", "cal_phase2_current_sense_offset", 0, True, True, True, 0x4C, 1, 2 ) + #self.gui_entry("PH3 Current Sense MA", "cal_phase3_current_sense_ma", 0, True, True, True, 0x4E, 1, 2 ) + #self.gui_entry("Ph3 Current Sense Offset", "cal_phase3_current_sense_offset", 0, True, True, True, 0x50, 1, 2 ) + #self.gui_entry("Calibration Voltage sensor", "cal_voltage_sensor", 0, True, True, True, 0x52, 1, 2 ) + #self.gui_entry("EWMA encoder", "ewma_encoder", 0, True, True, True, 0x54, 1, 1 ) # update button button_update = Button(self,text="Update",command = self.read_all) @@ -131,111 +131,112 @@ def gui_spacer(self,text_label): self.row += 1 def read_all(self): - print("real all EEPROM...") - # send read command - error, result = self.protocol.read_byte_command( - self.id.current_id, # ID - 0x00, # from EEPROM - 85, # byte number to read - verbose=1 - ) # TODO change ID through GUI - if error != 0 : - print("error:"+str(error)) - elif len(result)==85: - self.variables['model_number_servo'].set(str(result[0] + (result[1]<<8))) - self.variables['version_servo'].set(str(result[2])) - self.variables['id_local'].set(str(result[3])) - self.variables['id_servo'].set(str(result[3])) - self.variables['baud_rate_local'].set(str(result[4])) - self.variables['baud_rate_servo'].set(str(result[4])) - #self.variables['return_delay_local'].set(str(result[5])) - #self.variables['return_delay_servo'].set(str(result[5])) - self.variables['min_position_local'].set(str(sign(result[16] + (result[17]<<8)))) - self.variables['min_position_servo'].set(str(sign(result[16] + (result[17]<<8)))) - self.variables['max_position_local'].set(str(sign(result[18] + (result[19]<<8)))) - self.variables['max_position_servo'].set(str(sign(result[18] + (result[19]<<8)))) - self.variables['max_velocity_local'].set(str(result[20] + (result[21]<<8))) - self.variables['max_velocity_servo'].set(str(result[20] + (result[21]<<8))) - #self.variables['max_acceleration_local'].set(str(result[22] + (result[23]<<8))) - #self.variables['max_acceleration_servo'].set(str(result[22] + (result[23]<<8))) - self.variables['max_current_local'].set(str(result[24] + (result[25]<<8))) - self.variables['max_current_servo'].set(str(result[24] + (result[25]<<8))) - self.variables['max_temperature_local'].set(str(result[28])) - self.variables['max_temperature_servo'].set(str(result[28])) - self.variables['min_voltage_local'].set(str(result[29])) - self.variables['min_voltage_servo'].set(str(result[29])) - self.variables['max_voltage_local'].set(str(result[30])) - self.variables['max_voltage_servo'].set(str(result[30])) - self.variables['moving_threshold_local'].set(str(result[31])) - self.variables['moving_threshold_servo'].set(str(result[31])) - #self.variables['status_return_level_local'].set(str(result[32])) - #self.variables['status_return_level_servo'].set(str(result[32])) - #self.variables['alarm_led_local'].set(str(result[33])) - #self.variables['alarm_led_servo'].set(str(result[33])) - #self.variables['alarm_shutdown_local'].set(str(result[34])) - #self.variables['alarm_shutdown_servo'].set(str(result[34])) - - self.variables['encoder_bits_local'].set(str(result[35])) - self.variables['encoder_bits_servo'].set(str(result[35])) - self.variables['motor_pole_pairs_local'].set(str(result[36])) - self.variables['motor_pole_pairs_servo'].set(str(result[36])) - self.variables['motor_synchro_local'].set(str(result[37] + (result[38]<<8))) - self.variables['motor_synchro_servo'].set(str(result[37] + (result[38]<<8))) - self.variables['inv_phase_motor_local'].set(str(result[40])) - self.variables['inv_phase_motor_servo'].set(str(result[40])) - self.variables['field_weaknening_k_local'].set(str(result[41])) - self.variables['field_weaknening_k_servo'].set(str(result[41])) - - self.variables['pid_position_kp_local'].set(str(result[42] + (result[43]<<8))) - self.variables['pid_position_kp_servo'].set(str(result[42] + (result[43]<<8))) - #self.variables['pid_position_ki_local'].set(str(result[44] + (result[45]<<8))) - #self.variables['pid_position_ki_servo'].set(str(result[44] + (result[45]<<8))) - self.variables['pid_position_kd_local'].set(str(result[46] + (result[47]<<8))) - self.variables['pid_position_kd_servo'].set(str(result[46] + (result[47]<<8))) - - #self.variables['pid_velocity_kp_local'].set(str(result[48] + (result[49]<<8))) - #self.variables['pid_velocity_kp_servo'].set(str(result[48] + (result[49]<<8))) - #self.variables['pid_velocity_ki_local'].set(str(result[50] + (result[51]<<8))) - #self.variables['pid_velocity_ki_servo'].set(str(result[50] + (result[51]<<8))) - #self.variables['pid_velocity_kd_local'].set(str(result[52] + (result[53]<<8))) - #self.variables['pid_velocity_kd_servo'].set(str(result[52] + (result[53]<<8))) - #self.variables['pid_velocity_kff_local'].set(str(result[54] + (result[55]<<8))) - #self.variables['pid_velocity_kff_servo'].set(str(result[54] + (result[55]<<8))) - #self.variables['pid_acceleration_kff_local'].set(str(result[56] + (result[57]<<8))) - #self.variables['pid_acceleration_kff_servo'].set(str(result[56] + (result[57]<<8))) - - self.variables['pid_flux_current_kp_local'].set(str(result[58] + (result[59]<<8))) - self.variables['pid_flux_current_kp_servo'].set(str(result[58] + (result[59]<<8))) - #self.variables['pid_flux_current_ki_local'].set(str(result[60] + (result[61]<<8))) - #self.variables['pid_flux_current_ki_servo'].set(str(result[60] + (result[61]<<8))) - #self.variables['pid_flux_current_kff_local'].set(str(result[62] + (result[63]<<8))) - #self.variables['pid_flux_current_kff_servo'].set(str(result[62] + (result[63]<<8))) - - self.variables['pid_torque_current_kp_local'].set(str(result[64] + (result[65]<<8))) - self.variables['pid_torque_current_kp_servo'].set(str(result[64] + (result[65]<<8))) - #self.variables['pid_torque_current_ki_local'].set(str(result[66] + (result[67]<<8))) - #self.variables['pid_torque_current_ki_servo'].set(str(result[66] + (result[67]<<8))) - #self.variables['pid_torque_current_kff_local'].set(str(result[68] + (result[69]<<8))) - #self.variables['pid_torque_current_kff_servo'].set(str(result[68] + (result[69]<<8))) - - self.variables['cal_phase1_current_sense_ma_local'].set(str(result[70] + (result[71]<<8))) - self.variables['cal_phase1_current_sense_ma_servo'].set(str(result[70] + (result[71]<<8))) - self.variables['cal_phase1_current_sense_offset_local'].set(str(result[72] + (result[73]<<8))) - self.variables['cal_phase1_current_sense_offset_servo'].set(str(result[72] + (result[73]<<8))) - - self.variables['cal_phase2_current_sense_ma_local'].set(str(result[74] + (result[75]<<8))) - self.variables['cal_phase2_current_sense_ma_servo'].set(str(result[74] + (result[75]<<8))) - self.variables['cal_phase2_current_sense_offset_local'].set(str(result[76] + (result[77]<<8))) - self.variables['cal_phase2_current_sense_offset_servo'].set(str(result[76] + (result[77]<<8))) - - self.variables['cal_phase3_current_sense_ma_local'].set(str(result[78] + (result[79]<<8))) - self.variables['cal_phase3_current_sense_ma_servo'].set(str(result[78] + (result[79]<<8))) - self.variables['cal_phase3_current_sense_offset_local'].set(str(result[80] + (result[81]<<8))) - self.variables['cal_phase3_current_sense_offset_servo'].set(str(result[80] + (result[81]<<8))) - - self.variables['cal_voltage_sensor_local'].set(str(result[82] + (result[83]<<8))) - self.variables['cal_voltage_sensor_servo'].set(str(result[82] + (result[83]<<8))) - - - self.variables['ewma_encoder_local'].set(str(result[84])) - self.variables['ewma_encoder_servo'].set(str(result[84])) + if self.protocol: + print("real all EEPROM...") + # send read command + error, result = self.protocol.read_byte_command( + self.id.current_id, # ID + 0x00, # from EEPROM + 85, # byte number to read + verbose=1 + ) # TODO change ID through GUI + if error != 0 : + print("error:"+str(error)) + elif len(result)==85: + self.variables['model_number_servo'].set(str(result[0] + (result[1]<<8))) + self.variables['version_servo'].set(str(result[2])) + self.variables['id_local'].set(str(result[3])) + self.variables['id_servo'].set(str(result[3])) + self.variables['baud_rate_local'].set(str(result[4])) + self.variables['baud_rate_servo'].set(str(result[4])) + #self.variables['return_delay_local'].set(str(result[5])) + #self.variables['return_delay_servo'].set(str(result[5])) + self.variables['min_position_local'].set(str(sign(result[16] + (result[17]<<8)))) + self.variables['min_position_servo'].set(str(sign(result[16] + (result[17]<<8)))) + self.variables['max_position_local'].set(str(sign(result[18] + (result[19]<<8)))) + self.variables['max_position_servo'].set(str(sign(result[18] + (result[19]<<8)))) + self.variables['max_velocity_local'].set(str(result[20] + (result[21]<<8))) + self.variables['max_velocity_servo'].set(str(result[20] + (result[21]<<8))) + #self.variables['max_acceleration_local'].set(str(result[22] + (result[23]<<8))) + #self.variables['max_acceleration_servo'].set(str(result[22] + (result[23]<<8))) + self.variables['max_current_local'].set(str(result[24] + (result[25]<<8))) + self.variables['max_current_servo'].set(str(result[24] + (result[25]<<8))) + self.variables['max_temperature_local'].set(str(result[28])) + self.variables['max_temperature_servo'].set(str(result[28])) + self.variables['min_voltage_local'].set(str(result[29])) + self.variables['min_voltage_servo'].set(str(result[29])) + self.variables['max_voltage_local'].set(str(result[30])) + self.variables['max_voltage_servo'].set(str(result[30])) + #self.variables['moving_threshold_local'].set(str(result[31])) + #self.variables['moving_threshold_servo'].set(str(result[31])) + #self.variables['status_return_level_local'].set(str(result[32])) + #self.variables['status_return_level_servo'].set(str(result[32])) + #self.variables['alarm_led_local'].set(str(result[33])) + #self.variables['alarm_led_servo'].set(str(result[33])) + #self.variables['alarm_shutdown_local'].set(str(result[34])) + #self.variables['alarm_shutdown_servo'].set(str(result[34])) + + self.variables['encoder_bits_local'].set(str(result[35])) + self.variables['encoder_bits_servo'].set(str(result[35])) + self.variables['motor_pole_pairs_local'].set(str(result[36])) + self.variables['motor_pole_pairs_servo'].set(str(result[36])) + self.variables['motor_synchro_local'].set(str(result[37] + (result[38]<<8))) + self.variables['motor_synchro_servo'].set(str(result[37] + (result[38]<<8))) + self.variables['inv_phase_motor_local'].set(str(result[40])) + self.variables['inv_phase_motor_servo'].set(str(result[40])) + #self.variables['field_weaknening_k_local'].set(str(result[41])) + #self.variables['field_weaknening_k_servo'].set(str(result[41])) + + #self.variables['pid_position_kp_local'].set(str(result[42] + (result[43]<<8))) + #self.variables['pid_position_kp_servo'].set(str(result[42] + (result[43]<<8))) + #self.variables['pid_position_ki_local'].set(str(result[44] + (result[45]<<8))) + #self.variables['pid_position_ki_servo'].set(str(result[44] + (result[45]<<8))) + #self.variables['pid_position_kd_local'].set(str(result[46] + (result[47]<<8))) + #self.variables['pid_position_kd_servo'].set(str(result[46] + (result[47]<<8))) + + #self.variables['pid_velocity_kp_local'].set(str(result[48] + (result[49]<<8))) + #self.variables['pid_velocity_kp_servo'].set(str(result[48] + (result[49]<<8))) + #self.variables['pid_velocity_ki_local'].set(str(result[50] + (result[51]<<8))) + #self.variables['pid_velocity_ki_servo'].set(str(result[50] + (result[51]<<8))) + #self.variables['pid_velocity_kd_local'].set(str(result[52] + (result[53]<<8))) + #self.variables['pid_velocity_kd_servo'].set(str(result[52] + (result[53]<<8))) + #self.variables['pid_velocity_kff_local'].set(str(result[54] + (result[55]<<8))) + #self.variables['pid_velocity_kff_servo'].set(str(result[54] + (result[55]<<8))) + #self.variables['pid_acceleration_kff_local'].set(str(result[56] + (result[57]<<8))) + #self.variables['pid_acceleration_kff_servo'].set(str(result[56] + (result[57]<<8))) + + self.variables['pid_flux_current_kp_local'].set(str(result[58] + (result[59]<<8))) + self.variables['pid_flux_current_kp_servo'].set(str(result[58] + (result[59]<<8))) + self.variables['pid_flux_current_ki_local'].set(str(result[60] + (result[61]<<8))) + self.variables['pid_flux_current_ki_servo'].set(str(result[60] + (result[61]<<8))) + #self.variables['pid_flux_current_kff_local'].set(str(result[62] + (result[63]<<8))) + #self.variables['pid_flux_current_kff_servo'].set(str(result[62] + (result[63]<<8))) + + self.variables['pid_torque_current_kp_local'].set(str(result[64] + (result[65]<<8))) + self.variables['pid_torque_current_kp_servo'].set(str(result[64] + (result[65]<<8))) + self.variables['pid_torque_current_ki_local'].set(str(result[66] + (result[67]<<8))) + self.variables['pid_torque_current_ki_servo'].set(str(result[66] + (result[67]<<8))) + #self.variables['pid_torque_current_kff_local'].set(str(result[68] + (result[69]<<8))) + #self.variables['pid_torque_current_kff_servo'].set(str(result[68] + (result[69]<<8))) + + #self.variables['cal_phase1_current_sense_ma_local'].set(str(result[70] + (result[71]<<8))) + #self.variables['cal_phase1_current_sense_ma_servo'].set(str(result[70] + (result[71]<<8))) + #self.variables['cal_phase1_current_sense_offset_local'].set(str(result[72] + (result[73]<<8))) + #self.variables['cal_phase1_current_sense_offset_servo'].set(str(result[72] + (result[73]<<8))) + + #self.variables['cal_phase2_current_sense_ma_local'].set(str(result[74] + (result[75]<<8))) + #self.variables['cal_phase2_current_sense_ma_servo'].set(str(result[74] + (result[75]<<8))) + #self.variables['cal_phase2_current_sense_offset_local'].set(str(result[76] + (result[77]<<8))) + #self.variables['cal_phase2_current_sense_offset_servo'].set(str(result[76] + (result[77]<<8))) + + #self.variables['cal_phase3_current_sense_ma_local'].set(str(result[78] + (result[79]<<8))) + #self.variables['cal_phase3_current_sense_ma_servo'].set(str(result[78] + (result[79]<<8))) + #self.variables['cal_phase3_current_sense_offset_local'].set(str(result[80] + (result[81]<<8))) + #self.variables['cal_phase3_current_sense_offset_servo'].set(str(result[80] + (result[81]<<8))) + + #self.variables['cal_voltage_sensor_local'].set(str(result[82] + (result[83]<<8))) + #self.variables['cal_voltage_sensor_servo'].set(str(result[82] + (result[83]<<8))) + + + #self.variables['ewma_encoder_local'].set(str(result[84])) + #self.variables['ewma_encoder_servo'].set(str(result[84])) diff --git a/02-GUI/SBSUtility/id_frame.py b/02-GUI/SBSUtility/id_frame.py index ce5a2c8..0ab5566 100644 --- a/02-GUI/SBSUtility/id_frame.py +++ b/02-GUI/SBSUtility/id_frame.py @@ -3,13 +3,15 @@ from tkinter import * #from tkinter.ttk import * -from protocol2 import sign +from protocol2 import * + import time +import serial class id_frame(LabelFrame): def __init__(self,window,protocol): - super().__init__(text="ID") + super().__init__(text="COM / ID") self.protocol = protocol self.labels = {} self.entries = {} @@ -18,20 +20,34 @@ def __init__(self,window,protocol): self.row = 0 self.current_id = 1 + self.current_port = 1 + + # update button + button_update = Button(self,text="Update COM",command = self.update_com) + button_update.grid(column = 0, row = self.row, sticky='we') + self.row += 1 + + # list COM + self.lists["com"] = Listbox(self) + self.lists["com"].grid(column = 0, row = self.row, sticky='w') + self.lists["com"].bind("<>", self.select_com) + self.row += 1 # update button - button_update = Button(self,text="Update",command = self.update) + button_update = Button(self,text="Update ID",command = self.update_ids) button_update.grid(column = 0, row = self.row, sticky='we') self.row += 1 - # list + + # list ID self.lists["ids"] = Listbox(self) self.lists["ids"].grid(column = 0, row = self.row, sticky='w') self.lists["ids"].bind("<>", self.select_id) self.row += 1 # startup auto ping and update list - self.update() + self.update_com() + self.update_ids() def gui_spacer(self,text_label): @@ -39,7 +55,28 @@ def gui_spacer(self,text_label): label.grid(column = 0, row = self.row, sticky='w') self.row += 1 - def update(self): + + def update_com(self): + ports = ['COM%s' % (i + 1) for i in range(1, 256)] # avoid port 1 + print("COM:"+str(ports)) + counter = 0 + for port in ports: + try: + s = serial.Serial(port) + s.close() + self.lists["com"].insert(counter,str(port)) + counter += 1 + except (OSError, serial.SerialException): + pass + self.lists["com"].selection_set(0) + if self.lists["com"].curselection(): + self.current_port = self.lists["com"].get(self.lists["com"].curselection()) + print("self.current_port:"+str(self.current_port)) + self.protocol.open(self.current_port,1000000) ##eeprom baud = 3 + + + def update_ids(self): + self.lists["ids"].delete(0,END) counter = 0 for i in range(1,20): print("ping servo ID:" + str(i) + '...') @@ -57,3 +94,15 @@ def select_id(self,event): if self.lists["ids"].curselection(): self.current_id = int(self.lists["ids"].get(self.lists["ids"].curselection())) print("self.current_id:"+str(self.current_id)) + + def select_com(self,event): + if self.lists["com"].curselection(): + self.current_port = self.lists["com"].get(self.lists["com"].curselection()) + print("self.current_port:"+str(self.current_port)) + self.protocol.open(self.current_port,1000000) ##eeprom baud = 3 + self.update_ids() +# else: +# self.current_port = 0 +# print("self.current_port:"+str(self.current_port)) +# self.protocol = None + diff --git a/02-GUI/SBSUtility/protocol2.py b/02-GUI/SBSUtility/protocol2.py index a179a6e..3b55e72 100644 --- a/02-GUI/SBSUtility/protocol2.py +++ b/02-GUI/SBSUtility/protocol2.py @@ -70,14 +70,8 @@ def updateCRC(crc_accum, data_blk_ptr, data_blk_size): class servo_protocol2: - def __init__(self,port,baud): + def __init__(self): self.serial = serial.Serial() - self.serial.baudrate = baud - self.serial.port = port - self.serial.open() - if self.serial.is_open: - print("Serial openend...") - self.timeout_input = 20 # recv packet @@ -87,8 +81,27 @@ def __init__(self,port,baud): self.rx_packet_payload_length = 0 def __del__(self): + if self.serial: + self.serial.close() + print("Serial "+ self.serial.port + " closed!") + + # helper + + def open(self,port,baud): + if self.serial: + self.serial.close() + self.serial.baudrate = baud + self.serial.port = port + self.serial.open() + if self.serial.is_open: + print("Serial "+ self.serial.port + " openend...") + + def opened(self): + return self.serial.is_open + + def close(self): self.serial.close() - print("Serial closed!") + print("Serial "+ self.serial.port + " closed!") # base instructions diff --git a/02-GUI/SBSUtility/ram_frame.py b/02-GUI/SBSUtility/ram_frame.py index aa32f75..a332023 100644 --- a/02-GUI/SBSUtility/ram_frame.py +++ b/02-GUI/SBSUtility/ram_frame.py @@ -25,37 +25,38 @@ def __init__(self,window,protocol,trace,id): self.start_time = time.time()*1000.0 self.gui_spacer("") - self.gui_entry("Torque Enable", "torque_enable", 0, False, True, False, 0x80, 1, 1 ) + self.gui_entry("Torque Enable", "torque_enable", 0, True, True, True, 0x80, 1, 1 ) self.gui_entry("LED", "led", 0, True, True, True, 0x81, 1, 1 ) - self.gui_entry("Control Mode", "control_mode", 0, True, True, True, 0x82, 1, 1 ) + self.gui_entry("Control Mode", "control_mode", 0, False, True, False, 0x82, 1, 1 ) self.gui_spacer("---") - self.gui_entry("Goal Position", "goal_position", 0, True, True, True, 0x83, 10, 2 ) - self.gui_entry("Goal Velocity", "goal_velocity", 0, True, True, True, 0x85, 1, 2 ) - self.gui_entry("FeedForward Torque Current", "goal_torque_current", 0, True, True, True, 0x87, 1, 2 ) - self.gui_entry("Goal Flux Current", "goal_flux_current", 0, True, True, True, 0x89, 1, 2 ) - self.gui_entry("Position Kp", "goal_kp", 0, True, True, True, 0x8B, 1, 1 ) - self.gui_entry("Velocity Kd", "goal_kd", 0, True, True, True, 0x8C, 1, 1 ) + self.gui_entry("Goal Position (deg)", "goal_position", 0, True, True, True, 0x83, 10, 2 ) + self.gui_entry("Goal Velocity (dps)", "goal_velocity", 0, True, True, True, 0x85, 1, 2 ) + self.gui_entry("FFTorque Current (mA)", "goal_torque_current", 0, True, True, True, 0x87, 1, 2 ) + self.gui_entry("Goal Flux Current (mA)", "goal_flux_current", 0, True, True, True, 0x89, 1, 2 ) + self.gui_entry("Position Kp", "goal_pos_kp", 0, True, True, True, 0x8B, 1, 1 ) + self.gui_entry("Position Kd", "goal_pos_kd", 0, True, True, True, 0x8C, 1, 1 ) + self.gui_entry("Velocity Kp", "goal_vel_kp", 0, True, True, True, 0x8D, 1, 1 ) self.gui_spacer("---") - self.gui_entry("Manual Synchro Offset", "goal_synchro_offset", 0, True, True, True, 0x8D, 1, 2 ) - self.gui_entry("Force Open Loop", "goal_open_loop", 0, True, True, True, 0x8F, 1, 1 ) + self.gui_entry("Manual Synchro Offset", "goal_synchro_offset", 0, True, True, True, 0x8E, 1, 2 ) self.gui_spacer("---") - self.gui_entry("Present Position", "present_position", 0, False, True, False, 0x90, 10, 2 ) - self.gui_entry("Present Velocity", "present_velocity", 0, False, True, False, 0x92, 1, 2 ) - self.gui_entry("Present Torque Current", "present_torque_current", 0, False, True, False, 0x94, 1, 2 ) - self.gui_entry("Present Flux Current", "present_flux_current", 0, False, True, False, 0x96, 1, 2 ) - self.gui_entry("Present Voltage", "present_voltage", 0, False, True, False, 0x98, 1, 1 ) - self.gui_entry("Present Temperature", "present_temperature", 0, False, True, False, 0x99, 1, 1 ) + self.gui_entry("Present Position (deg)", "present_position", 0, False, True, False, 0x90, 10, 2 ) + self.gui_entry("Present Velocity (dps)", "present_velocity", 0, False, True, False, 0x92, 1, 2 ) + self.gui_entry("Present Torque Current (mA)", "present_torque_current", 0, False, True, False, 0x94, 1, 2 ) + self.gui_entry("Present Flux Current (mA)", "present_flux_current", 0, False, True, False, 0x96, 1, 2 ) + self.gui_entry("Present Voltage (V)", "present_voltage", 0, False, True, False, 0x98, 1, 1 ) + self.gui_entry("Present Temperature (°C)", "present_temperature", 0, False, True, False, 0x99, 1, 1 ) self.gui_spacer("---") self.gui_entry("Moving", "moving", 0, False, True, False, 0x9A, 1, 1 ) self.gui_spacer("---") - self.gui_entry("Setpoint Position", "setpoint_position", 0, False, True, False, 0xA0, 10, 2 ) - self.gui_entry("Setpoint Velocity", "setpoint_velocity", 0, False, True, False, 0xA2, 1, 2 ) - self.gui_entry("Setpoint Torque Current", "setpoint_torque_current", 0, False, True, False, 0xA4, 1, 2 ) - self.gui_entry("Setpoint Flux Current", "setpoint_flux_current", 0, False, True, False, 0xA6, 1, 2 ) + self.gui_entry("Setpoint Position (deg)", "setpoint_position", 0, False, True, False, 0xA0, 10, 2 ) + self.gui_entry("Setpoint Velocity (dps)", "setpoint_velocity", 0, False, True, False, 0xA2, 1, 2 ) + self.gui_entry("Setpoint Torque Current (mA)", "setpoint_torque_current", 0, False, True, False, 0xA4, 1, 2 ) + self.gui_entry("Setpoint Flux Current (mA)", "setpoint_flux_current", 0, False, True, False, 0xA6, 1, 2 ) self.gui_spacer("---") self.gui_entry("FOC Processing Time (us)", "processing_time", 0, False, True, False, 0xAA, 1, 1 ) self.gui_entry("FOC Frequency (Khz)", "foc_frequency", 0, False, True, False, 0xAB, 1, 1 ) self.gui_entry("PID Fequency (Khz)", "pid_frequency", 0, False, True, False, 0xAC, 1, 1 ) + self.gui_entry("MLP Fequency (Khz)", "mlp_frequency", 0, False, True, False, 0xAD, 1, 1 ) self.gui_spacer("---") self.gui_entry("Protocol CRC Fail", "protocol_crc_fail", 0, False, True, False, 0xB0, 1, 1 ) self.gui_entry("Hardware Error Status", "hardware_error_status", 0, False, True, False, 0xB1, 1, 1 ) @@ -64,6 +65,12 @@ def __init__(self,window,protocol,trace,id): #button_update = Button(self,text="Update",command = self.read_all) #button_update.grid(column = 2, row = 0, sticky='we') + self.present_torque_current = 0 + self.present_flux_current = 0 + self.setpoint_torque_current = 0 + self.setpoint_flux_current = 0 + self.alpha = 0.99 #0.25 + self.read_all() def gui_entry(self,text_label,variable_name,variable_value,has_local,has_servo,has_callback,callback_reg_address,callback_reg_scale,callback_reg_size): @@ -114,114 +121,159 @@ def gui_spacer(self,text_label): self.row += 1 def read_all(self): - # write test - if self.trace.variables["square_position"].get() == 1: - value = self.trace.test_square_position() - if value != 0: - print("write RAM...") - self.protocol.write_word_command(self.id.current_id,0x83,[value],verbose=1) - elif self.trace.variables["triangle_position"].get() == 1: - value = self.trace.test_triangle_position() - if value != 0: - print("write RAM...") - self.protocol.write_word_command(self.id.current_id,0x83,[value],verbose=1) - - # send read command - if (self.counter%100)==0: - verb = 1 - end_time = time.time()*1000.0 - print("delay for 100 iterations:" + str(end_time-self.start_time) + "ms") - self.start_time = end_time - else: - verb = 0 - error, result = self.protocol.read_byte_command( - self.id.current_id, # ID - 0x80, # from EEPROM - 50, # byte number to read - verbose=verb - ) # TODO change ID through GUI - - - if error != 0 : - print("error:"+str(error)) - elif len(result)==50: - goal_position = float(sign(result[3] + (result[4]<<8))/10.0) - setpoint_position = float(sign(result[32] + (result[33]<<8))/10.0) - present_position = float(sign(result[16] + (result[17]<<8))/10.0) - goal_velocity = float(sign(result[5] + (result[6]<<8))) - setpoint_velocity = float(sign(result[34] + (result[35]<<8))) - present_velocity = float(sign(result[18] + (result[19]<<8))) - goal_torque_current = float(sign(result[7] + (result[8]<<8))) - setpoint_torque_current = float(sign(result[36] + (result[37]<<8))) - present_torque_current = float(sign(result[20] + (result[21]<<8))) - goal_flux_current = float(sign( result[9] + (result[10]<<8))) - setpoint_flux_current = float(sign( result[38] + (result[39]<<8))) - present_flux_current = float(sign( result[22] + (result[23]<<8))) - kp = result[11] - kd = result[12] - goal_synchro_offset = float(sign( result[13] + (result[14]<<8))) - - self.trace.update( - goal_position, - setpoint_position, - present_position, - goal_velocity, - setpoint_velocity, - present_velocity, - goal_torque_current, - setpoint_torque_current, - present_torque_current, - goal_flux_current, - setpoint_flux_current, - present_flux_current - ) + if self.protocol: + # write test + if self.trace.variables["square_position"].get() == 1: + value = self.trace.test_square_position() + if value != 0: + print("write RAM...") + self.protocol.write_word_command(self.id.current_id,0x83,[value],verbose=1) + elif self.trace.variables["triangle_position"].get() == 1: + value = self.trace.test_triangle_position() + if value != 0: + print("write RAM...") + self.protocol.write_word_command(self.id.current_id,0x83,[value],verbose=1) + elif self.trace.variables["sinus_position"].get() == 1: + value = self.trace.test_sinus_position() + if value != 0: + print("write RAM...") + self.protocol.write_word_command(self.id.current_id,0x83,[value],verbose=1) + + # send read command + if (self.counter%100)==0: + verb = 1 + end_time = time.time()*1000.0 + print("delay for 100 iterations:" + str(end_time-self.start_time) + "ms") + self.start_time = end_time + else: + verb = 0 + error, result = self.protocol.read_byte_command( + self.id.current_id, # ID + 0x80, # from EEPROM + 50, # byte number to read + verbose=verb + ) # TODO change ID through GUI + + + if error != 0 : + print("error:"+str(error)) + elif len(result)==50: + goal_position = float(sign(result[3] + (result[4]<<8))/10.0) + setpoint_position = float(sign(result[32] + (result[33]<<8))/10.0) + present_position = float(sign(result[16] + (result[17]<<8))/10.0) + goal_velocity = float(sign(result[5] + (result[6]<<8))) + setpoint_velocity = float(sign(result[34] + (result[35]<<8))) + present_velocity = float(sign(result[18] + (result[19]<<8))) + goal_torque_current = float(sign(result[7] + (result[8]<<8))) + self.setpoint_torque_current = (1.0-self.alpha)*self.setpoint_torque_current+self.alpha*float(sign(result[36] + (result[37]<<8))) + self.present_torque_current = (1.0-self.alpha)*self.present_torque_current+self.alpha*float(sign(result[20] + (result[21]<<8))) + goal_flux_current = float(sign( result[9] + (result[10]<<8))) + self.setpoint_flux_current = (1.0-self.alpha)*self.setpoint_flux_current+self.alpha*float(sign( result[38] + (result[39]<<8))) + self.present_flux_current = (1.0-self.alpha)*self.present_flux_current+self.alpha*float(sign( result[22] + (result[23]<<8))) + pos_kp = result[11] + pos_kd = result[12] + vel_kp = result[13] + goal_synchro_offset = float(sign( result[14] + (result[15]<<8))) + + self.trace.update( + goal_position, + setpoint_position, + present_position, + goal_velocity, + setpoint_velocity, + present_velocity, + goal_torque_current, + self.setpoint_torque_current, + self.present_torque_current, + goal_flux_current, + self.setpoint_flux_current, + self.present_flux_current + ) + + if self.counter == 0: + self.variables['torque_enable_local'].set(str(result[0])) + self.variables['led_local'].set(str(result[1])) + #self.variables['control_mode_local'].set(str(result[2])) + self.variables['goal_position_local'].set(str( goal_position )) + self.variables['goal_velocity_local'].set(str( goal_velocity )) + self.variables['goal_torque_current_local'].set(str( goal_torque_current )) + self.variables['goal_flux_current_local'].set(str( goal_flux_current )) + self.variables['goal_pos_kp_local'].set(str( pos_kp )) + self.variables['goal_pos_kd_local'].set(str( pos_kd )) + self.variables['goal_vel_kp_local'].set(str( vel_kp )) + self.variables['goal_synchro_offset_local'].set(str( goal_synchro_offset )) + #self.variables['goal_open_loop_local'].set(str( result[15] )) + + self.variables['torque_enable_servo'].set(str(result[0])) + self.variables['led_servo'].set(str(result[1])) + self.variables['control_mode_servo'].set(str(result[2])) + + self.variables['goal_position_servo'].set(str( goal_position )) + self.variables['goal_velocity_servo'].set(str( goal_velocity )) + self.variables['goal_torque_current_servo'].set(str( goal_torque_current )) + self.variables['goal_flux_current_servo'].set(str( goal_flux_current )) + + self.variables['goal_pos_kp_servo'].set(str( pos_kp )) + self.variables['goal_pos_kd_servo'].set(str( pos_kd )) + self.variables['goal_vel_kp_servo'].set(str( vel_kp )) + + self.variables['goal_synchro_offset_servo'].set(str( goal_synchro_offset )) + #self.variables['goal_open_loop_servo'].set(str( result[15] )) + + self.variables['present_position_servo'].set(str( present_position )) + self.variables['present_velocity_servo'].set(str( present_velocity )) + self.variables['present_torque_current_servo'].set(str( int(self.present_torque_current) )) + self.variables['present_flux_current_servo'].set(str( int(self.present_flux_current) )) + self.variables['present_voltage_servo'].set(str(result[24])) + self.variables['present_temperature_servo'].set(str(result[25])) + self.variables['moving_servo'].set(str(result[26])) + self.variables['setpoint_position_servo'].set(str( setpoint_position )) + self.variables['setpoint_velocity_servo'].set(str( setpoint_velocity )) + self.variables['setpoint_torque_current_servo'].set(str( int(self.setpoint_torque_current) )) + self.variables['setpoint_flux_current_servo'].set(str( int(self.setpoint_flux_current) )) + self.variables['processing_time_servo'].set(str(result[42])) + self.variables['foc_frequency_servo'].set(str(result[43])) + self.variables['pid_frequency_servo'].set(str(result[44])) + self.variables['mlp_frequency_servo'].set(str(result[45])) - if self.counter == 0: - self.variables['led_local'].set(str(result[1])) - self.variables['control_mode_local'].set(str(result[2])) - self.variables['goal_position_local'].set(str( goal_position )) - self.variables['goal_velocity_local'].set(str( goal_velocity )) - self.variables['goal_torque_current_local'].set(str( goal_torque_current )) - self.variables['goal_flux_current_local'].set(str( goal_flux_current )) - self.variables['goal_kd_local'].set(str( kd )) - self.variables['goal_kp_local'].set(str( kp )) - self.variables['goal_synchro_offset_local'].set(str( goal_synchro_offset )) - self.variables['goal_open_loop_local'].set(str( result[15] )) - - self.variables['torque_enable_servo'].set(str(result[0])) - self.variables['led_servo'].set(str(result[1])) - self.variables['control_mode_servo'].set(str(result[2])) - - self.variables['goal_position_servo'].set(str( goal_position )) - self.variables['goal_velocity_servo'].set(str( goal_velocity )) - self.variables['goal_torque_current_servo'].set(str( goal_torque_current )) - self.variables['goal_flux_current_servo'].set(str( goal_flux_current )) - - self.variables['goal_kp_servo'].set(str( kp )) - self.variables['goal_kd_servo'].set(str( kd )) - - self.variables['goal_synchro_offset_servo'].set(str( goal_synchro_offset )) - self.variables['goal_open_loop_servo'].set(str( result[15] )) - - self.variables['present_position_servo'].set(str( present_position )) - self.variables['present_velocity_servo'].set(str( present_velocity )) - self.variables['present_torque_current_servo'].set(str( present_torque_current )) - self.variables['present_flux_current_servo'].set(str( present_flux_current )) - self.variables['present_voltage_servo'].set(str(result[24])) - self.variables['present_temperature_servo'].set(str(result[25])) - self.variables['moving_servo'].set(str(result[26])) - self.variables['setpoint_position_servo'].set(str( setpoint_position )) - self.variables['setpoint_velocity_servo'].set(str( setpoint_velocity )) - self.variables['setpoint_torque_current_servo'].set(str( setpoint_torque_current )) - self.variables['setpoint_flux_current_servo'].set(str( setpoint_flux_current )) - self.variables['processing_time_servo'].set(str(result[42])) - self.variables['foc_frequency_servo'].set(str(result[43])) - self.variables['pid_frequency_servo'].set(str(result[44])) - - self.variables['protocol_crc_fail_servo'].set(str(result[48])) - self.variables['hardware_error_status_servo'].set(str(result[49])) - self.data_ready = 1 - - - self.counter += 1 + self.variables['protocol_crc_fail_servo'].set(str(result[48])) + #self.variables['hardware_error_status_servo'].set(str(result[49])) + + # process ERRORS + HW_ERROR_BIT_VOLTAGE = 0 + HW_ERROR_BIT_POSITION_SENSOR_STATUS_ERROR = 1 + HW_ERROR_BIT_POSITION_SENSOR_NOT_RESPONDING = 2 + HW_ERROR_BIT_POSITION_SENSOR_TIMESTAMP = 3 + HW_ERROR_BIT_FOC_TIMEOUT = 4 + HW_ERROR_BIT_OVERLOAD = 5 + HW_ERROR_BIT_OVERHEATING = 6 + + error_code = result[49] + error_str = "" + + if error_code & (1<= self.test_timer+20.0: self.test_timer = time.time()*1000.0 diff --git a/cd00298474-stm32f-pmsm-singledual-foc-sdk-v43-stmicroelectronics.pdf b/cd00298474-stm32f-pmsm-singledual-foc-sdk-v43-stmicroelectronics.pdf new file mode 100644 index 0000000..08874a8 Binary files /dev/null and b/cd00298474-stm32f-pmsm-singledual-foc-sdk-v43-stmicroelectronics.pdf differ