//------------------------------------------------------------------------------
// 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());
}