From 1230e96c1f43e42f47f1869cf7b5ce23d5dca5d9 Mon Sep 17 00:00:00 2001 From: Nicolas Trimborn Date: Mon, 23 Aug 2021 21:03:39 +0200 Subject: [PATCH] Got ADS1299 working in syncronous mode. --- .gitmodules | 3 + 2_Motor_Master/LAN9252-2_motor_master_mkV.xml | 867 ++++++++++++++++++ .../.atmelstart/atmel_start_config.atstart | 5 +- .../Motor_Master/Motor_Master/ADS1299.c | 220 +++-- .../Motor_Master/Motor_Master/ADS1299.h | 55 +- .../Motor_Master/EtherCAT_SlaveDef.h | 32 +- .../Motor_Master/Ethercat_SlaveDef.h | 32 +- .../Motor_Master/Motor_Master.cproj | 4 +- .../Motor_Master/Motor_Master/bldc.c | 1 + .../Motor_Master/Motor_Master/bldc_types.h | 1 + .../Motor_Master/Motor_Master/driver_init.c | 10 + .../Motor_Master/Motor_Master/interrupts.h | 18 +- .../Motor_Master/Motor_Master/main.c | 71 +- 2_Motor_Slave/Motor_Slave/Motor_Slave/main.c | 2 +- 14 files changed, 1171 insertions(+), 150 deletions(-) create mode 100644 2_Motor_Master/LAN9252-2_motor_master_mkV.xml diff --git a/.gitmodules b/.gitmodules index 2e44cd2..4127f6d 100644 --- a/.gitmodules +++ b/.gitmodules @@ -4,3 +4,6 @@ [submodule "Examples/OpenBCI_Cyton_Library"] path = Examples/OpenBCI_Cyton_Library url = https://github.com/OpenBCI/OpenBCI_Cyton_Library.git +[submodule "Examples/ADS129X"] + path = Examples/ADS129X + url = https://github.com/ferdinandkeil/ADS129X.git diff --git a/2_Motor_Master/LAN9252-2_motor_master_mkV.xml b/2_Motor_Master/LAN9252-2_motor_master_mkV.xml new file mode 100644 index 0000000..41faa92 --- /dev/null +++ b/2_Motor_Master/LAN9252-2_motor_master_mkV.xml @@ -0,0 +1,867 @@ + + + + #x000004D8 + Microchip Technology Inc + + + + + LAN9252-2_motor_Master + LAN9252-2_motor_Master + 424DD8020000000000003600000028000000100000000E0000000100180000000000A2020000120B0000120B000000000000000000001306E31306E3190CE42B1FE62B1FE61306E31F13E5190CE42519E51306E31306E3190CE42F24E7190CE41306E31306E31306E31306E35F56EC645CED645CED4137E91F13E5473DE95F57EC3227E71306E3473DE95A51EC271BE61306E31306E31409CA524CC68E8AD74F48C1615CC82218D03E36BF716BCE746FCE453DC01307CE3931BA7D78D27671D1150CB21409CA1712801B1D1D1B1D1D1B1D1D1B1D1D120B891B1D1D1B1D1D1B1D1D1B1D1D120B891B1D1D1B1D1D1B1D1D1B1D1D1712801712807F8080D4D5D5D4D5D5383939120B89545656D4D5D5D4D5D5626464130C89292B2BD4D5D5D4D5D56264641915801712804647471B1D1DAAAAAAD4D5D5130E82383939292B2B717272D4D5D5151183D4D5D57F80801B1D1D7172721E1C81191580464747D4D5D5D4D5D51B1D1D19158A292B2BD4D5D5D4D5D5292B2B1B1B8AD4D5D56264641B1D1D1B1D1D2427821E1D81D4D5D54647476264643839391E208BD4D5D57F8080464747545656242A8BD4D5D59B9C9C292B2BAAAAAA2D3683252882464747D4D5D5D4D5D51B1D1D272D85292B2BD4D5D5D4D5D5292B2B2E37861B1D1DD4D5D5D4D5D5464747394484323BB52324812122822426822526824554C0323883292B822A2D83353C84424CBF3238843940842E32834853865D6EBB5262EB3E43E83334E74147E94349E9535FEB4D56EA5662EB484DEA545DEB636FED545AEA5A63EC6671ED8CA0F290A5F2748AEF6B7BEE5D68EC6874ED788AEF8397F17684EF7986EF8C9FF2818FF1818EF08E9DF18A97F18791F19BA9F3B0C0F691A4F291A2F28390F192A1F29CACF3A3B3F498A6F3A4B3F4AEBDF5B0BEF59EA8F3A3ADF4BBC7F7C4D1F8CAD7F8CED9F9B4C4F6B8C8F6ACB8F59AA3F3B6C1F6C5D2F8C2CDF8CCD7F9D2DDF9D5E0FAD2DAF9D5DCF9DFE7FBE2E9FBE5EBFBE8EEFB0000 + + + + + LAN9252-2_motor_Master + + + + 4096 + 4 + 3 + + + MII + 0 + + + MII + 1 + + #x0012 + + LAN9252-2_motor_Master + Outputs + Inputs + Outputs + Inputs + + #x1a00 + ECAT2MCU + + #x3101 + 1 + 8 + M1__control_mode + BYTE + + + #x3102 + 1 + 8 + M1__control_set + BYTE + + + #x3103 + 1 + 16 + M1__desired_position + INT + + + #x3104 + 1 + 16 + M1__desired_speed + INT + + + #x3105 + 1 + 16 + M1__desired__current + INT + + + #x3106 + 1 + 16 + M1__Max_Pos + INT + + + #x3107 + 1 + 16 + M1__Max_velocity + INT + + + #x3108 + 1 + 16 + M1__Max_current + INT + + + #x3109 + 1 + 16 + M1__Spare + INT + + + #x310A + 1 + 8 + M2__control_mode + BYTE + + + #x310B + 1 + 8 + M2__control_set + BYTE + + + #x310C + 1 + 16 + M2__desired_position + INT + + + #x310D + 1 + 16 + M2__desired_speed + INT + + + #x310E + 1 + 16 + M2__desired__current + INT + + + #x310F + 1 + 16 + M2__Max_Pos + INT + + + #x3110 + 1 + 16 + M2__Max_velocity + INT + + + #x3111 + 1 + 16 + M2__Max_current + INT + + + #x3112 + 1 + 16 + M2__Spare + INT + + + #x3113 + 1 + 16 + SPARE__1_RX + INT + + + #x3114 + 1 + 16 + SPARE__2_RX + INT + + + #x3115 + 1 + 16 + SPARE__3_RX + INT + + + #x3116 + 1 + 16 + SPARE__4_RX + INT + + + #x3117 + 1 + 16 + SPARE__5_RX + INT + + + #x3118 + 1 + 16 + SPARE__6_RX + INT + + + #x3119 + 1 + 16 + SPARE__7_RX + INT + + + #x311A + 1 + 16 + SPARE__8_RX + INT + + + #x311B + 1 + 16 + SPARE__9_RX + INT + + + #x311C + 1 + 16 + SPARE__10_RX + INT + + + #x311D + 1 + 16 + SPARE__11_RX + INT + + + #x311E + 1 + 16 + SPARE__12_RX + INT + + + #x311F + 1 + 16 + SPARE__13_RX + INT + + + #x3120 + 1 + 16 + SPARE__14_RX + INT + + + #x3121 + 1 + 16 + SPARE__15_RX + INT + + + #x3122 + 1 + 16 + SPARE__16_RX + INT + + + #x3123 + 1 + 8 + M3__control_mode + BYTE + + + #x3124 + 1 + 8 + M3__control_set + BYTE + + + #x3125 + 1 + 16 + M3__desired_position + INT + + + #x3126 + 1 + 16 + M3__desired_speed + INT + + + #x3127 + 1 + 16 + M3__desired__current + INT + + + #x3128 + 1 + 16 + M3__Max_Pos + INT + + + #x3129 + 1 + 16 + M3__Max_velocity + INT + + + #x312A + 1 + 16 + M3__Max_current + INT + + + #x312B + 1 + 16 + M3__Spare + INT + + + #x312C + 1 + 8 + M4__control_mode + BYTE + + + #x312D + 1 + 8 + M4__control_set + BYTE + + + #x312E + 1 + 16 + M4__desired_position + INT + + + #x312F + 1 + 16 + M4__desired_speed + INT + + + #x3130 + 1 + 16 + M4__desired__current + INT + + + #x3131 + 1 + 16 + M4__Max_Pos + INT + + + #x3132 + 1 + 16 + M4__Max_velocity + INT + + + #x3133 + 1 + 16 + M4__Max_current + INT + + + #x3134 + 1 + 16 + M4__Spare + INT + + + + #x1600 + MCU2ECAT + + #x3001 + 1 + 8 + M1__status + BYTE + + + #x3002 + 1 + 8 + M1__mode + BYTE + + + #x3003 + 1 + 16 + M1__Joint_rel_position + INT + + + #x3004 + 1 + 16 + M1__Joint_abs_position + INT + + + #x3005 + 1 + 16 + M1__Motor_speed + INT + + + #x3006 + 1 + 16 + M1__Motor_current_bus + INT + + + #x3007 + 1 + 16 + M1__Motor_currentPhA + INT + + + #x3008 + 1 + 16 + M1__Motor_currentPhB + INT + + + #x3009 + 1 + 16 + M1__Motor_currentPhC + INT + + + #x300A + 1 + 16 + M1__Motor_hallState + INT + + + #x300B + 1 + 16 + M1__Motor_dutyCycle + INT + + + #x300C + 1 + 8 + M2__status + BYTE + + + #x300D + 1 + 8 + M2__mode + BYTE + + + #x300E + 1 + 16 + M2__Joint__rel_position + INT + + + #x300F + 1 + 16 + M2__Joint_abs_position + INT + + + #x3010 + 1 + 16 + M2__Motor_speed + INT + + + #x3011 + 1 + 16 + M2__Motor_current_bus + INT + + + #x3012 + 1 + 16 + M2__Motor_currentPhA + INT + + + #x3013 + 1 + 16 + M2__Motor_currentPhB + INT + + + #x3014 + 1 + 16 + M2__Motor_currentPhC + INT + + + #x3015 + 1 + 16 + M2__Motor_hallState + INT + + + #x3016 + 1 + 16 + M2__Motor_dutyCycle + INT + + + #x3017 + 1 + 32 + EMG__CH1 + DINT + + + #x3018 + 1 + 32 + EMG__CH2 + DINT + + + #x3019 + 1 + 32 + EMG__CH3 + DINT + + + #x301A + 1 + 32 + EMG__CH4 + DINT + + + #x301B + 1 + 16 + SPARE__1_TX + INT + + + #x301C + 1 + 16 + SPARE__2_TX + INT + + + #x301D + 1 + 16 + SPARE__3_TX + INT + + + #x301E + 1 + 16 + SPARE__4_TX + INT + + + #x301F + 1 + 8 + M3__status + BYTE + + + #x3020 + 1 + 8 + M3__mode + BYTE + + + #x3021 + 1 + 16 + M3__Joint__rel_position + INT + + + #x3022 + 1 + 16 + M3__Joint_abs_position + INT + + + #x3023 + 1 + 16 + M3__Motor_speed + INT + + + #x3024 + 1 + 16 + M3__Motor_current_bus + INT + + + #x3025 + 1 + 16 + M3__Motor_currentPhA + INT + + + #x3026 + 1 + 16 + M3__Motor_currentPhB + INT + + + #x3027 + 1 + 16 + M3__Motor_currentPhC + INT + + + #x3028 + 1 + 16 + M3__Motor_hallState + INT + + + #x3029 + 1 + 16 + M3__Motor_dutyCycle + INT + + + #x302A + 1 + 8 + M4__status + BYTE + + + #x302B + 1 + 8 + M4__mode + BYTE + + + #x302C + 1 + 16 + M4__Joint__rel_position + INT + + + #x302D + 1 + 16 + M4__Joint_abs_position + INT + + + #x302E + 1 + 16 + M4__Motor_speed + INT + + + #x302F + 1 + 16 + M4__Motor_current_bus + INT + + + #x3030 + 1 + 16 + M4__Motor_currentPhA + INT + + + #x3031 + 1 + 16 + M4__Motor_currentPhB + INT + + + #x3032 + 1 + 16 + M4__Motor_currentPhC + INT + + + #x3033 + 1 + 16 + M4__Motor_hallState + INT + + + #x3034 + 1 + 16 + M4__Motor_dutyCycle + INT + + + #x3035 + 1 + 16 + IMU__q_x0 + INT + + + #x3036 + 1 + 16 + IMU__q_y0 + INT + + + #x3037 + 1 + 16 + IMU__q_z0 + INT + + + #x3038 + 1 + 16 + IMU__q_w0 + INT + + + #x3039 + 1 + 16 + FSR__CH1 + INT + + + #x303A + 1 + 16 + FSR__CH2 + INT + + + #x303B + 1 + 16 + FSR__CH3 + INT + + + #x303C + 1 + 16 + FSR__CH4 + INT + + + #x303D + 1 + 16 + FSR__CH5 + INT + + + #x303E + 1 + 16 + Pressure__CH1 + INT + + + #x303F + 1 + 16 + Pressure__CH2 + INT + + + #x3040 + 1 + 16 + Pressure__CH3 + INT + + + + 2048 + 803100CC1000F0FF + + + + + \ No newline at end of file diff --git a/2_Motor_Master/Motor_Master/Motor_Master/.atmelstart/atmel_start_config.atstart b/2_Motor_Master/Motor_Master/Motor_Master/.atmelstart/atmel_start_config.atstart index 5ce51a4..7949299 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/.atmelstart/atmel_start_config.atstart +++ b/2_Motor_Master/Motor_Master/Motor_Master/.atmelstart/atmel_start_config.atstart @@ -2400,9 +2400,10 @@ pads: SPI2_SS: name: PA14 definition: Atmel:SAME51_Drivers:0.0.1::SAME51J19A-MF::pad::PA14 - mode: Peripheral IO + mode: Digital output user_label: SPI2_SS - configuration: null + configuration: + pad_initial_level: High SPI2_MISO: name: PA15 definition: Atmel:SAME51_Drivers:0.0.1::SAME51J19A-MF::pad::PA15 diff --git a/2_Motor_Master/Motor_Master/Motor_Master/ADS1299.c b/2_Motor_Master/Motor_Master/Motor_Master/ADS1299.c index 6bb7a1b..0860fac 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/ADS1299.c +++ b/2_Motor_Master/Motor_Master/Motor_Master/ADS1299.c @@ -7,6 +7,41 @@ #include "ADS1299.h" +/* Settings +* No daisy: +* 4 EMG channels channels at 1000.0 Hz. +* Gain 24, SRB2 Open, Normal electrode input +* Route all Channels_P to Bias derivation +* Route all Channels_P to Bias derivation +* Lead off functions off + +Expected ADS Board Registers +* ADS_ID, 00, 3C, 0, 0, 1, 1, 1, 1, 1, 0 +* CONFIG1, 01, 94, 1, 0, 0, 1, 0, 1, 0, 0 +* CONFIG2, 02, C0, 1, 1, 0, 0, 0, 0, 0, 0 +* CONFIG3, 03, EC, 1, 1, 1, 0, 1, 1, 0, 0 +* LOFF, 04, 02, 0, 0, 0, 0, 0, 0, 1, 0 +* CH1SET, 05, 68, 0, 1, 1, 0, 1, 0, 0, 0 +* CH2SET, 06, 68, 0, 1, 1, 0, 1, 0, 0, 0 +* CH3SET, 07, 68, 0, 1, 1, 0, 1, 0, 0, 0 +* CH4SET, 08, 68, 0, 1, 1, 0, 1, 0, 0, 0 +* CH5SET, 09, 68, 0, 1, 1, 0, 1, 0, 0, 0 +* CH6SET, 0A, 68, 0, 1, 1, 0, 1, 0, 0, 0 +* CH7SET, 0B, 68, 0, 1, 1, 0, 1, 0, 0, 0 +* CH8SET, 0C, 68, 0, 1, 1, 0, 1, 0, 0, 0 +* BIAS_SENSP, 0D, FF, 1, 1, 1, 1, 1, 1, 1, 1 +* BIAS_SENSN, 0E, FF, 1, 1, 1, 1, 1, 1, 1, 1 +* LOFF_SENSP, 0F, 00, 0, 0, 0, 0, 0, 0, 0, 0 +* LOFF_SENSN, 10, 00, 0, 0, 0, 0, 0, 0, 0, 0 +* LOFF_FLIP, 11, 00, 0, 0, 0, 0, 0, 0, 0, 0 +* LOFF_STATP, 12, 00, 0, 0, 0, 0, 0, 0, 0, 0 +* LOFF_STATN, 13, 00, 0, 0, 0, 0, 0, 0, 0, 0 +* GPIO, 14, 0F, 0, 0, 0, 0, 1, 1, 1, 1 +* MISC1, 15, 00, 0, 0, 0, 0, 0, 0, 0, 0 +* MISC2, 16, 00, 0, 0, 0, 0, 0, 0, 0, 0 +* CONFIG4, 17, 00, 0, 0, 0, 0, 0, 0, 0, 0 +*/ + void initialize_ads() { spi_m_sync_disable(ADS1299.SPI_descr); @@ -16,17 +51,41 @@ void initialize_ads() /* Init SPI*/ spi_m_sync_enable(ADS1299.SPI_descr); gpio_set_pin_level(ADS1299.SS_pin, true); + delay_us(20); /* Reset ADS1299 - Reset Active Low*/ gpio_set_pin_level(ADS1299.reset_pin, false); - delay_us(5); + delay_us(20); gpio_set_pin_level(ADS1299.reset_pin, true); delay_us(20); + + ADS1299_RESET(); // all registers set to default + ADS1299_SDATAC(); // stop Read Data Continuous mode to communicate with ADS + ADS1299_RREGS(0x00, 0x17); // read ADS registers starting at 0x00 and ending at 0x17 + + /* Write Config Registers */ + ADS1299_WREG(CONFIG1,0x94); // Set Config 1 Register + ADS1299_WREG(CONFIG2,0xC0); // Set Config 2 Register + ADS1299_WREG(CONFIG3,0xEC); // Set Config 3 Register + ADS1299_WREG(LOFF,0x02); // Set LOFF Register + for(uint8_t i=CH1SET; i<=CH4SET; i++) // set up to modify the 4 channel setting registers + { + ADS1299.regData[i] = 0x68; // the regData array mirrors the ADS1299 register addresses + } + ADS1299_WREGS(CH1SET,3); // write new channel settings + ADS1299_WREG(BIAS_SENSP,0xFF); // Set BIAS_SENSP Register + ADS1299_WREG(BIAS_SENSN,0xFF); // Set BIAS_SENSN Register + ADS1299_WREG(BIAS_SENSP,0xFF); // Set BIAS_SENSP Register + ADS1299_WREG(GPIO_REG,0x0F); // Set BIAS_SENSP Register + /* Remaining Registers left at 0x00 */ + ADS1299_RREGS(0x00, 0x17); // read ADS registers to verify settings + ADS1299_RDATAC(); // enter Read Data Continuous mode + volatile int x = 0; } uint8_t getDeviceID() { - uint8_t data = RREG(0x00); + uint8_t data = ADS1299_RREG(0x00); return data; } @@ -34,120 +93,179 @@ uint8_t getDeviceID() // AS5048 Registers & Commands // ---------------------------------------------------------------------- -void WAKEUP() { +void ADS1299_WAKEUP() { gpio_set_pin_level(ADS1299.SS_pin, false); - transfer_byte(ADS1299.SPI_descr, _WAKEUP); + _transfer_byte(ADS1299.SPI_descr, _WAKEUP); gpio_set_pin_level(ADS1299.SS_pin, true); delay_us(3); //must wait 4 tCLK cycles before sending another command (Datasheet, pg. 35) } -void STANDBY() { // only allowed to send WAKEUP after sending STANDBY +void ADS1299_STANDBY() { // only allowed to send WAKEUP after sending STANDBY gpio_set_pin_level(ADS1299.SS_pin, false); - transfer_byte(ADS1299.SPI_descr, _STANDBY); + _transfer_byte(ADS1299.SPI_descr, _STANDBY); gpio_set_pin_level(ADS1299.SS_pin, true); } -void RESET() { // reset all the registers to default settings +void ADS1299_RESET() { // reset all the registers to default settings gpio_set_pin_level(ADS1299.SS_pin, false); - transfer_byte(ADS1299.SPI_descr, _RESET); + _transfer_byte(ADS1299.SPI_descr, _RESET); delay_us(12); //must wait 18 tCLK cycles to execute this command (Datasheet, pg. 35) gpio_set_pin_level(ADS1299.SS_pin, true); } -void START() { //start data conversion +void ADS1299_START() { //start data conversion gpio_set_pin_level(ADS1299.SS_pin, false); - transfer_byte(ADS1299.SPI_descr, _START); + _transfer_byte(ADS1299.SPI_descr, _START); gpio_set_pin_level(ADS1299.SS_pin, true); } -void STOP() { //stop data conversion +void ADS1299_STOP() { //stop data conversion gpio_set_pin_level(ADS1299.SS_pin, false); - transfer_byte(ADS1299.SPI_descr, _STOP); + _transfer_byte(ADS1299.SPI_descr, _STOP); gpio_set_pin_level(ADS1299.SS_pin, true); } -void RDATAC() { +void ADS1299_RDATAC() { gpio_set_pin_level(ADS1299.SS_pin, false); - transfer_byte(ADS1299.SPI_descr, _RDATAC); + _transfer_byte(ADS1299.SPI_descr, _RDATAC); gpio_set_pin_level(ADS1299.SS_pin, true); delay_us(3); } -void SDATAC() { +void ADS1299_SDATAC() { gpio_set_pin_level(ADS1299.SS_pin, false); - transfer_byte(ADS1299.SPI_descr, _SDATAC); + _transfer_byte(ADS1299.SPI_descr, _SDATAC); gpio_set_pin_level(ADS1299.SS_pin, true); delay_us(3); //must wait 4 tCLK cycles after executing this command (Datasheet, pg. 37) } - // ---------------------------------------------------------------------- // SPI register Related Commands // ---------------------------------------------------------------------- - - // reads ONE register at _address -uint8_t RREG(uint8_t _address) +uint8_t ADS1299_RREG(uint8_t _address) { - uint8_t opcode1 = _address + 0x20; // RREG expects 001rrrrr where rrrrr = _address + uint8_t opcode1 = _address + 0x20; // RREG expects 001rrrrr where rrrrr = _address gpio_set_pin_level(ADS1299.SS_pin, false); - transfer_byte(ADS1299.SPI_descr, opcode1); - transfer_byte(ADS1299.SPI_descr, 0x00); - ADS1299.regData[_address] = transfer_byte(ADS1299.SPI_descr, 0x00); + _transfer_byte(ADS1299.SPI_descr, opcode1); + _transfer_byte(ADS1299.SPI_descr, 0x00); + ADS1299.regData[_address] = _transfer_byte(ADS1299.SPI_descr, 0x00); gpio_set_pin_level(ADS1299.SS_pin, true); return ADS1299.regData[_address]; } // Read more than one register starting at _address -void RREGS(uint8_t _address, uint8_t _numRegistersMinusOne) +void ADS1299_RREGS(uint8_t _address, uint8_t _numRegistersMinusOne) { - uint8_t opcode1 = _address + 0x20; // RREG expects 001rrrrr where rrrrr = _address + uint8_t opcode1 = _address + 0x20; // RREG expects 001rrrrr where rrrrr = _address gpio_set_pin_level(ADS1299.SS_pin, false); - transfer_byte(ADS1299.SPI_descr, opcode1); - transfer_byte(ADS1299.SPI_descr, _numRegistersMinusOne); - for(int i = 0; i <= _numRegistersMinusOne; i++){ - ADS1299.regData[_address + i] = transfer_byte(ADS1299.SPI_descr, 0x00); // add register uint8_t to mirror array + _transfer_byte(ADS1299.SPI_descr, opcode1); + _transfer_byte(ADS1299.SPI_descr, _numRegistersMinusOne); + for(int i = 0; i <= _numRegistersMinusOne; i++){ // add register uint8_t to mirror array + ADS1299.regData[_address + i] = _transfer_byte(ADS1299.SPI_descr, 0x00); } - ADS1299.regData[_address] = transfer_byte(ADS1299.SPI_descr, 0x00); + //ADS1299.regData[_address] = _transfer_byte(ADS1299.SPI_descr, 0x00); gpio_set_pin_level(ADS1299.SS_pin, true); } -void WREG(uint8_t _address, uint8_t _value) { // Write ONE register at _address +void ADS1299_WREG(uint8_t _address, uint8_t _value) { // Write ONE register at _address uint8_t opcode1 = _address + 0x40; // WREG expects 010rrrrr where rrrrr = _address gpio_set_pin_level(ADS1299.SS_pin, false); // open SPI - transfer_byte(ADS1299.SPI_descr, opcode1); // Send WREG command & address - transfer_byte(ADS1299.SPI_descr, 0x00); // Send number of registers to read -1 - transfer_byte(ADS1299.SPI_descr, _value); // Write the value to the register + _transfer_byte(ADS1299.SPI_descr, opcode1); // Send WREG command & address + _transfer_byte(ADS1299.SPI_descr, 0x00); // Send number of registers to read -1 + _transfer_byte(ADS1299.SPI_descr, _value); // Write the value to the register gpio_set_pin_level(ADS1299.SS_pin, true); // close SPI ADS1299.regData[_address] = _value; // update the mirror array } -void WREGS(uint8_t _address, uint8_t _numRegistersMinusOne) { +void ADS1299_WREGS(uint8_t _address, uint8_t _numRegistersMinusOne) { uint8_t opcode1 = _address + 0x40; // WREG expects 010rrrrr where rrrrr = _address gpio_set_pin_level(ADS1299.SS_pin, false); // open SPI - transfer_byte(ADS1299.SPI_descr, opcode1); // Send WREG command & address - transfer_byte(ADS1299.SPI_descr, _numRegistersMinusOne); // Send number of registers to read -1 + _transfer_byte(ADS1299.SPI_descr, opcode1); // Send WREG command & address + _transfer_byte(ADS1299.SPI_descr, _numRegistersMinusOne); // Send number of registers to read -1 for (int i=_address; i <=(_address + _numRegistersMinusOne); i++){ - transfer_byte(ADS1299.SPI_descr, ADS1299.regData[i]); // Write to the registers + _transfer_byte(ADS1299.SPI_descr, ADS1299.regData[i]); // Write to the registers } gpio_set_pin_level(ADS1299.SS_pin, true); // close SPI } -void updateChannelData() +//read data +void ADS1299_RDATA() { // use in Stop Read Continuous mode when DRDY goes low + uint8_t inByte; + ADS1299.stat_1 = 0; // clear the status registers + int nchan = 4; //assume 4 channel. + gpio_set_pin_level(ADS1299.SS_pin, false); // open SPI + _transfer_byte(ADS1299.SPI_descr, _RDATA); + + for(int i=0; i<3; i++){ // read 3 byte status register (1100+LOFF_STATP+LOFF_STATN+GPIO[7:4]) + inByte = _transfer_byte(ADS1299.SPI_descr, 0x00); + ADS1299.stat_1 = (ADS1299.stat_1<<8) | inByte; + } + + for(int i = 0; i - + @@ -417,7 +417,7 @@ True Maximum (-g3) True - -std=gnu99 -mfloat-abi=hard -mfpu=fpv4-sp-d16 + -std=gnu11 -mfloat-abi=hard -mfpu=fpv4-sp-d16 True diff --git a/2_Motor_Master/Motor_Master/Motor_Master/bldc.c b/2_Motor_Master/Motor_Master/Motor_Master/bldc.c index fe94b79..267ed03 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/bldc.c +++ b/2_Motor_Master/Motor_Master/Motor_Master/bldc.c @@ -104,6 +104,7 @@ void BldcInitStruct(BLDCMotor_t* const motor, BLDCMotor_param_t * const motor_pa motor->motor_status.actualDirection = 0; motor->motor_status.duty_cycle = 0; motor->motor_status.calc_rpm = 0; + motor->motor_status.abs_position = 0; motor->motor_status.Num_Steps = 0; motor->motor_status.cur_comm_step = 0; motor->motor_status.currentHallPattern = 1; diff --git a/2_Motor_Master/Motor_Master/Motor_Master/bldc_types.h b/2_Motor_Master/Motor_Master/Motor_Master/bldc_types.h index fc8b01a..e65b9fa 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/bldc_types.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/bldc_types.h @@ -62,6 +62,7 @@ volatile typedef struct volatile uint8_t actualDirection; //! The actual direction of rotation. volatile uint16_t duty_cycle; volatile int16_t calc_rpm; + volatile int16_t abs_position; volatile int16_t Num_Steps; /* Hall States */ //volatile uint8_t prevHallPattern; diff --git a/2_Motor_Master/Motor_Master/Motor_Master/driver_init.c b/2_Motor_Master/Motor_Master/Motor_Master/driver_init.c index f3a2693..c1fd65b 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/driver_init.c +++ b/2_Motor_Master/Motor_Master/Motor_Master/driver_init.c @@ -663,6 +663,16 @@ void system_init(void) // GPIO on PA14 + gpio_set_pin_level(SPI2_SS, + // Initial level + // pad_initial_level + // Low + // High + true); + + // Set pin direction to output + gpio_set_pin_direction(SPI2_SS, GPIO_DIRECTION_OUT); + gpio_set_pin_function(SPI2_SS, GPIO_PIN_FUNCTION_OFF); // GPIO on PA25 diff --git a/2_Motor_Master/Motor_Master/Motor_Master/interrupts.h b/2_Motor_Master/Motor_Master/Motor_Master/interrupts.h index 345b629..b3df411 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/interrupts.h +++ b/2_Motor_Master/Motor_Master/Motor_Master/interrupts.h @@ -60,7 +60,7 @@ void adc_sram_dma_callback(struct _dma_resource *adc_dma_res) } // ---------------------------------------------------------------------- -// EtherCAT Cycle Timer - 1kHz +// Speed Counter TC2 Overflow handler // ---------------------------------------------------------------------- void TC2_Handler(void) { @@ -71,7 +71,7 @@ void TC2_Handler(void) } // ---------------------------------------------------------------------- -// +// Speed Counter TC2 Overflow handler // ---------------------------------------------------------------------- void TC4_Handler(void) { @@ -81,7 +81,9 @@ void TC4_Handler(void) } } - +// ---------------------------------------------------------------------- +// BLDC Driver External Interrupt Handlers +// ---------------------------------------------------------------------- static void M1_RESET_BAR(void) { volatile int x = 0; @@ -92,7 +94,15 @@ static void M2_RESET_BAR(void) volatile int x = 0; } - +// ---------------------------------------------------------------------- +// ADS Data Ready Interrupt +// ---------------------------------------------------------------------- +void ADS1299_dataReadyISR(void) +{ + ADS1299.data_ReadyFlag = true; + //int32_t* temp = ADS1299_UPDATECHANNELDATA(); + volatile int x = 1; +} diff --git a/2_Motor_Master/Motor_Master/Motor_Master/main.c b/2_Motor_Master/Motor_Master/Motor_Master/main.c index 0c218a2..1b4cd03 100644 --- a/2_Motor_Master/Motor_Master/Motor_Master/main.c +++ b/2_Motor_Master/Motor_Master/Motor_Master/main.c @@ -6,6 +6,7 @@ #include "bldc.h" #include "bldc_types.h" #include "EtherCAT_QSPI.h" +#include "ADS1299.h" #include "EtherCAT_SlaveDef.h" //#include "MSIF_master.h" #include "configuration.h" @@ -13,7 +14,7 @@ #include "statemachine.h" #include "angle_sensors.h" -#include "ADS1299.h" + void process_currents() @@ -79,10 +80,12 @@ void enable_NVIC_IRQ(void) //NVIC_EnableIRQ(TC4_IRQn); // TC4: M2_Speed_Timer NVIC_EnableIRQ(DMAC_0_IRQn); NVIC_EnableIRQ(DMAC_1_IRQn); - NVIC_SetPriority(DMAC_0_IRQn, 2); + NVIC_SetPriority(DMAC_0_IRQn, 1); NVIC_SetPriority(ADC1_0_IRQn, 3); NVIC_EnableIRQ(TCC0_0_IRQn); NVIC_EnableIRQ(TCC1_0_IRQn); + NVIC_EnableIRQ(EIC_2_IRQn); + NVIC_SetPriority(EIC_2_IRQn, 3); //NVIC_SetPriority(TCC0_0_IRQn, 3); //NVIC_EnableIRQ(EIC_5_IRQn); } @@ -146,40 +149,38 @@ int main(void) { /* Initializes MCU, drivers and middleware */ atmel_start_init(); - //initMotorParameters(); - //configure_tcc_pwm(); - //BldcInitStruct(&Motor1, &FH_22mm24BXTR); - //BldcInitStruct(&Motor2, &FH_32mm24BXTR); - //Motor1.readHall = &readHallSensorM1; - //Motor2.readHall = &readHallSensorM2; - //read_zero_current_offset_value(&Motor1, &Motor2); - //adc_sync_enable_channel(&ADC_1, 6); - //adc_init_dma(); - //config_qspi(); - //custom_logic_enable(); - //enable_NVIC_IRQ(); - //ECAT_STATE_MACHINE(); - //One_ms_timer_init(); + __disable_irq(); + + /* BLDC INIT */ BldcInitStruct(&Motor1, &FH_22mm24BXTR); BldcInitStruct(&Motor2, &FH_32mm24BXTR); Motor1.readHall = &readHallSensorM1; Motor2.readHall = &readHallSensorM2; + + /* Current Offeset Calibration */ read_zero_current_offset_value(&Motor1, &Motor2); + + /* Configure Hardware */ config_qspi(); configure_tcc_pwm(); adc_sync_enable_channel(&ADC_1, 6); + + /* SPI & DMA Configs */ boardToBoardTransferInit(); init_spi_master_dma_descriptors(); adc_init_dma(); + ECAT_STATE_MACHINE(); One_ms_timer_init(); custom_logic_enable(); - //speed_timer_init(); - + angle_sensor_init(); initialize_ads(); - + /* External IRQ Config */ + ext_irq_register(GPIO_PIN(ADS_DATA_RDY), ADS1299_dataReadyISR); enable_NVIC_IRQ(); + __enable_irq(); + //ADS1299_START(); /* Replace with your application code */ while (1) { @@ -189,20 +190,15 @@ int main(void) update_telemetry(); update_setpoints(); PORT->Group[1].OUTCLR.reg = (1<Channel[0].CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE; + DMAC->Channel[1].CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE; + //_dma_enable_transaction(DMAC_CHANNEL_CONF_SERCOM_1_RECEIVE, false); + //_dma_enable_transaction(DMAC_CHANNEL_CONF_SERCOM_1_TRANSMIT, false); - // - int16_t* angles; - //int16_t* field; - //int16_t* temp; - angles = read_angle(); - *M1_Joint_abs_position = degrees(angles[0]); - *M2_Joint_abs_position = degrees(angles[1]); - - START(); - *EMG_CH1 = getDeviceID(); + //int16_t* angles; + //angles = read_angle(); + //Motor1.motor_status.abs_position = degrees(angles[0]); + //Motor2.motor_status.abs_position = degrees(angles[1]); ////field = ang_sense_read(AS_CMD_MAGNITUDE); //*Spare1_tx = (field[0] & AS_MASK); @@ -210,10 +206,7 @@ int main(void) //temp = ang_sense_read(AS_CMD_TEMP); //*Spare3_tx = (int16_t)(((float)(temp[0] & AS_MASK) / 8.0) - 273.15); //*Spare4_tx = (int16_t)(((float)(temp[1] & AS_MASK) / 8.0) - 273.15); - - volatile int y = 0; - - //spi_m_dma_transfer(&SPI_1_MSIF, (uint8_t*)Slave_1.tx_buffer, (uint8_t*)Slave_1.rx_buffer, MASTER_BUFFER_SIZE); + } if (Motor1.timerflags.current_loop_tic) { @@ -221,6 +214,12 @@ int main(void) exec_commutation(&Motor1); exec_commutation(&Motor2); } + + if (ADS1299.data_ReadyFlag){ + ADS1299.data_ReadyFlag = false; + //ADS1299_UPDATECHANNELDATA(); + } + if (run_ECAT) {ECAT_STATE_MACHINE();} } diff --git a/2_Motor_Slave/Motor_Slave/Motor_Slave/main.c b/2_Motor_Slave/Motor_Slave/Motor_Slave/main.c index 3633a90..5b529ca 100644 --- a/2_Motor_Slave/Motor_Slave/Motor_Slave/main.c +++ b/2_Motor_Slave/Motor_Slave/Motor_Slave/main.c @@ -239,7 +239,7 @@ int main(void) //as5048a_init(&SPI_3, &AS_1); - //ORIENTATION_SENSOR_0_init(); + ORIENTATION_SENSOR_0_init(); angle_sensor_init();