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 }