//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;
}
int main(void){
//Initialization of UART module
UART_init();
//Enable interrupts
DDRE &= ~(1 << PE4);
EIMSK |= (1 << INT4);
EICRB |= (1 << ISC41);
sei();
//Initialization of CAN module
SPI_MasterInit();
CANInit_normal();
//Initialization of PWM module
PWM_init();
//Initialization of ADC module
ADC_init();
//Initialization of Solenoid module
solenoid_init();
//Initialization of Motor module
motor_init();
//Variables
uint8_t ready = 0;
uint8_t game_over = 0;
uint8_t some_counter = 0; // Used for delay on IR, so that not a single low read causes game to end
uint8_t some_other_counter = 0; //Solenoid should not spam. It kills everything. Mad solenoid
//This is where new received joystick values are stored
joystick_position joy_pos;
//Message is passed back to node 1 upon game over
message.length = 1;
message.ID = 0x01;
while(1){
/* PHASE 0: INITIALIZATION */
ready = 0;
game_over = 0;
joy_pos.x = 128;
joy_pos.y = 128;
joy_pos.button_pressed = 0;
//Set to some start value
PWM_set_value(joy_pos);
motor_send(joy_pos);
/* PHASE 1: PRE-GAME */
/* Wait for node 1 to make contact */
printf("Waiting for node 1 to initiate game\n");
while(!ready){
cli();
if(CAN_received){
received = CAN_read();
CAN_received = 0;
if(received.data[3] == 1){ //If this is set, node 1 wants to start a game
received.data[3] = 0;
ready = 1;
}
}
sei();
_delay_ms(5);
}
printf("Node 1 is ready to go!\n");
printf("Starting game\n");
/* PHASE 2: IN-GAME */
/* Follow input over CAN and return a message when we have a game over. It is not a matter of if, only when! */
while(!game_over){
cli(); //Interrupts are disabled while we read from the CAN bus. Bad values were sometimes introduced otherwise
if(CAN_received){ //If updated controller info is ready, read it into our struct
received = CAN_read();
CAN_received = 0;
joy_pos.y = received.data[0]; //first spot contains y value
joy_pos.x = received.data[1]; //second spot contains x value
joy_pos.button_pressed = received.data[2]; //third spot contains button value
}
if(joy_pos.y > 128 - margin && joy_pos.y < 128 + margin){
joy_pos.y = 128; //This is done to avoid noisy PWM input
}
/* Apply new numbers to the system */
//Use information available to control PWM, motor and solenoid
some_other_counter++;
/********************************************************/
/* Solenoid pulse function is omitted to avoid spamming. Spamming is more likely to introduce errors */
if(joy_pos.button_pressed){
solenoid_push();
some_other_counter = 0;
//solenoid_pulse(); //Apply solenoid pulse. Good luck
joy_pos.button_pressed = 0;
}
if(some_other_counter > 10){
solenoid_pull();
}
/**********************************************************/
PWM_set_value(joy_pos); //update servo
motor_send(joy_pos); //update motor
if(ADC_check_goal()){ //Evaluates to 1 if goal is detected
some_counter++;
if(some_counter == 20){
some_counter = 0;
game_over = 1; //Start procedure all over
CAN_reset();
//Tell node 1 about the failure
message.data[0] = 1;
CAN_send(message);
}
}
sei(); //Allow interrupts to occur
_delay_ms(20);
}
/* GAME ENDED. WAIT FOR NEW ONE TO START */
}
}
