/* 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;
}
}
float SliceCache::getAngle(int whichSlice)
{
int sliceIdx = cacheIndex(whichSlice);
if (sliceIdx >= 0)
return mSliceAngles[sliceIdx];
return readAngle(whichSlice);
}
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);
}
}
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;
}
}
}
}
}
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);
}
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
}
