void HardwareInit()
{
IntMasterDisable();
// Set the system clock to run at 50MHz from the PLL. // PLL=400MHz
// sysc2000000lk = 400MHz/2/4 = 50MHz
//SysCtlClockSet(SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN | SYSCTL_XTAL_16MHZ);
ROM_SysCtlClockSet(SYSCTL_SYSDIV_2_5 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN | SYSCTL_XTAL_16MHZ);
// Set the system clock to run at 20MHz from the PLL. // PLL=100MHz
// sysc2000000lk = 100MHz/2/4 = 20MHz
//SysCtlClockSet(SYSCTL_SYSDIV_10| SYSCTL_USE_PLL | SYSCTL_OSC_MAIN | SYSCTL_XTAL_16MHZ);
ulSysClock = ROM_SysCtlClockGet();
if (InitUART(UART0_BASE,SysCtlClockGet(),19200,
(UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE)) == -1)
while (1); // hang
if (InitUART(UART1_BASE,SysCtlClockGet(),9600,
(UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE)) == -1)
while (1); // hang
SysTickInit(1000); // 1msec counter
TimerInit(1); // 100ms counter
BBLedInit();
DEBUG_LED1(1);
DEBUG_LED1(0);
DEBUG_LED2(1);
DEBUG_LED2(0);
MasterI2C0Init(SysCtlClockGet() >= 40000000L); // Need 40MHz for 400K
A3906Init();
RMBD01Init();
Encoder_Init(QEI0_BASE); // J6 PITCH Encoder
Encoder_Init(QEI1_BASE); // J8 ROLL Encoder
EncoderLinesSet(QEI0_BASE,64);
EncoderLinesSet(QEI1_BASE,64);
//HBridgeInit();
//GPIOPinInit('E',4,false,false); /*TP5 */
//GPIOPinInit('E',5,false,false); /*TP6 */
IntMasterEnable();
}
//*****************************************************************************
//
// This function processes all the commands in the command queue. Certain
// commands are ignored while the controller is in a fault condition.
//
//*****************************************************************************
void
CommandQueueProcess(unsigned long bInFault)
{
tCommandQueue *pCommand;
//
// Loop while there are commands in the command queue.
//
while(g_ulCommandQueueRead != g_ulCommandQueueWrite)
{
//
// Get a pointer to this command.
//
pCommand = &(g_psCommandQueue[g_ulCommandQueueRead]);
//
// See which command has been received.
//
switch(pCommand->ulCmd)
{
//
// Force the motor controller into neutral.
//
case COMMAND_FORCE_NEUTRAL:
{
//
// Set the output to neutral.
//
ControllerForceNeutral();
//
// This command has been handled.
//
break;
}
//
// Switch the controller into voltage control mode.
//
case COMMAND_VOLTAGE_MODE:
{
//
// Set the voltage control mode.
//
ControllerVoltageModeSet(pCommand->ulParam1);
//
// This command has been handled.
//
break;
}
//
// Set the target voltage for voltage control mode.
//
case COMMAND_VOLTAGE_SET:
{
//
// Ignore this command if in a fault condition.
//
if(!bInFault)
{
//
// Set the new target voltage.
//
ControllerVoltageSet((long)pCommand->ulParam1);
}
//
// This command has been handled.
//
break;
}
//
// Set the rate of change for voltage control mode.
//
case COMMAND_VOLTAGE_RATE:
{
//
// Set the rate of change for voltage control mode.
//
ControllerVoltageRateSet(pCommand->ulParam1);
//
// This command has been handled.
//
break;
}
//
// Switch the controller into speed control mode.
//
case COMMAND_SPEED_MODE:
{
//
// Set the speed control mode.
//
ControllerSpeedModeSet(pCommand->ulParam1);
//
// This command has been handled.
//
break;
}
//
// Set the target speed for speed control mode.
//
case COMMAND_SPEED_SET:
{
//
// Ignore this command if in a fault condition.
//
if(!bInFault)
{
//
// Set the new target speed.
//
ControllerSpeedSet((long)pCommand->ulParam1);
}
//
// This command has been handled.
//
break;
}
//
// Set the speed reference source.
//
case COMMAND_SPEED_SRC_SET:
{
//
// Set the new speed reference source.
//
ControllerSpeedSrcSet(pCommand->ulParam1);
//
// This command has been handled.
//
break;
}
//
// Set the P gain on the speed controller.
//
case COMMAND_SPEED_P_SET:
{
//
// Set the P gain of the speed PID controller.
//
ControllerSpeedPGainSet((long)pCommand->ulParam1);
//
// This command has been handled.
//
break;
}
//
// Set the I gain on the speed controller.
//
case COMMAND_SPEED_I_SET:
{
//
// Set the I gain of the speed PID controller.
//
ControllerSpeedIGainSet((long)pCommand->ulParam1);
//
// This command has been handled.
//
break;
}
//
// Set the D gain on the speed controller.
//
case COMMAND_SPEED_D_SET:
{
//
// Set the D gain of the speed PID controller.
//
ControllerSpeedDGainSet((long)pCommand->ulParam1);
//
// This command has been handled.
//
break;
}
//
// Switch the controller into position mode.
//
case COMMAND_POS_MODE:
{
//
// Set the position mode.
//
ControllerPositionModeSet(pCommand->ulParam1,
pCommand->ulParam2);
//
// This command has been handled.
//
break;
}
//
// Set the target position for position mode.
//
case COMMAND_POS_SET:
{
//
// Ignore this command if in a fault condition.
//
if(!bInFault)
{
//
// Set the new target position.
//
ControllerPositionSet((long)pCommand->ulParam1);
}
//
// This command has been handled.
//
break;
}
//
// Set the position reference source.
//
case COMMAND_POS_SRC_SET:
{
//
// Set the new position reference source.
//
ControllerPositionSrcSet(pCommand->ulParam1);
//
// This command has been handled.
//
break;
}
//
// Set the P gain on the position controller.
//
case COMMAND_POS_P_SET:
{
//
// Set the P gain of the position PID controller.
//
ControllerPositionPGainSet((long)pCommand->ulParam1);
//
// This command has been handled.
//
break;
}
//
// Set the I gain on the position controller.
//
case COMMAND_POS_I_SET:
{
//
// Set the I gain of the position PID controller.
//
ControllerPositionIGainSet((long)pCommand->ulParam1);
//
// This command has been handled.
//
break;
}
//
// Set the D gain on the position controller.
//
case COMMAND_POS_D_SET:
{
//
// Set the D gain of the position PID controller.
//
ControllerPositionDGainSet((long)pCommand->ulParam1);
//
// This command has been handled.
//
break;
}
//
// Switch the controller into current control mode.
//
case COMMAND_CURRENT_MODE:
{
//
// Set the current control mode.
//
ControllerCurrentModeSet(pCommand->ulParam1);
//
// This command has been handled.
//
break;
}
//
// Set the target current for current control mode.
//
case COMMAND_CURRENT_SET:
{
//
// Ignore this command if in a fault condition.
//
if(!bInFault)
{
//
// Set the new target current.
//
ControllerCurrentSet((long)pCommand->ulParam1);
}
//
// This command has been handled.
//
break;
}
//
// Set the P gain on the current controller.
//
case COMMAND_CURRENT_P_SET:
{
//
// Set the P gain of the current PID controller.
//
ControllerCurrentPGainSet((long)pCommand->ulParam1);
//
// This command has been handled.
//
break;
}
//
// Set the I gain on the current controller.
//
case COMMAND_CURRENT_I_SET:
{
//
// Set the I gain of the current PID controller.
//
ControllerCurrentIGainSet((long)pCommand->ulParam1);
//
// This command has been handled.
//
break;
}
//
// Set the D gain on the current controller.
//
case COMMAND_CURRENT_D_SET:
{
//
// Set the D gain of the current PID controller.
//
ControllerCurrentDGainSet((long)pCommand->ulParam1);
//
// This command has been handled.
//
break;
}
//
// Set the number of brushes in the motor, which determines the
// number of commutations per revolution for sensor-less speed
// detection.
//
case COMMAND_NUM_BRUSHES:
{
//
// This command has been handled.
//
break;
}
//
// Set the number of lines per revolution in the encoder.
//
case COMMAND_ENCODER_LINES:
{
//
// Set the number of lines per revolution.
//
EncoderLinesSet(pCommand->ulParam1);
//
// This command has been handled.
//
break;
}
//
// Set the number of turns in the potentiometer.
//
case COMMAND_POT_TURNS:
{
//
// Set the number of turns in the potentiometer.
//
ADCPotTurnsSet((long)pCommand->ulParam1);
//
// This command has been handled.
//
break;
}
//
// Set the brake/coast state.
//
case COMMAND_BRAKE_COAST_SET:
{
//
// Set the brake/coast state.
//
HBridgeBrakeCoastSet(pCommand->ulParam1);
//
// This command has been handled.
//
break;
}
//
// Enable or disable the position limit "switches".
//
case COMMAND_POS_LIMIT_MODE:
{
//
// See if the position limit "switches" should be enabled or
// disabled.
//
if(pCommand->ulParam1)
{
LimitPositionEnable();
}
else
{
LimitPositionDisable();
}
//
// This command has been handled.
//
break;
}
//
// Set the position of the forward position limit "switch".
//
case COMMAND_POS_LIMIT_FWD:
{
//
// Set the forward limit "switch".
//
LimitPositionForwardSet(pCommand->ulParam1,
pCommand->ulParam2);
//
// This command has been handled.
//
break;
}
//
// Set the position of the reverse position limit "switch".
//
case COMMAND_POS_LIMIT_REV:
{
//
// Set the reverse limit "switch".
//
LimitPositionReverseSet(pCommand->ulParam1,
pCommand->ulParam2);
//
// This command has been handled.
//
break;
}
//
// Set the maximum output voltage.
//
case COMMAND_MAX_VOLTAGE:
{
//
// Set the maximum H-bridge output voltage.
//
HBridgeVoltageMaxSet(pCommand->ulParam1);
//
// This command has been handled.
//
break;
}
//
// Switch the controller into voltage compensation control mode.
//
case COMMAND_VCOMP_MODE:
{
//
// Set the voltage compensation control mode.
//
ControllerVCompModeSet(pCommand->ulParam1);
//
// This command has been handled.
//
break;
}
//
// Set the target voltage for voltage compensation control mode.
//
case COMMAND_VCOMP_SET:
{
//
// Ignore this command if in a fault condition.
//
if(!bInFault)
{
//
// Set the new target voltage.
//
ControllerVCompSet((long)pCommand->ulParam1);
}
//
// This command has been handled.
//
break;
}
//
// Set the rate of set point change for voltage compensation
// control mode.
//
case COMMAND_VCOMP_IN_RAMP:
{
//
// Set the rate of set point change for voltage compensation
// control mode.
//
ControllerVCompInRateSet(pCommand->ulParam1);
//
// This command has been handled.
//
break;
}
//
// Set the rate of compensation change for voltage compensation
// control mode.
//
case COMMAND_VCOMP_COMP_RAMP:
{
//
// Set the rate of compensation change for voltage compensation
// control mode.
//
ControllerVCompCompRateSet(pCommand->ulParam1);
//
// This command has been handled.
//
break;
}
//
// Ignore any commands that are not recognized.
//
default:
{
break;
}
}
//
// Increment the command queue read pointer.
//
g_ulCommandQueueRead = (g_ulCommandQueueRead + 1) % COMMAND_QUEUE_SIZE;
}
}
