static void
MoveAll(uint32_t mask, int32_t cdeg)
{
for(int servo = 0; servo < NUM_SERVOS; servo++)
{
if(mask & (1 << servo))
{
MoveOne(servo, cdeg);
}
}
}
void GlobulesSystem::ProcessPhysics()
{
static ULONGLONG before_now = GetTimeInMS();
ULONGLONG now = GetTimeInMS();
double delta = static_cast<double>(now - before_now) / 1000.0;
before_now = now;
for(unsigned i = 0; i < globules.size(); ++i)
CollideWithWalls(globules[i]);
for(unsigned i = 0; i < globules.size(); ++i)
for(unsigned j = 0; j < i; ++j)
CollideThem(globules[i], globules[j]);
for(unsigned i = 0; i < globules.size(); ++i)
{
AccelerateOne(globules[i], delta);
MoveOne(globules[i], delta);
}
}
bool RecorderDlg::MoveOne(vector<bool>& chan_mask, int lv)
{
int i;
int cur_lv = 0;
int lv_pos = -1;
for (i=(int)chan_mask.size()-1; i>=0; i--)
{
if (chan_mask[i])
{
cur_lv++;
if (cur_lv == lv)
{
lv_pos = i;
break;
}
}
}
if (lv_pos >= 0)
{
if (lv_pos == (int)chan_mask.size()-lv)
return MoveOne(chan_mask, ++lv);
else
{
if (!chan_mask[lv_pos+1])
{
for (i=lv_pos; i<(int)chan_mask.size(); i++)
{
if (i==lv_pos)
chan_mask[i] = false;
else if (i<=lv_pos+lv)
chan_mask[i] = true;
else
chan_mask[i] = false;
}
return true;
}
else return false;//no space anymore
}
}
else return false;
}
bool RecorderDlg::GetMask(vector<bool>& chan_mask)
{
return MoveOne(chan_mask, 1);
}
/******************************************************************************
*
* 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
}
