/**
* Changes the motor speed when the semaphore
*/
void vMotorTask()
{
xSemaphoreTake(changeSpeed, 0);
for (;;)
{
xSemaphoreTake(changeSpeed, portMAX_DELAY);
if (lDirection == 0) //Going forward
{
MOTOR_L_DIR1_PORT &= ~MOTOR_L_DIR1_PIN;
MOTOR_L_DIR2_PORT |= MOTOR_L_DIR2_PIN;
}
else //Going backward
{
MOTOR_L_DIR2_PORT &= ~MOTOR_L_DIR2_PIN;
MOTOR_L_DIR1_PORT |= MOTOR_L_DIR1_PIN;
}
if (rDirection == 0) //Going forward
{
MOTOR_R_DIR1_PORT &= ~MOTOR_R_DIR1_PIN;
MOTOR_R_DIR2_PORT |= MOTOR_R_DIR2_PIN;
}
else //Going backward
{
MOTOR_R_DIR2_PORT &= ~MOTOR_R_DIR2_PIN;
MOTOR_R_DIR1_PORT |= MOTOR_R_DIR1_PIN;
}
ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_2, rSpeed);
ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_3, lSpeed);
//PWM write MOTOR_L_PWM_PORT |= MOTOR_L_PWM_PIN
//PWM write MOTOR_R_PWM_PORT |= MOTOR_R_PWM_PIN
}
}
void MCInitPwm(uint32_t DutyCycle)
{
// Clock PWM peripheral at SysClk/PWMDIV
ROM_SysCtlPWMClockSet(SYSCTL_PWMDIV_64);
// Enable peripherals
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_PWM1);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD);
// Configure pin PD0 as PWM output
ROM_GPIOPinTypePWM(GPIO_PORTD_BASE, GPIO_PIN_0|GPIO_PIN_1);
ROM_GPIOPinConfigure(GPIO_PD0_M1PWM0);
ROM_GPIOPinConfigure(GPIO_PD1_M1PWM1);
// Calculate PWM clock
uint32_t PWMClock = SysCtlClockGet() / 64;
// Value from/to which PWM counter counts (subtract 1 becouse counter counts from 0). This is PWM period
LoadValue = (PWMClock/PWM_FREQUENCY) - 1;
// Configure PWM
ROM_PWMGenConfigure(PWM1_BASE, PWM_GEN_0, PWM_GEN_MODE_DOWN);
ROM_PWMGenPeriodSet(PWM1_BASE, PWM_GEN_0, LoadValue);
// Set PWM signal width
ROM_PWMPulseWidthSet(PWM1_BASE, PWM_OUT_1, (DutyCycle*LoadValue)/DUTY_CYCLE_DIVIDER);
ROM_PWMPulseWidthSet(PWM1_BASE, PWM_OUT_0, (DutyCycle*LoadValue)/DUTY_CYCLE_DIVIDER);
// Enable PWM output 0 and 1
ROM_PWMOutputState(PWM1_BASE, PWM_OUT_0_BIT|PWM_OUT_1_BIT, true);
// Enable PWM generator
ROM_PWMGenEnable(PWM1_BASE, PWM_GEN_0);
}
int xMotorDriverInit()
{
SYSCTL_RCGCGPIO_R |= SYSCTL_RCGCGPIO_R10 | SYSCTL_RCGCGPIO_R5;
vSemaphoreCreateBinary(changeSpeed);
ROM_GPIOPinTypePWM(MOTOR_L_PWM_PORT, MOTOR_L_PWM_PIN);
ROM_GPIOPinConfigure(GPIO_PF2_M0PWM2);
ROM_GPIOPinTypePWM(MOTOR_R_PWM_PORT, MOTOR_R_PWM_PIN);
ROM_GPIOPinConfigure(GPIO_PF3_M0PWM3);
GPIO_PORTL_DIR_R |= 0xF;
GPIO_PORTL_DEN_R |= 0xF;
PWM0_1_GENA_R = 0x8C;
PWM0_1_GENB_R = 0x8C;
ROM_PWMGenConfigure(PWM0_BASE, PWM_GEN_1, PWM_GEN_MODE_DOWN | PWM_GEN_MODE_GEN_NO_SYNC);
ROM_PWMGenPeriodSet(PWM0_BASE, PWM_GEN_1, 256);
ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_2, 25);
ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_3, 25);
ROM_PWMGenEnable(PWM0_BASE, PWM_GEN_1);
MOTOR_L_DIR1_PORT &= ~MOTOR_L_DIR1_PIN;
MOTOR_L_DIR2_PORT |= MOTOR_L_DIR2_PIN;
MOTOR_R_DIR1_PORT &= ~MOTOR_R_DIR1_PIN;
MOTOR_R_DIR2_PORT |= MOTOR_R_DIR2_PIN;
PWM0_1_CTL_R |= 0x5;
PWM0_ENABLE_R |= 0x4 | 0x8;
return 0;
}
// * svm_init *****************************************************************
// * setup pins and PWM hardware to talk to servomotors *
// * Assumes system clock already configured *
// ****************************************************************************
void svm_init(void)
{
ROM_SysCtlPeripheralEnable(SVM_OUTPUT_PORT); // enable clock to GPIO port
ROM_SysCtlPeripheralReset(SVM_OUTPUT_PORT); // reset to clear any previous config
// configure output pads
GPIOPinTypeGPIOOutput(SVM_OUTPUT_PORT_BASE, SVM_OUTPUT_PINS);
// configure PWM
ROM_SysCtlPeripheralEnable(SVM_PWM_MODULE); // enable clock to pwm module
ROM_SysCtlPWMClockSet(SVM_PWM_FPWM_DIV); // configure clock divider to derive Fpwm from Fsys
// wrap 16b PWM counter at 1041 for 3kHz pwm output
ROM_PWMDeadBandDisable(SVM_PWM_BASE, SVM_PWM_GEN); // allow PWM0, PWM1 to behave independently
ROM_PWMGenConfigure(SVM_PWM_BASE, SVM_PWM_GEN, // configure pwm generator
PWM_GEN_MODE_DOWN | // up/down count for center timed PWM
PWM_GEN_MODE_NO_SYNC); // outputs from generator behave independently
ROM_PWMGenPeriodSet(SVM_PWM_BASE, SVM_PWM_GEN, // sets period for generator to appropriate period
SVM_PWM_PERIOD_TICKS);
ROM_PWMPulseWidthSet(SVM_PWM_BASE, SVM_1_PWM, 0); // set initial pulse widths to 0 for safety
ROM_PWMPulseWidthSet(SVM_PWM_BASE, SVM_2_PWM, 0); // set initial pulse widths to 0 for safety
ROM_GPIOPinConfigure(SVM_1_PWM_MUX); // select mux for pwm output hbridge 1
ROM_GPIOPinConfigure(SVM_2_PWM_MUX); // select mux for pwm output hbridge 2
ROM_GPIOPinTypePWM(SVM_OUTPUT_PORT_BASE, SVM_1_PWM_PIN);// do some other step configuring pwm...
ROM_GPIOPinTypePWM(SVM_OUTPUT_PORT_BASE, SVM_2_PWM_PIN);// ...exact function is unknown
ROM_PWMOutputState(SVM_PWM_BASE, SVM_1_PWM_BIT | SVM_2_PWM_BIT, true);
// enable outputs from pwm generator to pins
ROM_PWMGenEnable(SVM_PWM_BASE, SVM_PWM_GEN); // enable pwm output generator
}
//*****************************************************************************
//
// Esta tarefa faz o ponteiro do servo se movimentar para a esquerda ou direita.
// O usuario seleciona o lado a partir dos botoes SW1 ou SW2
//
//*****************************************************************************
static void
TarefaServo(void *pvParameters)
{
// Variaveis para programar PWM (Tipo volateis: compilador nao elimina-as)
volatile uint32_t ui32Load;
volatile uint32_t ui32PWMClock;
volatile uint8_t ui8Adjust;
ui8Adjust = 83; //83: posicao central pra criar um pulso de 1.5mS do PWM.
uint8_t i8Message;
ui32PWMClock = SysCtlClockGet() / 64; // divisor 64 roda o clock a 625kHz.
ui32Load = (ui32PWMClock / PWM_FREQUENCY) - 1;
PWMGenConfigure(PWM1_BASE, PWM_GEN_0, PWM_GEN_MODE_DOWN);
PWMGenPeriodSet(PWM1_BASE, PWM_GEN_0, ui32Load);
ROM_PWMPulseWidthSet(PWM1_BASE, PWM_OUT_0, ui8Adjust * ui32Load / 1000);
ROM_PWMOutputState(PWM1_BASE, PWM_OUT_0_BIT, true);
ROM_PWMGenEnable(PWM1_BASE, PWM_GEN_0);
while(1)
{
if(xQueueReceive(g_FilaServo, &i8Message, 0) == pdPASS)
{
if(i8Message == LEFT_BUTTON)
{
//Movimento sendo definido PWM
ui8Adjust += 15;
if (ui8Adjust > 155) ui8Adjust = 155;
// Ajuste do tamanho do pulso PWM
ROM_PWMPulseWidthSet(PWM1_BASE, PWM_OUT_0, ui8Adjust * ui32Load / 1000);
xSemaphoreTake(g_pUARTSemaphore, portMAX_DELAY);
UARTprintf("Servo moveu-se para a posicao %d.\n\n", ui8Adjust);
xSemaphoreGive(g_pUARTSemaphore);
}
if(i8Message == RIGHT_BUTTON)
{
// Movimento sendo definido PWM
ui8Adjust -= 15;
if (ui8Adjust < 40) ui8Adjust = 40;
// Ajuste do tamanho do pulso PWM
ROM_PWMPulseWidthSet(PWM1_BASE, PWM_OUT_0, ui8Adjust * ui32Load / 1000);
//
// Proteger o MUTEX do UART
xSemaphoreTake(g_pUARTSemaphore, portMAX_DELAY);
UARTprintf("Servo moveu-se para a posicao %d.\n\n", ui8Adjust);
xSemaphoreGive(g_pUARTSemaphore);
}
}
}
}
// * svm_set_pulse ************************************************************
// * update state of svm line *
// * ucSvm should be one of: SVM_1, SVM_2 *
// * ulWidth is in units of ticks and should not be larger than *
// * SVM_PWM_PERIOD_TICKS - 1 *
// * Assumes pins already set up (svm_init()). *
// ****************************************************************************
void svm_set_pulse(unsigned char ucSvm, unsigned long ulWidth)
{
if(ulWidth > (SVM_PWM_PERIOD_TICKS - 1)) // ceiling ulWidth at max legal value
ulWidth = (SVM_PWM_PERIOD_TICKS - 1);
if(ucSvm == SVM_1)
ROM_PWMPulseWidthSet(SVM_PWM_BASE, SVM_1_PWM, ulWidth);
// update ceilinged pulse width to appropriate svm
else if(ucSvm == SVM_2)
ROM_PWMPulseWidthSet(SVM_PWM_BASE, SVM_2_PWM, ulWidth);
// update ceilinged pulse width to appropriate svm
}
void pwm_duty(int duty){
volatile uint32_t ui32Load;
volatile uint32_t ui32PWMClock;
volatile uint32_t divisor;
ui32PWMClock = SysCtlClockGet() / 64;
ui32Load = (ui32PWMClock / PWM_FREQUENCY) - 1;
divisor = duty*ui32Load/100;
ROM_PWMGenDisable(PWM1_BASE, PWM_GEN_0);
ROM_PWMPulseWidthSet(PWM1_BASE, PWM_OUT_0, divisor);
ROM_PWMGenEnable(PWM1_BASE, PWM_GEN_0);
}
void motor_right(uint32_t pwm_value)
{
//PWMPulseWidthSet(PWM1_BASE, PWM_OUT_2, pwm_value*ui32Load/1000);
ROM_PWMPulseWidthSet(PWM1_BASE, PWM_OUT_2, pwm_value*ui32Load/1000);
}
void cfg_PWM()
{
ROM_SysCtlClockSet(SYSCTL_SYSDIV_5|SYSCTL_USE_PLL|SYSCTL_OSC_MAIN|SYSCTL_XTAL_16MHZ); //Configure clock to 40MHz
ROM_SysCtlPWMClockSet(SYSCTL_PWMDIV_64); //Configure PWM clock to 625kHz (40.000.000/64)
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_PWM0); //Enable PWM_0 output
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_PWM1); //Enable PWM_1 output
ui32PWMClock = SysCtlClockGet() / 64; //Configure clock for PWM - 50Hz (ESCs and Servo motor default)
ui32Load = (ui32PWMClock / PWM_FREQUENCY) - 1;
/*********************CONFIG_PWM_1*********************************/
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB); //Enable PWM_0/0 output
ROM_GPIOPinTypePWM(GPIO_PORTB_BASE, GPIO_PIN_6); //Set PB6 for PWM output
ROM_GPIOPinConfigure(GPIO_PB6_M0PWM0); //Configure PB6 as PWM_0/0 output
PWMGenConfigure(PWM0_BASE, PWM_GEN_0, PWM_GEN_MODE_DOWN | PWM_GEN_MODE_NO_SYNC);
PWMGenPeriodSet(PWM0_BASE, PWM_GEN_0, ui32Load);
ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_0,1); //MÁX=1562 MIN=937 - 55Hz (1.5ms----2.5ms)
/*********************END_CONFIG_PWM_1*****************************/
/*********************CONFIG_PWM_2*********************************/
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB); //Enable PWM_0/1 output
ROM_GPIOPinTypePWM(GPIO_PORTB_BASE, GPIO_PIN_7); //Set PB7 for PWM output
ROM_GPIOPinConfigure(GPIO_PB7_M0PWM1); //Configure PB7 as PWM_0/1 output
PWMGenConfigure(PWM0_BASE, PWM_GEN_1, PWM_GEN_MODE_DOWN | PWM_GEN_MODE_NO_SYNC);
PWMGenPeriodSet(PWM0_BASE, PWM_GEN_1, ui32Load);
ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_1,1); //MÁX=1562 MIN=937 - 55Hz (1.5ms----2.5ms)
/*********************END_CONFIG_PWM_2*****************************/
/*********************CONFIG_PWM_3*********************************/
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB); //Enable PWM_0/2 output
ROM_GPIOPinTypePWM(GPIO_PORTB_BASE, GPIO_PIN_4); //Set PB4 for PWM output
ROM_GPIOPinConfigure(GPIO_PB4_M0PWM2); //Configure PB7 as PWM_0/2 output
PWMGenConfigure(PWM0_BASE, PWM_GEN_2, PWM_GEN_MODE_DOWN | PWM_GEN_MODE_NO_SYNC);
PWMGenPeriodSet(PWM0_BASE, PWM_GEN_2, ui32Load);
ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_2,1); //MÁX=1562 MIN=937 - 55Hz (1.5ms----2.5ms)
/*********************END_CONFIG_PWM_3*****************************/
/*********************CONFIG_PWM_4*********************************/
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB); //Enable PWM_0/3 output
ROM_GPIOPinTypePWM(GPIO_PORTB_BASE, GPIO_PIN_5); //Set PB5 for PWM output
ROM_GPIOPinConfigure(GPIO_PB5_M0PWM3); //Configure PB5 as PWM_0/3 output
PWMGenConfigure(PWM0_BASE, PWM_GEN_3, PWM_GEN_MODE_DOWN | PWM_GEN_MODE_NO_SYNC);
PWMGenPeriodSet(PWM0_BASE, PWM_GEN_3, ui32Load);
ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_3,1); //MÁX=1562 MIN=937 - 55Hz (1.5ms----2.5ms)
/*********************END_CONFIG_PWM_4*****************************/
/*********************CONFIG_PWM_5*********************************/
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD); //Enable PWM_1/0 output
ROM_GPIOPinTypePWM(GPIO_PORTD_BASE, GPIO_PIN_0); //Set PD0 for PWM output
ROM_GPIOPinConfigure(GPIO_PD0_M1PWM0); //Configure PD0 as PWM_1/0 output
PWMGenConfigure(PWM1_BASE, PWM_GEN_0, PWM_GEN_MODE_DOWN | PWM_GEN_MODE_NO_SYNC);
PWMGenPeriodSet(PWM1_BASE, PWM_GEN_0, ui32Load);
ROM_PWMPulseWidthSet(PWM1_BASE, PWM_OUT_0,ui32Load/2); //MÁX=1562 MIN=937 - 55Hz (1.5ms----2.5ms)
/*********************END_CONFIG_PWM_5*****************************/
/*********************CONFIG_PWM_6*********************************/
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD); //Enable PWM_1/1 output
ROM_GPIOPinTypePWM(GPIO_PORTD_BASE, GPIO_PIN_1); //Set PD1 for PWM output
ROM_GPIOPinConfigure(GPIO_PD1_M1PWM1); //Configure PD1 as PWM_1/1 output
PWMGenConfigure(PWM1_BASE, PWM_GEN_1, PWM_GEN_MODE_DOWN | PWM_GEN_MODE_NO_SYNC);
PWMGenPeriodSet(PWM1_BASE, PWM_GEN_1, ui32Load);
ROM_PWMPulseWidthSet(PWM1_BASE, PWM_OUT_1,ui32Load/2); //MÁX=1562 MIN=937 - 55Hz (1.5ms----2.5ms)
/*********************END_CONFIG_PWM_6*****************************/
ROM_PWMOutputState(PWM0_BASE, PWM_OUT_0_BIT, true);
ROM_PWMOutputState(PWM0_BASE, PWM_OUT_1_BIT, true);
ROM_PWMOutputState(PWM0_BASE, PWM_OUT_2_BIT, true);
ROM_PWMOutputState(PWM0_BASE, PWM_OUT_3_BIT, true);
ROM_PWMOutputState(PWM1_BASE, PWM_OUT_0_BIT, true);
ROM_PWMOutputState(PWM1_BASE, PWM_OUT_1_BIT, true);
ROM_PWMGenEnable(PWM0_BASE, PWM_GEN_0);
ROM_PWMGenEnable(PWM0_BASE, PWM_GEN_1);
ROM_PWMGenEnable(PWM0_BASE, PWM_GEN_2);
ROM_PWMGenEnable(PWM0_BASE, PWM_GEN_3);
ROM_PWMGenEnable(PWM1_BASE, PWM_GEN_0);
ROM_PWMGenEnable(PWM1_BASE, PWM_GEN_1);
}
void SendServo(uint8_t SelServo,uint8_t ValServo)
{
ui16Adjust=(degree*ValServo+min_c*10);
ui16Adjust=ui16Adjust/10;
if(ui16Adjust < min_c) ui16Adjust = min_c;
else if(ui16Adjust > max_c) ui16Adjust = max_c;
#ifdef DEBUG
UARTprintf("\n\nSERVO:%d,VALOR:%d",SelServo,ui16Adjust);
#endif
switch(SelServo)
{
case 1:
if(ValServo >= 0 && ValServo <= 180)
{
ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_0, ui16Adjust);
}
else
UARTprintf("\n\rValor excedente aos limites!");
break;
case 2:
if(ValServo >= 0 && ValServo <= 180)
{
ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_1, ui16Adjust);
}
else
UARTprintf("\n\rValor excedente aos limites!");
break;
case 3:
if(ValServo >= 0 && ValServo <= 180)
{
ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_2, ui16Adjust);
}
else
UARTprintf("\n\rValor excedente aos limites!");
break;
case 4:
if(ValServo >= 0 && ValServo <= 180)
{
ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_3, ui16Adjust);
}
else
UARTprintf("\n\rValor excedente aos limites!");
break;
case 5:
if(ValServo >= 0 && ValServo <= 180)
{
ROM_PWMPulseWidthSet(PWM1_BASE, PWM_OUT_0, ui16Adjust);
}
else
UARTprintf("\n\rValor excedente aos limites!");
break;
case 6:
if(ValServo >= 0 && ValServo <= 180)
{
ROM_PWMPulseWidthSet(PWM1_BASE, PWM_OUT_1, ui16Adjust);
}
else
UARTprintf("\n\rValor excedente aos limites!");
break;
default:
UARTprintf("\n\rServo não reconhecido!");
break;
}
//delay_ms(15);
}
void read_pb_pwm(uint16_t *temp_min,uint16_t *temp_max)
{
if(ROM_GPIOPinRead(GPIO_PORTF_BASE,GPIO_PIN_4)==0x00)
{
ui16Adjust--;
if (ui16Adjust < *temp_min)
{
ui16Adjust = *temp_min;
}
ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_0, ui16Adjust);
ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_1, ui16Adjust);
ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_2, ui16Adjust);
ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_3, ui16Adjust);
ROM_PWMPulseWidthSet(PWM1_BASE, PWM_OUT_0, ui16Adjust);
ROM_PWMPulseWidthSet(PWM1_BASE, PWM_OUT_1, ui16Adjust);
}
if(ROM_GPIOPinRead(GPIO_PORTF_BASE,GPIO_PIN_0)==0x00)
{
ui16Adjust++;
if (ui16Adjust > *temp_max)
{
ui16Adjust = *temp_max;
}
ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_0, ui16Adjust);
ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_1, ui16Adjust);
ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_2, ui16Adjust);
ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_3, ui16Adjust);
ROM_PWMPulseWidthSet(PWM1_BASE, PWM_OUT_0, ui16Adjust);
ROM_PWMPulseWidthSet(PWM1_BASE, PWM_OUT_1, ui16Adjust);
}
//UARTprintf("\n\rValor do PWM:%d",ui16Adjust);
//delay_ms(1);
}
void MCPwmDutyCycleSet(Motor selectedMotor, uint32_t DutyCycle)
{
ROM_PWMPulseWidthSet(PWM1_BASE, selectedMotor, (DutyCycle*LoadValue)/100 );
}
//*****************************************************************************
//
// This example demonstrates how to setup the PWM block to generate signals.
//
//*****************************************************************************
int
main(void)
{
tRectangle sRect;
unsigned long ulPeriod;
//
// Set the clocking to run directly from the crystal.
//
ROM_SysCtlClockSet(SYSCTL_SYSDIV_1 | SYSCTL_USE_OSC | SYSCTL_OSC_MAIN |
SYSCTL_XTAL_8MHZ);
ROM_SysCtlPWMClockSet(SYSCTL_PWMDIV_1);
//
// Initialize the display driver.
//
Formike128x128x16Init();
//
// Turn on the backlight.
//
Formike128x128x16BacklightOn();
//
// Initialize the graphics context.
//
GrContextInit(&g_sContext, &g_sFormike128x128x16);
//
// Fill the top 15 rows of the screen with blue to create the banner.
//
sRect.sXMin = 0;
sRect.sYMin = 0;
sRect.sXMax = GrContextDpyWidthGet(&g_sContext) - 1;
sRect.sYMax = 14;
GrContextForegroundSet(&g_sContext, ClrDarkBlue);
GrRectFill(&g_sContext, &sRect);
//
// Put a white box around the banner.
//
GrContextForegroundSet(&g_sContext, ClrWhite);
GrRectDraw(&g_sContext, &sRect);
//
// Put the application name in the middle of the banner.
//
GrContextFontSet(&g_sContext, g_pFontFixed6x8);
GrStringDrawCentered(&g_sContext, "pwmgen", -1,
GrContextDpyWidthGet(&g_sContext) / 2, 7, 0);
//
// Tell the user what is happening.
//
GrStringDrawCentered(&g_sContext, "Generating PWM on", -1,
GrContextDpyWidthGet(&g_sContext) / 2, 56, 0);
GrStringDrawCentered(&g_sContext, "pins PWM0 and PWM1", -1,
GrContextDpyWidthGet(&g_sContext) / 2, 68, 0);
//
// Enable the peripherals used by this example.
//
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_PWM0);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
//
// Set GPIO F0 and F1 as PWM pins. They are used to output the PWM0 and
// PWM1 signals.
//
ROM_GPIOPinTypePWM(GPIO_PORTF_BASE, GPIO_PIN_0 | GPIO_PIN_1);
ROM_GPIOPadConfigSet(GPIO_PORTF_BASE, GPIO_PIN_0 | GPIO_PIN_1,
GPIO_STRENGTH_8MA, GPIO_PIN_TYPE_STD);
//
// Compute the PWM period based on the system clock.
//
ulPeriod = ROM_SysCtlClockGet() / 8000;
//
// Set the PWM period to 440 (A) Hz.
//
ROM_PWMGenConfigure(PWM0_BASE, PWM_GEN_0,
PWM_GEN_MODE_UP_DOWN | PWM_GEN_MODE_NO_SYNC);
ROM_PWMGenPeriodSet(PWM0_BASE, PWM_GEN_0, ulPeriod);
//
// Set PWM0 to a duty cycle of 50% and PWM1 to a duty cycle of 80%.
//
ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_0, ((ulPeriod * 8) / 10));
ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_1, ((ulPeriod * 2) / 10));
//
// Enable the PWM0 and PWM1 output signals.
//
ROM_PWMOutputState(PWM0_BASE, PWM_OUT_0_BIT | PWM_OUT_1_BIT, true);
//
// Enable the PWM generator.
//
ROM_PWMGenEnable(PWM0_BASE, PWM_GEN_0);
//
// Loop forever while the PWM signals are generated.
//
while(1)
{
}
}
// Asservissement en vitesse des moteurs pour qu'il atteignent leur propre consigne
void asservirMoteurs(void){
pos0 = position_m3; //pas inversé
pos1 = pos1 + QEIVelocityGet(QEI1_BASE)*QEIDirectionGet(QEI1_BASE); //pas inversé
pos2 = position_m2;//pas inversé
pos3 = pos3 - QEIVelocityGet(QEI0_BASE)*QEIDirectionGet(QEI0_BASE); //inversé
ajustementVitesse();
//Motor 0
measured_speed0 = (pos0 - previous_pos0)/dt;
if(measured_speed0 < 0){
measured_speed0 = -measured_speed0;
}
previous_pos0 = pos0;
if(consigne0 > 3200){
output0 = PIDHandler0(&consigne0, &measured_speed0, &I0, &previous_error0, dt);
}
else{
output0 = SlowPIDHandler0(&consigne0, &measured_speed0, &I0, &previous_error0, dt);
}
//Motor 1
measured_speed1 = QEIVelocityGet(QEI1_BASE)/dt;//(pos1 - previous_pos1)*10;
if(measured_speed1 < 0){
measured_speed1 = -measured_speed1;
}
previous_pos1 = pos1;
if(consigne1 > 3200){
output1 = PIDHandler1(&consigne1, &measured_speed1, &I1, &previous_error1, dt);
}
else{
output1 = SlowPIDHandler1(&consigne1, &measured_speed1, &I1, &previous_error1, dt);
}
//Motor 2
measured_speed2 = (pos2 - previous_pos2)/dt;
if(measured_speed2 < 0){
measured_speed2 = -measured_speed2;
}
previous_pos2 = pos2;
if(consigne2 > 3200){
output2 = PIDHandler2(&consigne2, &measured_speed2, &I2, &previous_error2, dt);
}
else{
output2 = SlowPIDHandler2(&consigne2, &measured_speed2, &I2, &previous_error2, dt);
}
//Motor 3
measured_speed3 = QEIVelocityGet(QEI0_BASE)/dt;//(pos3 - previous_pos3)*10;
if(measured_speed3 < 0){
measured_speed3 = -measured_speed3;
}
previous_pos3 = pos3;
if(consigne3 > 3200){
output3 = PIDHandler3(&consigne3, &measured_speed3, &I3, &previous_error3, dt);
}
else{
output3 = SlowPIDHandler3(&consigne3, &measured_speed3, &I3, &previous_error3, dt);
}
/*output0 = output0_old +(dt/Tf0)*(output0-output0_old);
output1 = output1_old +(dt/Tf1)*(output1-output1_old);
output2 = output2_old +(dt/Tf2)*(output2-output2_old);
output3 = output3_old +(dt/Tf3)*(output3-output3_old);
output0_old = output0;
output1_old = output1;
output2_old = output2;
output3_old = output3;*/
//output0_table[index%10] = output0;
//output1_table[index%10] = output1;
//output2_table[index%10] = output2;
//output3_table[index%10] = output3;
//Traduction 6400e de tour fraction appliqué au pulse width
float fraction0;
float fraction1;
float fraction2;
float fraction3;
//Une équation linéaire est utilisée x*0.5/7700 = % du duty cycle
fraction0 = ((output0*0.5)/7700);
if(fraction0 > 0.99){
fraction0 = 0.99;
}
else if(fraction0 < 0){
fraction0 = 0;
}
fraction1 = ((output1*0.5)/7700);
if(fraction1 > 0.99){
fraction1 = 0.99;
}else if(fraction1 < 0){
fraction1 = 0;
}
fraction2 = ((output2*0.5)/7700);
if(fraction2 > 0.99){
fraction2 = 0.99;
}else if(fraction2 < 0){
fraction2 = 0;
}
fraction3 = ((output3*0.5)/7700);
if(fraction3 > 0.99){
fraction3 = 0.99;
}else if(fraction3 < 0){
fraction3 = 0;
}
PWMPulseWidthSet(PWM_BASE, PWM_OUT_0, (periodPWM*fraction0));
PWMPulseWidthSet(PWM_BASE, PWM_OUT_1, (periodPWM*fraction1));
PWMPulseWidthSet(PWM_BASE, PWM_OUT_2, (periodPWM*fraction2));
PWMPulseWidthSet(PWM_BASE, PWM_OUT_3, (periodPWM*fraction3));
pos0_table[index%300]=GPIOPinRead(GPIO_PORTD_BASE, GPIO_PIN_4 | GPIO_PIN_5 | GPIO_PIN_6 | GPIO_PIN_7);
pos1_table[index%300]=GPIOPinRead(GPIO_PORTE_BASE, GPIO_PIN_4 | GPIO_PIN_5);
pos2_table[index%300]=pos2;
pos3_table[index%300]=pos3;
speed0_table[index%300]=measured_speed0;
speed1_table[index%300]=measured_speed1;
speed2_table[index%300]=measured_speed2;
speed3_table[index%300]=measured_speed3;
output0_table[index%300]=output0;
output1_table[index%300]=output1;
output2_table[index%300]=output2;
output3_table[index%300]=output3;
fraction0_table[index%300]=fraction0;
fraction1_table[index%300]=fraction1;
fraction2_table[index%300]=fraction2;
fraction3_table[index%300]=fraction3;
//Si le robot est immobile
if(a_atteint_consigne && measured_speed0==0 && measured_speed1==0 && measured_speed2==0 && measured_speed3==0){
resetVariables();
resetQEIs();
ROM_PWMPulseWidthSet(PWM_BASE, PWM_OUT_0, 0);//periodPWM / 4);
ROM_PWMPulseWidthSet(PWM_BASE, PWM_OUT_1, 0);
ROM_PWMPulseWidthSet(PWM_BASE, PWM_OUT_2, 0);
ROM_PWMPulseWidthSet(PWM_BASE, PWM_OUT_3, 0);
est_en_mouvement = false;
}
else{
est_en_mouvement = true;
}
}
//*****************************************************************************
//
// This example demonstrates how to setup the PWM block to generate signals.
//
//*****************************************************************************
int
main(void)
{
unsigned long ulPeriod;
//
// Set the clocking to run directly from the crystal.
//
ROM_SysCtlClockSet(SYSCTL_SYSDIV_1 | SYSCTL_USE_OSC | SYSCTL_OSC_MAIN |
SYSCTL_XTAL_16MHZ);
ROM_SysCtlPWMClockSet(SYSCTL_PWMDIV_1);
//
// Initialize the UART.
//
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
GPIOPinConfigure(GPIO_PA0_U0RX);
GPIOPinConfigure(GPIO_PA1_U0TX);
ROM_GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1);
UARTStdioInit(0);
//
// Tell the user what is happening.
//
UARTprintf("\033[2JGenerating PWM on PD0 and PD1\n");
//
// Enable the peripherals used by this example.
//
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_PWM0);
ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD);
//
// Set GPIO D0 and D1 as PWM pins. They are used to output the PWM0 and
// PWM1 signals.
//
GPIOPinConfigure(GPIO_PD0_PWM0);
GPIOPinConfigure(GPIO_PD1_PWM1);
ROM_GPIOPinTypePWM(GPIO_PORTD_BASE, GPIO_PIN_0 | GPIO_PIN_1);
//
// Compute the PWM period based on the system clock.
//
ulPeriod = ROM_SysCtlClockGet() / 440;
//
// Set the PWM period to 440 (A) Hz.
//
ROM_PWMGenConfigure(PWM0_BASE, PWM_GEN_0,
PWM_GEN_MODE_UP_DOWN | PWM_GEN_MODE_NO_SYNC);
ROM_PWMGenPeriodSet(PWM0_BASE, PWM_GEN_0, ulPeriod);
//
// Set PWM0 to a duty cycle of 25% and PWM1 to a duty cycle of 75%.
//
ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_0, ulPeriod / 4);
ROM_PWMPulseWidthSet(PWM0_BASE, PWM_OUT_1, (ulPeriod * 3) / 4);
//
// Enable the PWM0 and PWM1 output signals.
//
ROM_PWMOutputState(PWM0_BASE, PWM_OUT_0_BIT | PWM_OUT_1_BIT, true);
//
// Enable the PWM generator.
//
ROM_PWMGenEnable(PWM0_BASE, PWM_GEN_0);
//
// Loop forever while the PWM signals are generated.
//
while(1)
{
}
}
