static cyg_uint32 getShiftValueFlip(void)
{
cyg_uint32 value;
waitIdle();
ZY1000_PEEK(ZY1000_JTAG_BASE+0x18, value);
VERBOSE(LOG_INFO("getShiftValue %08x (flipped)", value));
return value;
}
ThreadPool::Worker::~Worker()
{
if (workerThread.joinable())
{
waitIdle();
lock.lock();
threadIsAlive = false;
cond.notify_one();
lock.unlock();
workerThread.join();
}
}
void btThreadPool::runTasks() {
btAssert(m_numThreads != 0);
if (m_taskArray.size() == 0) return;
btAssert(!m_bRunningTasks); // This class cannot be used recursively!
m_bRunningTasks = true;
if (m_taskArray.size() >= m_numThreads) {
int tasksPerThread = m_taskArray.size() / m_numThreads;
int remainder = m_taskArray.size() % m_numThreads;
int curTask = 0;
for (int i = 0; i < m_numThreads; i++) {
m_pThreadInfo[i]->pTaskArr = &m_taskArray[curTask];
m_pThreadInfo[i]->numTasks = tasksPerThread + (remainder ? 1 : 0);
curTask += m_pThreadInfo[i]->numTasks;
if (remainder > 0) remainder--;
}
// Start the threads!
for (int i = 0; i < m_numThreads; i++) {
m_pThreadInfo[i]->pIdleEvent->reset(); // Reset the idle event
m_pThreadInfo[i]->pStartEvent->trigger(); // Start it
}
} else if (m_taskArray.size() >= m_numThreads / 2) {
// Rare case where tasks are less than num threads
// Probably better to just run the task on the main thread if it's only one task, but whatever.
for (int i = 0; i < m_taskArray.size(); i++) {
m_pThreadInfo[i]->pTaskArr = &m_taskArray[i];
m_pThreadInfo[i]->numTasks = 1;
m_pThreadInfo[i]->pIdleEvent->reset(); // Reset the idle event
m_pThreadInfo[i]->pStartEvent->trigger(); // Go!
}
} else {
// Not enough tasks. Run sequentially!
for (int i = 0; i < m_taskArray.size(); i++) {
m_taskArray[i]->run();
}
}
m_bRunningTasks = false;
waitIdle();
}
int interface_jtag_execute_queue(void)
{
cyg_uint32 empty;
waitIdle();
ZY1000_PEEK(ZY1000_JTAG_BASE+0x10, empty);
/* clear JTAG error register */
ZY1000_POKE(ZY1000_JTAG_BASE+0x14, 0x400);
if ((empty&0x400)!=0)
{
LOG_WARNING("RCLK timeout");
/* the error is informative only as we don't want to break the firmware if there
* is a false positive.
*/
// return ERROR_FAIL;
}
return ERROR_OK;
}
void zy1000_reset(int trst, int srst)
{
LOG_DEBUG("zy1000 trst=%d, srst=%d", trst, srst);
if(!srst)
{
ZY1000_POKE(ZY1000_JTAG_BASE+0x14, 0x00000001);
}
else
{
/* Danger!!! if clk!=0 when in
* idle in TAP_IDLE, reset halt on str912 will fail.
*/
ZY1000_POKE(ZY1000_JTAG_BASE+0x10, 0x00000001);
}
if(!trst)
{
ZY1000_POKE(ZY1000_JTAG_BASE+0x14, 0x00000002);
}
else
{
/* assert reset */
ZY1000_POKE(ZY1000_JTAG_BASE+0x10, 0x00000002);
}
if (trst||(srst&&(jtag_reset_config & RESET_SRST_PULLS_TRST)))
{
waitIdle();
/* we're now in the RESET state until trst is deasserted */
ZY1000_POKE(ZY1000_JTAG_BASE+0x20, TAP_RESET);
} else
{
/* We'll get RCLK failure when we assert TRST, so clear any false positives here */
ZY1000_POKE(ZY1000_JTAG_BASE+0x14, 0x400);
}
/* wait for srst to float back up */
if (!srst)
{
int i;
for (i=0; i<1000; i++)
{
// We don't want to sense our own reset, so we clear here.
// There is of course a timing hole where we could loose
// a "real" reset.
if (!readSRST())
break;
/* wait 1ms */
alive_sleep(1);
}
if (i==1000)
{
LOG_USER("SRST didn't deassert after %dms", i);
} else if (i>1)
{
LOG_USER("SRST took %dms to deassert", i);
}
}
}
int main(int argc, char* argv[])
{
RndSharedData _data;
bool _shake = false;
if (YARPParseParameters::parse(argc, argv, "-shake"))
_shake = true;
RndBehavior _rnd(&_data);
RBInitMotion initMotion;
RBInitShake initShakeWrist(YVector(6, __wrist), "wrist");
RBInitShake initShakeForearm(YVector(6, __forearm), "forearm");
RBInitShake initShakeArm(YVector(6, __arm), "arm");
RBWaitIdle waitIdle("wait idle");
RBWaitIdle stopped("rnd stopped");
RBInit init;
RBWaitDeltaT waitDeltaT1(2);
RBWaitDeltaT waitDeltaT2(2);
RBWaitMotion waitMotion("waiting on arm random motion");
RBWaitMotion waitShakeWrist("waiting on wrist shake");
RBWaitMotion waitShakeForearm("waiting on forearm shake");
RBWaitMotion waitShakeArm("waiting on arm shake");
RBWaitMotion waitRest("wating on rest");
// input states
RBSimpleInput motionDone(YBVArmDone);
RBSimpleInput armStarted(YBVArmIssuedCmd);
RBSimpleInput armBusy(YBVArmIsBusy);
RBSimpleInput start(YBVArmRndStart);
RBSimpleInput stop(YBVArmRndStop);
RBSimpleInput rest(YBVArmRest);
RBSimpleInput restDone(YBVArmRestDone);
RBSimpleOutput inhibitRest(YBVArmInhibitResting);
RBSimpleOutput inhibitHead(YBVSinkInhibitAll);
RBSimpleOutput enableHead(YBVSinkEnableAll);
RBRndReset resetRnd;
///////////////////////////////////////
RBWaitDeltaT trState1(1);
RBWaitDeltaT trState2(1);
RBWaitDeltaT waitSomeTime(4);
// arm random movement
_rnd.setInitialState(&init);
_rnd.add(NULL, &init, &waitIdle);
_rnd.add(&stop, &waitIdle, &stopped);
_rnd.add(&rest, &waitIdle, &waitRest, &resetRnd);
_rnd.add(&start, &waitIdle, &initMotion);
// rest
_rnd.add(&rest, &initMotion, &waitRest, &resetRnd);
_rnd.add(&rest, &waitMotion, &waitRest, &resetRnd);
_rnd.add(&stop, &waitMotion, &stopped);
_rnd.add(&armBusy, &waitMotion, &waitRest, &resetRnd);
_rnd.add(&armStarted, &initMotion, &waitMotion);
_rnd.add(&armBusy, &initMotion, &waitRest, &resetRnd);
// no shake
if (!_shake)
{
_rnd.add(&motionDone, &waitMotion, &waitSomeTime);
_rnd.add(&rest, &waitSomeTime, &waitRest, &resetRnd);
_rnd.add(NULL, &waitSomeTime, &initMotion);
}
else
{
// shake sequences
// wrist only
// JUST ADDED
_rnd.add(&motionDone, &waitMotion, &trState1, &inhibitRest);
_rnd.add(NULL, &trState1, &initShakeWrist, &inhibitHead);
// _rnd.add(&motionDone, &waitMotion, &initShakeWrist, &inhibitRest);
_rnd.add(&rest, &waitMotion, &waitRest);
_rnd.add(NULL, &initShakeWrist, &waitShakeWrist);
// _rnd.add(&motionDone, &waitShakeWrist, &initMotion, &inhibitRest);
// JUST ADDED
_rnd.add(&motionDone, &waitShakeWrist, &trState2, &enableHead);
_rnd.add(&stop, &waitShakeWrist, &stopped);
_rnd.add(NULL, &trState2, &initMotion, &inhibitRest);
/* other joints
_rnd.add(&motionDone, &waitShakeWrist, &waitDeltaT1);
_rnd.add(NULL, &waitDeltaT1, &initShakeForearm);
_rnd.add(NULL, &initShakeForearm, &waitShakeForearm);
_rnd.add(&motionDone, &waitShakeForearm, &waitDeltaT2);
_rnd.add(NULL, &waitDeltaT2, &initShakeArm);
_rnd.add(NULL, &initShakeArm, &waitShakeArm);
_rnd.add(&motionDone, &waitShakeArm, &initMotion, &inhibitRest);
*/
// rest
_rnd.add(&rest, &waitShakeWrist, &waitRest);
_rnd.add(&rest, &waitShakeForearm, &waitRest);
_rnd.add(&rest, &waitShakeArm, &waitRest);
_rnd.add(&stop, &waitShakeForearm, &stopped);
_rnd.add(&stop, &waitShakeArm, &stopped);
}
// wait rest
_rnd.add(&stop, &waitRest, &stopped);
_rnd.add(&restDone, &waitRest, &initMotion);
_rnd.add(&start, &stopped, &init);
_rnd.Begin();
_rnd.loop();
return 0;
}
