Exemplo n.º 1
0
//------------------------------------------------------------------------------
// 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);
        }
    }
    }
}
Exemplo n.º 2
0
/*****************************************************************************
******************************************************************************
  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;

}
Exemplo n.º 3
0
//------------------------------------------------------------------------------
// 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());
}
Exemplo n.º 4
0
//------------------------------------------------------------------------------
// 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());
}
Exemplo n.º 5
0
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);
}
Exemplo n.º 6
0
//------------------------------------------------------------------------------
// 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());
}
Exemplo n.º 7
0
//------------------------------------------------------------------------------
// 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());
}