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;
}
