// ----------------------------------------------------------------------------------
//
// _reset_init() -- Reset entry point.
//
// The CPU reset vector points here. Initialize the CPU, and jump
// to the C runtime start, which will eventually invoke main()
//
void _reset_init(void)
{
// Copy values to initialize data segment
uint32_t *fr = __etext;
uint32_t *to = __data_start__;
unsigned int len = __data_end__ - __data_start__;
while(len--)
*to++ = *fr++;
FPUEnable();
FPUStackingDisable();
main();
}
/******************************************************************************
*
* Test program
*
*****************************************************************************/
int
main(void)
{
int ret;
int servo; // lame TI compiler cant handle loop var declaration
FPUStackingDisable();
/* Initialize the clock to run at 40 MHz
*/
SysCtlClockSet(SYSCTL_SYSDIV_5 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN);
gSysClock = SysCtlClockGet();
/* Initialize the UART.
*/
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
GPIOPinConfigure(GPIO_PA0_U0RX);
GPIOPinConfigure(GPIO_PA1_U0TX);
GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1);
#ifdef STELLARISWARE
UARTStdioInit(0);
#else
UARTStdioConfig(0, 115200, gSysClock);
#endif
UARTprintf("\n\nServoBoard-Test\n---------------\n");
/* Initialize the GPIO port for the RGB LED
*/
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, RGB_PINS);
GPIOPinWrite(GPIO_PORTF_BASE, RGB_PINS, 0);
/* Initialize the battery monitor
* Use zeroes for parameter so default calibration will be used
*/
Servo_BatteryInit(0, 0);
/* Initialize servos for 20 msec
*/
ret = Servo_Init(gSysClock, 20000);
if(ret)
{
UARTprintf("error calling ServoInit\n");
return 0;
}
/* Enter loop to initialize all the servos in the system
*/
for(servo = 0; servo < NUM_SERVOS; servo++)
{
/* Associate each servo ID with a hardware timer (and A or B half)
*/
hServo[servo] = Servo_Config(servoInfo[servo].timer, servoInfo[servo].half);
if(hServo[servo] == 0)
{
UARTprintf("error config servo %d\n", servo);
return 0;
}
/* Delay a bit before initting the next servo. This is done to
* spread out the servo pulses so they do not all happen at the
* same time and load down the power supply.
* The delay value was determined experimentally. If the
* system clock frequency is changed then the delay value needs to
* be changed
*/
SysCtlDelay(22000);
}
/* Set each servo position to 0 to start, with 100 ms delay
*/
for(servo = 0; servo < NUM_SERVOS; servo++)
{
/* Set the servo motion rate */
Servo_SetMotionParameters(hServo[servo], 200);
Servo_SetPosition(hServo[servo], 0);
SysCtlDelay((gSysClock / 10) / 3);
}
// MoveAll(0xFFF, 0);
/* In this loop we just move all the servos between +45 and
* -45 deg (uncalibrated). There is a 100 ms delay between each
* servo, so that if observed with a scope each servo does not have
* the exact same timing.
*/
while(1)
{
/* Move all servos to -45 deg, with 100 ms between each servo
*/
for(servo = 0; servo < NUM_SERVOS; servo++)
{
UpdateRGB();
MoveOne(servo, -450);
DelayMs(100);
}
/* Now move all servos to +45 deg, with 100 ms delay
*/
for(servo = 0; servo < NUM_SERVOS; servo++)
{
UpdateRGB();
MoveOne(servo, 450);
DelayMs(100);
}
/* Read the battery voltage and print to the terminal
*/
uint32_t bat = Servo_ReadBatteryMv();
UARTprintf("%u.%02u V\n", bat / 1000, (bat % 1000) / 10);
}
#ifndef ccs // prevent warning from TI ccs compiler
return(0);
#endif
}
