nhan pham an / Hobbyking_Cheetah_Compact_DRV8323_StepDir

Fork and fix for mwork

Dependencies:   mbed-dev-f303 FastPWM3 millis

DRV8323/DRV.cpp@59:568e7be5232f, 2020-06-29 (annotated)

Committer:
annhandt09
Date:
Mon Jun 29 09:36:26 2020 +0000
Revision:
59:568e7be5232f
Parent:
55:fee62d8fd8fb 
add controller.p_des = controller.theta_mech - (controller.t_ff / controller.kp) ;  for first enable.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
benkatz 44:8040fa2fcb0d 1 #include "mbed.h"
benkatz 44:8040fa2fcb0d 2 #include "DRV.h"
annhandt09 55:fee62d8fd8fb 3 #include "macro.h"
benkatz 44:8040fa2fcb0d 4 DRV832x::DRV832x(SPI *spi, DigitalOut *cs){
benkatz 44:8040fa2fcb0d 5 _spi = spi;
benkatz 44:8040fa2fcb0d 6 _cs = cs;
benkatz 44:8040fa2fcb0d 7 _cs->write(1);
benkatz 44:8040fa2fcb0d 8 wait_us(10);
benkatz 44:8040fa2fcb0d 9 _spi->format(16, 1);
benkatz 44:8040fa2fcb0d 10 _spi->frequency(500000);
benkatz 44:8040fa2fcb0d 11 }
benkatz 44:8040fa2fcb0d 12
benkatz 44:8040fa2fcb0d 13 uint16_t DRV832x::spi_write(uint16_t val)
benkatz 44:8040fa2fcb0d 14 {
benkatz 44:8040fa2fcb0d 15 _cs->write(0);
benkatz 44:8040fa2fcb0d 16 wait_us(10);
benkatz 44:8040fa2fcb0d 17 uint16_t reply = _spi->write(val);
benkatz 44:8040fa2fcb0d 18 _cs->write(1);
benkatz 44:8040fa2fcb0d 19 return reply;
benkatz 44:8040fa2fcb0d 20 }
benkatz 44:8040fa2fcb0d 21
benkatz 44:8040fa2fcb0d 22 int DRV832x::read_FSR1(void)
benkatz 44:8040fa2fcb0d 23 {
benkatz 44:8040fa2fcb0d 24 uint16_t val = (1<<15) | FSR1;
benkatz 44:8040fa2fcb0d 25 return spi_write(val);
benkatz 44:8040fa2fcb0d 26 }
benkatz 44:8040fa2fcb0d 27
benkatz 44:8040fa2fcb0d 28 int DRV832x::read_FSR2(void)
benkatz 44:8040fa2fcb0d 29 {
benkatz 44:8040fa2fcb0d 30 uint16_t val = (1<<15) | FSR2;
benkatz 44:8040fa2fcb0d 31 return spi_write(val);
benkatz 44:8040fa2fcb0d 32 }
benkatz 44:8040fa2fcb0d 33
benkatz 44:8040fa2fcb0d 34 int DRV832x::read_register(int reg)
benkatz 44:8040fa2fcb0d 35 {
benkatz 44:8040fa2fcb0d 36 return spi_write((1<<15) | (reg<<11));
benkatz 44:8040fa2fcb0d 37 }
benkatz 44:8040fa2fcb0d 38
benkatz 44:8040fa2fcb0d 39 void DRV832x::write_register(int reg, int val)
benkatz 44:8040fa2fcb0d 40 {
benkatz 44:8040fa2fcb0d 41 spi_write((reg<<11) | val);
benkatz 44:8040fa2fcb0d 42 }
benkatz 44:8040fa2fcb0d 43 void DRV832x::write_DCR(int DIS_CPUV, int DIS_GDF, int OTW_REP, int PWM_MODE, int PWM_COM, int PWM_DIR, int COAST, int BRAKE, int CLR_FLT)
benkatz 44:8040fa2fcb0d 44 {
benkatz 44:8040fa2fcb0d 45 uint16_t val = (DCR<<11) | (DIS_CPUV<<9) | (DIS_GDF<<8) | (OTW_REP<<7) | (PWM_MODE<<5) | (PWM_COM<<4) | (PWM_DIR<<3) | (COAST<<2) | (BRAKE<<1) | CLR_FLT;
benkatz 44:8040fa2fcb0d 46 spi_write(val);
benkatz 44:8040fa2fcb0d 47 }
benkatz 44:8040fa2fcb0d 48 void DRV832x::write_HSR(int LOCK, int IDRIVEP_HS, int IDRIVEN_HS)
benkatz 44:8040fa2fcb0d 49 {
benkatz 44:8040fa2fcb0d 50 uint16_t val = (HSR<<11) | (LOCK<<8) | (IDRIVEP_HS<<4) | IDRIVEN_HS;
benkatz 44:8040fa2fcb0d 51 spi_write(val);
benkatz 44:8040fa2fcb0d 52 }
benkatz 44:8040fa2fcb0d 53 void DRV832x::write_LSR(int CBC, int TDRIVE, int IDRIVEP_LS, int IDRIVEN_LS)
benkatz 44:8040fa2fcb0d 54 {
benkatz 44:8040fa2fcb0d 55 uint16_t val = (LSR<<11) | (CBC<<10) | (TDRIVE<<8) | (IDRIVEP_LS<<4) | IDRIVEN_LS;
benkatz 44:8040fa2fcb0d 56 spi_write(val);
benkatz 44:8040fa2fcb0d 57 }
benkatz 44:8040fa2fcb0d 58 void DRV832x::write_OCPCR(int TRETRY, int DEAD_TIME, int OCP_MODE, int OCP_DEG, int VDS_LVL)
benkatz 44:8040fa2fcb0d 59 {
benkatz 44:8040fa2fcb0d 60 uint16_t val = (OCPCR<<11) | (TRETRY<<10) | (DEAD_TIME<<8) | (OCP_MODE<<6) | (OCP_DEG<<4) | VDS_LVL;
benkatz 44:8040fa2fcb0d 61 spi_write(val);
benkatz 44:8040fa2fcb0d 62 }
benkatz 44:8040fa2fcb0d 63 void DRV832x::write_CSACR(int CSA_FET, int VREF_DIV, int LS_REF, int CSA_GAIN, int DIS_SEN, int CSA_CAL_A, int CSA_CAL_B, int CSA_CAL_C, int SEN_LVL)
benkatz 44:8040fa2fcb0d 64 {
benkatz 44:8040fa2fcb0d 65 uint16_t val = (CSACR<<11) | (CSA_FET<<10) | (VREF_DIV<<9) | (LS_REF<<8) | (CSA_GAIN<<6) | (DIS_SEN<<5) | (CSA_CAL_A<<4) | (CSA_CAL_B<<3) | (CSA_CAL_C<<2) | SEN_LVL;
benkatz 44:8040fa2fcb0d 66 spi_write(val);
benkatz 44:8040fa2fcb0d 67 }
benkatz 44:8040fa2fcb0d 68
benkatz 44:8040fa2fcb0d 69 void DRV832x::print_faults(void)
benkatz 44:8040fa2fcb0d 70 {
benkatz 44:8040fa2fcb0d 71 uint16_t val1 = read_FSR1();
benkatz 44:8040fa2fcb0d 72 wait_us(10);
benkatz 44:8040fa2fcb0d 73 uint16_t val2 = read_FSR2();
benkatz 44:8040fa2fcb0d 74 wait_us(10);
benkatz 44:8040fa2fcb0d 75
annhandt09 55:fee62d8fd8fb 76 if(val1 & (1<<10)){Serial_printf("\n\rFAULT\n\r");}
benkatz 44:8040fa2fcb0d 77
annhandt09 55:fee62d8fd8fb 78 if(val1 & (1<<9)){Serial_printf("VDS_OCP\n\r");} // VDS monitor overcurrent fault condition
annhandt09 55:fee62d8fd8fb 79 if(val1 & (1<<8)){Serial_printf("GDF\n\r");} // Indicates gate drive fault condition
annhandt09 55:fee62d8fd8fb 80 if(val1 & (1<<7)){Serial_printf("UVLO\n\r");} // undervoltage lockout fault condition
annhandt09 55:fee62d8fd8fb 81 if(val1 & (1<<6)){Serial_printf("OTSD\n\r");} // overtemperature shutdown
annhandt09 55:fee62d8fd8fb 82 if(val1 & (1<<5)){Serial_printf("VDS_HA\n\r");} // VDS overcurrent fault on the A high-side MOSFET
annhandt09 55:fee62d8fd8fb 83 if(val1 & (1<<4)){Serial_printf("VDS_LA\n\r");} // s VDS overcurrent fault on the A low-side MOSFET
annhandt09 55:fee62d8fd8fb 84 if(val1 & (1<<3)){Serial_printf("VDS_HB\n\r");} // VDS overcurrent fault on the B high-side MOSFET
annhandt09 55:fee62d8fd8fb 85 if(val1 & (1<<2)){Serial_printf("VDS_LB\n\r");} // VDS overcurrent fault on the B low-side MOSFET
annhandt09 55:fee62d8fd8fb 86 if(val1 & (1<<1)){Serial_printf("VDS_HC\n\r");} // VDS overcurrent fault on the C high-side MOSFET
annhandt09 55:fee62d8fd8fb 87 if(val1 & (1)){Serial_printf("VDS_LC\n\r");} // VDS overcurrent fault on the C low-side MOSFET
benkatz 44:8040fa2fcb0d 88
annhandt09 55:fee62d8fd8fb 89 if(val2 & (1<<10)){Serial_printf("SA_OC\n\r");} // overcurrent on phase A sense amplifier (DRV8323xS)
annhandt09 55:fee62d8fd8fb 90 if(val2 & (1<<9)){Serial_printf("SB_OC\n\r");} // overcurrent on phase B sense amplifier
annhandt09 55:fee62d8fd8fb 91 if(val2 & (1<<8)){Serial_printf("SC_OC\n\r");} // overcurrent on phase C sense amplifier
annhandt09 55:fee62d8fd8fb 92 if(val2 & (1<<7)){Serial_printf("OTW\n\r");} // overtemperature warning
annhandt09 55:fee62d8fd8fb 93 if(val2 & (1<<6)){Serial_printf("CPUV\n\r");} // charge pump undervoltage fault condition
annhandt09 55:fee62d8fd8fb 94 if(val2 & (1<<5)){Serial_printf("VGS_HA\n\r");}
annhandt09 55:fee62d8fd8fb 95 if(val2 & (1<<4)){Serial_printf("VGS_LA\n\r");}
annhandt09 55:fee62d8fd8fb 96 if(val2 & (1<<3)){Serial_printf("VGS_HB\n\r");}
annhandt09 55:fee62d8fd8fb 97 if(val2 & (1<<2)){Serial_printf("VGS_LB\n\r");}
annhandt09 55:fee62d8fd8fb 98 if(val2 & (1<<1)){Serial_printf("VGS_HC\n\r");}
annhandt09 55:fee62d8fd8fb 99 if(val2 & (1)){Serial_printf("VGS_LC\n\r");}
benkatz 44:8040fa2fcb0d 100 }
benkatz 44:8040fa2fcb0d 101
benkatz 44:8040fa2fcb0d 102 void DRV832x::enable_gd(void)
benkatz 44:8040fa2fcb0d 103 {
benkatz 44:8040fa2fcb0d 104 uint16_t val = (read_register(DCR)) & (~(0x1<<2));
benkatz 44:8040fa2fcb0d 105 write_register(DCR, val);
benkatz 44:8040fa2fcb0d 106 }
benkatz 44:8040fa2fcb0d 107
benkatz 44:8040fa2fcb0d 108 void DRV832x::disable_gd(void)
benkatz 44:8040fa2fcb0d 109 {
benkatz 44:8040fa2fcb0d 110 uint16_t val = (read_register(DCR)) | (0x1<<2);
benkatz 44:8040fa2fcb0d 111 write_register(DCR, val);
benkatz 45:26801179208e 112 }
benkatz 45:26801179208e 113
benkatz 45:26801179208e 114 void DRV832x::calibrate(void)
benkatz 45:26801179208e 115 {
benkatz 45:26801179208e 116 uint16_t val = 0x1<<4 + 0x1<<3 + 0x1<<2;
benkatz 45:26801179208e 117 write_register(CSACR, val);
benkatz 44:8040fa2fcb0d 118 }