//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 */ } }