void main (void) { vuint32_t i = 0; initModesAndClock(); /* Initialize mode entries and system clock */ disableWatchdog(); /* Disable watchdog */ initPeriClkGen(); /* Initialize peripheral clock generation for DSPIs */ initADC(); initLED(); //can stuff initDSPI_1(); /* Initialize DSPI_1 as Slave SPI and init CTAR0 */ canSetup(); initEnergizeButton(); energizePacket.ID = ENERGIZE_ID; energizePacket.DATA.W[0] = 0xFFFFFFFF; energizePacket.DATA.W[1] = 0xFFFFFFFF; deenergizePacket.ID = DEENERGIZE_ID; deenergizePacket.DATA.W[0] = 0x00000000; deenergizePacket.DATA.W[1] = 0x00000000; torquePacket.ID = TORQUE_ID; speedPacket.ID = SPEED_ID; shutdownPacket.ID = SHUTDOWN_ID; shutdownPacket.DATA.W[0] = 0xDEADBEEF; shutdownPacket.DATA.W[1] = 0xDEADBEEF; /* Loop forever */ while (1) { prevEnergizeButton = energizeButton; getEnergizeButton(); switch (currentState) { case NOT_ENERGIZED: if (!energizeButton && prevEnergizeButton) { currentState = ENERGIZED; canSend(energizePacket); } break; case ENERGIZED: if (!energizeButton && prevEnergizeButton) { currentState = NOT_ENERGIZED; canSend(deenergizePacket); } else { getADC(); processAnalog(); if (imp_count < 2 || torque0 < 20) { convertSpeed(); convertTorque(); if (MODE == 0) { canSend(speedPacket); } else { canSend(torquePacket); } } else { currentState = SHUTDOWN; canSend(deenergizePacket); } } break; case SHUTDOWN: canSend(shutdownPacket); SHUTDOWN_CIRCUIT = TRUE; break; } toLED(currentState); //canReceive(); delay(); i++; } }
void main (void) { volatile uint32_t i = 0; /* Dummy idle counter */ uint8_t option; initModesAndClock(); /* Initialize mode entries and system clock */ initPeriClkGen(); /* Initialize peripheral clock generation for DSPIs */ disableWatchdog(); /* Disable watchdog */ initPads(); /* Initialize pads used in example */ initADC(); /* Init. ADC for normal conversions but don't start yet*/ initCTU(); /* Configure desired CTU event(s) */ initEMIOS_0(); /* Initialize eMIOS channels as counter, SAIC, OPWM */ initEMIOS_0ch3(); /* Initialize eMIOS 0 channel 3 as OPWM and channel 2 as SAIC*/ initEMIOS_0ch0(); /* Initialize eMIOS 0 channel 0 as modulus counter*/ initEMIOS_0ch23(); /* Initialize eMIOS 0 channel 23 as modulus counter*/ initEMIOS_0ch4(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 4 as time base */ initEMIOS_0ch6(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 6 as time base */ initEMIOS_0ch7(); /* Initialize eMIOS 0 channel 1 as OPWM, ch 7 as time base */ init_LinFLEX_0_UART(); SIU.PCR[17].R = 0x0200; /* Program the drive enable pin of Right Motor as output*/ SIU.PCR[16].R = 0x0200; /* Program the drive enable pin of Left Motor as output*/ SIU.PGPDO[0].R = 0x00000000; /* Disable the motors */ // set switchs as inputs. .. SIU.PCR[64].R = 0x0100; /* Program the drive enable pin of S1 (PE0) as input*/ SIU.PCR[65].R = 0x0100; /* Program the drive enable pin of S2 (PE1) as input*/ SIU.PCR[68].R = 0x0200; /* Program the drive enable pin of LED1 (PE4) as output*/ SIU.PCR[69].R = 0x0200; /* Program the drive enable pin of LED2 (PE5) as output*/ SIU.PCR[70].R = 0x0200; /* Program the drive enable pin of LED3 (PE6) as output*/ SIU.PCR[71].R = 0x0200; /* Program the drive enable pin of LED4 (PE7) as output*/ SIU.PGPDO[2].R |= 0x0f000000; /* Disable LEDs*/ /* Loop forever */ for (;;) { if((SIU.PGPDI[2].R & 0x80000000) != 0x80000000) { if (correction <= 1300) { correction = 1300; } else { correction -=10; } } else if((SIU.PGPDI[2].R & 0x40000000) != 0x40000000) { if (correction >= 1900) { correction = 1900; } else { correction +=10; } } option = ReadData(); if (option == '6') { CAMERA_simar(); } SERVO (correction); } }
void main (void) { initModesAndClock(); /* Initialize mode entries and system clock */ initPeriClkGen(); /* Initialize peripheral clock generation for DSPIs */ disableWatchdog(); /* Disable watchdog */ initPads(); /* Initialize pads used in example */ initADC(); /* Init. ADC for normal conversions but don't start yet*/ initCTU(); /* Configure desired CTU event(s) */ initEMIOS_0(); /* Initialize eMIOS channels as counter, SAIC, OPWM */ initEMIOS_0ch3(); /* Initialize eMIOS 0 channel 3 as OPWM and channel 2 as SAIC*/ initEMIOS_0ch0(); /* Initialize eMIOS 0 channel 0 as modulus counter*/ initEMIOS_0ch23(); /* Initialize eMIOS 0 channel 23 as modulus counter*/ initEMIOS_0ch4(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 4 as time base */ initEMIOS_0ch6(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 6 as time base */ initEMIOS_0ch7(); /* Initialize eMIOS 0 channel 1 as OPWM, ch 7 as time base */ init_LinFLEX_0_UART(); SIU.PCR[17].R = 0x0200; /* Program the drive enable pin of Right Motor as output*/ SIU.PCR[16].R = 0x0200; /* Program the drive enable pin of Left Motor as output*/ SIU.PGPDO[0].R = 0x00000000; /* Disable the motors */ // Routines DC_Motors_on(); //******************************** INFINITE LOOP *********************************** for (;;) { // 0. DEBUGGING CODE option = ReadData(); printlistall(); // 1. Clean all variables //sensor_value_left =0; //sensor_value_right =0 ; // 2. Sense the line //option = ReadData(); //CAMERA(); CAMERA_simar(); // 3. Calculate Differential for (i=0; i<127; i++) // one less that 128 { diff_result[i] = (short int) (Result[i+1] - Result [i]); // dy.dx } // 4. Find maximum and minimum indices max_val = diff_result[centre]; // can be improved to centre + max_delta min_val = diff_result[centre]; max_I = centre; min_I = centre; for (i=centre-1; i>2; i--) //0 skipped { if (max_val < diff_result[i]) { max_val = diff_result[i]; max_I = i; } if (min_val > diff_result[i]) { min_val = diff_result[i]; min_I = i; } } for (i = centre+1; i<126; i++) { if (max_val < diff_result[i]) { max_val = diff_result[i]; max_I = i; } if (min_val > diff_result[i]) { min_val = diff_result[i]; min_I = i; } } // 6. MAIN STATE MACHINE ALGO width = max_I - min_I; if (width> LB_WIDTH && width < UB_WIDTH) { // Everything is assumed normal. // IF there is a spike (very less probability) // it should be allowed to process, it can't misguide the car. } else { // Width is not right. Check for any psuedo max/ mins. We are assuming that // there must be atleast one pseudo in this case. As width is not right. diff = max_val; if( diff > MIN_FINGER) // M is not pseudo { // sure (according to assumption above) m is pseudo. // Two possibilities when one is pseudo if (max_I < LEFT_GUARD) { // Guard detected min_I = 0; } else { // 5.5 DYNAMIC INTEGRATION TIME /* if ((max_val+ (-min_val))/2 <50 ) { int_time = int_time*1.5; } else if ((max_val+ (-min_val))/2 >90 ) { int_time = int_time*0.85; } */ // Means that there had been a spike // skip the case // Become cautious and increase integration time. //continue; } } else { // M is pseudo and m can also be pseudo // To test m is pseudo or not. diff = min_val; if ( diff > MIN_FINGER) { // m is not pseudo // only M is pseudo if (min_I > RIGHT_GUARD) { // Guard detected max_I = 128; } else { // Means that there had been a spike // skip the case // Become cautious and increase integration time. //continue; } } else { // 5.5 DYNAMIC INTEGRATION TIME if ((max_val+ (-min_val))/2 <50 ) { int_time = int_time*1.5; } else if ((max_val+ (-min_val))/2 >90 ) { int_time = int_time*0.85; } // both are pseudo // So either a ALL_BALCK (CROSS) or ALL_WHITE // Skip the case // Become cautious and increase integration time. continue; } } } // 7. Find centre centre = (max_I + min_I) /2; // 7.5 Filter if (centre-prev_centre >15 || centre-prev_centre <-15) { prev_centre = centre; centre = centre*0.30; } else { prev_centre = centre; } // 8. Calculate error error = centre - 64 ; pid_term = (int) ( kp * error ); // 9. Calculate PID term if (pid_term > SERVO_LIMIT) { pid_term = SERVO_LIMIT; } else if (pid_term <-SERVO_LIMIT) { pid_term = -SERVO_LIMIT; } // 10. Feed the new value to servo motor correction = SERVO_CENTRE + pid_term; SERVO (correction); } }
void main (void) { initModesAndClock(); /* Initialize mode entries and system clock */ initPeriClkGen(); /* Initialize peripheral clock generation for DSPIs */ disableWatchdog(); /* Disable watchdog */ initPads(); /* Initialize pads used in example */ initADC(); /* Init. ADC for normal conversions but don't start yet*/ initCTU(); /* Configure desired CTU event(s) */ initEMIOS_0(); /* Initialize eMIOS channels as counter, SAIC, OPWM */ initEMIOS_0ch3(); /* Initialize eMIOS 0 channel 3 as OPWM and channel 2 as SAIC*/ initEMIOS_0ch0(); /* Initialize eMIOS 0 channel 0 as modulus counter*/ initEMIOS_0ch23(); /* Initialize eMIOS 0 channel 23 as modulus counter*/ initEMIOS_0ch4(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 4 as time base */ initEMIOS_0ch6(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 6 as time base */ initEMIOS_0ch7(); /* Initialize eMIOS 0 channel 1 as OPWM, ch 7 as time base */ //init_LinFLEX_0_UART(); SIU.PCR[17].R = 0x0200; /* Program the drive enable pin of Right Motor as output*/ SIU.PCR[16].R = 0x0200; /* Program the drive enable pin of Left Motor as output*/ SIU.PGPDO[0].R = 0x00000000; /* Disable the motors */ // Routines DC_Motors_on(); //******************************** INFINITE LOOP *********************************** for (;;) { // 1. Clean all variables //sensor_value_left =0; //sensor_value_right =0 ; // 2. Sense the line //option = ReadData(); //CAMERA(); CAMERA_simar(); // 3. Calculate SUMs sum_left =0; sum_right=0; for (i=1; i<64 ; i++) { sum_left += Result[i]; sum_right += Result[i+64]; } // 4. Find Difference error = sum_left - sum_right; // 8. Calculate pid pid_term = (int) ( kp * error ); // 9. Calculate PID term if (pid_term > SERVO_LIMIT) { pid_term = SERVO_LIMIT; } else if (pid_term <-SERVO_LIMIT) { pid_term = -SERVO_LIMIT; } // 10. Feed the new value to servo motor correction = SERVO_CENTRE + pid_term; SERVO (correction); // 11. Debugging code //option = ReadData(); //printserialsingned (error); } }
void main (void) { initModesAndClock(); /* Initialize mode entries and system clock */ initPeriClkGen(); /* Initialize peripheral clock generation for DSPIs */ disableWatchdog(); /* Disable watchdog */ initPads(); /* Initialize pads used in example */ initADC(); /* Init. ADC for normal conversions but don't start yet*/ initCTU(); /* Configure desired CTU event(s) */ initEMIOS_0(); /* Initialize eMIOS channels as counter, SAIC, OPWM */ initEMIOS_0ch3(); /* Initialize eMIOS 0 channel 3 as OPWM and channel 2 as SAIC*/ initEMIOS_0ch0(); /* Initialize eMIOS 0 channel 0 as modulus counter*/ initEMIOS_0ch23(); /* Initialize eMIOS 0 channel 23 as modulus counter*/ initEMIOS_0ch4(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 4 as time base */ initEMIOS_0ch6(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 6 as time base */ initEMIOS_0ch7(); /* Initialize eMIOS 0 channel 1 as OPWM, ch 7 as time base */ init_LinFLEX_0_UART(); SIU.PCR[17].R = 0x0200; /* Program the drive enable pin of Right Motor as output*/ SIU.PCR[16].R = 0x0200; /* Program the drive enable pin of Left Motor as output*/ SIU.PGPDO[0].R = 0x00000000; /* Disable the motors */ // Routines DC_Motors_on(); //******************************** INFINITE LOOP *********************************** for (;;) { // 0. DEBUGGING CODE option = ReadData(); printlistall(); // 1. Clean all variables //sensor_value_left =0; //sensor_value_right =0 ; // 2. Sense the line //option = ReadData(); //CAMERA(); CAMERA_simar(); // 3. Calculate Differential for (i=0;i<127;i++) // one less than 128 { diff_result[i] = (short int) (Result[i+1] - Result [i]); // dy.dx } // 4. Find maximum and minimum indices max_val = diff_result[START]; // can be improved to centre + max_delta min_val = diff_result[START]; max_I = START; min_I = START; for (i=START+1;i<END;i++) //0 skipped { if (max_val < diff_result[i]) { max_val = diff_result[i]; max_I = i; } if (min_val > diff_result[i]) { min_val = diff_result[i]; min_I = i; } } // 6. MAIN STATE MACHINE ALGO width = max_I - min_I; if (width> LB_WIDTH && width < UB_WIDTH) { // Everything is assumed normal. // IF there is a spike (very less probability) // it should be allowed to process, it can't misguide the car. // Find centre if (min_I < L_BOUND) { SERVO (SERVO_CENTRE - SERVO_LIMIT ); continue; } if (max_I > R_BOUND) { SERVO (SERVO_CENTRE + SERVO_LIMIT ); continue; } centre = (max_I + min_I) /2; } else { continue; } // 7.5 if ((centre - prev_centre) > FILTER) { prev_centre = centre; centre = centre * 0.20; } else { prev_centre = centre; } // 8. Calculate error error = centre - 64 ; pid_term = (int) ( kp * error ); // 9. Calculate PID term if (pid_term > SERVO_LIMIT) { pid_term = SERVO_LIMIT; } else if (pid_term <-SERVO_LIMIT) { pid_term = -SERVO_LIMIT; } // 10. Feed the new value to servo motor correction = SERVO_CENTRE + pid_term; SERVO (correction); } }
void main (void) { volatile uint32_t i = 0; /* Dummy idle counter */ uint8_t success=0; uint8_t byteReceived=0; uint8_t opcode=0; uint8_t payload=0; char msg[32]; uint8_t msgLength=0; clock = 0; initModesAndClock(); /* MPC56xxP/B/S: Initialize mode entries, set sysclk = 64 MHz*/ initPeriClkGen(); /* Initialize peripheral clock generation for DSPIs */ disableWatchdog(); /* Disable watchdog */ EXCEP_InitExceptionHandlers(); /* Initialize exceptions: only need to load IVPR */ initADC(); initPIT(); /* Initialize PIT1 for 10Hz IRQ, priority 2 */ initPads(); /* Initialize software interrupt 4 */ initEMIOS_0(); /* Initialize eMIOS channels as counter, SAIC, OPWM */ initEMIOS_1(); /* Initialize eMIOS channels as counter, SAIC, OPWM */ initEMIOS_0ch0(); /* Initialize eMIOS 0 channel 0 as modulus counter*/ initEMIOS_0ch23(); /* Initialize eMIOS 0 channel 23 as modulus counter*/ initEMIOS_0ch8(); /* Initialize eMIOS 0 channel 8 as modulus counter*/ //just to make sure the wheels are facing straight initDrive(); Drive(); SIU.PCR[64].R = 0x0100; /* Program the drive enable pin of S1 (PE0) as input*/ while((SIU.PGPDI[2].R & 0x80000000) == 0x80000000); /*Wait until S1 switch is pressed*/ for(i=0;i<100000;i++); while((SIU.PGPDI[2].R & 0x80000000) != 0x80000000); /*Wait until S1 switch is released*/ INTC_InitVector(); INTC_InstallINTCInterruptHandler(&SwIrq4ISR,4,3); INTC_InstallINTCInterruptHandler(&EOC_ISR,62,5); INTC_InstallINTCInterruptHandler(&Pit1ISR,60,2); INTC_InstallINTCInterruptHandler(&Pit2ISR,61,4); INTC_InstallINTCInterruptHandler(&Pit3ISR,127,4); initSerial(); flag_lineDone = -1; initCamera(); initSteeringController(); INTC_InitINTCInterrupts(); INTC.CPR.B.PRI = 0; /* Single Core: Lower INTC's current priority */ initDrive(); // setPWMRw(48); // setPWMLw(48); setDirection(FORWARD); in = getInBuffer(); out = getOutBuffer(); MESSAGE("I'm running\n\r"); fifo_write(&out->fifo,msg,msgLength); while(isCameraReady()!=STATE_READY); for(;;) { if(!(flag_lineDone==1)) { Drive(); success=0; success = fifo_read(&in->fifo,&byteReceived,1); if(success==1) { if(opcode==0) { opcode = byteReceived; MESSAGE("Command Received: "); fifo_write(&out->fifo,msg,msgLength); } else { payload = byteReceived; switch(opcode) { case DRIVE: // TransmitData("Drive Command\n\r"); MESSAGE("Drive Command\n\r"); fifo_write(&out->fifo,msg,msgLength); setDirection(payload); break; case SET_SPEED: // TransmitData("Set Speed Command\n\r"); MESSAGE("Set Speed Command\n\r"); fifo_write(&out->fifo,msg,msgLength); setPWMRw(payload); setPWMLw(payload); break; case STEERING: // TransmitData("Set Steering Command\n\r"); MESSAGE("Set Steering Command\n\r"); fifo_write(&out->fifo,msg,msgLength); setAngle(payload); break; default: // TransmitData("Bad command\n\r"); MESSAGE("Bad command\n\r"); fifo_write(&out->fifo,msg,msgLength); break; } opcode = 0; } } } if(TRANSMISSION_DONE()&&(out->fifo.length>0)) Tx(); if(RECEPTION_DONE()&&(in->fifo.length<IN_BUFFER_SIZE)) Rx(); } }