// 29.48MHz
// str 23
void PWMinit()
{
CloseMCPWM();
// init PWM1
// Free Running mode
P1TCONbits.PTEN = 0;
PWM1CON1bits.PMOD1 = 1; // Indepedent output mode
PWM1CON1bits.PEN1H = 0; // enable pwm pin for pwm
PWM1CON1bits.PEN1L = 1; // enable pwm pin for pwm
P1TMR = 1;
P1TPER = 1627;//1599; // frekvencija PWM- a : 18425Hz
P1DC1 = 0;
P1TCONbits.PTEN = 1;
// init PWM2
P2TCONbits.PTEN = 0;
PWM2CON1bits.PMOD1 = 1;
PWM2CON1bits.PEN1H = 0;
PWM2CON1bits.PEN1L = 1;
P2TPER = 1627;//1599;
P2DC1 = 0;
P2TCONbits.PTEN = 1;
}
static void batDefaultCallback(void) {
unsigned char i;
// Disable all interrupts
CRITICAL_SECTION_START;
LED_1 = 1;
LED_2 = 1;
#if defined(__IMAGEPROC2)
LED_3 = 1;
#endif
#if defined(__LOWBATT_STOPS_MOTORS)
// Stop any running motors
for (i=1; i<=4; i++) { SetDCMCPWM(i, 0, 0); }
CloseMCPWM();
#endif
// Slowly blink all LEDs 5 times (1 second interval)
for (i=0; i<5; ++i) {
LED_1 = ~LED_1;
LED_2 = ~LED_2;
#if defined(__IMAGEPROC2)
LED_3 = ~LED_3;
#endif
delay_ms(1000);
}
CRITICAL_SECTION_END;
}
void PWMinit(void)
{
CloseMCPWM();
P1TCONbits.PTEN = 0;
PWM1CON1bits.PMOD1 = 1;
PWM1CON1bits.PEN1H = 1;
PWM1CON1bits.PEN1L = 0;
//P1DTCON1bits.DTBPS = 0;
//P1DTCON1bits.DTB = 5;
//P1DTCON1bits.DTAPS = 0;
//P1DTCON1bits.DTA = 5;
P1TMR = 1;
P1TPER = 1599;
P1DC1 = 0;
P1TCONbits.PTEN = 1;
// init PWM2
P2TCONbits.PTEN = 0;
PWM2CON1bits.PMOD1 = 1;
PWM2CON1bits.PEN1H = 1;
PWM2CON1bits.PEN1L = 0;
//P2DTCON1bits.DTBPS = 0;
//P2DTCON1bits.DTB = 5;
//P2DTCON1bits.DTAPS = 0;
//P2DTCON1bits.DTA = 5;
P2TPER = 1599;
P2DC1 = 0;
P2TCONbits.PTEN = 1;
//ukljuci mostove
LATBbits.LATB11 = 1;
LATBbits.LATB12 = 1;
}
//citanje serijskog porta tokom kretanja
static char getCommand(void)
{
char command;
unsigned char rxBuffer[8];
//U1STAbits.OERR = 0; //skrnavac usrani mora rucno da se resetuje
if(CAN_checkRX()) //proverava jel stigao karakter preko serijskog porta
{
CAN_read(rxBuffer);
command = rxBuffer[0];
switch(command)
{
case 'P':
sendStatusAndPosition();
break;
case 'S':
//ukopaj se u mestu
d_ref = L;
t_ref = orientation;
v_ref = 0;
currentStatus = STATUS_IDLE;
__delay_ms(10);
return 0;
case 's':
//stani i ugasi PWM
d_ref = L;
t_ref = orientation;
v_ref = 0;
CloseMCPWM();
currentStatus = STATUS_IDLE;
__delay_ms(10);
return 0;
default:
//case 'G' : case 'D' : case 'T' : case 'A' : case 'Q':
// primljena komanda za kretanje u toku kretanja
// stop, status ostaje MOVING
d_ref = L;
t_ref = orientation;
v_ref = 0;
__delay_ms(100);
return 0;
}// end of switch(command)
}
return 1;
}
int main(void)
{
/* Configure Oscillator to operate the device at 30Mhz
Fosc= Fin*M/(N1*N2), Fcy=Fosc/2
Fosc= 7.37*(32)/(2*2)=58.96Mhz for Fosc, Fcy = 29.48Mhz */
/* Configure PLL prescaler, PLL postscaler, PLL divisor */
//PLLFBDbits.PLLDIV=38; /* M = PLLFBD + 2 */ // izlazna frekvencija = 30Mhz
//Fin=8MHz, Fcy=30MHz
// Configure PLL prescaler, PLL postscaler, PLL divisor
PLLFBD = 28; // M=40 ---> PLLFBD + 2 = M
CLKDIVbits.PLLPOST = 0; // N2=2 ---> 2x(PLLPOST + 2) = N2
CLKDIVbits.PLLPRE = 0; // N1=2 ---> PLLPRE + 2 = N1
//new oscillator selection
__builtin_write_OSCCONH(0b011); //0b011 ---> XT with PLL
//enable oscillator source switch
__builtin_write_OSCCONL (OSCCONL | (1<<0)); //OSWEN
//wait for PLL lock -> wait to new settings become available
while (OSCCONbits.COSC != 0b011);
//wait for PLL lock
while (OSCCONbits.LOCK != 0b1);
AD1PCFGL = 0xFFFF;// all PORT Digital
RPINR18bits.U1RXR = 0; //UART1 RX na RP0- pin 4
RPOR0bits.RP1R = 3; //UART1 TX na RP1- pin 5
RPINR14bits.QEA1R = 2; //QEI1A na RP2
RPINR14bits.QEB1R = 3; //QEI1B na RP3
RPINR16bits.QEA2R = 4; //QEI2A na RP4
RPINR16bits.QEB2R = 7; //QEI2B na RP7
CAN_init(DRIVER_IDENTIFICATOR); // inicijalizacija CAN BUS- a-> argument je adresa drajvera
int tmp;
char komanda, v, smer;
int Xc, Yc, ugao;
NewLine();
PortInit();
//UARTinit();
TimerInit();
QEIinit();
PWMinit();
// CloseMCPWM();
resetDriver();
setSpeed(0x80);
setSpeedAccel(K2); //K2 je za 1m/s /bilo je 2
int tmpX, tmpY, tmpO;
unsigned char rxBuffer[8];
while(1)
{
__delay_ms(1000);
setSpeed(30);
// kretanje_pravo(-1000, 0);
if(getStatus() == STATUS_MOVING)
CAN_getLastMessage(rxBuffer);
else
CAN_read(rxBuffer);
komanda = rxBuffer[0];
switch(komanda)
{
// zadavanje pozicije
case 'I':
tmpX = rxBuffer[1] << 8;
tmpX |= rxBuffer[2];
tmpY = rxBuffer[3] << 8;
tmpY |= rxBuffer[4];
tmpO = rxBuffer[5] << 8;
tmpO |= rxBuffer[6];
setPosition(tmpX, tmpY, tmpO);
break;
// citanje pozicije i statusa
case 'P':
sendStatusAndPosition();
break;
//zadavanje max. brzine (default K2/2)
case 'V':
tmp = rxBuffer[1];
setSpeed(tmp);
break;
//kretanje pravo [mm]
case 'D':
tmp = rxBuffer[1] << 8;
tmp |= rxBuffer[2];
v = rxBuffer[3];
PWMinit();
kretanje_pravo(tmp, v);
break;
//relativni ugao [stepen]
case 'T':
tmp = rxBuffer[1] << 8;
tmp |= rxBuffer[2];
PWMinit();
okret(tmp);
break;
//apsolutni ugao [stepen]
case 'A':
tmp = rxBuffer[1] << 8;
tmp |= rxBuffer[2];
PWMinit();
apsolutni_ugao(tmp);
break;
//idi u tacku (Xc, Yc) [mm]
case 'G':
tmpX = rxBuffer[1] << 8;
tmpX |= rxBuffer[2];
tmpY = rxBuffer[3] << 8;
tmpY |= rxBuffer[4];
v = rxBuffer[5];
smer = rxBuffer[6];
PWMinit();
gotoXY(tmpX, tmpY, v, smer);
break;
//kurva
case 'Q':
tmpX = rxBuffer[1] << 8;
tmpX |= rxBuffer[2];
tmpY = rxBuffer[3] << 8;
tmpY |= rxBuffer[4];
tmpO = rxBuffer[5] << 8;
tmpO |= rxBuffer[6];
smer = rxBuffer[7];
PWMinit();
kurva(tmpX, tmpY, tmpO, smer);
break;
//ukopaj se u mestu
case 'S':
stop();
break;
//stani i ugasi PWM
case 's':
stop();
CloseMCPWM();
break;
case 'R':
resetDriver();
break;
default:
forceStatus(STATUS_ERROR);
break;
}
}
return 0;
}
