void setSpeed(int16_t left, int16_t right)
{
if(left > 100)
{
speedLeft = 100;
}
else if(left < -100)
{
speedLeft = -100;
}
else
{
speedLeft = left;
}
if(right > 100)
{
speedRight = 100;
}
else if(right < -100)
{
speedRight = -100;
}
else
{
speedRight = right;
}
// set interrupt call speed
uint32_t interruptLeft = (MOTOR_PRESCALER - MOTOR_OFFSET) * abs(speedLeft) / 100;
uint32_t interruptRight = (MOTOR_PRESCALER - MOTOR_OFFSET) * abs(speedRight) / 100;
MAP_TimerMatchSet(TIMERA0_BASE, TIMER_A, interruptLeft);
MAP_TimerMatchSet(TIMERA0_BASE, TIMER_B, interruptRight);
}
void motorSetup()
{
/* Set pin mode for Hbridge output pins */
MAP_PinTypeGPIO(AIN1, PIN_MODE_0, false); /* Ain 1 */
MAP_PinTypeGPIO(AIN2, PIN_MODE_0, false); /* Bin 1 */
MAP_PinTypeGPIO(BIN1, PIN_MODE_0, false); /* Bin 2 */
MAP_PinTypeGPIO(BIN2, PIN_MODE_0, false); /* Ain 2 */
/* Get port name and bin number from GPIO number (TI lookup table) */
GPIO_IF_GetPortNPin(AIN1x, &port_ain1, &pin_ain1);
GPIO_IF_GetPortNPin(AIN2x, &port_ain2, &pin_ain2);
GPIO_IF_GetPortNPin(BIN1x, &port_bin1, &pin_bin1);
GPIO_IF_GetPortNPin(BIN2x, &port_bin2, &pin_bin2);
/* Set pin direction */
GPIODirModeSet(port_ain1, pin_ain1, 1);
GPIODirModeSet(port_ain2, pin_ain2, 1);
GPIODirModeSet(port_bin1, pin_bin1, 1);
GPIODirModeSet(port_bin2, pin_bin2, 1);
/* Set value to write to PIN */
bitA1 = 1 << (AIN1x % 8);
bitA2 = 1 << (AIN2x % 8);
bitB1 = 1 << (BIN1x % 8);
bitB2 = 1 << (BIN2x % 8);
// Enable timer A peripheral
MAP_PRCMPeripheralClkEnable(PRCM_TIMERA0, PRCM_RUN_MODE_CLK);
MAP_PRCMPeripheralReset(PRCM_TIMERA0);
// Split channels and configure for periodic interrupts
MAP_TimerConfigure(TIMERA0_BASE, TIMER_CFG_SPLIT_PAIR | TIMER_CFG_A_PERIODIC | TIMER_CFG_B_PERIODIC);
MAP_TimerPrescaleSet(TIMERA0_BASE, TIMER_A, 0);
MAP_TimerPrescaleSet(TIMERA0_BASE, TIMER_B, 0);
// Set compare interrupt
MAP_TimerIntEnable(TIMERA0_BASE, TIMER_TIMA_MATCH);
MAP_TimerIntEnable(TIMERA0_BASE, TIMER_TIMB_MATCH);
// Configure compare interrupt, start with 0 speed
MAP_TimerMatchSet(TIMERA0_BASE, TIMER_A, 0);
MAP_TimerMatchSet(TIMERA0_BASE, TIMER_B, 0);
// Set timeout interrupt
MAP_TimerIntRegister(TIMERA0_BASE, TIMER_A, TimerBaseIntHandlerA);
MAP_TimerIntRegister(TIMERA0_BASE, TIMER_B, TimerBaseIntHandlerB);
MAP_TimerIntEnable(TIMERA0_BASE, TIMER_TIMA_TIMEOUT);
MAP_TimerIntEnable(TIMERA0_BASE, TIMER_TIMB_TIMEOUT);
// Turn on timers
MAP_TimerLoadSet(TIMERA0_BASE, TIMER_A, MOTOR_PRESCALER);
MAP_TimerLoadSet(TIMERA0_BASE, TIMER_B, MOTOR_PRESCALER);
MAP_TimerEnable(TIMERA0_BASE, TIMER_A);
MAP_TimerEnable(TIMERA0_BASE, TIMER_B);
}
void motorSetup()
{
pinMode(AIN1x, OUTPUT);
pinMode(AIN2x, OUTPUT);
pinMode(BIN1x, OUTPUT);
pinMode(BIN2x, OUTPUT);
bitA1 = digitalPinToBitMask(AIN1x);
bitA2 = digitalPinToBitMask(AIN2x);
bitB1 = digitalPinToBitMask(BIN1x);
bitB2 = digitalPinToBitMask(BIN2x);
portA1 = digitalPinToPort(AIN1x);
portA2 = digitalPinToPort(AIN2x);
portB1 = digitalPinToPort(BIN1x);
portB2 = digitalPinToPort(BIN2x);
baseA1 = (uint32_t) portBASERegister(portA1);
baseA2 = (uint32_t) portBASERegister(portA2);
baseB1 = (uint32_t) portBASERegister(portB1);
baseB2 = (uint32_t) portBASERegister(portB2);
// Enable timer A peripheral
MAP_PRCMPeripheralClkEnable(PRCM_TIMERA0, PRCM_RUN_MODE_CLK);
MAP_PRCMPeripheralReset(PRCM_TIMERA0);
// Split channels and configure for periodic interrupts
MAP_TimerConfigure(TIMERA0_BASE, TIMER_CFG_SPLIT_PAIR | TIMER_CFG_A_PERIODIC | TIMER_CFG_B_PERIODIC);
MAP_TimerPrescaleSet(TIMERA0_BASE, TIMER_A, 0);
MAP_TimerPrescaleSet(TIMERA0_BASE, TIMER_B, 0);
// Set compare interrupt
MAP_TimerIntEnable(TIMERA0_BASE, TIMER_TIMA_MATCH);
MAP_TimerIntEnable(TIMERA0_BASE, TIMER_TIMB_MATCH);
// Configure compare interrupt, start with 0 speed
MAP_TimerMatchSet(TIMERA0_BASE, TIMER_A, 0);
MAP_TimerMatchSet(TIMERA0_BASE, TIMER_B, 0);
// Set timeout interrupt
MAP_TimerIntRegister(TIMERA0_BASE, TIMER_A, TimerBaseIntHandlerA);
MAP_TimerIntRegister(TIMERA0_BASE, TIMER_B, TimerBaseIntHandlerB);
MAP_TimerIntEnable(TIMERA0_BASE, TIMER_TIMA_TIMEOUT);
MAP_TimerIntEnable(TIMERA0_BASE, TIMER_TIMB_TIMEOUT);
// Turn on timers
MAP_TimerLoadSet(TIMERA0_BASE, TIMER_A, MOTOR_PRESCALER);
MAP_TimerLoadSet(TIMERA0_BASE, TIMER_B, MOTOR_PRESCALER);
MAP_TimerEnable(TIMERA0_BASE, TIMER_A);
MAP_TimerEnable(TIMERA0_BASE, TIMER_B);
}
static i32 hwt32_update_monotone(struct hw_timer32 *hwt, u32 expires)
{
struct u64_val value64 = {0, 0};
u32 current = 0;
hwt32_process_monotone_current(hwt, &value64);
current = value64.lo_32;
value64.hi_32 = 0;
/* If 'expires' is too close to be missed, then push
out the 'expires' to ensure that IRQ is triggered */
if((v1_to_v2_offset32(current, expires) <
HWT32_ACCESS_CYCLES) ||
(v1_to_v2_offset32(expires, current) <
HWT32_ACCESS_CYCLES)) {
util_u32_data_add(HWT32_ACCESS_CYCLES, current,
&value64);
expires = value64.lo_32;
}
MAP_TimerMatchSet(hwt->base_addr, TIMER_A, expires);
hwt->mtone_expy.hi_32 = value64.hi_32 + hwt->n_rollovers;
hwt->mtone_expy.lo_32 = expires;
return 0;
}
//****************************************************************************
//
//! Setup the timer in PWM mode
//!
//! \param ulBase is the base address of the timer to be configured
//! \param ulTimer is the timer to be setup (TIMER_A or TIMER_B)
//! \param ulConfig is the timer configuration setting
//! \param ucInvert is to select the inversion of the output
//!
//! This function
//! 1. The specified timer is setup to operate as PWM
//!
//! \return None.
//
//****************************************************************************
void SetupTimerPWMMode(unsigned long ulBase, unsigned long ulTimer,
unsigned long ulConfig, unsigned char ucInvert)
{
//
// Set GPT - Configured Timer in PWM mode.
//
MAP_TimerConfigure(ulBase,ulConfig);
MAP_TimerPrescaleSet(ulBase,ulTimer,0);
//
// Inverting the timer output if required
//
MAP_TimerControlLevel(ulBase,ulTimer,ucInvert);
//
// Load value set to ~0.5 ms time period
//
MAP_TimerLoadSet(ulBase,ulTimer,TIMER_INTERVAL_RELOAD);
//
// Match value set so as to output level 0
//
MAP_TimerMatchSet(ulBase,ulTimer,TIMER_INTERVAL_RELOAD);
}
//****************************************************************************
//
//! Update the dutycycle of the PWM timer
//!
//! \param ulBase is the base address of the timer to be configured
//! \param ulTimer is the timer to be setup (TIMER_A or TIMER_B)
//! \param ucLevel translates to duty cycle settings (0:255)
//!
//! This function
//! 1. The specified timer is setup to operate as PWM
//!
//! \return None.
//
//****************************************************************************
void UpdateDutyCycle(unsigned long ulBase, unsigned long ulTimer,
unsigned char ucLevel)
{
//
// Match value is updated to reflect the new dutycycle settings
//
MAP_TimerMatchSet(ulBase,ulTimer,(ucLevel*DUTYCYCLE_GRANULARITY));
}
static i32 hwt32_config_monotone(struct hw_timer32 *hwt, u32 expires)
{
/* Count upwards until counter value matches 'expires' */
MAP_TimerConfigure(hwt->base_addr, TIMER_CFG_PERIODIC_UP);
MAP_TimerLoadSet(hwt->base_addr, TIMER_A, 0xFFFFFFFF); /* Rollover Val */
MAP_TimerMatchSet(hwt->base_addr, TIMER_A, expires); /* User match Val */
/* IRQ(s) when up counter rollsover and counter reaches 'expires' */
hwt->irq_mask = TIMER_TIMA_TIMEOUT | TIMER_TIMA_MATCH;
return 0;
}
void PWMWrite(uint8_t pin, uint32_t analog_res, uint32_t duty, uint32_t freq)
{
analog_res = analog_res * 1000;
freq;
uint32_t load = (F_CPU / freq) * 1000;
uint32_t match = load / (analog_res / duty);
match = match;
load = load / 1000;
uint16_t prescaler = load >> 16;
uint16_t prescaler_match = match >> 16;
uint8_t timer = digitalPinToTimer(pin);
if(timer == NOT_ON_TIMER)
return;
MAP_PRCMPeripheralClkEnable(PRCM_TIMERA0 + (timer/2), PRCM_RUN_MODE_CLK);
uint16_t pnum = digitalPinToPinNum(pin);
switch(timer) {
/* PWM0/1 */
case TIMERA0A:
case TIMERA0B:
MAP_PinTypeTimer(pnum, PIN_MODE_5);
break;
/* PWM2/3 */
case TIMERA1A:
case TIMERA1B:
MAP_PinTypeTimer(pnum, PIN_MODE_9);
break;
/* PWM4/5 */
case TIMERA2A:
case TIMERA2B:
MAP_PinTypeTimer(pnum, PIN_MODE_3);
break;
/* PWM6/7 */
case TIMERA3A:
case TIMERA3B:
MAP_PinTypeTimer(pnum, PIN_MODE_3);
break;
}
uint32_t base = TIMERA0_BASE + ((timer/2) << 12);
/* FIXME: If B is already opperational and configure A, B get's messed up. */
MAP_TimerConfigure(base, TIMER_CFG_SPLIT_PAIR | TIMER_CFG_A_PWM | TIMER_CFG_B_PWM);
uint16_t timerab = timer % 2 ? TIMER_B : TIMER_A;
MAP_TimerPrescaleSet(base, timerab, prescaler);
MAP_TimerPrescaleMatchSet(base, timerab, prescaler_match);
MAP_TimerControlLevel(base, timerab, 1);
MAP_TimerLoadSet(base, timerab, load);
MAP_TimerMatchSet(base, timerab, match);
MAP_TimerEnable(base, timerab);
}
