void TFC_InitMotorPWM() { //Clock Setup for the TPM requires a couple steps. //1st, set the clock mux //See Page 124 of f the KL25 Sub-Family Reference Manual, Rev. 3, September 2012 SIM_SOPT2 |= SIM_SOPT2_PLLFLLSEL_MASK;// We Want MCGPLLCLK/2 (See Page 196 of the KL25 Sub-Family Reference Manual, Rev. 3, September 2012) SIM_SOPT2 &= ~(SIM_SOPT2_TPMSRC_MASK); SIM_SOPT2 |= SIM_SOPT2_TPMSRC(1); //We want the MCGPLLCLK/2 (See Page 196 of the KL25 Sub-Family Reference Manual, Rev. 3, September 2012) //Enable the Clock to the FTM0 Module //See Page 207 of f the KL25 Sub-Family Reference Manual, Rev. 3, September 2012 SIM_SCGC6 |= SIM_SCGC6_TPM0_MASK; //The TPM Module has Clock. Now set up the peripheral //Blow away the control registers to ensure that the counter is not running TPM0_SC = 0; TPM0_CONF = 0; //Set TPM module to continue in debug mode TPM0_CONF |= 0x00000060; //While the counter is disabled we can setup the prescaler TPM0_SC = TPM_SC_PS(FTM0_CLK_PRESCALE); //Setup the mod register to get the correct PWM Period TPM0_MOD = FTM0_CLOCK/(1<<FTM0_CLK_PRESCALE)/FTM0_OVERFLOW_FREQUENCY; //Setup Channels 0,1,2,3 TPM0_C0SC = TPM_CnSC_MSB_MASK | TPM_CnSC_ELSB_MASK; TPM0_C1SC = TPM_CnSC_MSB_MASK | TPM_CnSC_ELSB_MASK; TPM0_C2SC = TPM_CnSC_MSB_MASK | TPM_CnSC_ELSB_MASK; TPM0_C3SC = TPM_CnSC_MSB_MASK | TPM_CnSC_ELSB_MASK; //Enable the Counter //Set the Default duty cycle to 50% duty cycle TFC_SetMotorPWM(0.0,0.0); //Enable the TPM Counter TPM0_SC |= TPM_SC_CMOD(1); //Enable the FTM functions on the the port PORTC_PCR1 = PORT_PCR_MUX(4); PORTC_PCR2 = PORT_PCR_MUX(4); PORTC_PCR3 = PORT_PCR_MUX(4); PORTC_PCR4 = PORT_PCR_MUX(4); }
void update() { motors[0] = (((commands[0]*10+commands[1])-10)/10.0); motors[1] = (((commands[2]*10+commands[3])-10)/10.0); motors[2] = (((commands[4]*10+commands[5])-10)/10.0); commands[6] = commands[6]; TFC_SetMotorPWM(motors[0],motors[1]); TFC_SetServo(0,motors[2]); TFC_SetBatteryLED_Level(commands[6]); }
void TFC_Init() { TFC_InitClock(); TFC_InitSysTick(); TFC_InitGPIO(); TFC_InitServos(); TFC_InitMotorPWM(); TFC_InitADCs(); TFC_InitLineScanCamera(); TFC_InitTerminal(); TFC_InitUARTs(); TFC_HBRIDGE_DISABLE; TFC_SetMotorPWM(0,0); }
void TFC_Init() { InitClock(); /* Initialize clock system for 48 MHz */ InitSysTick(); /* Configure the timer and the interrupt to be used to generate the tick of the scheduler */ TFC_InitGPIO(); TFC_InitServos(); TFC_InitMotorPWM(); TFC_InitADCs(); TFC_InitLineScanCamera(); TFC_InitTerminal(); TFC_InitUARTs(); TFC_HBRIDGE_DISABLE; TFC_SetMotorPWM(0,0); TFC_RGB_Init(); TFC_Accel_Init(); }
int main(void) { uint32_t t,i=0, ana=0; int guardar,dato=0; //int32_t guardar; TFC_Init(); for(;;) { //TFC_Task must be called in your main loop. This keeps certain processing happy (I.E. Serial port queue check) TFC_Task(); //This Demo program will look at the middle 2 switch to select one of 4 demo modes. //Let's look at the middle 2 switches switch(dato) { default: case 0 : TFC_SetMotorPWM(0,0); TFC_SetServo(0,0); //Demo mode 0 just tests the switches and LED's if(TFC_PUSH_BUTTON_1_PRESSED) dato=3; break; case 1: //Demo mode 1 will just move the servos with the on-board potentiometers if(TFC_Ticker[0]>=20) { TFC_Ticker[0] = 0; //reset the Ticker //Every 20 mSeconds, update the Servos TFC_SetServo(0,TFC_ReadPot(0)); TFC_SetServo(1,TFC_ReadPot(1)); } //Let's put a pattern on the LEDs if(TFC_Ticker[1] >= 125) { TFC_Ticker[1] = 0; t++; if(t>4) { t=0; } TFC_SetBatteryLED_Level(t); } TFC_SetMotorPWM(0,0); //Make sure motors are off TFC_HBRIDGE_DISABLE; break; case 2 : //Demo Mode 2 will use the Pots to make the motors move TFC_HBRIDGE_ENABLE; TFC_SetMotorPWM(TFC_ReadPot(0),TFC_ReadPot(0)); //Let's put a pattern on the LEDs if(TFC_Ticker[1] >= 125) { TFC_Ticker[1] = 0; t++; if(t>4) { t=0; } TFC_SetBatteryLED_Level(t); } break; case 3 : if(TFC_PUSH_BUTTON_0_PRESSED){ dato=0; } ana = 4096; if(TFC_Ticker[0]>100 && LineScanImageReady==1) { TFC_Ticker[0] = 0; LineScanImageReady=0; //TERMINAL_PRINTF("\r\n"); //TERMINAL_PRINTF("L:"); if(t==0) t=3; else t--; TFC_SetBatteryLED_Level(t); for(i=0;i<128;i++){ //TERMINAL_PRINTF("%x,",LineScanImage0[i]); //x hexa, i integer, f float...... if(LineScanImage0[i]<ana){ ana = LineScanImage0[i]; guardar = i; } } guardar = guardar - 64; TERMINAL_PRINTF("\r\n"); //TERMINAL_PRINTF("%i",guardar); TFC_SetServo(0,(float)guardar/64.0f); //Rescale to -1.0 to 1.0 } TFC_HBRIDGE_ENABLE; if((float)guardar < -5 && (float)guardar > 5){ if(guardar < 0){ guardar = guardar*(-1); } TFC_SetMotorPWM((float)guardar/100.0f,(float)guardar/100.0f); } break; } } return 0; }