void TFC_InitServos() { //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 FTM1 Module //See Page 207 of f the KL25 Sub-Family Reference Manual, Rev. 3, September 2012 SIM_SCGC6 |= SIM_SCGC6_TPM1_MASK; //The TPM Module has Clock. Now set up the peripheral //Blow away the control registers to ensure that the counter is not running TPM1_SC = 0; TPM1_CONF = 0; //While the counter is disabled we can setup the prescaler TPM1_SC = TPM_SC_PS(FTM1_CLK_PRESCALE); TPM1_SC |= TPM_SC_TOIE_MASK; //Enable Interrupts for the Timer Overflow //Setup the mod register to get the correct PWM Period TPM1_MOD = FTM1_CLOCK/(1<<(FTM1_CLK_PRESCALE+1))/FTM1_OVERFLOW_FREQUENCY; //Setup Channels 0 and 1 TPM1_C0SC = TPM_CnSC_MSB_MASK | TPM_CnSC_ELSB_MASK; TPM1_C1SC = TPM_CnSC_MSB_MASK | TPM_CnSC_ELSB_MASK; //Enable the Counter //Set the Default duty cycle to servo neutral TFC_SetServo(0, 0.0); TFC_SetServo(1, 0.0); //Enable the TPM COunter TPM1_SC |= TPM_SC_CMOD(1); //Enable TPM1 IRQ on the NVIC enable_irq (INT_TPM1-16); //Enable the FTM functions on the the port PORTB_PCR0 = PORT_PCR_MUX(3); PORTB_PCR1 = PORT_PCR_MUX(3); }
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]); }
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; }