/* Attach Servo by given servo number, SERVO_ROT_NUM, SERVO_LEFT_NUM, SERVO_RIGHT_NUM
* if servo has not been attached, attach the servo, and read the current Angle
*/
void uArmClass::attachServo(byte servo_number)
{
    switch(servo_number) {
        case SERVO_ROT_NUM:
        if(!g_servo_rot.attached()) {
            g_servo_rot.attach(SERVO_ROT_PIN);
            cur_rot = readAngle(SERVO_ROT_NUM);
        }
        break;
        case SERVO_LEFT_NUM:
        if (!g_servo_left.attached()) {
            g_servo_left.attach(SERVO_LEFT_PIN);
            cur_left = readAngle(SERVO_LEFT_NUM);
        }
        break;
        case SERVO_RIGHT_NUM:
        if (!g_servo_right.attached()) {
            g_servo_right.attach(SERVO_RIGHT_PIN);
            cur_right = readAngle(SERVO_RIGHT_NUM);
        }
        break;
        case SERVO_HAND_ROT_NUM:
        if (!g_servo_hand_rot.attached()) {
            g_servo_hand_rot.attach(SERVO_HAND_PIN);
            cur_hand = readAngle(SERVO_HAND_ROT_NUM);
        }
        break;
    }
}
Exemple #2
0
float SliceCache::getAngle(int whichSlice)
{
  int sliceIdx = cacheIndex(whichSlice);

  if (sliceIdx >= 0)
    return mSliceAngles[sliceIdx];
  return readAngle(whichSlice);
}
Exemple #3
0
void UF_uArm::init()
{
    // read offset data
    offsetL = EEPROM.read(1);
    offsetR = EEPROM.read(2);
    // initialization the pin
    pinMode(LIMIT_SW, INPUT);  digitalWrite(LIMIT_SW, HIGH);
    pinMode(BTN_D4,   INPUT);  digitalWrite(BTN_D4,   HIGH);
    pinMode(BTN_D7,   INPUT);  digitalWrite(BTN_D7,   HIGH);
    pinMode(BUZZER,   OUTPUT); digitalWrite(BUZZER,   LOW);
    pinMode(PUMP_EN,  OUTPUT); digitalWrite(PUMP_EN,  LOW);
    pinMode(VALVE_EN, OUTPUT); digitalWrite(VALVE_EN, LOW);
	if (EEPROM.read(0) == CALIBRATION_FLAG) // read of offset flag
    {
		// attaches the servo on pin to the servo object
		servoL.attach(SERVO_L, D150A_SERVO_MIN_PUL, D150A_SERVO_MAX_PUL);
		servoR.attach(SERVO_R, D150A_SERVO_MIN_PUL, D150A_SERVO_MAX_PUL);
		servoRot.attach(SERVO_ROT, D150A_SERVO_MIN_PUL, D150A_SERVO_MAX_PUL);
		servoHand.attach(SERVO_HAND, D009A_SERVO_MIN_PUL, D009A_SERVO_MAX_PUL);
		servoHandRot.attach(SERVO_HAND_ROT, D009A_SERVO_MIN_PUL, D009A_SERVO_MAX_PUL);
		servoHand.write(HAND_ANGLE_OPEN, 0, true);
		servoHand.detach();

		servoL.write(map(readAngle(SERVO_L), SERVO_MIN, SERVO_MAX, 0, 180));
		servoR.write(map(readAngle(SERVO_R), SERVO_MIN, SERVO_MAX, 0, 180));
		servoRot.write(map(readAngle(SERVO_ROT), SERVO_MIN, SERVO_MAX, 0, 180));
		// initialization postion
		setServoSpeed(SERVO_R,   20);  // 0=full speed, 1-255 slower to faster
		setServoSpeed(SERVO_L,   20);  // 0=full speed, 1-255 slower to faster
		setServoSpeed(SERVO_ROT, 20);  // 0=full speed, 1-255 slower to faster
		setPosition(stretch, height, rotation, handRot);
    }
    else
    {	// buzzer alert if calibration needed
		alert(3, 200, 200);
    }
}
Exemple #4
0
void UF_uArm::calibration()
{
    int initPosL = INIT_POS_L + 20; // Added 20 degrees here to start at reasonable point
    int initPosR = INIT_POS_R + 20; // Added 20 degrees here to start at reasonable point

	if(!digitalRead(BTN_D7))
	{
		delay(20);
		// buzzer alert
		alert(1, 20, 0);
	}

	lstTime = millis();
    while(!digitalRead(BTN_D7))
    {
        if(millis() - lstTime > BTN_TIMEOUT_3000)
        {
            // buzzer alert
            alert(2, 50, 100);
            while(!digitalRead(BTN_D7))
            {
                servoL.detach();
                servoR.detach();
            }
			while(1)
			{
				// SG-> Commentary: While user adjusts for minimum angles, keep track of angle and add
				//                  margin of analog reading value of 12, which is about 3 degrees.
				int minAngle_L = readAngle(SERVO_L) + 12;
				int minAngle_R = readAngle(SERVO_R) + 12;

				if(!digitalRead(BTN_D7))
				{
					delay(20);
					// buzzer alert
					alert(1, 20, 0);
					delay(500); //SG-> Added to delay for user to remove hand
					// buzzer alert
					servoL.attach(SERVO_L, D150A_SERVO_MIN_PUL, D150A_SERVO_MAX_PUL);
					servoR.attach(SERVO_R, D150A_SERVO_MIN_PUL, D150A_SERVO_MAX_PUL);
					servoL.write(initPosL);
					servoR.write(initPosR);
					delay(500);
					while(readAngle(SERVO_R) < minAngle_R - SAMPLING_DEADZONE)
					{
						servoR.write(++initPosR);
						delay(50);
					}
					while(readAngle(SERVO_R) > minAngle_R + SAMPLING_DEADZONE)
					{
						servoR.write(--initPosR);
						delay(50);
					}
					while(readAngle(SERVO_L) < minAngle_L - SAMPLING_DEADZONE)
					{
						servoL.write(++initPosL);
						delay(50);
					}
					while(readAngle(SERVO_L) > minAngle_L + SAMPLING_DEADZONE)
					{
						servoL.write(--initPosL);
						delay(50);
					}
					offsetL = initPosL - INIT_POS_L;
					offsetR = initPosR - INIT_POS_R;
					EEPROM.write(0, CALIBRATION_FLAG);  // Added flag to know if offset is done
					EEPROM.write(1, offsetL);			// offsetL
					EEPROM.write(2, offsetR);			// offsetR
					// buzzer alert
					alert(1, 500, 0);      
					// reset postion
					init();
					break;
				}
			}
        }
    }
}
Exemple #5
0
float *SliceCache::getHyperPS(int tileX, int tileY, int whichSlice,
                              double &mean)
{
  if (whichSlice < 0 || whichSlice >= mHeader.nz)
    exitError("slice Num is out of range");
  int sliceIdx = cacheIndex(whichSlice);
  int currCacheSize = mCachedPS.size();
  int tileIdx = tileX + tileY * numXtiles;
  float *retPS;
  int halfSize = mTileSize / 2;
  int tileXdim = mTileSize + 2;
  int fftXdim = tileXdim / 2;
  int idir = 0; //FFT direction;
  int ii, jj, ind, ind2, ix0, iy0;

  if (sliceIdx > -1) { // already in cache
    //if (debugLevel >= 2)
    //  printf("Slice %d is in cache and is included\n", whichSlice);

  } else if (currCacheSize < mMaxSliceNum) { //not in cache and cache not full
    float *newPS = (float *)malloc(mPsArraySize * sizeof(float));
    int *newDone = (int *)malloc(numXtiles * mNumYtiles * sizeof(int));
    float *newMeans = (float *)malloc(numXtiles * mNumYtiles * sizeof(double));
    if (!newPS || !newDone || !newMeans)
      exitError("Allocating memory for power spectra");

    if (debugLevel >= 2)
      printf("Slice %d is NOT in cache and is included \n", whichSlice);

    for (ii = 0; ii < numXtiles * mNumYtiles; ii++)
      newDone[ii] = 0;

    mCachedPS.push_back(newPS);
    mCachedMeans.push_back(newMeans);
    mCachedTileDone.push_back(newDone);
    mCachedSliceMap.push_back(whichSlice);
    mSliceAngles.push_back(readAngle(whichSlice));

    if (mOldestIdx == -1)
      mOldestIdx = currCacheSize;
    sliceIdx = currCacheSize;

  } else { // not in cache and cache is full
    if (debugLevel >= 2)
      printf("Slice %d is NOT in cache and replaces slice %d \n", whichSlice,
             mCachedSliceMap[mOldestIdx]);

    mCachedSliceMap[mOldestIdx] = whichSlice;
    mSliceAngles[mOldestIdx] = readAngle(whichSlice);
    sliceIdx = mOldestIdx;
    mOldestIdx = (mOldestIdx + 1) % mMaxSliceNum;
  }

  fflush(stdout);
  retPS = mCachedPS[sliceIdx] + tileIdx * mPsDim;
  if (mCachedTileDone[sliceIdx][tileIdx]) {
    mean = mCachedMeans[sliceIdx][tileIdx];
    return retPS;
  }

  // Have to compute the power spectrum
  // Read in slice if needed
  if (mCurSlice != whichSlice) {
    // Reallocate as floating point when necessary
    if (mDataOffset && mSliceMode != MRC_MODE_FLOAT) {
      free(mSliceData);
      mSliceData = (float *)malloc(mHeader.nx * mHeader.ny * 4);
      if (!mSliceData)
	exitError("Allocating memory for floating point image slice");
      mSliceMode = MRC_MODE_FLOAT;
    }

    if (mrc_read_slice(mSliceData, mFpStack, &mHeader, whichSlice, 'Z'))
      exitError("Reading slice %d", whichSlice);

    // Apply any offset needed. Done as float to avoid over/underflow.
    if (mDataOffset) {
      unsigned char *bdata = (unsigned char *)mSliceData;
      b3dUInt16 *usdata = (b3dUInt16 *)mSliceData;
      b3dInt16 *sdata = (b3dInt16 *)mSliceData;

      switch (mHeader.mode) {
      case MRC_MODE_BYTE:
	for (ii = mHeader.nx * mHeader.ny - 1; ii >= 0; ii--)
	  mSliceData[ii] = bdata[ii] + mDataOffset;
	break;
      case MRC_MODE_USHORT:
	for (ii = mHeader.nx * mHeader.ny - 1; ii >= 0; ii--)
	  mSliceData[ii] = usdata[ii] + mDataOffset;
	break;
      case MRC_MODE_SHORT:
	for (ii = mHeader.nx * mHeader.ny - 1; ii >= 0; ii--)
	  mSliceData[ii] = sdata[ii] + mDataOffset;
	break;
      case MRC_MODE_FLOAT:
	for (ii = 0; ii < mHeader.nx * mHeader.ny; ii++)
	  mSliceData[ii] += mDataOffset;
	break;
      }
    }
    mCurSlice = whichSlice;
  }

  // get the mTile
  ix0 = tileX * halfSize;
  iy0 = tileY * halfSize;
  sliceTaperInPad(mSliceData, mSliceMode, mHeader.nx, ix0, ix0 + mTileSize - 1,
                  iy0, iy0 + mTileSize - 1, mTile, tileXdim, mTileSize, mTileSize,
                  9, 9);

  // Get its mean and save it
  mean = 0.0;
  for (ii = 0; ii < mTileSize; ii++)
    for (jj = 0; jj < mTileSize; jj++)
      mean += mTile[ii * tileXdim + jj];
  mean /= (mTileSize * mTileSize);
  mCachedMeans[sliceIdx][tileIdx] = mean;

  // FFT and sum into the PS curve
  todfft(mTile, &mTileSize, &mTileSize, &idir);

  for (ii = 0; ii < mPsDim; ii++)
    mPsTmp[ii] = 0.;
  for (ii = 0; ii < mTileSize / 2; ii++) {
    for (jj = 0; jj < fftXdim; jj++) {
      ix0 = ii * fftXdim + jj;
      ind = ii * tileXdim + 2 * jj;
      ind2 = (mTileSize - 1 - ii) * tileXdim + 2 * jj;
      mPsTmp[mTileToPsInd[ix0]] +=
        mTile[ind] * mTile[ind] + mTile[ind + 1] * mTile[ind + 1] +
        mTile[ind2] * mTile[ind2] + mTile[ind2 + 1] * mTile[ind2 + 1];
    }
  }

  // Store into floats and return
  for (ii = 0; ii < mPsDim; ii++) {
    retPS[ii] = mPsTmp[ii];
    //printf("%d  %f\n", mFreqCount[ii], retPS[ii]);
  }
  mCachedTileDone[sliceIdx][tileIdx] = 1;
  return retPS;
}
/** read Angle by servo_num, SERVO_ROT_NUM, SERVO_LEFT_NUM, SERVO_RIGHT_NUM, SERVO_HAND_ROT_NUM
** without offset
**/
double uArmClass::readAngle(byte servo_num)
{
    return readAngle(servo_num, false);
}
Exemple #7
0
void startSlam(int v)
{
	FileStorage fs("config.yml", FileStorage::READ);
	Mat cam, dist, rpos, q, s;

	fs["cameraMatrix"] >> cam;
	fs["distMatrix"] >> dist;
	fs["sensorPos"] >> rpos;
	fs["q"] >> q;
	fs["s"] >> s;

	fs.release();

	ml = new MarkerLocator(cam, dist, rpos, 100.);

//	Mat q = (Mat_<double>(2,1) << .01, .1*M_PI/180);
//	Mat s = (Mat_<double>(2,1) << 10, .5*M_PI/180);

	slam = new EkfSlam(&scan, q, s);
	#ifdef GUI
	namedWindow("EKFSlam");
	disp = new MapDisplay("Map");
	waitKey();
	#endif

	Mat u = (Mat_<double>(2,1) << 0, 0);

	int fd = openPort();
	oiStart();
	setSafeMode();
	readDist();
	readAngle();

	VideoCapture cap(0);
	if(!cap.isOpened()) {
		printf("Failed capture\n");
		return;
	}

	VideoWriter record("/tmp/demo.avi", CV_FOURCC('D','I','V','X'), 30, Size(1340,600), true);
	if(!record.isOpened()) {
		printf("Failed writer\n");
		return;
	}

	clock_t t;
	time_t stime;
	while(1) {
		t = clock();
		setDrive(v,v);
		double mindist = 99999;

		while(msElap(t) < 100 && !hitWall()) {
			u.at<double>(0) = (double)readDist();
			u.at<double>(1) = M_PI*(double)readAngle()/180;
			mindist = slamStep(u, cap, record);
		}

		// backup before turn when running into something
		if(hitWall() || mindist < 700) {
			if(hitWall()) {
				// backup
				setDrive(-15,-15);
				t = clock();
				while(msElap(t) < 500) {
					u.at<double>(0) = (double)readDist();
					u.at<double>(1) = M_PI*(double)readAngle()/180;
					slamStep(u, cap, record);
				}
			}

			// turn
			if(time(&stime)%60 < 30) {
				setDrive(-5, 5);
			}
			else
				setDrive(5,-5);

			t = clock();
			while(msElap(t) < 250) {
				u.at<double>(0) = 0;
				u.at<double>(1) = M_PI*(double)readAngle()/180;
				slamStep(u, cap, record);
			}
		}
		if(waitKey(10) == 27)
			break;
	}

	setDrive(0,0);
	imwrite("lastFrame.png", g_outFrame);

	close(fd);

	delete ml;
	delete slam;
	#ifdef GUI
	delete disp;
	#endif
}