unsigned char ccpcon; volatile unsigned int duty; unsigned int maxduty; volatile unsigned char step; unsigned char mode; signed char states[6][3] = {{0,-1,1}, {1,-1,0}, {1,0,-1}, {0,1,-1}, {-1,1,0}, {-1,0,1}}; // komutační kroky každé fáze unsigned char hallStates[6][3] = {{0,0,1}, {1,0,1}, {1,0,0}, {1,1,0}, {0,1,0}, {0,1,1}}; // výstup hallových senzorů při dané fázi char x; unsigned char Ichannel; // kanál ADC pro snímání proudu unsigned int current; unsigned int Ilimit = 205; // při zesílení 10 by to mělo odpovídat 20 A unsigned int Ucap; unsigned int h, l; volatile unsigned int period; unsigned int MINDUTY = 300; char fail = 0; void Start(char m); void GetADC(); void DriveMode(unsigned char); void SetDuty(unsigned int d, signed char s, unsigned char ccp); void InterruptSetDuty(unsigned int d, signed char s, unsigned char ccp); void SendTelemetry(); void SendInt(unsigned int msg); void InitECCP(); void InitMCP(); void MCPSendByte(unsigned char dat); void interrupt(); void interrupt_low(); void main() { TRISA = 0xFF; TRISB = 0xFF; TRISC = 0xFF; TRISD = 0xFF; ANSELA = 0xff; // RA0 - potenciometr, RA1-3 - proud fáze, RA5 - napětí kondenzátoru ANSELB = 0; ANSELC = 0; ANSELD = 0; LATA = 0; LATB = 0; LATC = 0; LATD = 0; INTCON = 0; INTCON2 = 0; INTCON3 = 0; RBIE_bit = 0; // Zatím nezapínat přerušení na port B RBIP_bit = 1; // vyšší priorita IOCB7_bit = 1; // přerušení při změně na B7 zapnnuta IOCB6_bit = 1; // B6 IOCB4_bit = 1; // B4 IPEN_bit = 1; // zapnití rozlišování prority přerušení PEIE_bit = 1; // peripheral interrupt enable GIEH_bit = 1; // global high priority interrupt enable GIEL_bit = 1; // global low priority interrupt enable RBPU_bit = 0; // zapnutí pullupů na portu B WPUB7_bit = 1; // zapnutí na B7, B6, B4 - Hally; B2, B1, B0 - tlačítka WPUB6_bit = 1; WPUB4_bit = 1; WPUB3_bit = 1; WPUB2_bit = 1; WPUB1_bit = 1; WPUB0_bit = 1; ADCON0 = 0b00000000; // kanál AN0 - potenciometr AN1 - regen potenciometr ADCON1 = 0; // vref+ = avdd; vref- = avss ADCON2 = 0b10111110; // výsledek zarovnán doprava; 20 cykly acquisition time; Tad = Fosc/64 ADON_bit = 1; // zapnutí ADC // inicializace UART2 pro MCP8026 (9600, 8bit, 1 stop bit) BAUDCON2.ABDEN = 0; // baud autodetect vypnuto BAUDCON2.BRG16 = 0; // 8bit baud rate generator BAUDCON2.CKTXP = 0; // normální polarita Tx BAUDCON2.DTRXP = 0; // normální polarita Rx SPBRG2 = 51; // tohle by mělo znamenat baudrate 9600 TXSTA2.BRGH = 0; TXSTA2.SENDB = 0; TXSTA2.SYNC = 0; // asynchronní přenos RCSTA2.RX9 = 0; // 8bitový paket RCSTA2.SPEN = 1; // serial port enable RCSTA2.CREN = 1; // zapnuté Rx TXSTA2.TXEN = 1; // zapnutí Tx RC2IE_bit = 1; // zapnutí přerušení při přijetí bytu RC2IP_bit = 0; // nízká priorita // inicializace UART1 pro komunikaci s PC BAUDCON1.ABDEN = 0; // baud autodetect vypnuto BAUDCON1.BRG16 = 0; // 8bit baud rate generator BAUDCON1.CKTXP = 0; // normální polarita Tx BAUDCON1.DTRXP = 0; // normální polarita Rx SPBRG1 = 25; // tohle by mělo znamenat baudrate 19200 TXSTA1.BRGH = 0; TXSTA1.SENDB = 0; TXSTA1.SYNC = 0; // asynchronní přenos RCSTA1.RX9 = 0; // 8bitový paket RCSTA1.SPEN = 1; // serial port enable RCSTA1.CREN = 1; // zapnutí Rx TXSTA1.TXEN = 1; // zapnutí Tx RC1IE_bit = 1; // zapnutí přerušení při přijetí bytu RC1IP_bit = 0; // nízká priorita InitECCP(); // inicializace PWM pro 3fáz. můstek T0CON = 0b10000101; // Timer0 enabled; prescaler 64; instruction clock TMR0IP_bit = 0; // Low priotity interrupt TMR0IF_bit = 0; // Timer0 použit pro snímání periody komutace TMR0IE_bit = 1; T1CON = 0b00100001; // Timer1 enabled; prescaler 4; instruction clock TMR1IP_bit = 0; // Low priority TMR1IF_bit = 0; // Timer1 použit pro ramp up TMR1IE_bit = 0; TRISB = 0b11011111; // RB5 pwm, RB4,6,7 vstupy, RB3 vstup TRISC = 0b11000000; // RC1, RC2 pwm, RC0,4,5 výstupy, RC6,7 uart TRISD = 0b11010010; // RD2, RD5 pwm, RD0 výstup, RD6,7 uart TRISE = 0b11111100; // RE1 pwm RE2 capture InitMCP(); // nahrání nastavení MCP8026 DriveMode(0); // STANDBY režim duty = MINDUTY; period = 0; while(1) { mode = PORTB & 0b00001110; // vyhodnocení stavu tlačítek if(mode == 0b00001010) { // DRIVE if(period == 0) { // pokud motor stojí, začít startovací sekvencí Start(1); CCP1ASE_bit = 0; CCP2ASE_bit = 0; CCP3ASE_bit = 0; // odblokování výstutpů RBIF_bit = 1; // vynucené spuštění přerušení a zjištění polohy rotoru RBIE_bit = 1; TMR1IE_bit = 1; LATE.B0 = 1; LATD.B3 = 0; } else { DriveMode(1); // sepnutí příslušných mosfetů obvodu superkondenzátoru CCP1ASE_bit = 0; CCP2ASE_bit = 0; CCP3ASE_bit = 0; RBIF_bit = 1; // vynucené spuštění přerušení a zjištění polohy rotoru RBIE_bit = 1; TMR1IE_bit = 1; LATE.B0 = 1; LATD.B3 = 0; } duty = MINDUTY; while(mode == 0b00001010) { GetADC(); // zjištění polohy potenciometru SetDuty(duty, states[step][0], 1); // nastavení výstupů pro driver můstku SetDuty(duty, states[step][1], 2); SetDuty(duty, states[step][2], 3); SendTelemetry(); // poslat data PC mode = PORTB & 0b00001110; // znovuvyhodnocení stavu tlačítek } TMR1IE_bit = 0; } else if(mode == 0b00001000) { // BOOST if(period == 0) { Start(2); CCP1ASE_bit = 0; CCP2ASE_bit = 0; CCP3ASE_bit = 0; RBIF_bit = 1; // vynucené spuštění přerušení a zjištění polohy rotoru RBIE_bit = 1; TMR1IE_bit = 1; LATD.B3 = 1; LATE.B0 = 0; } else { DriveMode(2); CCP1ASE_bit = 0; CCP2ASE_bit = 0; CCP3ASE_bit = 0; RBIF_bit = 1; // vynucené spuštění přerušení a zjištění polohy rotoru RBIE_bit = 1; TMR1IE_bit = 1; LATD.B3 = 1; LATE.B0 = 0; } while(mode == 0b00001000) { GetADC(); //if(Ucap > 70 && Ucap < 512) SetDuty(duty, states[step][0], 1); SetDuty(duty, states[step][1], 2); SetDuty(duty, states[step][2], 3); /*else { // pokud neni napětí kondenzátoru v mezích, přejít na obyč drive DriveMode(1); LATE.B0 = 0; break; }*/ SendTelemetry(); mode = PORTB & 0b00001110; } TMR1IE_bit = 0; } else if(mode == 0b00000010) { // REGEN DriveMode(3); duty = 1000; // nejvyšší možné duty cycle LATE.B0 = 1; LATD.B3 = 1; while((mode == 0b00000010) && (period < 3500)) { // jen při otáčkách > 350 rpm GetADC(); duty = 1000; SetDuty(duty, states[step][0], 1); SetDuty(duty, states[step][1], 2); SetDuty(duty, states[step][2], 3); SendTelemetry(); mode = PORTB & 0b00001110; } DriveMode(0); // doběh dynamickým brzděním LATE.B0 = 0; LATD.B3 = 0; CCP1ASE_bit = 1; // všechny dolní mosfety můstku sepnuty CCP2ASE_bit = 1; CCP3ASE_bit = 1; } else { DriveMode(0); LATE.B0 = 0; LATD.B3 = 0; SetDuty(0, -1, 1); SetDuty(0, -1, 2); SetDuty(0, -1, 3); GetADC(); SendTelemetry(); } } } void interrupt() { if (RBIF_bit == 1) { // při změně jednoho z hallových senzorů x = PORTB; // reset spouštěče přerušení při změně na pinu fail = 0; switch(PORTB & 0b11010000) { // Dekódování pozice podle hall. senzorů case 0b00000000: fail = 1; break; case 0b00010000: step = 2; Ichannel = 0b00001111; break; case 0b01000000: step = 4; Ichannel = 0b00000111; break; case 0b01010000: step = 3; Ichannel = 0b00001111; break; case 0b10000000: step = 0; Ichannel = 0b00001011; break; case 0b10010000: step = 1; Ichannel = 0b00001011; break; case 0b11000000: step = 5; Ichannel = 0b00000111; break; case 0b11010000: fail = 1; break; } if(!fail) { // pokud jsou výstupy hall senzorů v pořádku, aktualizovat PWM výstupy InterruptSetDuty(duty, states[step][0], 1); InterruptSetDuty(duty, states[step][1], 2); InterruptSetDuty(duty, states[step][2], 3); } else { // pokud jsou výstupy hall senzorů špatně, odpojit všechny fáze InterruptSetDuty(0, 0, 1); InterruptSetDuty(0, 0, 2); InterruptSetDuty(0, 0, 3); TXREG1 = 0b00001111; } period = (unsigned int) TMR0L; // čas mezi komutcemi pro výpočet rychlosti period = (unsigned int) (period | (TMR0H << 8)); TMR0H = 0; // vynulování čítače pro další měření TMR0L = 0; /*if (period > (setpoint - 50)) duty += 1; else if (period < (setpoint + 50)) duty -= 1; if (duty > maxduty) duty = maxduty; else if (duty < minduty) duty = minduty;*/ /*if ((period != 0) && ((mode == 0b00001010) || (mode == 0b00001000))) duty = duty + 1; if ((duty > maxduty) && ((mode == 0b00001010) || (mode == 0b00001000))) duty = maxduty; TXREG1 = (unsigned char)(duty & 0xff); */ RBIF_bit = 0; // vynulovat flag bit } } void interrupt_low() { if(RC2IF_bit == 1) TXREG1 = RCREG2; // komunikaci mcu s driverem přeposílat na PC (buď toto nebo telemetrie) else if(RC1IF_bit) { // příkaz z PC přeposlat driveru while(TXSTA2.TRMT == 0) ; // počká, pokud se zrovna něco posílá TXREG2 = RCREG1; } else if(TMR0IF_bit) { // pokud časovač periody přetekl, motor stojí period = 0; TMR0IF_bit = 0; } else if(TMR1IF_bit) { // postupné zvyšování duty cycle, aby se předešlo vysokým proudům při rozběhu duty += 1; if (duty > maxduty) duty = maxduty; // zvyšovat postupně do hodnoty nastavené potencimetrem TMR1IF_bit = 0; } } void Start(char m) { LATD.B0 = 0; // driver ve standby CCP1ASE_bit = 0; // odblokování PWM výstupů CCP2ASE_bit = 0; CCP3ASE_bit = 0; SetDuty(0, 0, 1); // všechny dolní mosfety zatím vypnuty SetDuty(0, 0, 2); SetDuty(0, 0, 3); LATD.B0 = 1; // driver zapnut DriveMode(m); // nastaví řídící Fety na požadovyný mód Delay_ms(100); SetDuty(0, -1, 1); // postupné zapínání dolních mosfetů, aby se před startem nabily bootstrap kondenzátory Delay_ms(10); SetDuty(0, -1, 2); Delay_ms(10); SetDuty(0, -1, 3); Delay_ms(10); duty = MINDUTY; } void GetADC() { //snímání potenciometru a napětí kondenzátoru if (ADCON0 == 0b00000001) { // pokud je hotov sample potenciometru maxduty = (unsigned int)((ADRESH<<8)|ADRESL); ADCON0 = 0b00010011; } else { // pokud je hotov sample kondenzátoru Ucap = (unsigned int)((ADRESH<<8)|ADRESL); ADCON0 = 0b00000011; } } void SetDuty(unsigned int d, signed char s, unsigned char ccp) { /* - zapíše duty cycle do příslušných registrů příslušného ECCP */ if (s == -1) d = 0; // pokud je v tomto kroku celou dobu zapnut dolní fet == duty cycle 0 if (s == 0) ccpcon = 0; // pokud je fáze odpojená, vypnout PWM modul else ccpcon = 0b10001100; // jinak probíhá modulace PWM if (ccp == 1) { CCPR1L = (unsigned char) (d >> 2); // nastavení horních 8 bitů duty cyle CCP1CON = (unsigned char) (ccpcon | ((d & 3) << 4)); // nastavení dolních 2 bitů } else if (ccp == 2) { CCPR2L = (unsigned char) (d >> 2); CCP2CON = (unsigned char) (ccpcon | ((d & 3) << 4)); } else if (ccp == 3) { CCPR3L = (unsigned char) (d >> 2); CCP3CON = (unsigned char) (ccpcon | ((d & 3) << 4)); } } void InterruptSetDuty(unsigned int d, signed char s, unsigned char ccp) { /* - zapíše duty cycle do příslušných registrů příslušného ECCP - zvlášť funkce pro zapsání duty v interruptu, kde se nesmí použít funkce co se už používá jinde */ if (s == -1) d = 0; if (s == 0) ccpcon = 0; else ccpcon = 0b10001100; if (ccp == 1) { CCPR1L = (unsigned char) (d >> 2); CCP1CON = (unsigned char) (ccpcon | ((d & 3) << 4)); } else if (ccp == 2) { CCPR2L = (unsigned char) (d >> 2); CCP2CON = (unsigned char) (ccpcon | ((d & 3) << 4)); } else if (ccp == 3) { CCPR3L = (unsigned char) (d >> 2); CCP3CON = (unsigned char) (ccpcon | ((d & 3) << 4)); } } void DriveMode(unsigned char m) { // nastavení obvodu superkondenzátoru if (m == 0) { // STANDBY režim LATC.B0 = 1; // BAT_SD - driver zapnut LATC.B4 = 0; // BAT_IN - dolní BAT fet zapnut Delay_us(10); LATC.B3 = 1; // CAP_L zapnut Delay_us(10); LATC.B5 = 0; // CAP_H vypnut LATD.B0 = 1; // MCP8026 pořád zapnuto } else if (m == 1) { // DRIVE režim LATC.B0 = 1; // BAT_SD - driver zapnut LATC.B4 = 1; // BAT_IN - horní BAT fet zapnut Delay_us(10); LATC.B3 = 1; // CAP_L zapnut Delay_us(10); LATC.B5 = 0; // CAP_H vypnut LATD.B0 = 1; // MCP8026 zapnuto } else if (m == 2) { // BOOST režim - kond. v sérii LATC.B0 = 1; // BAT_SD - driver zapnut LATC.B4 = 1; // BAT_IN - horní BAT fet zapnut Delay_us(10); LATC.B3 = 0; // CAP_L vypnut Delay_us(10); LATC.B5 = 1; // CAP_H zapnut LATD.B0 = 1; // MCP8026 zapnuto } else if (m == 3) { // REGEN režim - rekuperace do kond. LATC.B0 = 1; // BAT_SD - driver zapnut LATC.B4 = 0; // BAT_IN - dolní BAT fet zapnut Delay_us(10); LATC.B3 = 0; // CAP_L vypnut Delay_us(10); LATC.B5 = 1; // CAP_H zapnut LATD.B0 = 1; // MCP8026 zapnuto } } void SendTelemetry() { // posílá data jako csv SendInt(duty); UART1_Write(0x2C); // symbol čárky SendInt(Ucap); UART1_Write(0x2C); SendInt(6*period); TXREG1 = 0x0D; // konec řádky TXREG1 = 0x0A; } void SendInt(unsigned int msg) { char text[6]; WordToStr(msg, text); UART1_Write_Text(text); } void InitECCP() { // nastavení ECCP modulů // ECCP1 setup PSTR1CON = 0b00000011; // enabling P1A and P1B as PWM outputs CCPTMRS0 = 0b00000000; // timer2 as clock source for ECCP1,2,3 PR2 = 0xFF; // setting the timer period ECCP1AS = 0b00000001; // autoshutdown OFF; shutdown states - P1A-0 P1B-1 PWM1CON = 0b00000100; // deadband 4 cycles; disabling shutdown autoreset ECCP1AS |= 0b10000000; // force shutdown event CCP1CON = 0b10001100; // halfbridge mode; setting DC1B - 2 LSB of duty cycle CCPR1L = 0x00; // setting 8 MSB of duty cycle T2CON = 0b00000100; // timer2 enable; no prescaler, no postscaler ECCP1AS = ECCP1AS & 0b01111111; // clearing shutdown bit // ECCP2 setup PSTR2CON = 0b00000011; // enabling P1A and P1B as PWM outputs ECCP2AS = 0b00000001; // autoshutdown OFF; shutdown states - P1A-0 P1B-1 PWM2CON = 0b00000100; // deadband 4 cycles; disabling shutdown autoreset ECCP2AS |= 0b10000000; // force shutdown event CCP2CON = 0b10001100; // halfbridge mode; setting DC1B - 2 LSB of duty cycle CCPR2L = 0x00; // setting 8 MSB of duty cycle ECCP2AS = ECCP2AS & 0b01111111; // clearing shutdown bit // ECCP3 setup PSTR3CON = 0b00000011; // enabling P1A and P1B as PWM outputs ECCP3AS = 0b00000001; // autoshutdown OFF; shutdown states - P1A-0 P1B-1 PWM3CON = 0b00000100; // deadband 4 cycles; disabling shutdown autoreset ECCP3AS |= 0b10000000; // force shutdown event CCP3CON = 0b10001100; // halfbridge mode; setting DC1B - 2 LSB of duty cycle CCPR3L = 0x00; // setting 8 MSB of duty cycle ECCP3AS = ECCP3AS & 0b01111111; // clearing shutdown bit T2CKPS0_bit = 0; T2CKPS1_bit = 0; // 1x prescaler - finální frekvence 31,25 kHz } void InitMCP() { // nahrání nastavení driveru můstku MCP8026 LATD.B0 = 0; // standby režim MCPSendByte(0); Delay_ms(10); MCPSendByte(0b10000001); // SET_CFG_0 MCPSendByte(0b00000111); // vypnuto hlídání napětí na mosfetech Delay_ms(10); Delay_ms(10); MCPSendByte(0b10000011); // SET_CFG_1 MCPSendByte(0xFF); // limit proudu 0,991 V Delay_ms(10); Delay_ms(10); MCPSendByte(0b10000111); // SET_CFG_2 MCPSendByte(0b00000000); // deadtime 2000 ns, driver blanking 4 us Delay_ms(10); LATD.B0 = 1; // normální režim } void MCPSendByte(unsigned char dat) { // komunikace je po jednom drátě, je třeba vypínat rx nebo tx TXSTA2.CREN = 0; // vypnutí Rx TXSTA2.TXEN = 1; // zapnutí Tx TXREG2 = dat; while(TXSTA2.TRMT == 0); } /* POUŽITÉ PINY RA0, 1, 2, 3, 5 RB1, 2, 3, 4, 5, 6, 7 RC0, 1, 2, 3, 4, 5 RD0, 1, 2, 3, 5, 6, 7 RE0, 1, 2 /* P1A P1B ECCP1 RC2 RD5 ECCP2 RC1 RD2 ECCP3 RB5 RE1 */ /* komutační kroky : -1 = dolní mosfet 0 = oba vypnuté 1 = horní mosfet pwm na fázi, kde je zapnut horní mosfet */ /* RB7, RB6, RB4 použity pro přerušení při změně hallova senzoru polohy rotoru C, B, A */ /* RD1 je vstup CCP4 pro snímání otáček - připojit k jednomu z hall. senzorů nebo možná k opt. senzoru na setrvačníku */ /* RA0 je vstup ADC od potenciometru */ /* RC0 - BAT_SD RC4 - BAT_IN !!BACHA PŘEMÍSTĚNO!! RC3 - CAP_L !!BACHA PŘEMÍSTĚNO!! RC5 - CAP_H RD0 - CE */ /* RC6 - tx; RC7 - rx; */ /* RD6 je taky TX2 RD7 je taky RX2 takže DE2 driveru asi připojit k Rx i Tx UART2 a zapínat vždycky jenom Rx nebo Tx a UART1 nechat k připojení k Serial mointoru */ /* RB2 spouští Drive RB1 Boost RB0 Regen */ /* RA0 - potenciometr RA1 - proud A RA2 - proud B RA3 - proud C RA4 - supercap */ /* RD3, RE0 - LEDky */