// Turn to a point based on gyroscopic target
// Test status:
// Tested, works great.
// TODO Add in breakout time parameter, because it is constant at 2 seconds
// right now
void pLoopTurnPointRaw(int angleTarget, double p, double d, int thresh,
int threshCount) {
int error; // error in current position
int lastError = 0; // error from last iteration of the loop
int speed = 0; // speed for the motors to run at
int stopCount = 0; // Amount of time spent within threshold
int iterations = 0;
bprintf(1, "---\r\nang:%d\r\np:%f\r\nd:%f\r\nthresh%d\r\ncount:%d\r\n",
angleTarget, p, d, thresh, threshCount);
while (iterations++ < 40) { // Was 263
error = angleTarget - gyroGet(getGyro());
speed = (error * p) + ((error - lastError) * d);
speed = (abs(speed) > 127) ? (speed < 0) ? -127 : 127 : speed;
speed = (abs(speed) < 30) ? (speed < 0) ? -30 : 30 : speed;
speed = linSpeed(speed); // Linearize speed
driveRaw(-speed, -speed, speed, speed);
if (abs(error) < thresh) {
stopCount++;
if (stopCount >= threshCount)
break;
}
lastError = error;
delay(20);
}
driveRaw(speed, speed, -speed, -speed); // Slam the breaks
delay(10);
driveRaw(0, 0, 0, 0);
}
// Drive an amount of ticks as straight as possible
// Test status:
// completely untested
void pLoopDriveStraightRaw(int tickDiff, bool correctBackwards, bool correctDir,
double pSpeed, double dSpeed, double pCorrect,
int thresh, int threshCount) {
int leftInit = encoderGet(getEncoderBL()); // Initial left value
int rightInit = encoderGet(getEncoderBR()); // Initial right value
int errorL; // Error in the left side
int errorR; // Error in the right side
int error; // Averaged Error
int lastError = 0; // The Error from the Previous Iteration
int speed; // Calculated speed to drive at
int stopCount = 0; // Amount of time spent within threshold
int initGyro = gyroGet(getGyro()); // Initial value of the gyro
int speedModif = 0; // How much to modify the speed for angle
int angleOffset; // How much the robot is curving in its motion
int iterations = 0;
while (iterations++ <= (P_LOOP_DRIVE_BREAK_TIME / 20)) {
errorL = tickDiff - (encoderGet(getEncoderBL()) - leftInit);
errorR = tickDiff - (encoderGet(getEncoderBR()) - rightInit);
error = errorR; // round((errorL + errorR) / 2); // errorL;
speed = error * pSpeed + ((error - lastError) * dSpeed);
speed = (abs(speed) > 127) ? (speed < 0) ? -127 : 127 : speed;
speed = (abs(speed) < 25) ? (speed < 0) ? -25 : 25 : speed;
angleOffset = gyroGet(getGyro()) - initGyro;
speedModif = (correctDir) ? angleOffset * pCorrect : 0;
// Set motors to linearized version of input speeds
driveRaw(linSpeed(speed + speedModif), linSpeed(speed + speedModif),
linSpeed(speed - speedModif), linSpeed(speed - speedModif));
if (abs(error) < thresh) {
stopCount++;
if (stopCount >= threshCount || !correctBackwards)
break;
}
lastError = error;
delay(20);
}
driveRaw(-speed, -speed, -speed, -speed); // Slam the breaks
delay(10);
driveRaw(0, 0, 0, 0);
}
void ImuAdis16448::processMeasurements() {
while (1) {
boost::this_thread::interruption_point();
//get the newest measurement
Measurement::Ptr meas = Sensor::measurement_queue_.pop();
//VISENSOR_DEBUG("in imu_adis thread %d\n", meas.buffersize);
//check for missed frames
// TODO(schneith): use a timer to check for missed frames (currently we only detected sequences of MISSED-RECEIVED-MISSED, not MISSED-MISSED,...)
if( checkTimestamp(meas->timestamp) )
{
//publish an empty missed frame
ViImuMsg::Ptr missed_imu_ptr = boost::make_shared<ViImuMsg>();
missed_imu_ptr->imu_id = Imu::imu_id_;
publishImuData(missed_imu_ptr, ViErrorCodes::MEASUREMENT_DROPPED);
}
// //TODO adapt
// create new shared pointer for the imu message
ViImuMsg::Ptr imu_msg_ptr = boost::make_shared<ViImuMsg>();
// Add imu information to the msg
imu_msg_ptr->imu_id = Imu::imu_id_;
imu_msg_ptr->timestamp=meas->timestamp;
imu_msg_ptr->timestamp_host = meas->timestamp_host;
imu_msg_ptr->timestamp_fpga_counter = meas->timestamp_fpga_counter;
// get imu data
getGyro(meas->data.get(), &imu_msg_ptr->gyro[0]);
getAcc(meas->data.get(), &imu_msg_ptr->acc[0]);
getMag(meas->data.get(), &imu_msg_ptr->mag[0]);
getBaro(meas->data.get(), &imu_msg_ptr->baro);
publishImuData(imu_msg_ptr, ViErrorCodes::NO_ERROR);
}
}
