diff --git a/OncillaMotordriver.X/nbproject/configurations.xml b/OncillaMotordriver.X/nbproject/configurations.xml
index d0b98bc..23f587f 100644
--- a/OncillaMotordriver.X/nbproject/configurations.xml
+++ b/OncillaMotordriver.X/nbproject/configurations.xml
@@ -1,224 +1,230 @@
<?xml version="1.0" encoding="UTF-8"?>
<configurationDescriptor version="62">
<logicalFolder name="root" displayName="root" projectFiles="true">
<logicalFolder name="HeaderFiles"
displayName="Header Files"
projectFiles="true">
<itemPath>../src/config.h</itemPath>
<itemPath>../src/magnetic_encoder.h</itemPath>
<itemPath>../src/mdv_control_modes.h</itemPath>
<itemPath>../src/mdv_internal.h</itemPath>
<itemPath>../src/mdv_speed_pid.h</itemPath>
<itemPath>../src/misc_math.h</itemPath>
<itemPath>../src/motordriver.h</itemPath>
<itemPath>../src/pindefs.h</itemPath>
<itemPath>../src/pwm.h</itemPath>
<itemPath>../src/sbcp_mdv.h</itemPath>
<itemPath>../src/timer.h</itemPath>
<itemPath>../src/load_cell.h</itemPath>
+ <itemPath>../src/cos_table.h</itemPath>
+ <itemPath>../src/ring-buffer.h</itemPath>
</logicalFolder>
<logicalFolder name="LinkerScript"
displayName="Linker Files"
projectFiles="true">
</logicalFolder>
<logicalFolder name="SourceFiles"
displayName="Source Files"
projectFiles="true">
<itemPath>../src/magnetic_encoder.c</itemPath>
<itemPath>../src/main.c</itemPath>
<itemPath>../src/mdv_control_modes.c</itemPath>
<itemPath>../src/mdv_internal.c</itemPath>
<itemPath>../src/mdv_speed_pid.c</itemPath>
<itemPath>../src/motordriver.c</itemPath>
<itemPath>../src/pindefs.c</itemPath>
<itemPath>../src/pwm.c</itemPath>
<itemPath>../src/sbcp_mdv.c</itemPath>
<itemPath>../src/timer.c</itemPath>
<itemPath>../src/load_cell.c</itemPath>
+ <itemPath>../src/cos_table.c</itemPath>
+ <itemPath>../src/ring-buffer.c</itemPath>
</logicalFolder>
<logicalFolder name="ExternalFiles"
displayName="Important Files"
projectFiles="false">
<itemPath>Makefile</itemPath>
</logicalFolder>
</logicalFolder>
<sourceRootList>
<Elem>../src</Elem>
</sourceRootList>
<projectmakefile>Makefile</projectmakefile>
<confs>
<conf name="default" type="2">
<toolsSet>
<developmentServer>localhost</developmentServer>
<targetDevice>dsPIC33FJ128MC804</targetDevice>
<targetHeader></targetHeader>
<targetPluginBoard></targetPluginBoard>
<platformTool>PICkit3PlatformTool</platformTool>
<languageToolchain>XC16</languageToolchain>
<languageToolchainVersion>1.11</languageToolchainVersion>
<platform>2</platform>
</toolsSet>
<compileType>
<linkerTool>
<linkerLibItems>
- <linkerLibFileItem>../../sbcp-uc/libsbcp-uc.X/dist/128MC804_MDV/production/libsbcp-uc.X.a</linkerLibFileItem>
+ <linkerLibFileItem>../../../sbcp-uc/libsbcp-uc.X/dist/128MC804_MDV/production/libsbcp-uc.X.a</linkerLibFileItem>
</linkerLibItems>
</linkerTool>
<loading>
<useAlternateLoadableFile>false</useAlternateLoadableFile>
<alternateLoadableFile></alternateLoadableFile>
</loading>
</compileType>
<makeCustomizationType>
<makeCustomizationPreStepEnabled>false</makeCustomizationPreStepEnabled>
<makeCustomizationPreStep></makeCustomizationPreStep>
<makeCustomizationPostStepEnabled>false</makeCustomizationPostStepEnabled>
<makeCustomizationPostStep></makeCustomizationPostStep>
<makeCustomizationPutChecksumInUserID>false</makeCustomizationPutChecksumInUserID>
<makeCustomizationEnableLongLines>false</makeCustomizationEnableLongLines>
<makeCustomizationNormalizeHexFile>false</makeCustomizationNormalizeHexFile>
</makeCustomizationType>
<C30>
<property key="code-model" value="default"/>
<property key="const-model" value="default"/>
<property key="data-model" value="default"/>
<property key="enable-all-warnings" value="true"/>
<property key="enable-ansi-std" value="false"/>
<property key="enable-ansi-warnings" value="false"/>
<property key="enable-fatal-warnings" value="true"/>
<property key="enable-large-arrays" value="false"/>
<property key="enable-omit-frame-pointer" value="false"/>
<property key="enable-procedural-abstraction" value="false"/>
<property key="enable-short-double" value="false"/>
<property key="enable-symbols" value="true"/>
<property key="enable-unroll-loops" value="false"/>
- <property key="extra-include-directories" value="../../sbcp-uc/include"/>
+ <property key="extra-include-directories" value="../../../sbcp-uc/include"/>
<property key="isolate-each-function" value="false"/>
+ <property key="keep-inline" value="false"/>
<property key="oXC16gcc-align-arr" value="false"/>
<property key="oXC16gcc-cnsts-mauxflash" value="false"/>
<property key="oXC16gcc-data-sects" value="false"/>
<property key="oXC16gcc-errata" value=""/>
<property key="oXC16gcc-fillupper" value=""/>
<property key="oXC16gcc-large-aggregate" value="false"/>
<property key="oXC16gcc-mauxflash" value="false"/>
<property key="oXC16gcc-mpa-lvl" value=""/>
<property key="oXC16gcc-name-text-sec" value=""/>
<property key="oXC16gcc-near-chars" value="false"/>
<property key="oXC16gcc-no-isr-warn" value="false"/>
<property key="oXC16gcc-sfr-warn" value="true"/>
<property key="oXC16gcc-smar-io-lvl" value="1"/>
<property key="oXC16gcc-smart-io-fmt" value=""/>
<property key="optimization-level" value="0"/>
<property key="post-instruction-scheduling" value="default"/>
<property key="pre-instruction-scheduling" value="default"/>
<property key="preprocessor-macros"
value="SBCP_INST_TABLE_SIZE=8;SBCP_REG_TABLE_SIZE=128;SBCP_LOW_LATENCY_IN_SIZE=2;SBCP_LOW_LATENCY_OUT_SIZE=10"/>
<property key="scalar-model" value="default"/>
+ <property key="use-cci" value="false"/>
</C30>
<C30-AS>
<property key="assembler-symbols" value=""/>
<property key="expand-macros" value="false"/>
<property key="extra-include-directories-for-assembler" value=""/>
<property key="extra-include-directories-for-preprocessor" value=""/>
<property key="false-conditionals" value="false"/>
<property key="keep-locals" value="false"/>
<property key="list-assembly" value="false"/>
<property key="list-file" value=""/>
<property key="list-section-info" value="false"/>
<property key="list-source" value="false"/>
<property key="list-symbols" value="false"/>
<property key="omit-debug-dirs" value="false"/>
<property key="omit-forms" value="false"/>
<property key="preprocessor-macros" value=""/>
<property key="relax" value="false"/>
<property key="warning-level" value="emit-warnings"/>
</C30-AS>
<C30-LD>
<property key="boot-eeprom" value="no_eeprom"/>
<property key="boot-flash" value="no_flash"/>
<property key="boot-ram" value="no_ram"/>
<property key="boot-write-protect" value="no_write_protect"/>
<property key="enable-check-sections" value="false"/>
<property key="enable-data-init" value="true"/>
<property key="enable-default-isr" value="true"/>
<property key="enable-handles" value="true"/>
<property key="enable-pack-data" value="true"/>
<property key="extra-lib-directories" value=""/>
<property key="general-code-protect" value="no_code_protect"/>
<property key="general-write-protect" value="no_write_protect"/>
<property key="generate-cross-reference-file" value="false"/>
<property key="heap-size" value=""/>
<property key="input-libraries" value=""/>
<property key="linker-symbols" value=""/>
<property key="map-file" value=""/>
<property key="preprocessor-macros" value=""/>
<property key="remove-unused-sections" value="false"/>
<property key="report-memory-usage" value="false"/>
<property key="secure-eeprom" value="no_eeprom"/>
<property key="secure-flash" value="no_flash"/>
<property key="secure-ram" value="no_ram"/>
<property key="secure-write-protect" value="no_write_protect"/>
<property key="stack-size" value=""/>
<property key="symbol-stripping" value=""/>
<property key="trace-symbols" value=""/>
<property key="warn-section-align" value="false"/>
</C30-LD>
<C30Global>
<property key="fast-math" value="false"/>
<property key="legacy-libc" value="false"/>
<property key="output-file-format" value="elf"/>
</C30Global>
<PICkit3PlatformTool>
<property key="ADC 1" value="true"/>
<property key="AutoSelectMemRanges" value="auto"/>
<property key="CRC" value="true"/>
<property key="DUAL COMPARATOR" value="true"/>
<property key="Freeze All Other Peripherals" value="true"/>
<property key="I2C1" value="true"/>
<property key="INPUT CAPTURE 1" value="true"/>
<property key="INPUT CAPTURE 2" value="true"/>
<property key="INPUT CAPTURE 7" value="true"/>
<property key="INPUT CAPTURE 8" value="true"/>
<property key="OUTPUT COMPARE 1" value="true"/>
<property key="OUTPUT COMPARE 2" value="true"/>
<property key="OUTPUT COMPARE 3" value="true"/>
<property key="OUTPUT COMPARE 8" value="true"/>
<property key="PARALLEL MASTER/SLAVE PORT" value="true"/>
<property key="REAL TIME CLOCK AND CALENDAR" value="true"/>
<property key="SPI 1" value="true"/>
<property key="SPI 2" value="true"/>
<property key="SecureSegment.SegmentProgramming" value="FullChipProgramming"/>
<property key="TIMER1" value="true"/>
<property key="TIMER2" value="true"/>
<property key="TIMER3" value="true"/>
<property key="TIMER4" value="true"/>
<property key="TIMER5" value="true"/>
<property key="ToolFirmwareFilePath"
value="Press to browse for a specific firmware version"/>
<property key="ToolFirmwareOption.UseLatestFirmware" value="true"/>
<property key="UART 1" value="true"/>
<property key="UART 2" value="true"/>
<property key="hwtoolclock.frcindebug" value="false"/>
<property key="memories.aux" value="false"/>
<property key="memories.eeprom" value="false"/>
<property key="memories.flashdata" value="true"/>
<property key="memories.id" value="false"/>
<property key="memories.programmemory" value="true"/>
<property key="memories.programmemory.end" value="0x157ff"/>
<property key="memories.programmemory.start" value="0x0"/>
<property key="poweroptions.powerenable" value="false"/>
<property key="programmertogo.imagename" value=""/>
<property key="programoptions.eraseb4program" value="true"/>
<property key="programoptions.pgmspeed" value="2"/>
<property key="programoptions.preserveeeprom" value="false"/>
<property key="programoptions.preserveprogramrange" value="false"/>
<property key="programoptions.preserveprogramrange.end" value="0xff"/>
<property key="programoptions.preserveprogramrange.start" value="0x0"/>
<property key="programoptions.preserveuserid" value="false"/>
<property key="programoptions.usehighvoltageonmclr" value="false"/>
<property key="programoptions.uselvpprogramming" value="false"/>
<property key="voltagevalue" value="3.25"/>
</PICkit3PlatformTool>
</conf>
</confs>
</configurationDescriptor>
diff --git a/OncillaMotordriver.X/nbproject/project.xml b/OncillaMotordriver.X/nbproject/project.xml
index a844272..0a264e9 100644
--- a/OncillaMotordriver.X/nbproject/project.xml
+++ b/OncillaMotordriver.X/nbproject/project.xml
@@ -1,16 +1,16 @@
<?xml version="1.0" encoding="UTF-8"?>
<project xmlns="http://www.netbeans.org/ns/project/1">
<type>com.microchip.mplab.nbide.embedded.makeproject</type>
<configuration>
<data xmlns="http://www.netbeans.org/ns/make-project/1">
- <name>OncillaMotordriver</name>
+ <name>OncillaMotordriver_lc</name>
<creation-uuid>0d773020-2f2f-4b98-89ff-c9a31e70eff1</creation-uuid>
<make-project-type>0</make-project-type>
<c-extensions>c</c-extensions>
<cpp-extensions/>
<header-extensions>h</header-extensions>
<sourceEncoding>UTF-8</sourceEncoding>
<make-dep-projects/>
</data>
</configuration>
</project>
diff --git a/src/config.h b/src/config.h
index 53676f3..15f7223 100644
--- a/src/config.h
+++ b/src/config.h
@@ -1,29 +1,27 @@
#ifndef __CONFIG_H__
#define __CONFIG_H__
// instruction cycle speed
-#define FCY_FOR_BAUDRATE 40000000UL
-#define FCY 40000000UL
+#define FCY_FOR_BAUDRATE 40000000UL
+#define FCY 40000000UL
// PWM frequency
-#define FPWM 60000UL
+#define FPWM 60000UL
// T1 interrupt frequency (Hz)
-#define FT1 1000UL
-#define FSYST 1000UL
-
-
+#define FT1 1000UL
+#define FSYST 1000UL
#define FSPI_ME_READING_LATENCY 1000000
// uart time out value (us) SHOULD NOT BE USED
//#define TIME_OUT 100
#define SBCP_MDV_BAUDRATE 3310000UL
#define SBCP_MDV_TIMEOUT 2000
-#define SBCP_MDV_CLASS 0xea
-#define SBCP_MDV_THIS_DEVICE_INITIAL_ID 0x01
+#define SBCP_MDV_CLASS 0xea
+#define SBCP_MDV_THIS_DEVICE_INITIAL_ID 0x42
-#define SBCP_MDV_FIRMWARE_VERSION 0x01
+#define SBCP_MDV_FIRMWARE_VERSION 0x01
#endif // __CONFIG_H__
diff --git a/src/load_cell.c b/src/load_cell.c
index 6523e9e..5ea699f 100644
--- a/src/load_cell.c
+++ b/src/load_cell.c
@@ -1,350 +1,421 @@
#include "load_cell.h"
#include "sbcp_mdv.h"
+#include <pps.h>
+#include <string.h>
+#include "timer.h"
+#include "pindefs.h"
+#include <libpic30.h>
+
+
struct load_cell {
int torque;
lc_error_flags flags;
};
load_cell lcs[3];
load_cell * lc1 = &(lcs[0]);
load_cell * lc2 = &(lcs[1]);
load_cell * lc3 = &(lcs[2]);
-
/**
* Th state machine states for Magnetic encoders reception
*/
-typedef enum DMA_SPI_LC_rx_state {
+typedef enum DMA_SPI_LC_state {
DMA_SPI_LC_IDLE, ///< No reading is beeing performed.
- DMA_SPI_LC_WAITING_FOR_DEVICES, ///< Reading is started, we send the start order to device, we putting a latency for them to read their data.
+ DMA_SPI_LC_WRITING_RESET,
+ DMA_SPI_LC_WAITING_500us,
+ DMA_SPI_LC_WAITING_TO_WRITE_SETTINGS,
+ DMA_SPI_LC_WRITING_SETTINGS,
+ DMA_SPI_LC_WRITING_INSTRUCTION_2,
+ DMA_SPI_LC_WRITING_INSTRUCTION_3,
+ DMA_SPI_LC_WAITING_FOR_READOUT,
DMA_SPI_LC_READING_DEVICES, ///< We are actually reading the data from the devices
DMA_SPI_LC_DATA_READY_TO_BE_PROCESSED ///< We have finish to read the data, and we need to process it to extract atual data in the spi_main function
-}DMA_SPI_LC_rx_state;
+}DMA_SPI_LC_state;
struct DMA_SPI_LC_data {
- gpio spi_cs;
- DMA_SPI_LC_rx_state state;
+ gpio spi_cs;
+ gpio spi_sync;
+ gpio spi_mosi;
+ gpio spi_miso;
+ gpio spi_clk;
+ DMA_SPI_LC_state state;
};
-struct DMA_SPI_LC_data dma_spi_lc;
-
-typedef union DMA_SPI_LC_rx_buffer{
- struct{
- //First word definition little endian representation
- unsigned Q1_LIN : 1; ///< their is a linearity read alarm, data invalid
- unsigned Q1_COF : 1; ///< their is CORDIC Overflow, data invalid
- unsigned Q1_OCF : 1; ///< Algorithm is finished. If zero, data invalid
- unsigned Q1_ENC_DATA_LOW : 8; ///< first 8 bits of data
- unsigned Q1_ENC_DATA_HIGH : 4; ///< last 4 bits of data
- unsigned Q1_DUMMY_BIT : 1; ///< unimplemented
- //------ 1 word size
- //second word definition, little endian representation
- unsigned Q2_ENC_DATA_LOW : 8; ///< first 8 bits of data
- unsigned Q2_ENC_DATA_HIGH : 4; ///< last 4 bits of data
- unsigned Q2_DUMMY_BIT : 1; ///< unimplemented
-
- unsigned Q1_EVEN_PAR : 1; ///< even parity bit
- unsigned Q1_MAG_DEC : 1; ///< Maginute amplitude decreased.
- unsigned Q1_MAG_INC : 1; ///< Maginute amplitude increased.
- //------ 1 word size
- //third word little endian representation
- unsigned Q3_ENC_DATA_MID : 5; ///<central 5 bits of data
- unsigned Q3_ENC_DATA_HIGH : 4; ///< last 4 bits of data
- unsigned Q3_DUMMY_BIT : 1; ///< unimplemented
- //19 bits for the first knee encoder.
- unsigned Q2_EVEN_PAR : 1; ///< even parity bit
- unsigned Q2_MAG_DEC : 1; ///< Maginute amplitude decreased.
- unsigned Q2_MAG_INC : 1; ///< Maginute amplitude increased.
- unsigned Q2_LIN : 1; ///< their is a linearity read alarm, data invalid
- unsigned Q2_COF : 1; ///< their is CORDIC Overflow, data invalid
- unsigned Q2_OCF : 1; ///< Algorithm is finished. If zero, data invalid
- //------ 1word size
-
- // 4th and last word, little endian representation
- //trailing 7 bits that should be ignored
- unsigned DUMMY_BITS : 7; ///< unimplemented
- //19 bits for the 2nd knee encoder.
- unsigned Q3_EVEN_PAR : 1; ///< even parity bit
- unsigned Q3_MAG_DEC : 1; ///< Maginute amplitude decreased.
- unsigned Q3_MAG_INC : 1; ///< Maginute amplitude increased.
- unsigned Q3_LIN : 1; ///< their is a linearity read alarm, data invalid
- unsigned Q3_COF : 1; ///< their is CORDIC Overflow, data invalid
- unsigned Q3_OCF : 1; ///< Algorithm is finished. If zero, data invalid
- unsigned Q3_ENC_DATA_LOW : 3; ///< first 3 bits of data
- //------ 1word size
- } b; ///< bits representation
- unsigned char B[8];///<byte representation
- unsigned int w[4];///<word representation
-} DMA_SPI_LC_rx_buffer;
-
-
-DMA_SPI_LC_rx_buffer dma_spi_lc_rx_buffer __attribute__((space(dma)));
-
-typedef union DMA_SPI_LC_tx_buffer{
- struct{
- //little endian representation
- unsigned WEN : 1;
- unsigned RW : 1;
- unsigned RS2 : 1;
- unsigned RS1 : 1;
- unsigned RS0 : 1;
- unsigned CREAD : 1;
- unsigned DUMMY : 2;
- } communication_bits; ///< bits rep
-
- struct{
- //little endian representation
- unsigned RDY : 1;
- unsigned ERR : 1;
- unsigned NOREF : 1;
- unsigned PARITY : 1;
- unsigned CHD3 : 4;
- } status_bits; ///< bits rep
-
- unsigned char byte;
- unsigned char w[1];///<word representation
-} DMA_SPI_LC_tx_buffer;
+volatile struct DMA_SPI_LC_data dma_spi_lc;
+
+typedef struct DMA_SPI_LC_read_value{
+ unsigned char DATA[3];
+ unsigned char STATUS;
+} DMA_SPI_LC_read_value;
+
+#define CIRCULAR_BUFFER_SIZE 3
+volatile unsigned char circular_buffer_ready = 0;
+volatile unsigned char circular_buffer_index = 0;
+volatile DMA_SPI_LC_read_value circular_buffer[CIRCULAR_BUFFER_SIZE];
+
+unsigned char dma_spi_lc_tx_buffer[16] __attribute__((space(dma)));
+
+volatile unsigned char dma_spi_lc_rx_buffer[16] __attribute__((space(dma)));
-DMA_SPI_LC_tx_buffer dma_spi_lc_tx_buffer __attribute__((space(dma)));
#define dma_spi_lc_start() do{\
- /*gpio_clear(dma_spi_lc.spi_cs);*/\
+ gpio_clear(dma_spi_lc.spi_cs);\
}while(0)
#define dma_spi_lc_stop() do{\
gpio_set(dma_spi_lc.spi_cs);\
}while(0)
-#define dma_spi_lc_write(byte_val) do{\
- dma_spi_lc_tx_buffer.byte = byte_val;\
- DMA4REQbits.FORCE = 1;\
+#define dma_spi_lc_write() do{\
dma_spi_lc_stop();\
- /*SPI2BUF = byte_val;*/ /*force the transfer of dma channel */\
- dma_spi_lc.state = DMA_SPI_LC_WAITING_FOR_DEVICES;\
-}while(0)
-
-#define dma_spi_lc_start_actual_reading() do{\
- /*SPI2BUF = 0x0000; force the transfer of dma channel \
- dma_spi_lc.state = DMA_SPI_LC_READING_DEVICES;*/\
+ dma_spi_lc_start();\
+ DMA4CONbits.CHEN = 1;\
+ DMA3CONbits.CHEN = 1;\
+ DMA4REQbits.FORCE = 1;\
}while(0)
-
-
void init_load_cell(load_cell * e){
e->torque = 0;
e->flags = LC_F_ONBOARD_PROCESSING_UNFINISHED;
}
-void init_load_cells(gpio spi_cs){
- dma_spi_lc.spi_cs = spi_cs;
- dma_spi_lc_stop();
- dma_spi_lc.state = DMA_SPI_LC_IDLE;
+void init_load_cells(){
+
+ //Peripheral Pin Select
+
+ PPSUnLock;
+ PPSOutput(0, OUT_PIN_PPS_RP25); // no output on rp5
+ IN_FN_PPS_SDI2 = 0;// no input on rp2
+ IN_FN_PPS_SCK2 = 0;// no input on rp5
+ IN_FN_PPS_SS2 = 0;// no input on rp23
+ OUT_PIN_PPS_RP2 = 0;
+ OUT_PIN_PPS_RP5 = 0;
+ OUT_PIN_PPS_RP23 = 0;
+ OUT_PIN_PPS_RP24 = 0;
+ OUT_PIN_PPS_RP25 = 0;
+ PPSOutput(OUT_FN_PPS_SDO2, OUT_PIN_PPS_RP23); // MOSI to rp23
+ PPSOutput(OUT_FN_PPS_SS2, OUT_PIN_PPS_RP2); // chip select to rp2
+ PPSOutput(OUT_FN_PPS_SCK2, OUT_PIN_PPS_RP5); // clock out to rp5
+ PPSInput(PPS_SDI2, PPS_RP25);// MISO to rp25
+ //PPSLock;
+
+ dma_spi_lc.spi_mosi = gpio_create(GPIO_PORT_C,GPIO_PIN_7,GPIO_OUTPUT);
+ gpio_clear(dma_spi_lc.spi_mosi);
- SPI2STATbits.SPIEN = 0; //Disable SPI 2 module during configuration
- //SPI 1 is for magentic encoders :
- IEC2bits.SPI2IE = 0; //disable interrupt. This is needed because we use DMA
- IFS2bits.SPI2IF = 0; //clear the interrupt flag.
- IPC8bits.SPI2IP = 0b111;
+ dma_spi_lc.spi_miso = gpio_create(GPIO_PORT_C,GPIO_PIN_9,GPIO_INPUT);
+ gpio_set(dma_spi_lc.spi_miso);
- SPI2STATbits.SPIROV = 0;
- SPI2CON1bits.DISSCK = 0; //Use the clock
- SPI2CON1bits.DISSDO = 0; //Disable data output. This is not needed by the application, no pin are routed
- SPI2CON1bits.MODE16 = 0; //Transmit bytes. Rx buffer is in byte, but we don't care because we are transmitting an even number of byte
-
- SPI2CON1bits.CKE = 0; //data valid on rising edge
- SPI2CON1bits.CKP = 1; //clock idle state is at high level
- SPI2CON1bits.MSTEN = 1; //enable master mode
- SPI2CON1bits.SMP = 1; //data is sampled at middle of clock time
+ dma_spi_lc.spi_clk = gpio_create(GPIO_PORT_B,GPIO_PIN_5,GPIO_OUTPUT);
+ gpio_set(dma_spi_lc.spi_clk);
- SPI2CON1bits.SSEN = 0; //enable chip select bit
- SPI2CON2bits.FRMEN = 0; //Frame support
+ dma_spi_lc.spi_sync = gpio_create(GPIO_PORT_C,GPIO_PIN_8,GPIO_OUTPUT);
+ gpio_set(dma_spi_lc.spi_sync);
- //Choose a clock frequency of 625 kHz, max accepted by AS5045 is 1Mhz
- SPI2CON1bits.SPRE = 0b000; //Select a secondary prescaler of 1:1
- SPI2CON1bits.PPRE = 0b00 ; //select a primary prescaler of 64:1
+ dma_spi_lc.spi_cs = gpio_create(GPIO_PORT_B,GPIO_PIN_2,GPIO_OUTPUT);
+ gpio_set(dma_spi_lc.spi_cs);
-
- SPI2STATbits.SPIEN = 1; //Enable SPI 2 module
+ dma_spi_lc.state = DMA_SPI_LC_IDLE;
- DMA3CONbits.CHEN = 0;
- DMA3CONbits.SIZE = 0;
- DMA3CONbits.DIR = 0;
- DMA3CONbits.HALF = 0;
+ /************************************
+ SPI2-MODULE
+ ************************************/
- DMA3CONbits.NULLW = 1;
+ SPI2STATbits.SPIEN = 0; //Disable SPI 2 module during configuration
+ //SPI 1 is for magentic encoders :
+ IEC2bits.SPI2IE = 0; //disable interrupt. This is needed because we use DMA
+ IFS2bits.SPI2IF = 0; //clear the interrupt flag.
+ IPC8bits.SPI2IP = 0b000;
+ SPI2CON1bits.MSTEN = 1; //enable master mode
+ SPI2STATbits.SPIROV = 0; // clear overflow flag
- DMA3CONbits.AMODE = 0b00;// Register indirect with posincrement
- DMA3CONbits.MODE = 0b01;// One - shot, No Ping Pong
+ SPI2CON1bits.DISSCK = 0; //Use the clock
+ SPI2CON1bits.DISSDO = 0; //Disable data output. This is not needed by the application, no pin are routed
+ SPI2CON1bits.MODE16 = 0; //Transmit bytes. Rx buffer is in byte, but we don't care because we are transmitting an even number of byte
- DMA3STA = __builtin_dmaoffset(dma_spi_lc_rx_buffer.w);
+ SPI2CON1bits.CKE = 0; //data valid on rising edge
+ SPI2CON1bits.CKP = 1; //clock idle state is at high level
+ SPI2CON1bits.SMP = 0; //0=data is sampled at middle of clock time
- DMA3PAD = (volatile unsigned int) & SPI2BUF;
+ SPI2CON1bits.SSEN = 0; //disable chip select bit (we do this ourselves)
+ SPI2CON2bits.FRMEN = 0; //Frame support
- DMA3REQ = 0;//0x000A;
- DMA3CNT = 3;
+ //Choose a clock frequency of 625 kHz, max accepted by AS5045 is 1Mhz
+ //TODO: set frequency
+ SPI2CON1bits.SPRE = 0b000; //Select a secondary prescaler of 1:1
+ SPI2CON1bits.PPRE = 0b10 ; //select a primary prescaler of 64:1
- IFS2bits.DMA3IF = 0;
- IEC2bits.DMA3IE = 0;
+ IEC2bits.SPI2IE = 0; //enable interrupt.
+ SPI2STATbits.SPIEN = 1; //Enable SPI 2 module
+ /************************************
+ DMA3-MODULE
+ ************************************/
- IFS2bits.DMA4IF = 0;
- IEC2bits.DMA4IE = 0;
- IPC11bits.DMA4IP = 0b111; //maximum priority
+ IEC2bits.DMA3IE = 0;
+ IFS2bits.DMA3IF = 0;
- DMA4CONbits.CHEN = 0;
- DMA4CONbits.SIZE = 1; //send per byte
- DMA4CONbits.DIR = 1; //send, don't read
- DMA4CONbits.HALF = 0;
+ IPC9bits.DMA3IP = 0b001; //low priority, but needs to be higher than DMA4IP
- DMA4CONbits.NULLW = 0;
- DMA4CONbits.AMODE = 0b00;// Register indirect with post-increment
- DMA4CONbits.MODE = 0b01;// One - shot, No Ping Pong
+ DMA3CONbits.CHEN = 0;
+ DMA3CONbits.SIZE = 1;
+ DMA3CONbits.DIR = 0;//read, don't send
+ DMA3CONbits.HALF = 0;
- dma_spi_lc_tx_buffer.byte = 0xAA;
- DMA4STA = __builtin_dmaoffset(dma_spi_lc_tx_buffer.w); //transmission buffer
- DMA4PAD = (volatile unsigned int) & SPI2BUF;
+ DMA3CONbits.NULLW = 0;
- DMA4REQ = 0b00100001;
- //DMA4REQ = 0x00A;
- DMA4CNT = 2; //number of bytes to send
-
- IFS2bits.DMA4IF = 0;
- IEC2bits.DMA4IE = 1; //we don't need this interrupt
- DMA4CONbits.CHEN = 1;
-
- init_load_cell(&(lcs[0]));
- init_load_cell(&(lcs[1]));
- init_load_cell(&(lcs[2]));
-}
+ DMA3CONbits.AMODE = 0b00;// Register indirect with posincrement
+ DMA3CONbits.MODE = 0b00;// continuous, No Ping Pong
-#define fill_status(lc,ocf_bit,cof_bit,lin_bit,amp_incr_bit,amp_decr_bit) do{ \
- lc.flags = LC_F_OK; \
- if(!(ocf_bit)){ \
- lc.flags |= LC_F_ONBOARD_PROCESSING_UNFINISHED; \
- } \
- if(cof_bit){ \
- lc.flags |= LC_F_CORDIC_OVERFLOW; \
- } \
- if(lin_bit){ \
- lc.flags |= LC_F_LINEARITY_READ_ALARM; \
- } \
- if( (amp_incr_bit) && (amp_decr_bit) ) { \
- lc.flags |= LC_F_LOAD_CELL_AMPLITUDE_ERROR; \
- } \
-}while(0)
+ DMA3STA = __builtin_dmaoffset(dma_spi_lc_rx_buffer);
+ DMA3PAD = (volatile unsigned int) & SPI2BUF;
-void load_cell_start_reading(){
- if(dma_spi_lc.state == DMA_SPI_LC_IDLE){
- dma_spi_lc.state = DMA_SPI_LC_WAITING_FOR_DEVICES;
- //dma_spi_lc_start();
- dma_spi_lc_write(0xAA);//0x5c
- //dma_spi_lc_start_actual_reading();
- return;
- }
+ DMA3REQbits.IRQSEL = 0b0100001; //SPI2
+ DMA3CNT = 0;
+
+ IEC2bits.DMA3IE = 1;
+
+ /************************************
+ DMA4-MODULE
+ ************************************/
+
+ IEC2bits.DMA4IE = 0;
+ IFS2bits.DMA4IF = 0;
+ IPC11bits.DMA4IP = 0b000; //low priority
+
+ DMA4CONbits.CHEN = 0;
+ DMA4CONbits.SIZE = 1; //send per byte
+ DMA4CONbits.DIR = 1; //send, don't read
+ DMA4CONbits.HALF = 0; //only one buffer
+
+ DMA4CONbits.NULLW = 0;
+
+ DMA4CONbits.AMODE = 0b00;// Register indirect with post-increment
+ DMA4CONbits.MODE = 0b01;// 0b01 One - shot, No Ping Pong
+
+ DMA4REQbits.IRQSEL = 0b0100001; //SPI2
+ DMA4CNT = 0; //number of bytes to send
+
+ DMA4STA = __builtin_dmaoffset(dma_spi_lc_tx_buffer); //transmission buffer
+ DMA4PAD = (volatile unsigned int) & SPI2BUF;
+
+ IEC2bits.DMA4IE = 0; //we don't need this interrupt, we use the interupt when everything is received
+
+ /************************************
+ CN19-MODULE
+ ************************************/
+
+ CNEN2bits.CN19IE = 1;
+ CNPU2bits.CN19PUE = 0;
+ IFS1bits.CNIF = 0;
+ IEC1bits.CNIE = 0; //don't enable it yet
+
+ init_load_cell(&(lcs[0]));
+ init_load_cell(&(lcs[1]));
+ init_load_cell(&(lcs[2]));
+
+ load_cell_reset();
}
+inline void set_lc_torque(load_cell lc, int value, int adr) {
+ lc.flags = LC_F_OK;
+ lc.torque = value;
+ sbcp_me_reg(adr,LCX_TORQUE).u = value;
+ sbcp_mark_new_low_latency_data_available();
+}
-#define set_lc_torque(lc,value,adr) do{ \
- if((lc).flags == LC_F_OK){ \
- (lc).torque = (long) ((value)); \
- (lc).torque &= 0x3fff; \
- } \
- sbcp_lc_reg(adr,LCX_TORQUE).u = (lc).torque; \
- if((lc).flags & (LC_F_LINEARITY_READ_ALARM | LC_F_CORDIC_OVERFLOW | LC_F_ONBOARD_PROCESSING_UNFINISHED)) { \
- sbcp_lc_reg(adr,LCX_TORQUE).u |= 1 << 15; \
- } \
- if((lc).flags & LC_F_LOAD_CELL_AMPLITUDE_ERROR){ \
- sbcp_lc_reg(adr,LCX_TORQUE).u |= 1 << 14; \
- } \
+#define NUMARGS(...) (sizeof((char[]){__VA_ARGS__})/sizeof(char))
+#define spi_write_bytes(...) do{\
+ memcpy(dma_spi_lc_tx_buffer, (char [NUMARGS(__VA_ARGS__)]) {__VA_ARGS__}, NUMARGS(__VA_ARGS__)*sizeof(char));\
+ DMA3CNT = NUMARGS(__VA_ARGS__)-1;\
+ DMA4CNT = NUMARGS(__VA_ARGS__)-1;\
+ dma_spi_lc_write();\
}while(0)
+#define spi_led_on() spi_write_bytes(0x28,0x31)
+#define spi_led_off() spi_write_bytes(0x28,0x30)
+#define spi_led_blink() spi_write_bytes(0x28,0x30|BLINK)
+#define spi_settings 0b00001000,0b00011000,0b00101000,0b00000001,0b00010000,0b00000000,0b00000111,0b11010000,
+
+
+int lc_error_count=0;
+
+int reset_counter;
+volatile short start_reading = 0;
void load_cell_process(){
- if(dma_spi_lc.state == DMA_SPI_LC_DATA_READY_TO_BE_PROCESSED){
-
- //we assume that the parity is always correct.
- /// \todo : check the parity of the data, buts it's rather complicated.
-
- //first we check the status
- fill_status(lcs[0],
- dma_spi_lc_rx_buffer.b.Q1_OCF,
- dma_spi_lc_rx_buffer.b.Q1_COF,
- dma_spi_lc_rx_buffer.b.Q1_LIN,
- dma_spi_lc_rx_buffer.b.Q1_MAG_INC,
- dma_spi_lc_rx_buffer.b.Q1_MAG_DEC);
-
- fill_status(lcs[1],
- dma_spi_lc_rx_buffer.b.Q2_OCF,
- dma_spi_lc_rx_buffer.b.Q2_COF,
- dma_spi_lc_rx_buffer.b.Q2_LIN,
- dma_spi_lc_rx_buffer.b.Q2_MAG_INC,
- dma_spi_lc_rx_buffer.b.Q2_MAG_DEC);
-
- fill_status(lcs[2],
- dma_spi_lc_rx_buffer.b.Q3_OCF,
- dma_spi_lc_rx_buffer.b.Q3_COF,
- dma_spi_lc_rx_buffer.b.Q3_LIN,
- dma_spi_lc_rx_buffer.b.Q3_MAG_INC,
- dma_spi_lc_rx_buffer.b.Q3_MAG_DEC);
-
-
- set_lc_torque(lcs[0], (dma_spi_lc_rx_buffer.b.Q1_ENC_DATA_HIGH << 8 ) + dma_spi_lc_rx_buffer.b.Q1_ENC_DATA_LOW , MDV_LC_AXIS_1);
- set_lc_torque(lcs[1], (dma_spi_lc_rx_buffer.b.Q2_ENC_DATA_HIGH << 8 ) + dma_spi_lc_rx_buffer.b.Q2_ENC_DATA_LOW , MDV_LC_AXIS_2);
- set_lc_torque(lcs[2], (dma_spi_lc_rx_buffer.b.Q3_ENC_DATA_HIGH << 8 ) + ( dma_spi_lc_rx_buffer.b.Q3_ENC_DATA_MID << 3 ) + dma_spi_lc_rx_buffer.b.Q3_ENC_DATA_LOW , MDV_LC_AXIS_3);
-
- //mark that new data is available in the table.
- sbcp_mark_new_low_latency_data_available();
- //we have readen the data, nothing more to do
- dma_spi_lc.state = DMA_SPI_LC_IDLE;
-
- }
+ int v;
+ int index;
+ DMA_SPI_LC_read_value value;
+ unsigned short p;
+ if(start_reading){
+ start_reading=0;
+ dma_spi_lc.state = DMA_SPI_LC_READING_DEVICES;
+ spi_write_bytes(0x58,0x00,0x00,0x00,0x00); // read 4 bytes
+ }
+ if(circular_buffer_ready){
+ for(index = 0; index<CIRCULAR_BUFFER_SIZE; index++){
+
+ IEC2bits.DMA3IE = 0;//disable writing to circular buffer while reading from it, without losing interrupts.
+ value = circular_buffer[index]; //copy to remove race conditions with interupt
+ IEC2bits.DMA3IE = 1;
+ // step 1: check parity
+ v = value.DATA[0] ^ value.DATA[1] ^ value.DATA[2];
+ v = (v ^ v>>4) & 0x0f;
+ p = (0x6996 >> v) & 1; //Look-up table
+ if(value.DATA[0] == 0 && value.DATA[1] == 0 && value.DATA[2] == 0 && value.STATUS == 0){//while strictly speaking, this is a correct packet, it is more likely something is going wrong
+ lc_error_count++;
+ }
+ if(p != (value.STATUS & 0b00010000)>>4){
+ lc_error_count++;
+ }else if(value.STATUS & 0b11100000){ //RDY, ERR, NOREF
+ lc_error_count++;
+ }else {
+ switch(value.STATUS & 0b00001111){
+ case 0:
+ set_lc_torque(lcs[0], ((unsigned long) value.DATA[0])<<8 | ((unsigned long) value.DATA[1]),MDV_LC_AXIS_1);
+ break;
+ case 1:
+ set_lc_torque(lcs[1], ((unsigned long) value.DATA[0])<<8 | ((unsigned long) value.DATA[1]),MDV_LC_AXIS_2);
+ break;
+ case 2:
+ set_lc_torque(lcs[2], ((unsigned long) value.DATA[0])<<8 | ((unsigned long) value.DATA[1]),MDV_LC_AXIS_3);
+ break;
+ default:
+ lc_error_count++;
+ }
+ }
+ }
+ if(lc_error_count>100){
+ if(dma_spi_lc.state == DMA_SPI_LC_WAITING_FOR_READOUT
+ || dma_spi_lc.state == DMA_SPI_LC_READING_DEVICES){
+ lc_error_count=0;
+ load_cell_reset();
+ }
+ }
+ }
+}
+
+void load_cell_reset(){
+ IEC1bits.CNIE = 0;
+ circular_buffer_ready = 0;
+ reset_counter = 0;
+ start_reading=0;
+ dma_spi_lc.state = DMA_SPI_LC_WRITING_RESET;
+ spi_write_bytes(0xff,0xff,0xff,0xff,0xff);
+}
+
+void load_cell_start_reading(){
+ if(dma_spi_lc.state == DMA_SPI_LC_IDLE){
+ //start reading
+ dma_spi_lc.state = DMA_SPI_LC_WAITING_FOR_READOUT;
+ IEC1bits.CNIE = 1;
+ }
+ if(dma_spi_lc.state == DMA_SPI_LC_WAITING_TO_WRITE_SETTINGS){
+ if(reset_counter == 0){ //you need to wait at least 500us to start writing the settings. We wait for an additional 1ms so we are sure we waited long enough.
+ reset_counter++;
+ }else{
+ dma_spi_lc.state = DMA_SPI_LC_WRITING_SETTINGS;
+ spi_write_bytes(spi_settings);
+ }
+ }
}
/*******************************************************************************
*
- * Interruption
+ * Interrupt
*
******************************************************************************/
/**
- * Interruption when the SPI 2 finished its reading
+ * Interrupt when the DMA finished reading all the bytes
*/
void __attribute__((__interrupt__ , no_auto_psv)) _DMA3Interrupt(){
- dma_spi_lc.state = DMA_SPI_LC_DATA_READY_TO_BE_PROCESSED;
- //dma_spi_lc_stop();
- //clear the interruption flag
- IFS2bits.DMA3IF = 0;
+
+ switch(dma_spi_lc.state){
+ case DMA_SPI_LC_WRITING_RESET:
+ dma_spi_lc.state = DMA_SPI_LC_WAITING_TO_WRITE_SETTINGS;
+ break;
+ case DMA_SPI_LC_WRITING_SETTINGS:
+ spi_led_on();
+ dma_spi_lc.state = DMA_SPI_LC_WRITING_INSTRUCTION_2; //todo,
+ break;
+ case DMA_SPI_LC_WRITING_INSTRUCTION_2:
+ dma_spi_lc.state = DMA_SPI_LC_IDLE; //todo,
+ break;
+ case DMA_SPI_LC_WRITING_INSTRUCTION_3:
+ dma_spi_lc.state = DMA_SPI_LC_IDLE; //todo,
+ break;
+ case DMA_SPI_LC_READING_DEVICES:
+ dma_spi_lc.state = DMA_SPI_LC_WAITING_FOR_READOUT;
+ circular_buffer[circular_buffer_index].DATA[0] = dma_spi_lc_rx_buffer[1];
+ circular_buffer[circular_buffer_index].DATA[1] = dma_spi_lc_rx_buffer[2];
+ circular_buffer[circular_buffer_index].DATA[2] = dma_spi_lc_rx_buffer[3];
+ circular_buffer[circular_buffer_index].STATUS = dma_spi_lc_rx_buffer[4];
+ circular_buffer_index++;
+ if(circular_buffer_index >= CIRCULAR_BUFFER_SIZE){
+ circular_buffer_index = 0;
+ circular_buffer_ready = 1;
+ }
+ IEC1bits.CNIE = 1;// re-enable the read interrupt after communication is done
+ break;
+ default:
+ dma_spi_lc.state = DMA_SPI_LC_IDLE; //todo,
+ break;
+ }
+ //gpio_set(dma_spi_lc.spi_mosi);//Idle high
+ IFS2bits.DMA3IF = 0;//clear the interrupt flag
}
+/**
+ * Interrupt when the DMA finished its writing of all the bytes
+*/
+void __attribute__((__interrupt__ , no_auto_psv)) _DMA4Interrupt(){
+ //clear the interruption flag
+ //DMA4CONbits.CHEN = 0;
+ //dma_spi_lc.state = DMA_SPI_LC_IDLE;
+ //dma_spi_lc_stop();
+ IFS2bits.DMA4IF = 0;
+}
/**
- * Interruption when the SPI 2 finished its writing
+ * Interrupt when the SPI 2 finished writing and reading a byte
+*/
+void __attribute__((__interrupt__, no_auto_psv)) _SPI2Interrupt(void)
+{
+ //SPI2STATbits.SPIROV = 0;
+ IFS2bits.SPI2IF = 0; // Clear the SPI1 Interrupt Flag
+ //SPI1BUF=0; // send next byte, not necassary if NULLW set
+}
-void __attribute__((__interrupt__ , no_auto_psv)) _DMA4Interrupt(){
- //clear the interruption flag
- //DMA4CONbits.CHEN = 0;
- IFS2bits.DMA4IF = 0;
-}*/
+void __attribute__((__interrupt__ , no_auto_psv)) _CNInterrupt(){
+ if(dma_spi_lc.state == DMA_SPI_LC_WAITING_FOR_READOUT){
+ if(gpio_read(dma_spi_lc.spi_miso) == 0){
+ IEC1bits.CNIE = 0;// disable this interrupt during communication
+ start_reading = 1;
+ }
+ }
+ IFS1bits.CNIF = 0;
+}
int lc_get_torque(load_cell * e){
return e->torque;
}
lc_error_flags lc_get_error(load_cell * e){
return e->flags;
}
-
void lc_load_persistent_sbcp_settings(){
}
diff --git a/src/load_cell.h b/src/load_cell.h
index 1df07e1..0b1bbe2 100644
--- a/src/load_cell.h
+++ b/src/load_cell.h
@@ -1,44 +1,41 @@
/*
* File: load_cell.h
* Author: jonas
*
* Created on March 26, 2013, 11:07 AM
*/
#ifndef LOAD_CELL_H
#define LOAD_CELL_H
#include <gpio.h>
+#include <libpic30.h>
+
struct load_cell;
typedef struct load_cell load_cell;
-void init_load_cells(gpio spi_cs);
-
+void init_load_cells();
+void load_cell_reset();
void load_cell_process();
void load_cell_start_reading();
void lc_load_persistent_sbcp_settings();
-enum lc_error_flags {
+typedef enum lc_error_flags {
LC_F_OK = 0 ,
- LC_F_LINEARITY_READ_ALARM = 1 << 0,
- LC_F_CORDIC_OVERFLOW = 1 << 1,
- LC_F_ONBOARD_PROCESSING_UNFINISHED = 1 << 2,
- LC_F_LOAD_CELL_AMPLITUDE_ERROR = 1 << 3
-};
-
-typedef enum lc_error_flags lc_error_flags;
+ LC_F_ONBOARD_PROCESSING_UNFINISHED = 1
+} lc_error_flags;
int lc_get_torque(load_cell * e);
lc_error_flags lc_get_error(load_cell * e);
extern load_cell * lc1,* lc2,* lc3;
#endif /* LOAD_CELL_H */
diff --git a/src/magnetic_encoder.c b/src/magnetic_encoder.c
index f25fd9f..1d184b7 100644
--- a/src/magnetic_encoder.c
+++ b/src/magnetic_encoder.c
@@ -1,336 +1,342 @@
#include "magnetic_encoder.h"
#include "sbcp_mdv.h"
+#include "misc_math.h"
#define timer_2_start() do { T2CONbits.TON = 1; }while(0)
#define timer_2_stop() do { T2CONbits.TON = 0; }while(0)
-#define timer_2_stop_and_clear() do{ \
- timer_2_stop(); \
- TMR2 = 0x00; \
-}while(0)
+#define timer_2_stop_and_clear() do{ \
+ timer_2_stop(); \
+ TMR2 = 0x00; \
+ }while(0)
struct magnetic_encoder {
int position;
me_error_flags flags;
int mult;
int div;
};
magnetic_encoder mes[3];
magnetic_encoder * me1 = &(mes[0]);
magnetic_encoder * me2 = &(mes[1]);
magnetic_encoder * me3 = &(mes[2]);
/**
* Th state machine states for Magnetic encoders reception
*/
typedef enum DMA_SPI_ME_rx_state {
- DMA_SPI_ME_IDLE, ///< No reading is beeing performed.
- DMA_SPI_ME_WAITING_FOR_DEVICES, ///< Reading is started, we send the start order to device, we putting a latency for them to read their data.
- DMA_SPI_ME_READING_DEVICES, ///< We are actually reading the data from teh devices
- DMA_SPI_ME_DATA_READY_TO_BE_PROCESSED ///< We have finish to read the data, and we need to process it to extract atual data in the spi_main function
+ DMA_SPI_ME_IDLE, ///< No reading is beeing performed.
+ DMA_SPI_ME_WAITING_FOR_DEVICES, ///< Reading is started, we send the start order to device, we putting a latency for them to read their data.
+ DMA_SPI_ME_READING_DEVICES, ///< We are actually reading the data from teh devices
+ DMA_SPI_ME_DATA_READY_TO_BE_PROCESSED ///< We have finish to read the data, and we need to process it to extract atual data in the spi_main function
}DMA_SPI_ME_rx_state;
struct DMA_SPI_ME_data {
gpio spi_cs;
DMA_SPI_ME_rx_state state;
};
struct DMA_SPI_ME_data dma_spi_me;
typedef union DMA_SPI_ME_rx_buffer{
- struct{
- //First word definition little endian representation
- unsigned Q1_LIN : 1; ///< their is a linearity read alarm, data invalid
- unsigned Q1_COF : 1; ///< their is CORDIC Overflow, data invalid
- unsigned Q1_OCF : 1; ///< Algorithm is finished. If zero, data invalid
- unsigned Q1_ENC_DATA_LOW : 8; ///< first 8 bits of data
- unsigned Q1_ENC_DATA_HIGH : 4; ///< last 4 bits of data
- unsigned Q1_DUMMY_BIT : 1; ///< unimplemented
- //------ 1 word size
- //second word definition, little endian representation
- unsigned Q2_ENC_DATA_LOW : 8; ///< first 8 bits of data
- unsigned Q2_ENC_DATA_HIGH : 4; ///< last 4 bits of data
- unsigned Q2_DUMMY_BIT : 1; ///< unimplemented
-
- unsigned Q1_EVEN_PAR : 1; ///< even parity bit
- unsigned Q1_MAG_DEC : 1; ///< Maginute amplitude decreased.
- unsigned Q1_MAG_INC : 1; ///< Maginute amplitude increased.
- //------ 1 word size
- //third word little endian representation
- unsigned Q3_ENC_DATA_MID : 5; ///<central 5 bits of data
- unsigned Q3_ENC_DATA_HIGH : 4; ///< last 4 bits of data
- unsigned Q3_DUMMY_BIT : 1; ///< unimplemented
- //19 bits for the first knee encoder.
- unsigned Q2_EVEN_PAR : 1; ///< even parity bit
- unsigned Q2_MAG_DEC : 1; ///< Maginute amplitude decreased.
- unsigned Q2_MAG_INC : 1; ///< Maginute amplitude increased.
- unsigned Q2_LIN : 1; ///< their is a linearity read alarm, data invalid
- unsigned Q2_COF : 1; ///< their is CORDIC Overflow, data invalid
- unsigned Q2_OCF : 1; ///< Algorithm is finished. If zero, data invalid
- //------ 1word size
-
- // 4th and last word, little endian representation
- //trailing 7 bits that should be ignored
- unsigned DUMMY_BITS : 7; ///< unimplemented
- //19 bits for the 2nd knee encoder.
- unsigned Q3_EVEN_PAR : 1; ///< even parity bit
- unsigned Q3_MAG_DEC : 1; ///< Maginute amplitude decreased.
- unsigned Q3_MAG_INC : 1; ///< Maginute amplitude increased.
- unsigned Q3_LIN : 1; ///< their is a linearity read alarm, data invalid
- unsigned Q3_COF : 1; ///< their is CORDIC Overflow, data invalid
- unsigned Q3_OCF : 1; ///< Algorithm is finished. If zero, data invalid
- unsigned Q3_ENC_DATA_LOW : 3; ///< first 3 bits of data
- //------ 1word size
- } b; ///< bits representation
- unsigned char B[8];///<byte representation
- unsigned int w[4];///<word representation
+ struct{
+ //First word definition little endian representation
+ unsigned Q1_LIN : 1; ///< their is a linearity read alarm, data invalid
+ unsigned Q1_COF : 1; ///< their is CORDIC Overflow, data invalid
+ unsigned Q1_OCF : 1; ///< Algorithm is finished. If zero, data invalid
+ unsigned Q1_ENC_DATA_LOW : 8; ///< first 8 bits of data
+ unsigned Q1_ENC_DATA_HIGH : 4; ///< last 4 bits of data
+ unsigned Q1_DUMMY_BIT : 1; ///< unimplemented
+ //------ 1 word size
+ //second word definition, little endian representation
+ unsigned Q2_ENC_DATA_LOW : 8; ///< first 8 bits of data
+ unsigned Q2_ENC_DATA_HIGH : 4; ///< last 4 bits of data
+ unsigned Q2_DUMMY_BIT : 1; ///< unimplemented
+
+ unsigned Q1_EVEN_PAR : 1; ///< even parity bit
+ unsigned Q1_MAG_DEC : 1; ///< Maginute amplitude decreased.
+ unsigned Q1_MAG_INC : 1; ///< Maginute amplitude increased.
+ //------ 1 word size
+ //third word little endian representation
+ unsigned Q3_ENC_DATA_MID : 5; ///<central 5 bits of data
+ unsigned Q3_ENC_DATA_HIGH : 4; ///< last 4 bits of data
+ unsigned Q3_DUMMY_BIT : 1; ///< unimplemented
+ //19 bits for the first knee encoder.
+ unsigned Q2_EVEN_PAR : 1; ///< even parity bit
+ unsigned Q2_MAG_DEC : 1; ///< Maginute amplitude decreased.
+ unsigned Q2_MAG_INC : 1; ///< Maginute amplitude increased.
+ unsigned Q2_LIN : 1; ///< their is a linearity read alarm, data invalid
+ unsigned Q2_COF : 1; ///< their is CORDIC Overflow, data invalid
+ unsigned Q2_OCF : 1; ///< Algorithm is finished. If zero, data invalid
+ //------ 1word size
+
+ // 4th and last word, little endian representation
+ //trailing 7 bits that should be ignored
+ unsigned DUMMY_BITS : 7; ///< unimplemented
+ //19 bits for the 2nd knee encoder.
+ unsigned Q3_EVEN_PAR : 1; ///< even parity bit
+ unsigned Q3_MAG_DEC : 1; ///< Maginute amplitude decreased.
+ unsigned Q3_MAG_INC : 1; ///< Maginute amplitude increased.
+ unsigned Q3_LIN : 1; ///< their is a linearity read alarm, data invalid
+ unsigned Q3_COF : 1; ///< their is CORDIC Overflow, data invalid
+ unsigned Q3_OCF : 1; ///< Algorithm is finished. If zero, data invalid
+ unsigned Q3_ENC_DATA_LOW : 3; ///< first 3 bits of data
+ //------ 1word size
+ } b; ///< bits representation
+ unsigned char B[8];///<byte representation
+ unsigned int w[4];///<word representation
} DMA_SPI_ME_rx_buffer;
DMA_SPI_ME_rx_buffer dma_spi_me_rx_buffer __attribute__((space(dma)));
-#define dma_spi_me_start() do{\
- gpio_clear(dma_spi_me.spi_cs);\
-}while(0)
+#define dma_spi_me_start() do{ \
+ gpio_clear(dma_spi_me.spi_cs); \
+ }while(0)
-#define dma_spi_me_stop() do{\
- gpio_set(dma_spi_me.spi_cs);\
-}while(0)
+#define dma_spi_me_stop() do{ \
+ gpio_set(dma_spi_me.spi_cs); \
+ }while(0)
-#define dma_spi_me_start_actual_reading() do{\
- SPI1BUF = 0x0000; /*force the transfer of dma channel */\
- dma_spi_me.state = DMA_SPI_ME_READING_DEVICES;\
-}while(0)
+#define dma_spi_me_start_actual_reading() do{ \
+ SPI1BUF = 0x0000; /*force the transfer of dma channel */ \
+ dma_spi_me.state = DMA_SPI_ME_READING_DEVICES; \
+ }while(0)
void init_magnetic_encoder(magnetic_encoder * e){
e->position = 0;
e->flags = ME_F_ONBOARD_PROCESSING_UNFINISHED;
e->mult = 1;
e->div = 1;
}
void init_magnetic_encoders(gpio spi_cs){
dma_spi_me.spi_cs = spi_cs;
dma_spi_me_stop();
dma_spi_me.state = DMA_SPI_ME_IDLE;
//SPI 1 is for magentic encoders :
IEC0bits.SPI1IE = 0; //disable interrupt. This is needed because we use DMA
IFS0bits.SPI1IF = 0; //clear the interrupt flag.
SPI1CON1bits.DISSCK = 0; //Us the clock
SPI1CON1bits.DISSDO = 1; //Disable data output. This is not needed by the application, no pin are routed
SPI1CON1bits.MODE16 = 1; //Transmit word. Rx buffer is in byte, but we don't care because we are transmitting an even number of byte
SPI1CON1bits.SMP = 0; //data is sampled at middle of clock time
SPI1CON1bits.CKE = 1; //clock cycle start when going from active to idle clock state.
SPI1CON1bits.CKP = 1; //clock idle state is at high level
SPI1CON1bits.MSTEN = 1; //enable master mode
//Choose a clock frequency of 625 kHz, max accepted by AS5045 is 1Mhz
SPI1CON1bits.SPRE = 0b111; //Select a secondary prescaler of 1:1
SPI1CON1bits.PPRE = 0b00 ; //select a primary prescaler of 64:1
SPI1CON2bits.FRMEN = 0; //No frame support
SPI1STATbits.SPIEN = 1; //Enable SPI 1 module
+
DMA2CONbits.CHEN = 0;
DMA2CONbits.SIZE = 0;
DMA2CONbits.DIR = 0;
DMA2CONbits.HALF = 0;
DMA2CONbits.NULLW = 1;
DMA2CONbits.AMODE = 0b00;// Register indirect with posincrement
DMA2CONbits.MODE = 0b01;// One - shot, No Ping Pong
DMA2STA = __builtin_dmaoffset(dma_spi_me_rx_buffer.w);
DMA2PAD = (volatile unsigned int) & SPI1BUF;
DMA2REQ = 0x000A;
DMA2CNT = 3;
IFS1bits.DMA2IF = 0;
IEC1bits.DMA2IE = 1;
init_magnetic_encoder(&(mes[0]));
init_magnetic_encoder(&(mes[1]));
init_magnetic_encoder(&(mes[2]));
}
#define fill_status(me,ocf_bit,cof_bit,lin_bit,amp_incr_bit,amp_decr_bit) do{ \
- me.flags = ME_F_OK; \
- if(!(ocf_bit)){ \
- me.flags |= ME_F_ONBOARD_PROCESSING_UNFINISHED; \
- } \
- if(cof_bit){ \
- me.flags |= ME_F_CORDIC_OVERFLOW; \
- } \
- if(lin_bit){ \
- me.flags |= ME_F_LINEARITY_READ_ALARM; \
- } \
- if( (amp_incr_bit) && (amp_decr_bit) ) { \
- me.flags |= ME_F_MAGNETIC_AMPLITUDE_ERROR; \
- } \
-}while(0)
+ me.flags = ME_F_OK; \
+ if(!(ocf_bit)){ \
+ me.flags |= ME_F_ONBOARD_PROCESSING_UNFINISHED; \
+ } \
+ if(cof_bit){ \
+ me.flags |= ME_F_CORDIC_OVERFLOW; \
+ } \
+ if(lin_bit){ \
+ me.flags |= ME_F_LINEARITY_READ_ALARM; \
+ } \
+ if( (amp_incr_bit) && (amp_decr_bit) ) { \
+ me.flags |= ME_F_MAGNETIC_AMPLITUDE_ERROR; \
+ } \
+ }while(0)
void magnetic_encoders_start_reading(){
if(dma_spi_me.state == DMA_SPI_ME_IDLE){
dma_spi_me.state = DMA_SPI_ME_WAITING_FOR_DEVICES;
DMA2CONbits.CHEN = 1;
dma_spi_me_start();
timer_2_start();
return;
}
}
#define set_me_value(me,value,adr) do{ \
- if((me).flags == ME_F_OK){ \
- (me).position = (long) ((value)) * (me).mult / (me).div; \
- (me).position &= 0x3fff; \
- } \
- sbcp_me_reg(adr,MEX_POSITION).u = (me).position; \
- if((me).flags & (ME_F_LINEARITY_READ_ALARM | ME_F_CORDIC_OVERFLOW | ME_F_ONBOARD_PROCESSING_UNFINISHED)) { \
- sbcp_me_reg(adr,MEX_POSITION).u |= 1 << 15; \
- } \
- if((me).flags & ME_F_MAGNETIC_AMPLITUDE_ERROR){ \
- sbcp_me_reg(adr,MEX_POSITION).u |= 1 << 14; \
- } \
-}while(0)
+ if((me).flags == ME_F_OK){ \
+ unsigned int _TMP_val = value; \
+ if((me).mult < 0){ \
+ _TMP_val = 4096 - _TMP_val; \
+ } \
+ (me).position = (long) _TMP_val * abs((me).mult) / (me).div; \
+ (me).position &= 0x3fff; \
+ } \
+ sbcp_me_reg(adr,MEX_POSITION).u = (me).position; \
+ if((me).flags & (ME_F_LINEARITY_READ_ALARM | ME_F_CORDIC_OVERFLOW | ME_F_ONBOARD_PROCESSING_UNFINISHED)) { \
+ sbcp_me_reg(adr,MEX_POSITION).u |= 1 << 15; \
+ } \
+ if((me).flags & ME_F_MAGNETIC_AMPLITUDE_ERROR){ \
+ sbcp_me_reg(adr,MEX_POSITION).u |= 1 << 14; \
+ } \
+ }while(0)
void magnetic_encoders_process(){
if(dma_spi_me.state == DMA_SPI_ME_DATA_READY_TO_BE_PROCESSED){
//we assume that the parity is always correct.
/// \todo : check the parity of the data, buts it's rather complicated.
//first we check the status
fill_status(mes[0],
dma_spi_me_rx_buffer.b.Q1_OCF,
dma_spi_me_rx_buffer.b.Q1_COF,
dma_spi_me_rx_buffer.b.Q1_LIN,
dma_spi_me_rx_buffer.b.Q1_MAG_INC,
dma_spi_me_rx_buffer.b.Q1_MAG_DEC);
fill_status(mes[1],
dma_spi_me_rx_buffer.b.Q2_OCF,
dma_spi_me_rx_buffer.b.Q2_COF,
dma_spi_me_rx_buffer.b.Q2_LIN,
dma_spi_me_rx_buffer.b.Q2_MAG_INC,
dma_spi_me_rx_buffer.b.Q2_MAG_DEC);
fill_status(mes[2],
dma_spi_me_rx_buffer.b.Q3_OCF,
dma_spi_me_rx_buffer.b.Q3_COF,
dma_spi_me_rx_buffer.b.Q3_LIN,
dma_spi_me_rx_buffer.b.Q3_MAG_INC,
dma_spi_me_rx_buffer.b.Q3_MAG_DEC);
set_me_value(mes[0], (dma_spi_me_rx_buffer.b.Q1_ENC_DATA_HIGH << 8 ) + dma_spi_me_rx_buffer.b.Q1_ENC_DATA_LOW , MDV_ME_Q1);
set_me_value(mes[1], (dma_spi_me_rx_buffer.b.Q2_ENC_DATA_HIGH << 8 ) + dma_spi_me_rx_buffer.b.Q2_ENC_DATA_LOW , MDV_ME_Q2);
set_me_value(mes[2], (dma_spi_me_rx_buffer.b.Q3_ENC_DATA_HIGH << 8 ) + ( dma_spi_me_rx_buffer.b.Q3_ENC_DATA_MID << 3 ) + dma_spi_me_rx_buffer.b.Q3_ENC_DATA_LOW , MDV_ME_Q3);
//mark that new data is available in the table.
sbcp_mark_new_low_latency_data_available();
//we have readen the data, nothing more to do
dma_spi_me.state = DMA_SPI_ME_IDLE;
}
}
/*******************************************************************************
*
* Interruption
*
******************************************************************************/
/**
* Interruption when the SPI 1 finished its reading
*/
void __attribute__((__interrupt__ , no_auto_psv)) _DMA2Interrupt(){
dma_spi_me.state = DMA_SPI_ME_DATA_READY_TO_BE_PROCESSED;
dma_spi_me_stop();
//clear the interruption flag
IFS1bits.DMA2IF = 0;
}
/* Timer 2 is used to create the delay to communicate over the SPI1 bus */
void __attribute__((__interrupt__, no_auto_psv)) _T2Interrupt(void){
/* Interrupt Service Routine code goes here */
timer_2_stop_and_clear();
dma_spi_me_start_actual_reading();
IFS0bits.T2IF = 0; // Clear Timer 2 Interrupt Flag
}
void me_set_gear_ratio(magnetic_encoder * e, int mult, int div){
if(div == 0) {
div = 1;
}
e->mult = mult;
e->div = div;
}
int me_get_gear_mult(magnetic_encoder * e){
return e->mult;
}
int me_get_gear_div(magnetic_encoder * e){
return e->div;
}
int me_get_value(magnetic_encoder * e){
return e->position;
}
me_error_flags me_get_error(magnetic_encoder * e){
return e->flags;
}
void me_load_persistent_sbcp_settings(){
me_set_gear_ratio(me1,
sbcp_me_reg(MDV_ME_Q1,MEX_GEAR_MULT).i,
sbcp_me_reg(MDV_ME_Q1,MEX_GEAR_DIV).i);
me_set_gear_ratio(me2,
sbcp_me_reg(MDV_ME_Q2,MEX_GEAR_MULT).i,
sbcp_me_reg(MDV_ME_Q2,MEX_GEAR_DIV).i);
me_set_gear_ratio(me3,
sbcp_me_reg(MDV_ME_Q3,MEX_GEAR_MULT).i,
sbcp_me_reg(MDV_ME_Q3,MEX_GEAR_DIV).i);
}
diff --git a/src/magnetic_encoder.h b/src/magnetic_encoder.h
index 4dc2447..332ef5c 100644
--- a/src/magnetic_encoder.h
+++ b/src/magnetic_encoder.h
@@ -1,44 +1,50 @@
#ifndef MAGNETIC_ENCODER_H_
#define MAGNETIC_ENCODER_H_
#include <gpio.h>
struct magnetic_encoder;
typedef struct magnetic_encoder magnetic_encoder;
void init_magnetic_encoders(gpio spi_cs);
void magnetic_encoders_process();
void magnetic_encoders_start_reading();
void me_load_persistent_sbcp_settings();
enum me_error_flags {
ME_F_OK = 0 ,
ME_F_LINEARITY_READ_ALARM = 1 << 0,
ME_F_CORDIC_OVERFLOW = 1 << 1,
ME_F_ONBOARD_PROCESSING_UNFINISHED = 1 << 2,
ME_F_MAGNETIC_AMPLITUDE_ERROR = 1 << 3
};
typedef enum me_error_flags me_error_flags;
void me_set_gear_ratio(magnetic_encoder * e, int mult, int div);
int me_get_gear_mult(magnetic_encoder * e);
int me_get_gear_div(magnetic_encoder * e);
int me_get_value(magnetic_encoder * e);
me_error_flags me_get_error(magnetic_encoder * e);
+
+
extern magnetic_encoder * me1,* me2,* me3;
+
+
+
+
#endif //MAGNETIC_ENCODER_H_
diff --git a/src/main.c b/src/main.c
index c1de3bd..0cd8c71 100644
--- a/src/main.c
+++ b/src/main.c
@@ -1,286 +1,285 @@
-#include "config.h"
+ #include "config.h"
#include <libpic30.h>
#include "pindefs.h"
#include "motordriver.h"
#include "timer.h"
#include "sbcp_mdv.h"
#include "magnetic_encoder.h"
#include "load_cell.h"
/** C O N F I G ****************************************************/
// See "p33fj128mc804.h" for all config settings.
// Code Protect off, Write protect disabled
_FGS(GSS_OFF & GWRP_OFF);
// Prim. Osc (XT, HS, EC) w/ PLL, Two speed osc off, Temp protect off
_FOSCSEL(FNOSC_PRIPLL);
// Clock Switch & Clock Monitor off, OSC2 is IO, external clock generation
-_FOSC(FCKSM_CSDCMD & OSCIOFNC_ON & POSCMD_EC);
+_FOSC(FCKSM_CSDCMD & OSCIOFNC_ON & POSCMD_EC & IOL1WAY_OFF );
// Watchdog Timer Disabled
_FWDT(FWDTEN_OFF);
// No PWM off delay, PWM controlled by port configuration at device reset
_FPOR(FPWRT_PWR1);
// No JTAG, debugging on PGD1 pins
_FICD(JTAGEN_OFF & ICS_PGD1);
extern unsigned int volatile systime;
unsigned int prev_systime;
void clock_init() {
// Configure PLL prescaler, PLL postscaler and PLL divisor
// 8MHz oscillator
// frequency = Fin * M / (N1 * N2)
// Maximum frequency is 80MHz = 40MIPS
// => 8*40/(2*2)
PLLFBD = 38; // M = 40
CLKDIVbits.PLLPOST=0; // N2 = 2
CLKDIVbits.PLLPRE=0; // N1 = 2
}
/*void pwm_init() {
// initialize PWM channels
// ***************** PWM section *****************
// P1TCON, P2TCON
// -> pwm enabled, pwm stops in idle mode, postscale 1:1, prescale 1:1, free run mode
P1TCON = 0x00; // PTEN (1), DC (1), PTSIDL (1), DC (5), PTOPS (4), PTCKPS (2), PTMOD (2)
P2TCON = 0x00; // PTEN (1), DC (1), PTSIDL (1), DC (5), PTOPS (4), PTCKPS (2), PTMOD (2)
P1TCONbits.PTEN = 1;
P2TCONbits.PTEN = 1;
// PWM1CON1, PWM2CON1
// -> only high side pwm channel is used
PWM1CON1 = 0b0000011101110000; // DC (5), independent mode (3), DC (1), enable PWM1Hx (3), DC (1), PWM1Lx is IO-pin (3)
PWM2CON1 = 0b0000000100010000; // DC (7), independent mode (1), DC (3), enable PWM2Hx (1), DC (3), PWM2Lx is IO-pin (1)
// P1TPER, P2TPER
// see p.14-25 ref manual motorcontrol dspic33f
P1TPER = MAX_PWM;
P2TPER = MAX_PWM;
// initialize as 0 voltage
P1DC1 = 0;
P1DC2 = 0;
P1DC3 = 0;
P2DC1 = 0;
// PWM_TRQ1 = 0;
// M1_SET_PWM(0);
// PWM_TRQ2 = 0;
// M2_SET_PWM(0);
}*/
void qei_init() {
// Enable pullup on B8-B11 (encoder inputs)
// __builtin_write_OSCCONL(0b10000000);//unlock peripheral pinsettings (p.168 manual)
_QEA1R = 20;
_QEB1R = 21;
_QEA2R = 6;
_QEB2R = 7;
// __builtin_write_OSCCONL(0b11000000);//lock pereriphal pinsettings
// ***************** QEI section *****************
QEI1CON = 0b0000011100000000; // CNTERR(1), unused(1), QEISIDL(1), INDEX(1), UPDN(1), QEIM(3), SWPAB(1), PCDOUT(1), TQGATE(1), POSRES(2) (prescale), POSRES(1), TQCS(1), UPDN_SRC(1)
DFLT1CON = 0b0000000110100000; // unused(5), IMV(2), CEID(1), QEOUT(1), QECK(3), unused(4) -> digital filter 1:4
MAX1CNT = 0xffff;//maxcount for qei position.
QEI2CON = 0b0000011100000000;
DFLT2CON = 0b0000000110100000;
MAX2CNT = 0xffff;//maxcount for qei position.
mdv_reset_count(mdv1);
mdv_reset_count(mdv2);
}
void delay_ms(unsigned int t) {
uint16_t i;
for (i=0; i<t; i++) {
__delay_ms(1);
}
}
///////////////////////////////////////////////////////////////////////////////
int main (void) {
// wait a little bit
delay_ms(100);
clock_init();
pin_init();
// enable motordrivers reset
mdv_master_reset_enable();
mdv_pin pins[2] =
{
{
gpio_create(GPIO_PORT_A,GPIO_PIN_3,GPIO_OUTPUT), //brake
gpio_create(GPIO_PORT_A,GPIO_PIN_4,GPIO_OUTPUT), //coast
gpio_create(GPIO_PORT_B,GPIO_PIN_11,GPIO_OUTPUT), //dir
gpio_create(GPIO_PORT_C,GPIO_PIN_3,GPIO_OUTPUT), //mode
},
{
gpio_create(GPIO_PORT_A,GPIO_PIN_10,GPIO_OUTPUT), //brake
gpio_create(GPIO_PORT_A,GPIO_PIN_8,GPIO_OUTPUT), //coast
gpio_create(GPIO_PORT_A,GPIO_PIN_7,GPIO_OUTPUT), //dir
gpio_create(GPIO_PORT_A,GPIO_PIN_9,GPIO_OUTPUT), //mode
}
};
pwm_init(&(pins[0].torque),PWM_1_1);
pwm_set_cycle(pins[0].torque, MAX_PWM);
pwm_set_value(pins[0].torque, 0);
pwm_init(&(pins[0].speed),PWM_1_3);
pwm_set_cycle(pins[0].speed, MAX_PWM);
pwm_set_value(pins[0].speed, 0);
pwm_init(&(pins[1].torque),PWM_2_1);
pwm_set_cycle(pins[1].torque, MAX_PWM);
pwm_set_value(pins[1].torque, 0);
pwm_init(&(pins[1].speed),PWM_1_2);
pwm_set_cycle(pins[1].speed, MAX_PWM);
pwm_set_value(pins[1].speed, 0);
qei_init();
init_motordrivers(&(pins[0]),&(pins[1]));
//now set the right default value for motor
mdv_get_parameters(mdv1)->p_gain = 400;
mdv_get_parameters(mdv1)->i_gain = 20;
mdv_get_parameters(mdv1)->d_gain = 200;
mdv_get_parameters(mdv1)->preload = MAX_DUTY;
mdv_get_parameters(mdv1)->stiffness = 0;
mdv_get_parameters(mdv1)->damping = 0;
- mdv_set_maximal_torque(mdv1 , 10/256 * MAX_DUTY);
- mdv_set_maximal_acceleration(mdv1 , 1000);
- mdv_set_maximal_speed(mdv1 , 10000);
+ mdv_set_gear_ratio(mdv1,675,8);
+
+
+ mdv_set_maximal_torque(mdv1 , (long) 100 * (long)MAX_DUTY / (long)256);
+ mdv_set_maximal_acceleration(mdv1 , 32767);
+ mdv_set_maximal_speed(mdv1 , 32767);
//limit position set by calibration !!!
- mdv_set_gear_ratio(mdv1,675,8);
mdv_get_parameters(mdv2)->p_gain = 400;
mdv_get_parameters(mdv2)->i_gain = 20;
mdv_get_parameters(mdv2)->d_gain = 200;
mdv_get_parameters(mdv2)->preload = MAX_DUTY;
mdv_get_parameters(mdv2)->stiffness = 0;
mdv_get_parameters(mdv2)->damping = 0;
- mdv_set_maximal_torque(mdv2 , 10/256 * MAX_DUTY);
- mdv_set_maximal_acceleration(mdv2 , 1000);
- mdv_set_maximal_speed(mdv2 , 10000);
-
-
+ mdv_set_gear_ratio(mdv2,225,4);
+ mdv_set_maximal_torque(mdv2 , (long)100 * (long)MAX_DUTY / (long)256);
+ mdv_set_maximal_acceleration(mdv2 , 32767);
+ mdv_set_maximal_speed(mdv2 , 32767);
- mdv_set_gear_ratio(mdv2,225,4);
-
// RS485 communication init
timer_init();
init_magnetic_encoders(gpio_create(GPIO_PORT_B,GPIO_PIN_4,GPIO_OUTPUT));
- me_set_gear_ratio(me1,25,12);
+ me_set_gear_ratio(me1,12,25);
me_set_gear_ratio(me2,1,1);
me_set_gear_ratio(me3,1,1);
- init_load_cells(gpio_create(GPIO_PORT_B,GPIO_PIN_5,GPIO_OUTPUT));
-
+ init_load_cells();
+
sbcp_settings s;
gpio send_enable = gpio_create(GPIO_PORT_C,GPIO_PIN_2,GPIO_OUTPUT);
//we init with receive_disable == send_enable since there is only one pin.
sbcp_init_settings(&s,
SBCP_MDV_CLASS,
SBCP_MDV_THIS_DEVICE_INITIAL_ID,
SBCP_MDV_FIRMWARE_VERSION,send_enable,send_enable);
s.baudrate = SBCP_MDV_BAUDRATE;
s.clock_frequency = FCY_FOR_BAUDRATE;
s.uart = UART_1;
s.timeout_frequency = SBCP_MDV_TIMEOUT;
s.init_register = &init_sbcp_mdv_registers;
s.init_instruction = &init_sbcp_mdv_instructions;
map_pin_to_periph_output(17,RP_U1TX);
uart_map_rx_input(UART_1,16);
sbcp_init(&s);
//now set again the persistent parameters !!!
mdv_load_persistent_sbcp_settings();
me_load_persistent_sbcp_settings();
lc_load_persistent_sbcp_settings();
prev_systime = 0;
// disable motordrivers reset, we are running boys!!
mdv_master_reset_disable();
while (1) {
// run indefinetly
// check if system time has changed
- if (systime != prev_systime) {
+ if (systime != prev_systime) { // every 1ms
prev_systime = systime;
// order of processes determines priority
// motor control process is most sensitive to timing
mdv_process(mdv1);
mdv_process(mdv2);
- //sensor reading process, just meke one reading each 1ms
+ //sensor reading process, just make one reading each 1ms
// cpg_main(&cpg);
// sine_main(&sine);
magnetic_encoders_start_reading();
load_cell_start_reading();
- if(PORTBbits.RB13){
+ if(BLINK){
LED_ENABLE;
} else {
LED_DISABLE;
}
}
// put here processes which do not depend on the system timer
//no need to fucking check prev_systime == systime, if this is not the
//case it means that the critical part last more than one loop
//increment,so we are fubar
magnetic_encoders_process();
load_cell_process();
sbcp_process();
}
}
diff --git a/src/mdv_control_modes.c b/src/mdv_control_modes.c
index bbc3123..4955d20 100644
--- a/src/mdv_control_modes.c
+++ b/src/mdv_control_modes.c
@@ -1,229 +1,317 @@
#include "mdv_control_modes.h"
#include "mdv_speed_pid.h"
#include "misc_math.h"
+#include "cos_table.h"
+
+#include "sbcp_mdv.h"
void mdv_coast_init(motordriver * mdv){
gpio_set(mdv->pins.brake);
gpio_clear(mdv->pins.coast);
}
void mdv_brake_init(motordriver * mdv){
gpio_set(mdv->pins.coast);
gpio_clear(mdv->pins.brake);
}
void mdv_position_process(motordriver * mdv){
if(mdv_flag(mdv,MDV_F_NEW_POSITION_AVAILABLE)){
- int abs_speed = abs(mdv->goal_speed > 0);
+ mdv->goal_pos = mdv_e2i_position(mdv,mdv->user_goal_pos);
+
+ mdv->goal_pos = clamp(mdv->goal_pos,0,mdv->pos_limit);
+ mdv->goal_speed = clamp_absolute(mdv_lo2hi_res_speed(mdv->goal_pos - mdv->moving_pos) / mdv->params.smooth_interval ,mdv->max_speed );
+
+ int abs_speed = abs(mdv->goal_speed);
+ //mdv->goal_pos = mdv->moving_pos + (mdv->goal_pos - mdv->moving_pos)*(long)(SMOOTH_POSITION_OVERSHOOT + 1 );
+
//disable moving during play with parameter
mdv_clear_flag(mdv,MDV_F_MOVING_ENABLE);
//checks that the set speed is correct !!!!!! users could be morrons
// and move motor if needed
- if(mdv->goal_pos != mdv-> moving_pos ){
+ if(mdv->goal_pos != mdv-> moving_pos ) {
if (mdv->goal_pos > mdv-> moving_pos) {
mdv->goal_speed = abs_speed;
} else {
mdv->goal_speed = -abs_speed;
}
mdv_set_flag(mdv,MDV_F_MOVING_ENABLE);
}
mdv_clear_flag(mdv,MDV_F_NEW_POSITION_AVAILABLE);
}
//now process normally
mdv_speed_pid_with_compliance_process(mdv);
}
void mdv_position_init(motordriver * mdv){
gpio_set(mdv->pins.coast);
gpio_set(mdv->pins.brake);
}
void mdv_velocity_process(motordriver * mdv){
if(mdv_flag(mdv,MDV_F_NEW_SPEED_AVAILABLE)){
mdv_clear_flag(mdv,MDV_F_MOVING_ENABLE);
//set flag if needed, and set the right goal position if user
// modify it
if(mdv->goal_speed > 0){
mdv->goal_pos = mdv->pos_limit;
if(mdv->moving_pos < mdv->goal_pos ){
mdv_set_flag(mdv,MDV_F_MOVING_ENABLE);
}
} else if (mdv->goal_speed < 0){
mdv->goal_pos = 0;
if(mdv->moving_pos > 0 ){
mdv_set_flag(mdv,MDV_F_MOVING_ENABLE);
}
}
mdv_clear_flag(mdv, MDV_F_NEW_SPEED_AVAILABLE);
}
mdv_speed_pid_with_compliance_process(mdv);
}
void mdv_velocity_init(motordriver * mdv){
gpio_set(mdv->pins.coast);
gpio_set(mdv->pins.brake);
}
-
void mdv_smooth_position_process(motordriver * mdv){
/// \todo implement safety where if no data is available for an
/// amount of time, we switch to COAST mode
- if(mdv_flag(mdv,MDV_F_NEW_POSITION_AVAILABLE)){
- // compute new desired speed, clamp within the range of integers
+ if(mdv_flag(mdv,MDV_F_NEW_POSITION_AVAILABLE))
+ {
+ pushIntoBuffer(&mdv->commandsBuffer, mdv->user_goal_pos);
+ }
- int new_speed = clamp_absolute(mdv_lo2hi_res_speed(mdv->goal_pos - mdv->moving_pos) / mdv->params.smooth_interval ,
- mdv->max_speed );
+ // starts the logging of commands whend half the capacity is reached.
+ // can be changed to 3/4 or any other fraction of the capacity.
+ // this fraction can also be precomputed for more efficiency
+ if(!mdv->startSendOrders && mdv->commandsBuffer.numVals >= BUFFER_CAPACITY/2)
+ {
+ mdv->startSendOrders = 1;
+ }
- mdv_clear_flag(mdv,MDV_F_MOVING_ENABLE);
- mdv->goal_speed = new_speed;
+ if(!mdv->startSendOrders)
+ {
+ mdv_speed_pid_with_compliance_process(mdv);
+ return;
+ }
+
+ if(mdv->timeCounter < mdv->params.smooth_interval)
+ {
+ ++(mdv->timeCounter);
+ mdv_speed_pid_with_compliance_process(mdv);
+ return;
+ }
- mdv->goal_pos = mdv->moving_pos + (mdv->goal_pos - mdv->moving_pos)*(long)(SMOOTH_POSITION_OVERSHOOT + 1 );
+ //actually sends the orders if the buffer is not empty.
+ if(mdv->commandsBuffer.numVals > 0)
+ {
+ //gets the goal position from the buffer.
+ mdv->goal_pos = mdv_e2i_position(mdv,popFromBuffer(&mdv->commandsBuffer));
- // limit position
- mdv->goal_pos = clamp(mdv->goal_pos,0,mdv->pos_limit);
+ // compute new desired speed, clamp within the range of integers
+ long new_speed = clamp_absolute(mdv_lo2hi_res_speed(mdv->goal_pos - mdv->moving_pos) / mdv->params.smooth_interval ,
+ mdv->max_speed );
- // update goal position
- if (mdv->goal_pos != mdv->moving_pos) {
- // enable new movement
- mdv_set_flag(mdv, MDV_F_MOVING_ENABLE);
- }
+ mdv_clear_flag(mdv,MDV_F_MOVING_ENABLE);
+ mdv->goal_speed = new_speed;
- mdv_clear_flag(mdv,MDV_F_NEW_POSITION_AVAILABLE);
- }
+ mdv->goal_pos = mdv->moving_pos + (mdv->goal_pos - mdv->moving_pos)*(long)(SMOOTH_POSITION_OVERSHOOT + 1 );
- mdv_speed_pid_with_compliance_process(mdv);
-}
+ // limit position
+ mdv->goal_pos = clamp(mdv->goal_pos,0,mdv->pos_limit);
+ // update goal position
+ if (mdv->goal_pos != mdv->moving_pos) {
+ // enable new movement
+ mdv_set_flag(mdv, MDV_F_MOVING_ENABLE);
+ }
+
+ mdv_clear_flag(mdv,MDV_F_NEW_POSITION_AVAILABLE);
+ }
+ mdv->timeCounter = 0;
+ mdv_speed_pid_with_compliance_process(mdv);
+}
void mdv_smooth_position_init(motordriver * mdv){
gpio_set(mdv->pins.coast);
gpio_set(mdv->pins.brake);
+
+ initBuffer(&mdv->commandsBuffer);
+ mdv->timeCounter = 0;
+ mdv->startSendOrders = 0;
}
#define MDV_RUNTIME_CALIBRATION_SUCCESSFUL 0xffff
#define mdv_rc_in_calibration_region(min,max,pos) ( (min) < (max) ? ( ( (min) <= (pos) ) && ( (pos) <= (max) ) ) : ( ( (pos) <= (max) ) || ( (min) <= (pos) ) ) )
/**
* returns 0xffff if calibration is over, otherwise a 12 bits unsigned
* int being the real position.
*/
int mdv_rc_check_and_calibrate(motordriver * mdv){
/* readout the real position */
int pos = (*(mdv->calib.pos_evaluator))();
if(mdv_rc_in_calibration_region(mdv->calib.min_region,mdv->calib.max_region,pos)){
//we check how much far away we are from the min position, in increments, and we map it to
//the 0 position, and add the offset.
-
- long actual_position = mdv_e2i_position(mdv, pos - mdv->calib.min_region) + mdv->calib.offset;
+ long actual_position = (long)pos - (long)mdv->calib.min_region + (long)mdv->calib.offset;
+ if(actual_position < 0 ){
+ actual_position += mdv->calib.pos_evaluator_max_value;
+ }
+ actual_position = mdv_e2i_position(mdv,actual_position);
mdv->pos_limit = mdv_e2i_position(mdv,mdv->calib.pos_limit);
mdv_set_pos_count(mdv,actual_position);
return MDV_RUNTIME_CALIBRATION_SUCCESSFUL;
}
return pos;
}
void mdv_runtime_calibration_init(motordriver * mdv){
gpio_set(mdv->pins.brake);
- gpio_set(mdv->pins.coast);
+ gpio_clear(mdv->pins.coast);
- //this sets an update of 10 ms, and make sure that the next process will
- //compute an update.
+ //this sets an update of 10 ms, and make sure that the next process will
+ //compute an update.
mdv->calib.update_period = 10;
mdv->calib.ellapsed_ts = 10;
}
void mdv_runtime_calibration_process(motordriver * mdv){
//first we check if we can calibrate
int pos = mdv_rc_check_and_calibrate(mdv);
if (pos == MDV_RUNTIME_CALIBRATION_SUCCESSFUL ){
//yes, and calibrated , so go back to COAST
- mdv_set_control_mode(mdv,MDV_M_COAST);
+ mdv->sc.status |= MDV_RUNTIME_CALIBRATED;
+ mdv_set_control_mode_private(mdv,MDV_M_COAST);
return;
}
//now check if we need an update of the target (every mdv->calib.update_period)
if( (++(mdv->calib.ellapsed_ts)) <= mdv->calib.update_period ) {
//no update needed, just process PID and return
mdv_speed_pid_with_compliance_process(mdv);
return;
}
//we need to compute a new update update.
mdv->calib.ellapsed_ts = 0;
- unsigned int mini = mdv->calib.min_region;
- unsigned int maxi = mdv->calib.max_region;
+ int mini = mdv->calib.min_region;
+ int maxi = mdv->calib.max_region;
long target_position;
/* first put the actual absolute error in target position */
- // the boundary are clean
+ // the boundary are clean
if (mini < maxi){
target_position = (mini + maxi) / 2 - pos ;
} else { //max has an overshoot, we should take it into account
target_position = (mini + maxi + mdv->calib.pos_evaluator_max_value + 1) / 2 - pos;
}
- //check for the shortest path !!!
- if(abs(target_position) > ANGULAR_RESOLUTION / 2){
- //we should make more than half a turn, so we go in the other direction !!!
- //case target > 0 remove ANGULAR_RESOLUTION
- //case target < 0 add ANGULAR_RESOLUTION
- target_position += ANGULAR_RESOLUTION * ((target_position > 0) ? -1 : 1);
- }
+ //check for the shortest path !!!
+ if(abs(target_position) > ANGULAR_RESOLUTION / 2){
+ //we should make more than half a turn, so we go in the other direction !!!
+ //case target > 0 remove ANGULAR_RESOLUTION
+ //case target < 0 add ANGULAR_RESOLUTION
+ target_position += ANGULAR_RESOLUTION * ((target_position > 0) ? -1 : 1);
+ }
- //don't forget to put this in the right dimension
+ //don't forget to put this in the right dimension
target_position = mdv_e2i_position(mdv,target_position);
//IGNORE POSITION_LIMIT !!!!!
//we now now our next target
mdv->goal_pos = target_position + mdv->pos_cnt ;
- if(mdv->goal_pos != mdv->moving_pos){
+ if(mdv->goal_pos != mdv->pos_cnt){
mdv_set_flag(mdv,MDV_F_MOVING_ENABLE);
} else {
mdv_clear_flag(mdv,MDV_F_MOVING_ENABLE);
}
- mdv->goal_speed = mdv_lo2hi_res_speed((mdv->goal_pos - mdv->moving_pos) / mdv->calib.update_period);
+ mdv->goal_speed = mdv_lo2hi_res_speed((mdv->goal_pos - mdv->pos_cnt) / mdv->calib.update_period);
+
+ //prevent the speed to exceed 2 turn per minute of the effector i.e. ANGULAR_RESOLUTION / 30.
+ long max_speed = mdv_e2i_speed(mdv, ANGULAR_RESOLUTION / 30);
+ mdv->goal_speed = clamp_absolute(mdv->goal_speed,max_speed);
- //prevent the speed to exceed 2 turn per minute of the effector i.e. ANGULAR_RESOLUTION / 30.
- mdv->goal_speed = clamp_absolute(mdv->goal_speed,
- mdv_e2i_speed(mdv,ANGULAR_RESOLUTION / 30));
//now process the speed pid
mdv_speed_pid_with_compliance_process(mdv);
+ //activate move now
+ gpio_set(mdv->pins.coast);
+
+}
+void mdv_sine_generator_init(motordriver * mdv){
+ mdv->sine.t = COS_TABLE_SIZE / 4 - 1;
+ mdv->sine.t_residue = 0;
+ mdv->sine.iter = 4;
+ mdv_set_smooth_interval(mdv,5);
+ mdv_smooth_position_init(mdv);
+}
+
+
+void mdv_sine_generator_process(motordriver * mdv){
+ sbcp_reg_address r = mdv == mdv1 ? MDV_SINE_1 : MDV_SINE_2;
+ int16_t frequency = sbcp_reg_table[r + SINE_FREQUENCY].i;
+ mdv->sine.t += frequency / 100;
+ mdv->sine.t_residue += frequency % 100;
+ if(mdv->sine.t_residue > 100){
+ mdv->sine.t += 1;
+ mdv->sine.t_residue -= 100;
+ }
+
+ if(mdv->sine.t > COS_TABLE_SIZE){
+ mdv->sine.t -= COS_TABLE_SIZE;
+ }
+
+ mdv->sine.iter += 1;
+
+ //seems strange but we clear the flag, only us could move the motor
+ //if we need.
+ mdv_clear_flag(mdv,MDV_F_NEW_POSITION_AVAILABLE);
+
+ if(mdv->sine.iter % 5 == 0){
+ mdv->sine.iter = 0;
+ int16_t amp = sbcp_reg_table[r + SINE_AMPLITUDE].i;
+ int16_t offset = sbcp_reg_table[r + SINE_OFFSET].i;
+ long pos = (long)amp * (long)cos_table[mdv->sine.t] / (long)2048 + (long)offset ;
+ mdv_set_goal_position(mdv, pos & 0xffff);
+ }
+ mdv_smooth_position_process(mdv);
}
diff --git a/src/mdv_control_modes.h b/src/mdv_control_modes.h
index ac851c7..a0388c5 100644
--- a/src/mdv_control_modes.h
+++ b/src/mdv_control_modes.h
@@ -1,33 +1,35 @@
#ifndef MDV_CONTROL_MODES_H_
#define MDV_CONTROL_MODES_H_
#include "mdv_internal.h"
/**
* A mdv_ctrl_process_fptr function that does nothing.
*/
void mdv_null_process(motordriver * mdv);
void mdv_coast_init(motordriver * mdv);
//Null process for coast mode
void mdv_brake_init(motordriver * mdv);
//Null process for brake mode
void mdv_position_init(motordriver * mdv);
void mdv_position_process(motordriver * mdv);
void mdv_velocity_init(motordriver * mdv);
void mdv_velocity_process(motordriver * mdv);
void mdv_smooth_position_init(motordriver * mdv);
void mdv_smooth_position_process(motordriver * mdv);
void mdv_runtime_calibration_init(motordriver * mdv);
void mdv_runtime_calibration_process(motordriver * mdv);
+void mdv_sine_generator_init(motordriver * mdv);
+void mdv_sine_generator_process(motordriver * mdv);
#endif //MDV_CONTROL_MODES_H_
diff --git a/src/mdv_internal.c b/src/mdv_internal.c
index 2bb967b..bb9eaa6 100644
--- a/src/mdv_internal.c
+++ b/src/mdv_internal.c
@@ -1,46 +1,54 @@
#include "mdv_internal.h"
#include "mdv_control_modes.h"
mdv_ctrl_fptr mdv_ctrl[MDV_NUMBER_OF_CONTROL_MODE] = {
- {&mdv_null_process,&mdv_coast_init},
- {&mdv_null_process,&mdv_brake_init},
- {&mdv_position_process,&mdv_position_init},
- {&mdv_velocity_process,&mdv_velocity_init},
- {&mdv_smooth_position_process,&mdv_smooth_position_init},
- {&mdv_null_process,&mdv_coast_init},
- {&mdv_runtime_calibration_init, &mdv_runtime_calibration_process}
+ {&mdv_null_process,&mdv_coast_init},
+ {&mdv_null_process,&mdv_brake_init},
+ {&mdv_position_process,&mdv_position_init},
+ {&mdv_velocity_process,&mdv_velocity_init},
+ {&mdv_smooth_position_process,&mdv_smooth_position_init},
+ {&mdv_null_process,&mdv_coast_init},
+ {&mdv_runtime_calibration_process, &mdv_runtime_calibration_init},
+ {&mdv_sine_generator_process,&mdv_sine_generator_init}
};
void mdv_null_process(motordriver * mdv){
pwm_set_value(mdv->pins.speed,MAX_PWM / 2);
pwm_set_value(mdv->pins.torque,MAX_PWM / 2);
}
void mdv_cleanup(motordriver * mdv) {
- mdv->goal_pos = mdv->pos_cnt;
- mdv->moving_pos = mdv->pos_cnt;
- mdv->goal_speed = 0;
- mdv->moving_speed = 0;
- mdv->output_trq = 0;
- mdv->prev_p_error = 0;
- mdv->i_error = 0;
- mdv_clear_flag(mdv,MDV_F_MOVING_ENABLE |
- MDV_F_UPDATE_I_ERROR |
- MDV_F_NEW_POSITION_AVAILABLE |
- MDV_F_NEW_SPEED_AVAILABLE);
+ mdv->goal_pos = mdv->pos_cnt;
+ mdv->moving_pos = mdv->pos_cnt;
+ mdv->goal_speed = 0;
+ mdv->moving_speed = 0;
+ mdv->output_trq = 0;
+ mdv->prev_p_error = 0;
+ mdv->i_error = 0;
+ mdv_clear_flag(mdv,MDV_F_MOVING_ENABLE |
+ MDV_F_UPDATE_I_ERROR |
+ MDV_F_NEW_POSITION_AVAILABLE |
+ MDV_F_NEW_SPEED_AVAILABLE);
}
void mdv_set_pos_count(motordriver * mdv, long pos){
IEC0bits.T1IE = 0;
mdv->previous_cnt = pos;
mdv->pos_cnt = pos;
- *(mdv->pos_counter) = pos;
+ *(mdv->pos_counter) = pos;
IEC0bits.T1IE = 1;
}
+
+
+void mdv_set_control_mode_private(motordriver * mdv , mdv_control_mode c){
+ mdv_cleanup(mdv);
+ mdv->sc.mode = c;
+ (*(mdv_ctrl[c].init))(mdv);
+}
diff --git a/src/mdv_internal.h b/src/mdv_internal.h
index d32658d..bdcdd79 100644
--- a/src/mdv_internal.h
+++ b/src/mdv_internal.h
@@ -1,276 +1,287 @@
#ifndef MDV_INTERNAL_H_
#define MDV_INTERNAL_H_
#include "motordriver.h"
#include "config.h"
+#include "ring-buffer.h"
+
//==============================================================================
// DEFINES
//==============================================================================
//nb increment of QEI encoder
#define QEI_RES 512UL
#define QEI_CNT_PER_TURN (QEI_RES * 4)
//compute the gear coeff, not done automatically !!!
// GEAR_MULT_COEFF 512 * 4 1
//----------------- = --------- = ---
// GEAR_DIV_COEFF 4096 2
#define GEAR_MULT_COEFF 1
#define GEAR_DIV_COEFF 2
#define HALL_RES 12UL
//max speed of the motor in RPM
#define MAX_SPEED_RPM 16200UL
//==============================================================================
// CONTROL DATA FOR MOTORDRIVER
//==============================================================================
enum mdv_flags {
- MDV_F_NO_FLAGS = 0,
- MDV_F_MOVING_ENABLE = 1 << 0,
- MDV_F_UPDATE_I_ERROR = 1 << 1,
- MDV_F_NEW_POSITION_AVAILABLE = 1 << 2,
- MDV_F_NEW_SPEED_AVAILABLE = 1 << 3
+ MDV_F_NO_FLAGS = 0,
+ MDV_F_MOVING_ENABLE = 1 << 0,
+ MDV_F_UPDATE_I_ERROR = 1 << 1,
+ MDV_F_NEW_POSITION_AVAILABLE = 1 << 2,
+ MDV_F_NEW_SPEED_AVAILABLE = 1 << 3
};
typedef enum mdv_flags mdv_flags;
#define mdv_set_flag(m,f) do{ (m)->flags |= (f); }while(0)
#define mdv_clear_flag(m,f) do { (m)->flags &= ~(f); }while(0)
#define mdv_flag(m,f) ( (m->flags) & f )
-#define mdv_set_fast_decay(mdv) do{\
- gpio_clear(mdv->pins.mode); /*fast decay when low */ \
-}while(0)
+#define mdv_set_fast_decay(mdv) \
+ do{ \
+ gpio_clear(mdv->pins.mode); /*fast decay when low */ \
+ }while(0)
-#define mdv_set_slow_decay(mdv) do{\
- gpio_set(mdv->pins.mode); /* slow decay when high */ \
-}while(0)
+#define mdv_set_slow_decay(mdv) \
+ do{ \
+ gpio_set(mdv->pins.mode); /* slow decay when high */ \
+ }while(0)
#define mdv_in_fast_decay_mode(mdv) (! gpio_read(mdv->pins.mode))
#define mdv_in_slow_decay_mode(mdv) ( gpio_read(mdv->pins.mode))
struct mdv_calibration_parameters {
unsigned int pos_limit;
unsigned int min_region;
unsigned int max_region;
int offset;
pos_evaluator_fptr pos_evaluator;
unsigned int update_period;
unsigned int ellapsed_ts;
unsigned int pos_evaluator_max_value;
};
struct motordriver {
- mdv_pin pins;
- mdv_parameters params;
+ mdv_pin pins;
+ mdv_parameters params;
struct mdv_calibration_parameters calib;
- mdv_status_and_control sc;
+ mdv_status_and_control sc;
- mdv_flags flags;
+ mdv_flags flags;
sbcp_reg_address address_start;
- volatile unsigned int * pos_counter;
+ volatile unsigned int * pos_counter;
/**
* \name qei position parameters
*/
///@{
/**
* previous (relative) position counter of quadrature
* input. dimension QEI increments
*/
- volatile int previous_cnt;
+ volatile int previous_cnt;
/**
* relative present position counter of quadrature
* input. dimension QEI increments
*/
- volatile long pos_cnt;
+ volatile long pos_cnt;
/**
* current speed of quadrature input. dimension QEI increments per
* 1/FT1
*/
- volatile int current_speed;
+ volatile int current_speed;
/**
* current acceleration of quadrature input. dimension QEI
* increments per (1/FT1) ^2
*/
- volatile int current_accel;
+ volatile int current_accel;
///@}
- // move controlling parameters
- long goal_pos; // goal position
- long goal_speed; // desired speed of the motor during the motion
- // QEI incremet / ts (1ms)
-
+ // move controlling parameters
+ int user_goal_pos; //buffer goal position
+ long goal_pos; // goal position
+ long goal_speed; // desired speed of the motor during the motion
+ // QEI incremet / ts (1ms)
+ //commands buffer to avoid jitter
+ RingBuffer commandsBuffer; //the ring buffer containing the commands
+ int startSendOrders; //boolean to start sending the orders when half the capacity is reached
+ int timeCounter; //a counter to log commands only every 5ms.
-
- // internally controlled variables
- long moving_pos; // current "desired" position during move
- long moving_speed; // current "desired" moving speed of the motor
+ // internally controlled variables
+ long moving_pos; // current "desired" position during move
+ long moving_speed; // current "desired" moving speed of the motor
// 1
- int resolution_error; // compensates for rounding errors
+ int resolution_error; // compensates for rounding errors
+
+ long i_error; // speed integral error
+ long prev_p_error; // previous proportional error, used to compute the derivative error
- long i_error; // speed integral error
- long prev_p_error; // previous proportional error, used to compute the derivative error
+ int max_trq; // maximum torque setting
- int max_trq; // maximum torque setting
+ // control output parameters
+ long output_trq; // current torque request
- // control output parameters
- long output_trq; // current torque request
+ // motor limits
+ long pos_limit; // maximum positive limit
+ long max_speed; // maximum speed the motor may reach
+ int max_accel; // 1 / 2 ^
- // motor limits
- long pos_limit; // maximum positive limit
- long max_speed; // maximum speed the motor may reach
- int max_accel; // 1 / 2 ^
+ int gear_mult;
+ int gear_div;
- int gear_mult;
- int gear_div;
+ struct {
+ int t;
+ int t_residue;
+ int iter;
+ } sine;
};
/**
* \name conversion macro
* Thes macro convert dimension for several value
*/
///@{
/**
* Transforms 1/4096 of turn in QEI increments
* \params mdv the motordriver
* \params pos the position to transform
*/
#define mdv_e2i_position(mdv,pos) ((long) ( (mdv)->gear_mult * ((long) (pos)) / (mdv)->gear_div ))
/**
* Transforms QEI increments per (1/FT1) to 1 / 2 ^ SPEED_BINARY_PRECISION
* QEI increments per per (1/FT1)
* \params speed the speed to transform
*/
#define mdv_lo2hi_res_speed(spd) ( (spd) * (1 << SPEED_BINARY_PRECISION) )
/**
* Transforms 1/4096 of turn per second to 1/ 2 ^
* SPEED_BINARY_PRECISION QEI increments per (1/FT1) (with higher
* precision)
* \param mdv the motordriver
* \param speed the speed
*/
#define mdv_e2i_speed(mdv,spd) (mdv_lo2hi_res_speed(mdv_e2i_position(mdv,spd)) / FT1)
/**
* Transforms 1/4096 of turn pers second^2 to 1 / 2 ^
* SPEED_BINARY_PRECISION QEI per (1/ FT1)^2
* \param mdv the motordriver
* \param a the acceleration
*/
-#define mdv_e2i_accel(mdv,a) (mdv_e2i_speed(mdv, a ) / FT1 )
+#define mdv_e2i_accel(mdv,a) (mdv_e2i_speed(mdv, a ) * 128 / FT1 )
/**
*
*/
#define mdv_e2i_torque(mdv,trq) (trq)
-
-
-
/**
* Transforms QEI increments in 1/4096 of turn.
* \params mdv the motordriver
* \params pos the position to transform
*/
#define mdv_i2e_position(mdv,pos) ((long) ( (mdv)->gear_div * ((long) (pos)) / (mdv)->gear_mult ))
/**
* Transforms 1 / 2 ^ SPEED_BINARY_PRECISION QEI increments per (1/FT1) in QEI
* increments per (1/FT1).
* \params speed the speed to transform
*/
#define mdv_hi2lo_res_speed(spd) ( (spd) / ( 1 << SPEED_BINARY_PRECISION ) )
/**
* Transforms 1/ 2 ^ SPEED_BINARY_PRECISION QEI increments per (1/FT1) in
* 1/4096 of turn per second.
* \param mdv the motordriver
* \param speed the speed
*/
#define mdv_i2e_speed(mdv,speed) ( FT1 * mdv_i2e_position(mdv,mdv_hi2lo_res_speed(speed)) )
/**
* Transforms 1 / 2 ^ SPEED_BINARY_PRECISION QEI per (1/FT1)^2 in 1/4096 of
* turn pers second^2.
* \param mdv the motordriver
* \param a the acceleration
*/
-#define mdv_i2e_accel(mdv,a) ( mdv_i2e_speed(mdv,a) * FT1 )
+#define mdv_i2e_accel(mdv,a) ( mdv_i2e_speed(mdv,a) / 128 * FT1)
/**
*
*/
#define mdv_i2e_torque(mdv,trq) (trq)
///@}
//==============================================================================
// CONTROL MODE SWITCHING
//==============================================================================
/**
* Process function for a control mode.
*/
typedef void (*mdv_ctrl_process_fptr)(motordriver *);
/**
* Init function for a control mode.
*/
typedef void (*mdv_ctrl_init_fptr)(motordriver *);
typedef struct mdv_ctrl_fptr {
- mdv_ctrl_process_fptr process;
- mdv_ctrl_init_fptr init;
+ mdv_ctrl_process_fptr process;
+ mdv_ctrl_init_fptr init;
} mdv_ctrl_fptr;
/**
* General cleanup function. It resets a mode so all PID is clean and
* unintegrated.
*/
void mdv_cleanup(motordriver * mdv);
/**
* A mdv_ctrl_process_fptr function that does nothing.
*/
void mdv_null_process(motordriver * mdv);
void mdv_set_pos_count(motordriver * mdv, long pos);
extern mdv_ctrl_fptr mdv_ctrl[MDV_NUMBER_OF_CONTROL_MODE];
+void mdv_set_control_mode_private(motordriver * mdv , mdv_control_mode c);
#endif
diff --git a/src/mdv_speed_pid.c b/src/mdv_speed_pid.c
index 0040056..9adf6ff 100644
--- a/src/mdv_speed_pid.c
+++ b/src/mdv_speed_pid.c
@@ -1,253 +1,250 @@
#include "mdv_speed_pid.h"
#include "mdv_internal.h"
-
#include "config.h"
-
#include "misc_math.h"
#define SMALLEST_HIGH_RES_SPEED mdv_lo2hi_res_speed(1L)
unsigned int mdv_trq_to_pwm(motordriver * mdv, long desired_trq, uint8_t force_fast_decay);
void mdv_update_position(motordriver * mdv){
int max_accel;
- // The desired position moves according to the speed and acceleration settings...
+ // The desired position moves according to the speed and acceleration settings...
- //if not moving, we skip :P
+ //if not moving, we skip :P
if (!mdv_flag(mdv,MDV_F_MOVING_ENABLE)) {
- return;
+ return;
}
// if no acceleration curve is used, then moving_speed is just the set_speed
// M->moving_speed = M->set_speed;
// => Acceleration curve computation
// compute when to switch off the set-speed in order to start descelerating such that zero speed is reached at the goal position
// maximum acceleration must not exceed the current moving speed (for correctly computing the speed auto-switch-off position)
max_accel = min(abs(mdv->moving_speed), (int)mdv->max_accel);
- // auto-switching off the set_speed should only be done if the current speed has the same direction as the set_speed
-
- if( (mdv->moving_speed > 0 && mdv->goal_speed > 0)
- || (mdv->moving_speed < 0 && mdv->goal_speed < 0)){
-
- ///\todo : find what these two mean
- long first_operand = abs(mdv->moving_pos - mdv->goal_pos) * (long) max_accel;
- long second_operand = mdv_hi2lo_res_speed( (long) abs(mdv->moving_speed)
- * ( (long) abs(mdv->moving_speed)
- + (long) max_accel -1 )) / 2 ;
- if (first_operand < second_operand ) {
- // we don't set the desired speed to zero, but to the minimum acceleration setting in order to guarantee convergence
- if (mdv->goal_speed > 0){
- mdv->goal_speed = min((int)mdv->max_accel, mdv->goal_speed);
- } else {
- mdv->goal_speed = - min((int)mdv->max_accel, - mdv->goal_speed);
- }
- }
- }
-
- // compute new speed according to the acceleration parameter
- if (mdv->moving_speed != mdv->goal_speed) {
-
- if (mdv->moving_speed < mdv->goal_speed) {
- mdv->moving_speed += (int)mdv->max_accel;
- if (mdv->moving_speed > mdv->goal_speed) {
- mdv->moving_speed = mdv->goal_speed;
- }
- } else {
- mdv->moving_speed -= (int)mdv->max_accel;
- if (mdv->moving_speed < mdv->goal_speed) {
- mdv->moving_speed = mdv->goal_speed;
- }
- }
- }
-
-
- // compute new desired position for the current step
- mdv->moving_pos += mdv_hi2lo_res_speed(mdv->moving_speed);
-
- // compensate for the bit errors due to the shifting action
- mdv->resolution_error += mdv->moving_speed - mdv_lo2hi_res_speed(mdv_hi2lo_res_speed(mdv->moving_speed)) ;
- if (mdv->resolution_error > SMALLEST_HIGH_RES_SPEED ) {
- mdv->resolution_error -= SMALLEST_HIGH_RES_SPEED;
- mdv->moving_pos += 1;
- } else if( mdv->resolution_error < - SMALLEST_HIGH_RES_SPEED) {
- mdv->resolution_error += SMALLEST_HIGH_RES_SPEED;
- mdv->moving_pos -= 1;
- }
-
- // check if the goal position is reached
- if (((mdv->goal_speed > 0) && (mdv->moving_pos >= mdv->goal_pos)) || ((mdv->goal_speed < 0) && (mdv->moving_pos <= mdv->goal_pos)) ) {
- mdv->moving_pos = mdv->goal_pos;
- mdv->moving_speed = 0;
- mdv_clear_flag(mdv,MDV_F_MOVING_ENABLE);
- }
+ // auto-switching of the set_speed should only be done if the current speed has the same direction as the set_speed
+
+ if( (mdv->moving_speed > 0 && mdv->goal_speed > 0)
+ || (mdv->moving_speed < 0 && mdv->goal_speed < 0)){
+
+ ///\todo : find what these two means
+ long first_operand = abs(mdv->moving_pos - mdv->goal_pos) * (long) max_accel;
+ long second_operand = mdv_hi2lo_res_speed( (long) abs(mdv->moving_speed)
+ * ( (long) abs(mdv->moving_speed)
+ + (long) max_accel - 1 )) / 2 ;
+ if (first_operand < second_operand ) {
+ // we don't set the desired speed to zero, but to the minimum acceleration setting in order to guarantee convergence
+ if (mdv->goal_speed > 0){
+ mdv->goal_speed = min((int)mdv->max_accel, mdv->goal_speed);
+ } else {
+ mdv->goal_speed = - min((int)mdv->max_accel, - mdv->goal_speed);
+ }
+ }
+ }
+
+ // compute new speed according to the acceleration parameter
+ if (mdv->moving_speed != mdv->goal_speed) {
+
+ if (mdv->moving_speed < mdv->goal_speed) {
+ mdv->moving_speed += (int)mdv->max_accel;
+ if (mdv->moving_speed > mdv->goal_speed) {
+ mdv->moving_speed = mdv->goal_speed;
+ }
+ } else {
+ mdv->moving_speed -= (int)mdv->max_accel;
+ if (mdv->moving_speed < mdv->goal_speed) {
+ mdv->moving_speed = mdv->goal_speed;
+ }
+ }
+ }
+
+
+ // compute new desired position for the current step
+ mdv->moving_pos += mdv_hi2lo_res_speed(mdv->moving_speed);
+
+ // compensate for the bit errors due to the shifting action
+ mdv->resolution_error += mdv->moving_speed - mdv_lo2hi_res_speed(mdv_hi2lo_res_speed(mdv->moving_speed)) ;
+ if (mdv->resolution_error > SMALLEST_HIGH_RES_SPEED ) {
+ mdv->resolution_error -= SMALLEST_HIGH_RES_SPEED;
+ mdv->moving_pos += 1;
+ } else if( mdv->resolution_error < - SMALLEST_HIGH_RES_SPEED) {
+ mdv->resolution_error += SMALLEST_HIGH_RES_SPEED;
+ mdv->moving_pos -= 1;
+ }
+
+ // check if the goal position is reached
+ if (((mdv->goal_speed > 0) && (mdv->moving_pos >= mdv->goal_pos)) || ((mdv->goal_speed < 0) && (mdv->moving_pos <= mdv->goal_pos)) ) {
+ mdv->moving_pos = mdv->goal_pos;
+ mdv->moving_speed = 0;
+ mdv_clear_flag(mdv,MDV_F_MOVING_ENABLE);
+ }
}
void mdv_compute_and_apply_output(motordriver * mdv){
long desired_speed;
long p_error, d_error;
long pid_output;
unsigned int speed_output;
unsigned int scaled_speed_output;
long i_max;
long required_trq;
// ***************** Speed control section: velocity error *****************
desired_speed = mdv->moving_pos - mdv->pos_cnt;
// compute new PID output
p_error = (long)mdv->current_speed - desired_speed;
- // only integrate action when not in complinat mode
+ // only integrate action when not in compliant mode
if (mdv_flag(mdv,MDV_F_UPDATE_I_ERROR)){
mdv->i_error += p_error;
}
d_error = p_error - mdv->prev_p_error;
-// if (M1.update_i_error) d_error = p_error - M1.prev_p_error; else d_error = 0;
+ // if (M1.update_i_error) d_error = p_error - M1.prev_p_error; else d_error = 0;
pid_output = ((p_error*(long) (mdv->params.p_gain)) + (mdv->i_error*(long)mdv->params.i_gain) + (d_error*(long)mdv->params.d_gain))/(long)1024;
mdv->prev_p_error = p_error;
// anti-windupif
- if(mdv->params.i_gain != 0){
- i_max = ((long)(MAX_PWM-1)*(long)1024)/(mdv->params.i_gain*2);
- mdv->i_error = clamp_absolute(mdv->i_error,i_max);
- }
+ if(mdv->params.i_gain != 0){
+ i_max = ((long)(MAX_PWM-1)*(long)1024)/(mdv->params.i_gain*2);
+ mdv->i_error = clamp_absolute(mdv->i_error,i_max);
+ }
// clamp output
speed_output = min(abs(pid_output),(long)(MAX_PWM-1));
- if(mdv_in_fast_decay_mode(mdv)){
- speed_output += MAX_DUTY / 2;
- } else {
- speed_output *= 2;
- }
+ if(mdv_in_fast_decay_mode(mdv)){
+ speed_output += MAX_DUTY / 2;
+ } else {
+ speed_output *= 2;
+ }
// set pwm output
//(MODE1==FAST_DECAY) ? (_PWM) + MAX_DUTY/2 : (_PWM)*2);
pwm_set_value(mdv->pins.speed,speed_output);
// ***************** Output torque control section *****************
// torque as feeled by the leg = spring force - spring damping + rotor inertia compensation
long output_spring = (((desired_speed)*(long)mdv->params.stiffness)/512) ;
long output_damping = - (((long)(mdv->current_speed - mdv_hi2lo_res_speed(mdv->moving_speed)) * (long)mdv->params.damping)/64);
mdv->output_trq = output_spring + output_damping;
// Add the spring preload term
if (desired_speed > 0){
mdv->output_trq += mdv->params.preload;
} else {
mdv->output_trq -= mdv->params.preload;
}
// required torque on the motor is the requested torque + a compensation term for the rotor inertia
///\todo remove hardcoded 64.
required_trq = mdv->output_trq + (long)mdv->current_accel* (long) MDV_INERTIA_COMP/64;
// scale speed to torque dimensions
scaled_speed_output = (long)((long)speed_output*MAX_DUTY)/(long)MAX_PWM;
// Set direction of torque, depending on which factor is smallest
-// if ((long)_ABS(required_trq) < (long)(speed_output*MAX_DUTY)/MAX_PWM) {
+ // if ((long)_ABS(required_trq) < (long)(speed_output*MAX_DUTY)/MAX_PWM) {
if ((unsigned int)abs(mdv->params.preload) < scaled_speed_output) {
mdv_clear_flag(mdv, MDV_F_UPDATE_I_ERROR);
if (required_trq>0) {
gpio_clear(mdv->pins.dir);
} else {
gpio_set(mdv->pins.dir);
}
///\todo remove this hardcoded value
if (mdv->params.stiffness > 500) { // problematic switching between fast and slow decay for very stiff spring settings
pwm_set_value(mdv->pins.torque, mdv_trq_to_pwm(mdv,abs(required_trq), 1));
} else {
pwm_set_value(mdv->pins.torque, mdv_trq_to_pwm(mdv,abs(required_trq), 0));
// LED_TX_ENABLE;
}
}
else {
mdv_set_flag(mdv,MDV_F_UPDATE_I_ERROR);
-// if (speed_output > 2)
+ // if (speed_output > 2)
if (pid_output<0) {
gpio_clear(mdv->pins.dir);
} else {
gpio_set(mdv->pins.dir);
}
// force fast decay mode
-// if (scaled_speed_output*4 < (unsigned int)_ABS(M1.compliance_preload))
-// PWM_TRQ1 = M1_trq_to_pwm((long)scaled_speed_output*4, TRUE);// (_ABS(required_trq)*(long)speed_output)/50, TRUE);
-// else
-// PWM_TRQ1 = M1_trq_to_pwm((long)scaled_speed_output, TRUE);
+ // if (scaled_speed_output*4 < (unsigned int)_ABS(M1.compliance_preload))
+ // PWM_TRQ1 = M1_trq_to_pwm((long)scaled_speed_output*4, TRUE);// (_ABS(required_trq)*(long)speed_output)/50, TRUE);
+ // else
+ // PWM_TRQ1 = M1_trq_to_pwm((long)scaled_speed_output, TRUE);
pwm_set_value(mdv->pins.torque, mdv_trq_to_pwm(mdv,abs(required_trq), 1));
-// LED_TX_DISABLE;
+ // LED_TX_DISABLE;
}
-
}
unsigned int mdv_trq_to_pwm(motordriver * mdv, long desired_trq, uint8_t force_fast_decay) {
unsigned int pwm_out;
desired_trq = (desired_trq*4096)/MAX_DUTY;
// clamp desired_trq within safe range
if (desired_trq > 8192) {
desired_trq = 8192;
}
// desired torque is a 12-bit value
// translate to 12-bit pwm-output using a piece-wise linear function
// this function compensates for the non-linear pwm to torque relation
if (desired_trq < 2) pwm_out = ((unsigned int)desired_trq)*215;
else if (desired_trq < 40) pwm_out = ((unsigned int)desired_trq - 2)*7/2 + 430;
else if (desired_trq < 86) pwm_out = ((unsigned int)desired_trq -40)*3/2 + 563;
else pwm_out = ((unsigned int)desired_trq -86)*7/8 + 632;
// full pwm scale is now 12-bit, convert to pwm-range
pwm_out = (unsigned int)(((unsigned long)pwm_out*(unsigned long)MAX_DUTY)/TORQUE_RESOLUTION);
// saturate at maximum torque
if ((mdv->max_trq > ((MAX_DUTY * MDV_SLOWFAST_THRES)/256)) && (!force_fast_decay)) {
if (pwm_out > mdv->max_trq + MDV_SLOWFAST_COMP) pwm_out = mdv->max_trq + MDV_SLOWFAST_COMP;
}
else { // motor remains in the fast decay regime
if (pwm_out > mdv->max_trq) pwm_out = mdv->max_trq;
}
if (force_fast_decay) {
mdv_set_fast_decay(mdv);
-// LED_TX_DISABLE;
+ // LED_TX_DISABLE;
}
else {
// choose appropriate decay mode
if (pwm_out > (unsigned int)((MAX_DUTY*MDV_SLOWFAST_THRES)/256)) {
mdv_set_slow_decay(mdv);
-// LED_TX_ENABLE;
+ // LED_TX_ENABLE;
}
else if (pwm_out < (unsigned int)((MAX_DUTY*(MDV_SLOWFAST_THRES-10))/256)) { // small hysteresis
mdv_set_fast_decay(mdv);
-// LED_TX_DISABLE;
+ // LED_TX_DISABLE;
}
}
// compensate for the slightly higher efficiency of the slow decay mode
if ( mdv_in_slow_decay_mode(mdv) ) {
pwm_out -= (unsigned int)MDV_SLOWFAST_COMP;
}
return pwm_out;
}
void mdv_speed_pid_with_compliance_process(motordriver * mdv){
- mdv_update_position(mdv);
- mdv_compute_and_apply_output(mdv);
+ mdv_update_position(mdv);
+ mdv_compute_and_apply_output(mdv);
}
diff --git a/src/motordriver.c b/src/motordriver.c
index 3da24f6..dccfbef 100644
--- a/src/motordriver.c
+++ b/src/motordriver.c
@@ -1,311 +1,319 @@
#include "motordriver.h"
#include "mdv_internal.h"
#include "mdv_control_modes.h"
#include "timer.h"
#include "config.h"
#include "sbcp_mdv.h"
#include "misc_math.h"
motordriver _NEAR MDV_1;
motordriver _NEAR MDV_2;
motordriver * mdv1 = &(MDV_1);
motordriver * mdv2 = &(MDV_2);
gpio mdv_master_reset;
void init_motordrivers(mdv_pin* mdv1_pin, mdv_pin* mdv2_pin){
init_motordriver(mdv1,mdv1_pin, MDV_M1, &POS1CNT);
init_motordriver(mdv2,mdv2_pin, MDV_M2, &POS2CNT);
}
void init_motordriver(motordriver * mdv,
- mdv_pin * pins,
- sbcp_reg_address address_start,
- volatile unsigned int * pos_counter ){
- mdv->pins = *pins;
- /*set coast mode as default and init it*/
- mdv->sc.status = MDV_IDLE;
- mdv->sc.mode = MDV_M_COAST;
- mdv->address_start = address_start;
- mdv->pos_counter = pos_counter;
- mdv_coast_init(mdv);
+ mdv_pin * pins,
+ sbcp_reg_address address_start,
+ volatile unsigned int * pos_counter ){
+ mdv->pins = *pins;
+ /*set coast mode as default and init it*/
+ mdv->sc.status = MDV_IDLE;
+ mdv->sc.mode = MDV_M_COAST;
+ mdv->address_start = address_start;
+ mdv->pos_counter = pos_counter;
+ mdv_coast_init(mdv);
- mdv->previous_cnt = 0;
- mdv->pos_cnt = 0;
- mdv->goal_pos = 0;
- mdv->goal_speed = 0;
- mdv_clear_flag(mdv,MDV_F_MOVING_ENABLE | MDV_F_UPDATE_I_ERROR);
+ mdv->previous_cnt = 0;
+ mdv->pos_cnt = 0;
+ mdv->goal_pos = 0;
+ mdv->goal_speed = 0;
+ mdv_clear_flag(mdv,MDV_F_MOVING_ENABLE | MDV_F_UPDATE_I_ERROR);
- mdv->params.p_gain = 0;
- mdv->params.i_gain = 0;
- mdv->params.d_gain = 0;
+ mdv->params.p_gain = 0;
+ mdv->params.i_gain = 0;
+ mdv->params.d_gain = 0;
- mdv->i_error = 0;
- mdv->prev_p_error = 0;
+ mdv->i_error = 0;
+ mdv->prev_p_error = 0;
- mdv->params.stiffness = 0;
- mdv->params.damping = 0;
- mdv->params.preload = 0;
+ mdv->params.stiffness = 0;
+ mdv->params.damping = 0;
+ mdv->params.preload = 0;
- mdv->max_trq = 0;
- mdv->output_trq = 0;
- mdv->pos_limit = 0;
+ mdv->max_trq = 0;
+ mdv->output_trq = 0;
+ mdv->pos_limit = 0;
- mdv->max_speed = 0;
- mdv->max_accel = 0;
+ mdv->max_speed = 0;
+ mdv->max_accel = 0;
}
void mdv_reset_count(motordriver * mdv){
mdv_set_pos_count(mdv,0);
}
-
-
-
void mdv_update_sbcp_register(motordriver * mdv){
if(mdv_flag(mdv,MDV_F_MOVING_ENABLE)){
mdv->sc.status |= MDV_MOVING;
} else {
mdv->sc.status &= ~(MDV_MOVING);
}
- sbcp_mdv_reg(mdv,MX_STATUS_AND_CONTROL_MODE) = mdv->sc.reg_val;
+ sbcp_mdv_reg(mdv ,MX_STATUS_AND_CONTROL_MODE) = mdv->sc.reg_val;
- sbcp_mdv_reg(mdv, MX_TARGETED_POSITION).i = mdv_i2e_position(mdv,mdv->moving_pos);
- sbcp_mdv_reg(mdv , MX_PRESENT_POSITION).i = mdv_i2e_position(mdv, mdv->pos_cnt);
+ sbcp_mdv_reg(mdv , MX_TARGETED_POSITION).i = mdv_i2e_position(mdv,mdv->moving_pos);
+ sbcp_mdv_reg(mdv , MX_PRESENT_POSITION).i = mdv_i2e_position(mdv, mdv->pos_cnt);
- sbcp_mdv_reg(mdv , MX_TARGETED_SPEED).i = mdv_i2e_speed(mdv, mdv->moving_speed);
- sbcp_mdv_reg(mdv , MX_PRESENT_SPEED).i = mdv_i2e_speed(mdv,mdv_lo2hi_res_speed(mdv->current_speed));
+ sbcp_mdv_reg(mdv , MX_TARGETED_SPEED).i = mdv_i2e_speed(mdv, mdv->moving_speed);
+ sbcp_mdv_reg(mdv , MX_PRESENT_SPEED).i = mdv_i2e_speed(mdv,mdv_lo2hi_res_speed(mdv->current_speed));
- sbcp_mdv_reg(mdv , MX_TARGETED_TORQUE).i = mdv_i2e_torque(mdv, mdv->output_trq);
+ sbcp_mdv_reg(mdv , MX_TARGETED_TORQUE).i = mdv_i2e_torque(mdv, mdv->output_trq);
- sbcp_mark_new_low_latency_data_available();
+ sbcp_mark_new_low_latency_data_available();
};
void mdv_process(motordriver * mdv){
- (*(mdv_ctrl[mdv->sc.mode].process))(mdv);
- mdv_update_sbcp_register(mdv);
+ (*(mdv_ctrl[mdv->sc.mode].process))(mdv);
+ mdv_update_sbcp_register(mdv);
}
-void mdv_set_control_mode_private(motordriver * mdv , mdv_control_mode c){
- mdv_cleanup(mdv);
- mdv->sc.mode = c;
- (*(mdv_ctrl[c].init))(mdv);
-}
mdv_error mdv_set_control_mode(motordriver * mdv, int c){
c = abs(c);
if(c >= MDV_NUMBER_OF_CONTROL_MODE){
return MDV_ERR_UNKNOWN_CONTROL_MODE;
}
if( c == MDV_M_RUNTIME_CALIBRATION ){
return MDV_ERR_RESERVED_CONTROL_MODE;
}
if( mdv->sc.mode == MDV_M_RUNTIME_CALIBRATION) {
return MDV_ERR_IN_CALIBRATION;
}
+ if( ! (mdv->sc.status & MDV_RUNTIME_CALIBRATED) ){
+ return MDV_ERR_MOTOR_NOT_RUNTIME_CALIBRATED;
+ }
+
if(c == mdv->sc.mode) {
return MDV_ERR_NO_ERROR;
}
mdv_set_control_mode_private(mdv,c);
return MDV_ERR_NO_ERROR;
}
int mdv_get_control_mode(motordriver * mdv){
return mdv->sc.mode;
}
-#define mdv_update_position_and_speed(mdv,pos_counter) do{\
-int _MC_tmp_speed = pos_counter - mdv->previous_cnt;\
-mdv->current_accel = _MC_tmp_speed - mdv->current_speed;\
-mdv->current_speed = _MC_tmp_speed;\
-mdv->previous_cnt = pos_counter;\
-mdv->pos_cnt += mdv->current_speed;\
-}while(0)
+#define mdv_update_position_and_speed(mdv,pos_counter) do{ \
+ int _MC_tmp_speed = pos_counter - mdv->previous_cnt; \
+ mdv->current_accel = _MC_tmp_speed - mdv->current_speed; \
+ mdv->current_speed = _MC_tmp_speed; \
+ mdv->previous_cnt = pos_counter; \
+ mdv->pos_cnt += mdv->current_speed; \
+ }while(0)
/* Timer 1 is used as system timer */
void __attribute__((__interrupt__, no_auto_psv)) _T1Interrupt(void) {
systime ++;
// timing critical part of motor control
mdv_update_position_and_speed(mdv1,POS1CNT);
mdv_update_position_and_speed(mdv2,POS2CNT);
IFS0bits.T1IF = 0; // Clear Timer 1 Interrupt Flag
}
int mdv_get_status_and_control_mode(motordriver * mdv) {
return mdv->sc.reg_val.i;
}
mdv_error mdv_set_goal_position(motordriver * mdv, int position){
- mdv->goal_pos = mdv_e2i_position(mdv,position);
+ mdv->user_goal_pos = position;
mdv_set_flag(mdv,MDV_F_NEW_POSITION_AVAILABLE);
- return MDV_ERR_NO_ERROR;
+ return MDV_ERR_NO_ERROR;
}
mdv_error mdv_set_goal_speed(motordriver * mdv , int speed){
mdv->goal_speed = mdv_e2i_speed(mdv,speed);
mdv_set_flag(mdv,MDV_F_NEW_SPEED_AVAILABLE);
- return MDV_ERR_NO_ERROR;
+ return MDV_ERR_NO_ERROR;
}
//mdv_error mdv_set_goal_torque(motordriver * mdv, mdv_torque_t torque);
int mdv_get_goal_position(motordriver * mdv){
return mdv_i2e_position(mdv,mdv->goal_pos);
}
int mdv_get_goal_speed(motordriver * mdv){
return mdv_i2e_speed(mdv,mdv->goal_speed);
}
//mdv_torque_t mdv_get_goal_torque(motordriver * mdv);
int mdv_get_position(motordriver * mdv){
return mdv_i2e_position(mdv,mdv->pos_cnt);
}
int mdv_get_speed(motordriver * mdv){
return mdv_i2e_speed(mdv,mdv_lo2hi_res_speed(mdv->current_speed));
}
int mdv_get_targeted_torque(motordriver * mdv){
return mdv_i2e_torque(mdv,mdv->output_trq);
}
int mdv_get_current(motordriver * mdv){
return 0;
}
int mdv_get_virtual_goal_position(motordriver * mdv){
return mdv_i2e_position(mdv,mdv->moving_pos);
}
int mdv_get_virtual_goal_speed(motordriver * mdv){
return mdv_i2e_speed(mdv,mdv->moving_speed);
}
mdv_parameters * mdv_get_parameters(motordriver * mdv){
return &(mdv->params);
}
int mdv_get_maximal_acceleration(motordriver * mdv){
return mdv_i2e_accel(mdv,mdv->max_accel);
}
int mdv_get_maximal_speed(motordriver * mdv){
return mdv_i2e_speed(mdv,mdv->max_speed);
}
int mdv_get_maximal_torque(motordriver * mdv){
return mdv_i2e_torque(mdv,mdv->max_trq);
}
mdv_error mdv_set_maximal_acceleration(motordriver * mdv, int value){
mdv->max_accel = mdv_e2i_accel(mdv,max(0,value));
- return MDV_ERR_NO_ERROR;
+ return MDV_ERR_NO_ERROR;
}
mdv_error mdv_set_maximal_speed(motordriver * mdv, int value){
- mdv->max_speed = mdv_e2i_speed(mdv,max(0,value));
- return MDV_ERR_NO_ERROR;
+ mdv->max_speed = mdv_e2i_speed(mdv,max((int)0,value));
+ return MDV_ERR_NO_ERROR;
}
mdv_error mdv_set_maximal_torque(motordriver * mdv, int value){
- mdv->max_trq = mdv_e2i_torque(mdv,max(0,value));
- return MDV_ERR_NO_ERROR;
+ mdv->max_trq = mdv_e2i_torque(mdv,max((int)0,value));
+ return MDV_ERR_NO_ERROR;
}
mdv_error mdv_set_gear_ratio(motordriver * mdv, int gear_mult, int gear_div){
mdv->gear_mult = GEAR_MULT_COEFF * gear_mult;
mdv->gear_div = GEAR_DIV_COEFF * gear_div;
- return MDV_ERR_NO_ERROR;
+ return MDV_ERR_NO_ERROR;
}
int mdv_get_gear_mult(motordriver * mdv){
return mdv->gear_mult / GEAR_MULT_COEFF;
}
int mdv_get_gear_div(motordriver * mdv){
return mdv->gear_div / GEAR_DIV_COEFF;
}
int mdv_get_limit_position(motordriver * mdv){
return mdv_i2e_position(mdv,mdv->pos_limit);
}
mdv_error mdv_calibrate_motor(motordriver * mdv,
unsigned int limit_pos,
unsigned int min_calibration_region,
unsigned int max_calibration_region,
int calibration_offset,
pos_evaluator_fptr position_evaluator,
unsigned int pos_evaluator_max_value,
uint8_t statically_calibrated){
/* only calibrate once */
if(mdv->sc.status & MDV_RUNTIME_CALIBRATED){
return MDV_ERR_ALREADY_RUNTIME_CALIBRATED;
}
if(! statically_calibrated){
- mdv->pos_limit = mdv_e2i_position(mdv,limit_pos);
- mdv_set_pos_count(mdv,mdv->pos_limit / 2);
- mdv_set_control_mode(mdv, MDV_M_COAST);
-
-
- return MDV_ERR_NO_ERROR;
+ return MDV_ERR_MOTOR_NOT_MANUFACTURER_CALIBRATED;
}
if(position_evaluator == 0x00){
return MDV_ERR_BAD_PARAM;
}
mdv->calib.pos_limit = limit_pos;
mdv->calib.min_region = clamp(min_calibration_region , 0 , ANGULAR_RESOLUTION);
mdv->calib.max_region = clamp(max_calibration_region , 0 , ANGULAR_RESOLUTION);
mdv->calib.offset = clamp_absolute(calibration_offset,
mdv_e2i_position(mdv,ANGULAR_RESOLUTION));
mdv->calib.pos_evaluator = position_evaluator;
mdv->calib.pos_evaluator_max_value = pos_evaluator_max_value;
mdv_set_control_mode_private(mdv,MDV_M_RUNTIME_CALIBRATION);
- return MDV_ERR_NO_ERROR;
+ return MDV_ERR_NO_ERROR;
}
void mdv_load_persistent_sbcp_settings(){
mdv_set_gear_ratio(mdv1,
sbcp_reg_table[MDV_M1 + MX_GEAR_MULT].i,
sbcp_reg_table[MDV_M1 + MX_GEAR_DIV].i);
mdv_set_gear_ratio(mdv2,
sbcp_reg_table[MDV_M2 + MX_GEAR_MULT].i,
sbcp_reg_table[MDV_M2 + MX_GEAR_DIV].i);
}
+
+
+
+void mdv_uncalibrate_motor(motordriver * mdv){
+
+ mdv->sc.status &= ~(MDV_RUNTIME_CALIBRATED);
+ mdv->pos_limit = 0;
+ mdv_set_pos_count(mdv,0);
+ mdv_set_control_mode_private(mdv,MDV_M_COAST);
+
+}
+
+
+mdv_error mdv_set_smooth_interval(motordriver* mdv, int ts){
+ mdv->params.smooth_interval = ts;
+ return MDV_ERR_NO_ERROR;
+}
\ No newline at end of file
diff --git a/src/motordriver.h b/src/motordriver.h
index 62fc96a..f1e3481 100644
--- a/src/motordriver.h
+++ b/src/motordriver.h
@@ -1,228 +1,234 @@
/*
* File: motordriver.h
* Author: tuleu
*
* Created on August 22, 2012, 3:43 PM
*/
#ifndef MOTORDRIVER_H_
#define MOTORDRIVER_H_
#include <gpio.h>
#include <register.h>
#include "pwm.h"
// number of counts per rotation
#define ANGULAR_RESOLUTION 4096
// maximum torque output value
#define TORQUE_RESOLUTION 4096
// spring force scaling: for a given SPRING_FORCE, maximum torque output is reached after TORQUE_PER_ROTATION_SCALING/SPRING_FORCE rotations
#define TORQUE_PER_ROTATION_SCALING 16
// since we have really low resolution speed
// in order to improve movement resolution, we multiply by a multiple of two
// the speed. this is the power of the multiplication. aim is to improve the precision
#define SPEED_BINARY_PRECISION 4
// common constants and macro's
#define MAX_PID_RANGE 32767
// communication protocol parameters
// Maximum counter value of HW PWM counter
#define MAX_PWM (FCY/FPWM-1)
#define MAX_DUTY (2*FCY/FPWM-1)
///\todo these macro as parameters
#define MDV_INERTIA_COMP 300
#define MDV_SLOWFAST_COMP 200
#define MDV_SLOWFAST_THRES 220
#define SMOOTH_POSITION_OVERSHOOT 0
enum mdv_status {
- MDV_IDLE = 0,
- MDV_MOVING = 1 << 0,
- MDV_RUNTIME_CALIBRATED = 1 << 1,
- MDV_ERROR = 1 << 2
+ MDV_IDLE = 0,
+ MDV_MOVING = 1 << 0,
+ MDV_RUNTIME_CALIBRATED = 1 << 1,
+ MDV_ERROR = 1 << 2
};
typedef enum mdv_status mdv_status;
typedef enum mdv_control_mode {
- MDV_M_COAST = 0,
- MDV_M_BRAKE = 1,
- MDV_M_POSITION_CONTROL = 2,
- MDV_M_VELOCITY_CONTROL = 3,
- MDV_M_SMOOTH_POSITION_CONTROL = 4,
- MDV_M_TORQUE_CONTROL = 5,
- MDV_M_RUNTIME_CALIBRATION = 6,
-
- MDV_NUMBER_OF_CONTROL_MODE
+ MDV_M_COAST = 0,
+ MDV_M_BRAKE = 1,
+ MDV_M_POSITION_CONTROL = 2,
+ MDV_M_VELOCITY_CONTROL = 3,
+ MDV_M_SMOOTH_POSITION_CONTROL = 4,
+ MDV_M_TORQUE_CONTROL = 5,
+ MDV_M_RUNTIME_CALIBRATION = 6,
+ MDV_M_SINE_GENERATOR = 7,
+ MDV_NUMBER_OF_CONTROL_MODE
} mdv_control_mode;
union mdv_status_and_control{
- struct {
- mdv_control_mode mode;
- mdv_status status;
- };
- unsigned int uint;
- sbcp_reg_val reg_val;
+ struct {
+ uint8_t mode;
+ uint8_t status;
+ };
+ unsigned int uint;
+ sbcp_reg_val reg_val;
};
typedef union mdv_status_and_control mdv_status_and_control;
typedef unsigned int mdv_pid_t;
typedef unsigned int mdv_stiffness_t;
typedef unsigned int mdv_damping_t;
typedef unsigned int mdv_timestep_t;
typedef int mdv_torque_t;
struct mdv_parameters {
mdv_pid_t p_gain;
mdv_pid_t i_gain;
mdv_pid_t d_gain;
mdv_stiffness_t stiffness;
mdv_damping_t damping;
mdv_torque_t preload;
mdv_timestep_t smooth_interval;
};
typedef struct mdv_parameters mdv_parameters;
struct mdv_pin {
- gpio brake;
- gpio coast;
- gpio dir;
- gpio mode;
- pwm_t torque;
- pwm_t speed;
+ gpio brake;
+ gpio coast;
+ gpio dir;
+ gpio mode;
+ pwm_t torque;
+ pwm_t speed;
};
typedef struct mdv_pin mdv_pin;
enum mdv_error {
- MDV_ERR_NO_ERROR = 0,
- MDV_ERR_UNKNOWN_CONTROL_MODE = 1,
- MDV_ERR_BAD_PARAM = 2,
- MDV_ERR_ALREADY_RUNTIME_CALIBRATED = 3,
- MDV_ERR_RESERVED_CONTROL_MODE = 4,
- MDV_ERR_IN_CALIBRATION = 5,
- MDV_ERR_NB_ERRORS = 6
+ MDV_ERR_NO_ERROR = 0,
+ MDV_ERR_UNKNOWN_CONTROL_MODE = 1,
+ MDV_ERR_BAD_PARAM = 2,
+ MDV_ERR_ALREADY_RUNTIME_CALIBRATED = 3,
+ MDV_ERR_RESERVED_CONTROL_MODE = 4,
+ MDV_ERR_IN_CALIBRATION = 5,
+ MDV_ERR_MOTOR_NOT_MANUFACTURER_CALIBRATED = 6,
+ MDV_ERR_MOTOR_NOT_RUNTIME_CALIBRATED = 7,
+ MDV_ERR_NB_ERRORS = 8
};
typedef enum mdv_error mdv_error;
struct motordriver;
typedef struct motordriver motordriver;
// motordrivers reset output
extern gpio mdv_master_reset;
#define mdv_master_reset_enable() do{ gpio_clear(mdv_master_reset);}while(0)
#define mdv_master_reset_disable() do{ gpio_set(mdv_master_reset);} while(0)
void init_motordrivers(mdv_pin * mdv1_pin, mdv_pin * mdv2_pin);
void init_motordriver(motordriver * mdv,
- mdv_pin * pin,
- sbcp_reg_address address_start,
- volatile unsigned int * pos_counter);
+ mdv_pin * pin,
+ sbcp_reg_address address_start,
+ volatile unsigned int * pos_counter);
void mdv_reset_count(motordriver *m);
void mdv_process(motordriver * mdv);
int mdv_get_status_and_control_mode(motordriver *m);
#define mdv_get_status(m) (mdv_get_status_and_control_mode(m).status)
int mdv_get_control_mode(motordriver * mdv);
mdv_error mdv_set_control_mode(motordriver * mdv, int ctrl);
mdv_error mdv_runtime_calibrate(motordriver * mdv, int actual_position);
mdv_error mdv_set_smooth_interval(motordriver * mdv, int ts);
int mdv_get_smooth_interval(motordriver * mdv);
mdv_error mdv_set_goal_position(motordriver * mdv, int position);
mdv_error mdv_set_goal_speed(motordriver * mdv , int speed);
//mdv_error mdv_set_goal_torque(motordriver * mdv, mdv_torque_t torque);
int mdv_get_goal_position(motordriver * mdv);
int mdv_get_goal_speed(motordriver * mdv);
//mdv_torque_t mdv_get_goal_torque(motordriver * mdv);
int mdv_get_position(motordriver * mdv);
int mdv_get_speed(motordriver * mdv);
int mdv_get_targeted_torque(motordriver * mdv);
int mdv_get_current(motordriver * mdv);
int mdv_get_virtual_goal_position(motordriver *m);
int mdv_get_virtual_goal_speed(motordriver *m);
mdv_parameters * mdv_get_parameters(motordriver * m);
int mdv_get_maximal_acceleration(motordriver * m);
int mdv_get_maximal_speed(motordriver * m);
int mdv_get_maximal_torque(motordriver * m);
mdv_error mdv_set_maximal_acceleration(motordriver * m, int value);
mdv_error mdv_set_maximal_speed(motordriver * m, int value);
mdv_error mdv_set_maximal_torque(motordriver * m, int value);
mdv_error mdv_set_gear_ratio(motordriver * m, int gear_mult, int gear_div);
int mdv_get_gear_mult(motordriver * m);
int mdv_get_gear_div(motordriver * m);
int mdv_get_limit_position(motordriver * m);
void mdv_load_persistent_sbcp_settings();
typedef int ( * pos_evaluator_fptr )( );
mdv_error mdv_calibrate_motor(motordriver * mdv,
unsigned int limit_pos,
unsigned int min_calibration_region,
unsigned int max_calibration_region,
int calibration_offset,
pos_evaluator_fptr pos_evaluator,
unsigned int pos_evaluator_max_value,
uint8_t statically_calibrated);
+
+/* never fails */
+void mdv_uncalibrate_motor(motordriver * mdv);
+
extern motordriver * mdv1, * mdv2;
#endif /* MOTORDRIVER_H */
diff --git a/src/pindefs.c b/src/pindefs.c
index 5f9b5be..71997db 100644
--- a/src/pindefs.c
+++ b/src/pindefs.c
@@ -1,85 +1,85 @@
#include "p33FJ128MC804.h"
#include "pindefs.h"
#include "motordriver.h"
void pin_init()
{
// pin default configuration
ADPCFG = 0xffff; // Set all Analog ports as Digital I/O
-// TRISA = 0xffff; // Everything is input
-// TRISB = 0xffff; // Everything is input
- mdv_master_reset = gpio_create(GPIO_PORT_B,GPIO_PIN_15,GPIO_OUTPUT);
+ // TRISA = 0xffff; // Everything is input
+ // TRISB = 0xffff; // Everything is input
+ mdv_master_reset = gpio_create(GPIO_PORT_B,GPIO_PIN_15,GPIO_OUTPUT);
// set motor 1 control pins to output
-// TRISCbits.TRISC3 = 0; // MODE1 is output
-// TRISBbits.TRISB11 = 0; // DIR1 is output
+ // TRISCbits.TRISC3 = 0; // MODE1 is output
+ // TRISBbits.TRISB11 = 0; // DIR1 is output
TRISBbits.TRISB14 = 0; // TRQ1 is output
TRISBbits.TRISB10 = 0; // SPD1 is output
-// TRISAbits.TRISA4 = 0; // COAST1 is output
-// TRISAbits.TRISA3 = 0; // BREAK1 is output
-// TRISAbits.TRISA0 = 1; // CSOUT1 is input (analog input)
-//
-// // set motor 2 control pins to output
-// TRISAbits.TRISA9 = 0; // MODE2 is output
-// TRISAbits.TRISA7 = 0; // DIR2 is output
+ // TRISAbits.TRISA4 = 0; // COAST1 is output
+ // TRISAbits.TRISA3 = 0; // BREAK1 is output
+ // TRISAbits.TRISA0 = 1; // CSOUT1 is input (analog input)
+ //
+ // // set motor 2 control pins to output
+ // TRISAbits.TRISA9 = 0; // MODE2 is output
+ // TRISAbits.TRISA7 = 0; // DIR2 is output
TRISCbits.TRISC6 = 0; // TRQ2 is output
TRISBbits.TRISB12 = 0; // SPD2 is output
-// TRISAbits.TRISA8 = 0; // COAST2 is output
-// TRISAbits.TRISA10 = 0; // BREAK2 is output
-// TRISAbits.TRISA1 = 1; // CSOUT2 is input (analog input)
+ // TRISAbits.TRISA8 = 0; // COAST2 is output
+ // TRISAbits.TRISA10 = 0; // BREAK2 is output
+ // TRISAbits.TRISA1 = 1; // CSOUT2 is input (analog input)
// set motors reset pin to output
mdv_master_reset_enable(); // set the motor drivers in reset-state
// set motors error feedback pin to input
TRISBbits.TRISB13 = 1; // FF2 is input
// quadrature encoder inputs
TRISCbits.TRISC4 = 1; // QEI1_A is input
TRISCbits.TRISC5 = 1; // QEI1_B is input
TRISBbits.TRISB6 = 1; // QEI2_A is input
TRISBbits.TRISB7 = 1; // QEI2_B is input
// RC servo pins
TRISBbits.TRISB0 = 0; // SERVO1 is output
TRISBbits.TRISB1 = 0; // SERVO2 is output
// SPI 1 interface: absolute magnetic encoders
TRISBbits.TRISB8 = 0; // SPI1_CLK is output
TRISBbits.TRISB4 = 0; // SPI1_CS is output
TRISBbits.TRISB9 = 1; // SPI1_DI is input
// SPI 2 interface: force/torque sensors
- TRISCbits.TRISC7 = 0; // SPI2_CLK is output
- TRISBbits.TRISB5 = 0; // SPI2_CS is output
- TRISCbits.TRISC8 = 1; // SPI2_DI is input
- TRISCbits.TRISC9 = 0; // SPI2_DO is output
- TRISBbits.TRISB2 = 0; // SPI2_SYNC is output
+ TRISBbits.TRISB5 = 0; // SPI2_CLK is output
+ TRISBbits.TRISB2 = 0; // SPI2_CS is output
+ TRISCbits.TRISC9 = 1; // SPI2_DI is input
+ TRISCbits.TRISC7 = 0; // SPI2_DO is output
+ TRISCbits.TRISC8 = 0; // SPI2_SYNC is output
// General Purpose led
TRISBbits.TRISB3 = 0; // LED is output
// RS485 interface pins
-// TRISCbits.TRISC2 = 0; // RS485 enable pin is output
-// TRISCbits.TRISC1 = 0; // RS485 transmit pin is output
-// TRISCbits.TRISC0 = 1; // RS485 receive pin is input
-// PORT_RS485_RECEIVER_DISABLE;
+ // TRISCbits.TRISC2 = 0; // RS485 enable pin is output
+ // TRISCbits.TRISC1 = 0; // RS485 transmit pin is output
+ // TRISCbits.TRISC0 = 1; // RS485 receive pin is input
+ // PORT_RS485_RECEIVER_DISABLE;
- //UART1 connects to RS485
+ //UART1 connects to RS485
-// RPINR18 = 0b0001111100000000; //tie U1CTS to Vss, tie UART1RX to RP0
-// RPOR0 = 0b0000001100000000; //tie UART1TX to RP1
+ // RPINR18 = 0b0001111100000000; //tie U1CTS to Vss, tie UART1RX to RP0
+ // RPOR0 = 0b0000001100000000; //tie UART1TX to RP1
- //SPI 1 configuration. It may already be configured, but better to repeat
+ //SPI 1 configuration. It may already be configured, but better to repeat
_SDI1R = 9; // Map SPI 1 DI to RP 9
_RP8R = 8; // Map RP8 to SPI 1 Clock Output
//SPI 2 configuration, it may already be configured, but better to repeat
- _SDI2R = 24; //Map SPI 2 DI to RP24
+ /*_SDI2R = 24; //Map SPI 2 DI to RP24
_RP23R = 11; //Map RP23 to SPI 2 Clock Output
- _RP25R = 10; //Map RP25 to SPI2 DO
+ _RP25R = 10; //Map RP25 to SPI2 DO*/
}
diff --git a/src/pindefs.h b/src/pindefs.h
index 84e1102..b6294fd 100644
--- a/src/pindefs.h
+++ b/src/pindefs.h
@@ -1,34 +1,40 @@
#ifndef PINDEFS_H
#define PINDEFS_H
#include <gpio.h>
// SPI 2 interface: force/torque sensors
//#define SPI2_CLK LATCbits.LATC7
//#define SPI2_CS LATBbits.LATB5
//#define SPI2_DI LATCbits.LATC8
//#define SPI2_DO LATCbits.LATC9
//#define SPI2_SYNC LATBbits.LATB2
// General Purpose led
+#define BLINK ((systime>>8)&0x01)
#define LED LATBbits.LATB3
#define LED_ENABLE (LED = 1)
#define LED_DISABLE (LED = 0)
#define LED_TOGGLE (LED = !LED)
+#define led_disable() do{ LED = 0; }while(0)
-// debug led
-#define LED_TX LATCbits.LATC1
-#define LED_TX_ENABLE (LED_TX = 0)
-#define LED_TX_DISABLE (LED_TX = 1)
-#define LED_TX_TOGGLE (LED_TX = !LED_TX)
+#define led_toggle() do{ LED = !LED; }while(0)
+
+
+// tx led
+#define LED_TX (LATCbits.LATC1)
+#define led_tx_enable() do{ LED_TX = 0; }while(0)
+
+#define led_tx_disable() do { LED_TX = 1; } while(0)
+#define led_tx_toggle() do { LED_TX = !LED_TX; } while(0)
void pin_init();
#endif // PINDEFS_H
diff --git a/src/pwm.c b/src/pwm.c
index c779f8c..d55c2dd 100644
--- a/src/pwm.c
+++ b/src/pwm.c
@@ -1,49 +1,49 @@
#include "pwm.h"
#include <gpio.h>
void pwm_init(pwm_t * pwm, pwm_device_t device){
- switch(device){
- case PWM_1_1 :
- pwm->DC = &(P1DC1);
- pwm->TPER = &(P1TPER);
- pwm->TCON = &(P1TCON);
- pwm->CON1 = &(PWM1CON1);
- pwm->pin = 0;
- break;
- case PWM_1_2 :
- pwm->DC = &(P1DC2);
- pwm->TPER = &(P1TPER);
- pwm->TCON = &(P1TCON);
- pwm->CON1 = &(PWM1CON1);
- pwm->pin = 1;
- break;
- case PWM_1_3 :
- pwm->DC = &(P1DC3);
- pwm->TPER = &(P1TPER);
- pwm->TCON = &(P1TCON);
- pwm->CON1 = &(PWM1CON1);
- pwm->pin = 2;
- break;
- case PWM_2_1 :
- pwm->DC = &(P2DC1);
- pwm->TPER = &(P2TPER);
- pwm->TCON = &(P2TCON);
- pwm->CON1 = &(PWM2CON1);
- pwm->pin = 0;
- break;
- default :
- pwm->DC = 0;
- pwm->TCON = 0;
- pwm->CON1 = 0;
- pwm->TPER = 0;
- pwm->pin = 0;
- break;
- }
- //enable the channel globally
- atomic_or(pwm->TCON , 1 << 15);
- //initialize the pin as an output !!!
- atomic_or(pwm->CON1, ( 1 << (pwm->pin + 4)) | (1 << (pwm->pin + 8)));
+ switch(device){
+ case PWM_1_1 :
+ pwm->DC = &(P1DC1);
+ pwm->TPER = &(P1TPER);
+ pwm->TCON = &(P1TCON);
+ pwm->CON1 = &(PWM1CON1);
+ pwm->pin = 0;
+ break;
+ case PWM_1_2 :
+ pwm->DC = &(P1DC2);
+ pwm->TPER = &(P1TPER);
+ pwm->TCON = &(P1TCON);
+ pwm->CON1 = &(PWM1CON1);
+ pwm->pin = 1;
+ break;
+ case PWM_1_3 :
+ pwm->DC = &(P1DC3);
+ pwm->TPER = &(P1TPER);
+ pwm->TCON = &(P1TCON);
+ pwm->CON1 = &(PWM1CON1);
+ pwm->pin = 2;
+ break;
+ case PWM_2_1 :
+ pwm->DC = &(P2DC1);
+ pwm->TPER = &(P2TPER);
+ pwm->TCON = &(P2TCON);
+ pwm->CON1 = &(PWM2CON1);
+ pwm->pin = 0;
+ break;
+ default :
+ pwm->DC = 0;
+ pwm->TCON = 0;
+ pwm->CON1 = 0;
+ pwm->TPER = 0;
+ pwm->pin = 0;
+ break;
+ }
+ //enable the channel globally
+ atomic_or(pwm->TCON , 1 << 15);
+ //initialize the pin as an output !!!
+ atomic_or(pwm->CON1, ( 1 << (pwm->pin + 4)) | (1 << (pwm->pin + 8)));
}
diff --git a/src/pwm.h b/src/pwm.h
index 288c020..efadf4e 100644
--- a/src/pwm.h
+++ b/src/pwm.h
@@ -1,51 +1,53 @@
/*
* File: pwm.h
* Author: tuleu
*
* Created on September 13, 2012, 2:24 PM
*/
#ifndef PWM_H
#define PWM_H
#include <p33Fxxxx.h>
enum pwm_device_t {
- PWM_1_1,
- PWM_1_2,
- PWM_1_3,
- PWM_2_1
+ PWM_1_1,
+ PWM_1_2,
+ PWM_1_3,
+ PWM_2_1
};
typedef enum pwm_device_t pwm_device_t;
struct pwm_t {
- volatile unsigned int * DC;
- volatile unsigned int * TCON;
- volatile unsigned int * CON1;
- volatile unsigned int * TPER;
- unsigned char pin;
+ volatile unsigned int * DC;
+ volatile unsigned int * TCON;
+ volatile unsigned int * CON1;
+ volatile unsigned int * TPER;
+ unsigned char pin;
};
typedef struct pwm_t pwm_t;
void pwm_init(pwm_t * pwm, pwm_device_t device);
-#define pwm_set_cycle(pwm,value) do{\
-*(pwm.TPER) = value;\
-}while(0)
+#define pwm_set_cycle(pwm,value) \
+ do{ \
+ *(pwm.TPER) = value; \
+ }while(0)
-#define pwm_set_value(pwm,value) do{\
-*(pwm.DC) = value;\
-}while(0)
+#define pwm_set_value(pwm,value) \
+ do{ \
+ *(pwm.DC) = value; \
+ }while(0)
#endif /* PWM_H */
diff --git a/src/sbcp_mdv.c b/src/sbcp_mdv.c
index 2aec0de..c6c4ca5 100644
--- a/src/sbcp_mdv.c
+++ b/src/sbcp_mdv.c
@@ -1,392 +1,419 @@
#include "sbcp_mdv.h"
#include "motordriver.h"
#include "magnetic_encoder.h"
#include "load_cell.h"
#include <packet.h>
+#include "misc_math.h"
/*******************************************************************************
* C A L L B A C K S
*******************************************************************************/
typedef mdv_error (*set_param_fptr)(motordriver * mdv, int value);
typedef int (*get_param_fptr)(motordriver * mdv);
struct sbcp_mdv_setter_and_getter {
set_param_fptr setter;
get_param_fptr getter;
};
struct sbcp_mdv_setter_and_getter mdv_gAs[MX_REGISTER_SIZE];
#ifndef NDEBUG
enum sbcp_mdv_communication_error{
SBCP_MDV_CERR_NOT_A_MDV_REGISTER = MDV_ERR_NB_ERRORS, //6
SBCP_MDV_CERR_GETTER_UNSET, //7
SBCP_MDV_CERR_SETTER_UNSET // 8
};
#endif
sbcp_error sbcp_mdv_generic_clbk(sbcp_reg_address address, sbcp_reg_val * value){
#ifndef NDEBUG
if(address < MDV_M1 || address > MDV_M2 + MX_REGISTER_SIZE){
return SBCP_MDV_CERR_NOT_A_MDV_REGISTER;
}
#endif
enum sbcp_mdv_mX_register reg = address < MDV_M2 ? address - MDV_M1 : address - MDV_M2;
motordriver * mdv = address < MDV_M2 ? mdv1 : mdv2;
struct sbcp_mdv_setter_and_getter gAs = mdv_gAs[reg];
#ifndef NDEBUG
if(gAs.setter == 0x00){
return SBCP_MDV_CERR_SETTER_UNSET;
}
if(gAs.getter == 0x00){
return SBCP_MDV_CERR_GETTER_UNSET;
}
#endif
sbcp_error err = (*(gAs.setter))(mdv,value->i);
value->i = (*(gAs.getter))(mdv);
return err;
}
sbcp_error sbcp_mdv_generic_param_clbk(sbcp_reg_address address, sbcp_reg_val * value){
///\todo add min max value
mdv_parameters * params = (address < MDV_M2) ? mdv_get_parameters(mdv1) : mdv_get_parameters(mdv2);
switch( address < MDV_M2 ? address - MDV_M1 : address - MDV_M2){
case MX_PID_PGAIN :
params->p_gain = value->u;
break;
case MX_PID_IGAIN :
params->i_gain = value->u;
break;
case MX_PID_DGAIN :
params->d_gain = value->u;
break;
case MX_COMPLIANCE_PRELOAD :
params->preload = value->i;
break;
case MX_COMPLIANCE_STIFFNESS :
params->stiffness = value->u;
break;
case MX_COMPLIANCE_DAMPING :
params->damping = value->u;
break;
case MX_SMOOTH_POSITION_UPDATE :
params->smooth_interval = value->u;
break;
default :
return 42;
}
return SBCP_NO_ERROR;
}
sbcp_error sbcp_mdv_gear_clbk(sbcp_reg_address address, sbcp_reg_val * value){
motordriver * mdv = address < MDV_M2 ? mdv1 : mdv2;
enum sbcp_mdv_mX_register reg = address < MDV_M2 ? address - MDV_M1 : address - MDV_M2;
//get the gear either from the table, or the new value if this the one being set
int gear_mult = reg == MX_GEAR_MULT ? value->i : sbcp_reg_int(address);
int gear_div = reg == MX_GEAR_DIV ? value->i : sbcp_reg_int(address);
return mdv_set_gear_ratio(mdv,gear_mult,gear_div);
}
void init_sbcp_register_for_mdv(motordriver * mdv , sbcp_reg_address address_start){
#ifndef NDEBUG
unsigned int i;
for(i = 0; i < MX_REGISTER_SIZE; ++i){
mdv_gAs[i].setter = (set_param_fptr) 0x00;
mdv_gAs[i].getter = (get_param_fptr) 0x00;
}
#endif
- mdv_gAs[MX_STATUS_AND_CONTROL_MODE].setter = & mdv_set_control_mode;
- mdv_gAs[MX_STATUS_AND_CONTROL_MODE].getter = & mdv_get_control_mode;
+ mdv_gAs[MX_STATUS_AND_CONTROL_MODE].setter = & mdv_set_control_mode;
+ mdv_gAs[MX_STATUS_AND_CONTROL_MODE].getter = & mdv_get_control_mode;
mdv_gAs[MX_GOAL_POSITION].setter = &mdv_set_goal_position;
mdv_gAs[MX_GOAL_POSITION].getter = &mdv_get_goal_position;
mdv_gAs[MX_GOAL_SPEED].setter = &mdv_set_goal_speed;
mdv_gAs[MX_GOAL_SPEED].getter = &mdv_get_goal_speed;
mdv_gAs[MX_MAX_ACCELERATION].setter = &mdv_set_maximal_acceleration;
mdv_gAs[MX_MAX_ACCELERATION].getter = &mdv_get_maximal_acceleration;
mdv_gAs[MX_MAX_SPEED].setter = &mdv_set_maximal_speed;
mdv_gAs[MX_MAX_SPEED].getter = &mdv_get_maximal_speed;
mdv_gAs[MX_MAX_TORQUE].setter = &mdv_set_maximal_torque;
mdv_gAs[MX_MAX_TORQUE].getter = &mdv_get_maximal_torque;
sbcp_add_register(address_start + MX_STATUS_AND_CONTROL_MODE,
mdv_get_status_and_control_mode(mdv),
SBCP_REG_READABLE | SBCP_REG_WRITABLE,
&sbcp_mdv_generic_clbk);
sbcp_add_register(address_start + MX_GOAL_POSITION,
mdv_get_goal_position(mdv),
SBCP_REG_READABLE | SBCP_REG_WRITABLE,
&sbcp_mdv_generic_clbk);
sbcp_add_register(address_start + MX_TARGETED_POSITION,
mdv_get_virtual_goal_position(mdv),
SBCP_REG_READABLE ,
SBCP_REG_NO_CALLBACK);
sbcp_add_register(address_start + MX_PRESENT_POSITION,
mdv_get_position(mdv),
SBCP_REG_READABLE ,
SBCP_REG_NO_CALLBACK);
sbcp_add_register(address_start + MX_GOAL_SPEED,
mdv_get_goal_speed(mdv),
SBCP_REG_READABLE | SBCP_REG_WRITABLE,
&sbcp_mdv_generic_clbk);
sbcp_add_register(address_start + MX_TARGETED_SPEED,
mdv_get_virtual_goal_speed(mdv),
SBCP_REG_READABLE ,
SBCP_REG_NO_CALLBACK);
sbcp_add_register(address_start + MX_PRESENT_SPEED,
mdv_get_speed(mdv),
SBCP_REG_READABLE ,
SBCP_REG_NO_CALLBACK);
sbcp_add_register(address_start + MX_TARGETED_TORQUE,
mdv_get_targeted_torque(mdv),
SBCP_REG_READABLE ,
SBCP_REG_NO_CALLBACK);
sbcp_add_register(address_start + MX_PID_PGAIN,
mdv_get_parameters(mdv)->p_gain,
SBCP_REG_READABLE | SBCP_REG_WRITABLE,
&sbcp_mdv_generic_param_clbk);
sbcp_add_register(address_start + MX_PID_IGAIN,
mdv_get_parameters(mdv)->i_gain,
SBCP_REG_READABLE | SBCP_REG_WRITABLE,
&sbcp_mdv_generic_param_clbk);
sbcp_add_register(address_start + MX_PID_DGAIN,
mdv_get_parameters(mdv)->d_gain,
SBCP_REG_READABLE | SBCP_REG_WRITABLE,
&sbcp_mdv_generic_param_clbk);
- sbcp_add_register(address_start + MX_COMPLIANCE_PRELOAD,
+ sbcp_add_register(address_start + MX_COMPLIANCE_PRELOAD,
mdv_get_parameters(mdv)->preload,
SBCP_REG_READABLE | SBCP_REG_WRITABLE,
&sbcp_mdv_generic_param_clbk);
sbcp_add_register(address_start + MX_COMPLIANCE_STIFFNESS,
mdv_get_parameters(mdv)->stiffness,
SBCP_REG_READABLE | SBCP_REG_WRITABLE,
&sbcp_mdv_generic_param_clbk);
sbcp_add_register(address_start + MX_COMPLIANCE_DAMPING,
mdv_get_parameters(mdv)->damping,
SBCP_REG_READABLE | SBCP_REG_WRITABLE,
&sbcp_mdv_generic_param_clbk);
sbcp_add_register(address_start + MX_MAX_ACCELERATION,
mdv_get_maximal_acceleration(mdv),
SBCP_REG_READABLE | SBCP_REG_WRITABLE,
&sbcp_mdv_generic_clbk);
sbcp_add_register(address_start + MX_MAX_SPEED,
mdv_get_maximal_speed(mdv),
SBCP_REG_READABLE | SBCP_REG_WRITABLE,
&sbcp_mdv_generic_clbk);
sbcp_add_register(address_start + MX_MAX_TORQUE,
mdv_get_maximal_torque(mdv),
SBCP_REG_READABLE | SBCP_REG_WRITABLE,
&sbcp_mdv_generic_clbk);
sbcp_add_register(address_start + MX_GEAR_MULT,
mdv_get_gear_mult(mdv),
SBCP_REG_READABLE | SBCP_REG_WRITABLE | SBCP_REG_PERSISTENT,
&sbcp_mdv_gear_clbk);
sbcp_add_register(address_start + MX_GEAR_DIV,
mdv_get_gear_div(mdv),
SBCP_REG_READABLE | SBCP_REG_WRITABLE | SBCP_REG_PERSISTENT,
&sbcp_mdv_gear_clbk);
sbcp_add_register(address_start + MX_LIMIT_POS_CW,
ANGULAR_RESOLUTION - 1,
SBCP_REG_READABLE | SBCP_REG_WRITABLE | SBCP_REG_PERSISTENT,
SBCP_REG_NO_CALLBACK);
sbcp_add_register(address_start + MX_CALIBRATION_REGION_MIN,
0,
SBCP_REG_READABLE | SBCP_REG_WRITABLE | SBCP_REG_PERSISTENT,
SBCP_REG_NO_CALLBACK);
sbcp_add_register(address_start + MX_CALIBRATION_REGION_MAX,
ANGULAR_RESOLUTION - 1,
SBCP_REG_READABLE | SBCP_REG_WRITABLE | SBCP_REG_PERSISTENT,
SBCP_REG_NO_CALLBACK);
sbcp_add_register(address_start + MX_CALIBRATION_OFFSET,
0,
SBCP_REG_READABLE | SBCP_REG_WRITABLE | SBCP_REG_PERSISTENT,
SBCP_REG_NO_CALLBACK);
sbcp_add_register(address_start + MX_SMOOTH_POSITION_UPDATE,
mdv_get_parameters(mdv)->smooth_interval,
SBCP_REG_READABLE | SBCP_REG_WRITABLE,
&sbcp_mdv_generic_param_clbk);
}
sbcp_error magnetic_encoder_gear_clbk(sbcp_reg_address addr, sbcp_reg_val * value){
magnetic_encoder * q = 0;
if(addr >= MDV_ME_Q1 && addr < MDV_ME_Q2){
q = me1;
addr -= MDV_ME_Q1;
} else if ( addr < MDV_ME_Q3) {
q = me2;
addr -= MDV_ME_Q2;
} else if ( addr < MDV_ME_Q3 + MEX_REGISTER_SIZE){
q = me3;
addr -= MDV_ME_Q3;
} else {
/** \todo replace this 42 */
return 42;
}
int gear_mult = addr == MEX_GEAR_MULT ? value->i : me_get_gear_mult(q);
int gear_div = addr == MEX_GEAR_DIV ? value->i : me_get_gear_div(q);
me_set_gear_ratio(q,gear_mult,gear_div);
return SBCP_NO_ERROR;
}
void init_sbcp_register_for_magnetic_encoder(magnetic_encoder * q, sbcp_reg_address start){
sbcp_add_register(start + MEX_POSITION,
me_get_value(q),
SBCP_REG_READABLE,
SBCP_REG_NO_CALLBACK);
sbcp_add_register(start + MEX_GEAR_MULT,
me_get_gear_mult(q),
SBCP_REG_READABLE | SBCP_REG_WRITABLE | SBCP_REG_PERSISTENT,
&magnetic_encoder_gear_clbk);
sbcp_add_register(start + MEX_GEAR_DIV,
me_get_gear_div(q),
SBCP_REG_READABLE | SBCP_REG_WRITABLE | SBCP_REG_PERSISTENT,
& magnetic_encoder_gear_clbk);
+
+
}
+void init_sbcp_register_for_sine(sbcp_reg_address start, int defaultAmplitude,
+ int defaultOffset,
+ int defaultFrequency){
+ sbcp_add_register_no_callback(start + SINE_FREQUENCY, defaultFrequency, SBCP_REG_READABLE | SBCP_REG_WRITABLE);
+ sbcp_add_register_no_callback(start + SINE_OFFSET, defaultOffset, SBCP_REG_READABLE | SBCP_REG_WRITABLE);
+ sbcp_add_register_no_callback(start + SINE_AMPLITUDE, defaultAmplitude, SBCP_REG_READABLE | SBCP_REG_WRITABLE);
+}
void init_sbcp_register_for_load_cell(load_cell * q, sbcp_reg_address start){
sbcp_add_register(start + LCX_TORQUE,
lc_get_torque(q),
SBCP_REG_READABLE,
SBCP_REG_NO_CALLBACK);
}
void init_sbcp_mdv_registers(){
init_sbcp_register_for_magnetic_encoder(me1,MDV_ME_Q1);
init_sbcp_register_for_magnetic_encoder(me2,MDV_ME_Q2);
init_sbcp_register_for_magnetic_encoder(me3,MDV_ME_Q3);
init_sbcp_register_for_load_cell(lc1,MDV_LC_AXIS_1);
init_sbcp_register_for_load_cell(lc2,MDV_LC_AXIS_2);
init_sbcp_register_for_load_cell(lc3,MDV_LC_AXIS_3);
init_sbcp_register_for_mdv(mdv1,MDV_M1);
init_sbcp_register_for_mdv(mdv2,MDV_M2);
sbcp_append_low_latency_in_register(MDV_M1 + MX_GOAL_POSITION);
sbcp_append_low_latency_in_register(MDV_M2 + MX_GOAL_POSITION);
- sbcp_append_low_latency_out_register(MDV_M1 + MX_PRESENT_POSITION);
- sbcp_append_low_latency_out_register(MDV_M1 + MX_TARGETED_TORQUE);
+ sbcp_append_low_latency_out_register(MDV_M1 + MX_GOAL_SPEED);
+ sbcp_append_low_latency_out_register(MDV_M2 + MX_GOAL_SPEED);
+ sbcp_append_low_latency_out_register(MDV_M1 + MX_PRESENT_POSITION);
sbcp_append_low_latency_out_register(MDV_M2 + MX_PRESENT_POSITION);
+
+ sbcp_append_low_latency_out_register(MDV_M1 + MX_PRESENT_SPEED);
+ sbcp_append_low_latency_out_register(MDV_M2 + MX_PRESENT_SPEED);
+
+ sbcp_append_low_latency_out_register(MDV_M1 + MX_TARGETED_TORQUE);
sbcp_append_low_latency_out_register(MDV_M2 + MX_TARGETED_TORQUE);
- sbcp_append_low_latency_out_register(MDV_ME_Q1 + MEX_POSITION);
+ sbcp_append_low_latency_out_register(MDV_ME_Q1 + MEX_POSITION);
sbcp_append_low_latency_out_register(MDV_ME_Q2 + MEX_POSITION);
sbcp_append_low_latency_out_register(MDV_ME_Q3 + MEX_POSITION);
sbcp_append_low_latency_out_register(MDV_LC_AXIS_1 + LCX_TORQUE);
sbcp_append_low_latency_out_register(MDV_LC_AXIS_2 + LCX_TORQUE);
sbcp_append_low_latency_out_register(MDV_LC_AXIS_3 + LCX_TORQUE);
}
int get_me1(){
return me_get_value(me1);
}
int get_me2(){
- return me_get_value(me1);
+ return me_get_value(me2);
}
-void sbcp_calibrate_motor(unsigned int * b){
+void sbcp_calibrate_motors(unsigned int * b){
sbcp_error err ;
err = mdv_calibrate_motor(mdv1,
sbcp_reg_table[MDV_M1 + MX_LIMIT_POS_CW].u & 0x7fff,
sbcp_reg_table[MDV_M1 + MX_CALIBRATION_REGION_MIN].u,
sbcp_reg_table[MDV_M1 + MX_CALIBRATION_REGION_MAX].u,
sbcp_reg_table[MDV_M1 + MX_CALIBRATION_OFFSET].i,
&get_me1,
- me_get_gear_mult(me1)* (ANGULAR_RESOLUTION - 1) / me_get_gear_div(me1),
+ abs((long)me_get_gear_mult(me1)* ((long)ANGULAR_RESOLUTION ) / (long)me_get_gear_div(me1)),
(sbcp_reg_table[MDV_M1 + MX_LIMIT_POS_CW].u & 0x8000) >> 15);
if(err != MDV_ERR_NO_ERROR){
sbcp_prepare_r_packet(0,err);
sbcp_mark_r_packet_ready();
return;
}
err = mdv_calibrate_motor(mdv2,
sbcp_reg_table[MDV_M2 + MX_LIMIT_POS_CW].u & 0x0fff,
sbcp_reg_table[MDV_M2 + MX_CALIBRATION_REGION_MIN].u,
sbcp_reg_table[MDV_M2 + MX_CALIBRATION_REGION_MAX].u,
sbcp_reg_table[MDV_M2 + MX_CALIBRATION_OFFSET].i,
&get_me2,
- me_get_gear_mult(me2)* (ANGULAR_RESOLUTION - 1) / me_get_gear_div(me2),
+ abs((long)me_get_gear_mult(me2)* ((long)ANGULAR_RESOLUTION) / (long)me_get_gear_div(me2)),
(sbcp_reg_table[MDV_M2 + MX_LIMIT_POS_CW].u & 0x8000) >> 15);
sbcp_prepare_r_packet(0,err);
sbcp_mark_r_packet_ready();
}
+void sbcp_uncalibrate_motors(){
+ mdv_uncalibrate_motor(mdv1);
+ mdv_uncalibrate_motor(mdv2);
+
+ sbcp_prepare_r_packet(0,SBCP_NO_ERROR);
+ sbcp_mark_r_packet_ready();
+}
+
void init_sbcp_mdv_instructions(){
sbcp_add_instruction(SBCP_MDV_CALIBRATE_MOTORS,
- &sbcp_calibrate_motor);
+ &sbcp_calibrate_motors);
+
+ sbcp_add_instruction(SBCP_MDV_UNCALIBRATE_MOTORS,
+ &sbcp_uncalibrate_motors);
}
diff --git a/src/sbcp_mdv.h b/src/sbcp_mdv.h
index aaf8bbd..48bacfe 100644
--- a/src/sbcp_mdv.h
+++ b/src/sbcp_mdv.h
@@ -1,101 +1,114 @@
/*
* File: sbcp_mdv.h
* Author: tuleu
*
* Created on September 12, 2012, 6:09 PM
*/
#ifndef SBCP_MDV_H_
#define SBCP_MDV_H_
#include <sbcp.h>
enum sbcp_mdv_mX_register {
MX_STATUS_AND_CONTROL_MODE = 0,
- MX_GOAL_POSITION =1,
- MX_TARGETED_POSITION = 2,
- MX_PRESENT_POSITION = 3,
+ MX_GOAL_POSITION = 1,
+ MX_TARGETED_POSITION = 2,
+ MX_PRESENT_POSITION = 3,
- MX_GOAL_SPEED = 4,
- MX_TARGETED_SPEED = 5,
- MX_PRESENT_SPEED = 6,
+ MX_GOAL_SPEED = 4,
+ MX_TARGETED_SPEED = 5,
+ MX_PRESENT_SPEED = 6,
- MX_TARGETED_TORQUE = 7,
+ MX_TARGETED_TORQUE = 7,
- MX_PID_PGAIN = 8,
- MX_PID_IGAIN = 9,
- MX_PID_DGAIN = 10,
+ MX_PID_PGAIN = 8,
+ MX_PID_IGAIN = 9,
+ MX_PID_DGAIN = 10,
- MX_COMPLIANCE_PRELOAD = 11,
- MX_COMPLIANCE_STIFFNESS = 12,
- MX_COMPLIANCE_DAMPING = 13,
- MX_MAX_ACCELERATION = 14,
- MX_MAX_SPEED = 15,
- MX_MAX_TORQUE = 16,
+ MX_COMPLIANCE_PRELOAD = 11,
+ MX_COMPLIANCE_STIFFNESS = 12,
+ MX_COMPLIANCE_DAMPING = 13,
+ MX_MAX_ACCELERATION = 14,
+ MX_MAX_SPEED = 15,
+ MX_MAX_TORQUE = 16,
- MX_GEAR_MULT = 17,
- MX_GEAR_DIV = 18,
- MX_LIMIT_POS_CW = 19,
- MX_CALIBRATION_REGION_MIN = 20,
- MX_CALIBRATION_REGION_MAX = 21,
- MX_CALIBRATION_OFFSET = 22,
+ MX_GEAR_MULT = 17,
+ MX_GEAR_DIV = 18,
- MX_SMOOTH_POSITION_UPDATE = 23,
+ MX_LIMIT_POS_CW = 19,
+ MX_CALIBRATION_REGION_MIN = 20,
+ MX_CALIBRATION_REGION_MAX = 21,
+ MX_CALIBRATION_OFFSET = 22,
+ MX_SMOOTH_POSITION_UPDATE = 23,
- MX_REGISTER_SIZE = 24
+
+ MX_REGISTER_SIZE = 24
};
typedef enum sbcp_mdv_mx_register sbcp_mdv_mX_register;
enum sbcp_mdv_meX_register {
- MEX_POSITION = 0,
- MEX_GEAR_MULT = 1,
- MEX_GEAR_DIV = 2,
+ MEX_POSITION = 0,
+ MEX_GEAR_MULT = 1,
+ MEX_GEAR_DIV = 2,
MEX_REGISTER_SIZE = 3
};
enum sbcp_mdv_lcX_register {
LCX_TORQUE = 0,
LCX_REGISTER_SIZE = 1
};
+enum sbcp_mdv_Sine_register {
+ SINE_FREQUENCY = 0,
+ SINE_AMPLITUDE = 1,
+ SINE_OFFSET = 2,
+ SINE_REGISTER_SIZE
+};
+
enum sbcp_mdv_register {
- MDV_ME_Q1 = SBCP_SPECIFIC_REG_START, //3
+ MDV_ME_Q1 = SBCP_SPECIFIC_REG_START, // 3
- MDV_ME_Q2 = MDV_ME_Q1 + MEX_REGISTER_SIZE, //6
+ MDV_ME_Q2 = MDV_ME_Q1 + MEX_REGISTER_SIZE, // 6
- MDV_ME_Q3 = MDV_ME_Q2 + MEX_REGISTER_SIZE, //9
+ MDV_ME_Q3 = MDV_ME_Q2 + MEX_REGISTER_SIZE, // 9
MDV_LC_AXIS_1 = MDV_ME_Q3 + MEX_REGISTER_SIZE, //12
MDV_LC_AXIS_2 = MDV_LC_AXIS_1 + LCX_REGISTER_SIZE, // 13
MDV_LC_AXIS_3 = MDV_LC_AXIS_2 + LCX_REGISTER_SIZE, // 14
- MDV_M1 = MDV_LC_AXIS_3 + LCX_REGISTER_SIZE, //15
- MDV_M2 = MDV_M1 + MX_REGISTER_SIZE //39
-
+ MDV_M1 = MDV_LC_AXIS_3 + LCX_REGISTER_SIZE, // 15
+ MDV_M2 = MDV_M1 + MX_REGISTER_SIZE, // 39
+ MDV_SINE_1 = MDV_M2 + MX_REGISTER_SIZE, // 63
+ MDV_SINE_2 = MDV_SINE_1 + SINE_REGISTER_SIZE // 66
};
typedef enum sbcp_mdv_register sbcp_mdv_register;
enum sbcp_mdv_instruction {
- SBCP_MDV_CALIBRATE_MOTORS = SBCP_INST_DEVICE_SPECIFIC_START
+ SBCP_MDV_CALIBRATE_MOTORS = SBCP_INST_DEVICE_SPECIFIC_START,
+ SBCP_MDV_UNCALIBRATE_MOTORS = SBCP_INST_DEVICE_SPECIFIC_START + 1
};
+
+
+
#define sbcp_mdv_reg(mdv,reg) (sbcp_reg_table[mdv->address_start + reg])
#define sbcp_me_reg(me,reg) (sbcp_reg_table[me + reg])
#define sbcp_lc_reg(lc,reg) (sbcp_reg_table[lc + reg])
void init_sbcp_mdv_registers();
void init_sbcp_mdv_instructions();
#endif /* SBCP_MDV_H */
diff --git a/src/timer.c b/src/timer.c
index b8477df..5a905d8 100644
--- a/src/timer.c
+++ b/src/timer.c
@@ -1,72 +1,71 @@
-
#include "timer.h"
#include "p33FJ128MC804.h"
#include "config.h"
#define NTOTPULSE (FCY/FSYST -1)
#define NTOTPULSE_SBCP (FCY/FSBCPT -1)
#define NTOTPULSE_SPI_ME_READING_LATENCY (FCY/FSPI_ME_READING_LATENCY - 1)
volatile unsigned int systime;
void timer_init()
{
// Timer 1 is currently implemented in motorcontrol.c
/* Set timer 1 as main clock */
T1CONbits.TON = 0; // Disable Timer
T1CONbits.TCS = 0; // Select internal instruction cycle clock
T1CONbits.TGATE = 0; // Disable Gated Timer mode
#if NTOTPULSE < 65535
/* 1x prescaler */
PR1 = NTOTPULSE;
T1CONbits.TCKPS = 0b00; // Select 1:1 Prescaler
#elif NTOTPULSE < (8*65535)
/* 8x prescaler */
PR1 = NTOTPULSE/8;
T1CONbits.TCKPS = 0b01; // Select 1:8 Prescaler
#else
#error "FSYST - Check the values and maybe use a greater prescaler value"
#endif
TMR1 = 0x00; // Clear timer register
IPC0bits.T1IP = 0x06; // Set Timer1 Interrupt Priority Level to 6 = very high priority
IFS0bits.T1IF = 0; // Clear Timer1 Interrupt Flag
IEC0bits.T1IE = 1; // Enable Timer1 interrupt
T1CONbits.TON = 1; // Start Timer
systime = 0;
-// T2CONbits.T32 = 0; // Select 16 bit mode
+ // T2CONbits.T32 = 0; // Select 16 bit mode
/* Set timer 2 for SBCP timeout */
T2CONbits.TON = 0; // Disable Timer
T2CONbits.TCS = 0; // Select internal instruction cycle clock
T2CONbits.TGATE = 0; // Disable Gated Timer mode
#if NTOTPULSE_SPI_ME_READING_LATENCY < 65535
/* 1x prescaler */
PR2 = NTOTPULSE_SPI_ME_READING_LATENCY;
T2CONbits.TCKPS = 0b00; // Select 1:1 Prescaler
#elif NTOTPULSE_SPI_ME_READING_LATENCY < (8*65535)
/* 8x prescaler */
PR2 = NTOTPULSE_SPI_ME_READING_LATENCY/8;
T2CONbits.TCKPS = 0b01; // Select 1:8 Prescaler
#else
#error "F_SPI_ME_REQADING_LATENCY - Check the values and maybe use a greater prescaler value"
#endif
TMR2 = 0x00; // Clear timer register
IPC1bits.T2IP = 0x01; // Set Timer2 Interrupt Priority Level to 1 = low priority
IFS0bits.T2IF = 0; // Clear Timer2 Interrupt Flag
IEC0bits.T2IE = 1; // Enable Timer2 interrupt
// We do not activate timer 2 but keep it stopped until it is needed
}