//------------------------------------------------------------------------------ // circularScanController() -- controls the circular scans //------------------------------------------------------------------------------ void ScanGimbal::circularScanController(const double) { static Basic::Integer iBar(1); // Depending on our scan state, we will either start or stop the bar switch(getScanState()) { // reset state: move to ref position case 0: { setServoMode(POSITION_SERVO); setFastSlewMode(true); setCmdPos(getRefPosition()); setScanState(1); } break; case 1: { // wait state if (isPositioned() || isAtLimits()) { setScanState(2); // out of state 1 } break;// fall into state 2 } case 2: { // start scan - switch to a rate servo, and begin spinning at a commanded rate // Trigger the SCAN_START event handler onStartScanEvent(&iBar); setServoMode(RATE_SERVO); setFastSlewMode(false); myLastAngle = Basic::Angle::aepcdRad(getPosition().x() - getRefPosition().x()); setScanState(3); } break; case 3: { // end scan - finished with one rotation, start over again bool onceAround = false; double myAngle = Basic::Angle::aepcdRad(getPosition().x() - getRefPosition().x()); // clockwise if(getCmdAzRate() >= 0.0) { onceAround = (myLastAngle < 0.0 && myAngle >= 0.0); } // we are going counter-clockwise else { onceAround = (myLastAngle >= 0.0 && myAngle < 0.0); } myLastAngle = myAngle; if (onceAround) { // Trigger the SCAN_END event handler onEndScanEvent(&iBar); // start over with our scan (another revolution) setScanState(2); } } } }
/***************************************************************************** ****************************************************************************** Function Name : init_test_sine_task Date : Dec. 1997 Remarks: initialization for task ****************************************************************************** Paramters: (i/o = input/output) none *****************************************************************************/ static int init_test_sine_task(void) { int j, i; char string[100]; double max_range=0; int ans; /* check whether any other task is running */ if (strcmp(current_task_name,NO_TASK) != 0) { printf("New task can only be run if no other task is running!\n"); return FALSE; } /* allow or speed adjustment */ get_double("Frequency Multiplier",speed,&speed); /* enable inverse dynamics control */ get_int("Which InvDyn: NewtonEuler=1 ArtBody=2",which_invdyn,&which_invdyn); /* read the script for this task */ if (!read_sine_script()) return FALSE; /* go to a save posture */ bzero((char *)&(target[1]),n_dofs*sizeof(target[1])); for (i=1; i<=n_dofs; ++i) { target[i].th = off[i]; for (j=1; j<=n_sine[i]; ++j) { target[i].th += amp[i][j]*sin(phase[i][j]); } } if (!go_target_wait_ID(target)) return FALSE; /* switch the servo mode */ if (invdyn_servo_flag) setServoMode(INVDYNSERVO); else setServoMode(MOTORSERVO); /* do we really want to do this task? */ ans = 999; while (ans == 999) { if (!get_int("Enter 1 to start or anthing else to abort ...",ans,&ans)) return FALSE; } if (ans != 1) return FALSE; task_time = 0.0; scd(); return TRUE; }
//------------------------------------------------------------------------------ // pseudoRandomScanController() -- steps through an array of vertices (fast slew) //------------------------------------------------------------------------------ void ScanGimbal::pseudoRandomScanController(const double) { static Basic::Integer iBar(1); // Depending on our scan state, we will either start or stop the bar switch(getScanState()) { // reset state, must be in electronic mode or we will not operate case 0: { if (prScanVertices != 0) { if ( isGimbalType(ELECTRONIC) ) { setServoMode(POSITION_SERVO); setFastSlewMode(true); setScanState(1); } else setScanMode(MANUAL_SCAN); } } break; case 1: { // start state - go to the desired pseudo random point (must be in electric mode) if (isPositioned() || isAtLimits()) { // make sure we have at least one vertice, then move to it if (nprv != 0 && cprv <= nprv) { // if this is the first vertice, send a start event if (cprv == 0) { // Trigger the SCAN_START event handler onStartScanEvent(&iBar); } else { setScanPos(prScanVertices[cprv].x(), prScanVertices[cprv].y()); cprv++; } } // when we reach the end else setScanState(2); } } break; case 2: { // end state - reset our vertice count and send an end event if (isPositioned() || isAtLimits()) { // Trigger the SCAN_END event handler onEndScanEvent(&iBar); // set us back to the first vertice cprv = 0; setScanState(1); } } break; } // now set our commanded position accordingly setCmdPos(getRefPosition()+getScanPos()); }
//------------------------------------------------------------------------------ // barScanController() -- control the bar scans //------------------------------------------------------------------------------ void ScanGimbal::barScanController(const double) { static Basic::Integer iBar(1); // Depending on our scan state, we will either start or stop the bar switch(getScanState()) { // reset state, we must set our bar number back to 1 case 0: { setBarNumber(1); computeNewBarPos(getBarNumber(), BEGINNING); setScanState(1); setServoMode(POSITION_SERVO); setFastSlewMode(true); } break; case 1: { // start state - slow slew and compute the end position if (isPositioned() || isAtLimits()) { iBar = getBarNumber(); // Trigger the SCAN_START event handler onStartScanEvent(&iBar); computeNewBarPos(getBarNumber(), ENDING); setScanState(2); setFastSlewMode(false); } } break; case 2: { // end state - fast slew and compute the next bar's position (if any) if (isPositioned() || isAtLimits()) { iBar = getBarNumber(); // Trigger the SCAN_END event handler onEndScanEvent(&iBar); nextBar(); computeNewBarPos(getBarNumber(), BEGINNING); setScanState(1); setFastSlewMode(true); } } break; } // now set our commanded position accordingly setCmdPos(getRefPosition()+getScanPos()); }
void initializeDynamixels( void ) { byte i =0; do { _delay_ms(50); uint16_t vol = DynamixelGetVoltage(i); if (vol == 0xffff) { TxRS232_Dec(i); TxRS232_String(" timed out.\r\n"); } else { TxRS232_Dec(i); TxRS232_String(" did not time out.\r\n"); dynamixelAddress = i; break; } i++; } while (i < 0xff); //Put Motors in Servo Mode setServoMode(dynamixelAddress,1); setComplianceSlopes(dynamixelAddress, 128); }
//------------------------------------------------------------------------------ // spiralScanController() -- controls the spiral scan //------------------------------------------------------------------------------ void ScanGimbal::spiralScanController(const double dt) { double degPerDT = (getRevPerSec() * 360.0) * dt; static Basic::Integer iBar(1); switch(getScanState()) { // reset state: move to ref position case 0: { setServoMode(POSITION_SERVO); setFastSlewMode(true); setScanState(1); setConAngle(0); setNumRevs(0.0); } break; case 1: { // wait state if (isPositioned() || isAtLimits()) { setScanState(2); // out of state 1 } break;// fall into state 2 } case 2: { // start scan setFastSlewMode(false); setConAngle(getConAngle() + degPerDT); if (fabs(getConAngle()) > 360.0) { setNumRevs(getNumRevs()+1); if (getConAngle() >= 0.0) { setConAngle(getConAngle() - 360.0); } else { setConAngle(getConAngle() + 360.0); } } // Trigger the SCAN_START event handler onStartScanEvent(&iBar); setScanState(3); } break; case 3: { // turn revolutions per second into degrees per sec per frame // now we get this to each time step setConAngle(getConAngle() + degPerDT); if (fabs(getConAngle()) > 360.0) { setNumRevs(getNumRevs()+1); if (getConAngle() >= 0.0) { setConAngle(getConAngle() - 360.0); } else { setConAngle(getConAngle() + 360.0); } } // end scan - finished with one rotation, check if our reference has moved bool onceAround = false; if (getNumRevs() >= getMaxNumRevs()) { onceAround = true; } // after one revolution if (onceAround) { // Trigger the SCAN_END event handler onEndScanEvent(&iBar); setConAngle(0.0); setNumRevs(0.0); setScanState(2); } } break; } double fullAngleRadians = getNumRevs() * 360.0; if (getRevPerSec() < 0.0) { fullAngleRadians = -fullAngleRadians; } fullAngleRadians = (fullAngleRadians + getConAngle()) * Basic::Angle::D2RCC; // azimuth double newX = getScanRadius() * (fullAngleRadians / (2.0 * PI)) * sin(fullAngleRadians); // elevation double newY = getScanRadius() * (fullAngleRadians / (2.0 * PI)) * cos(fullAngleRadians); setScanPos(newX, newY); // command our new position setCmdPos(getRefPosition() + getScanPos()); }
//------------------------------------------------------------------------------ // conicalScanController() -- controls the conical scans //------------------------------------------------------------------------------ void ScanGimbal::conicalScanController(const double dt) { double degPerDT = (getRevPerSec() * 360.0) * dt; static Basic::Integer iBar(1); switch(getScanState()) { // reset state: move to ref position case 0: { setServoMode(POSITION_SERVO); setFastSlewMode(true); setScanState(1); setConAngle(0); } break; case 1: { // wait state if (isPositioned() || isAtLimits()) { setScanState(2); // out of state 1 } break;// fall into state 2 } case 2: { // start scan setFastSlewMode(false); setConAngle( Basic::Angle::aepcdDeg(degPerDT + getConAngle()) ); // Trigger the SCAN_START event handler onStartScanEvent(&iBar); setScanState(3); } break; case 3: { // turn revolutions per second into degrees per sec per frame // now we get this to each time step double conAngleN1 = getConAngle(); setConAngle( Basic::Angle::aepcdDeg(degPerDT + getConAngle()) ); // end scan - finished with one rotation, check if our reference has moved bool onceAround = false; // clockwise rotation if (getRevPerSec() >= 0.0) { if (conAngleN1 < 0.0 && getConAngle() >= 0.0) { onceAround = true; } } // counter-clockwise rotation else { if (conAngleN1 < 0.0 && getConAngle() >= 0.0) { setConAngle(0); onceAround = true; } } // after one revolution if (onceAround) { // Trigger the SCAN_END event handler onEndScanEvent(&iBar); setScanState(2); } } break; } // azimuth double newX = getScanRadius() * sin(getConAngle() * Basic::Angle::D2RCC); // elevation double newY = getScanRadius() * cos(getConAngle() * Basic::Angle::D2RCC); setScanPos(newX, newY); // command our new position setCmdPos(getRefPosition() + getScanPos()); }