bool checkIsLeaf(void) {
int direction = currentDirection();
while(!isDark()) {
if( abs(angleDifference(currentDirection(), direction)) > 170) {
return true;
}
}
return false;
}
void faceForward(int forwards){
if(angleDifference(currentDirection(), forwards) > 0){//too far to the right
motor[left] = -SPEED; //turn left
} else {
motor[right] = -SPEED; //turn right
}
while(abs(angleDifference(currentDirection(), forwards)) != 0){
wait1Msec(1);
}
}
void turnToAngle(int angle, int undershoot) {
if( angleDifference(angle, currentDirection()) < 0 ) {
direction = LEFT;
} else {
direction = RIGHT;
}
spinInDirection();
while(abs(angleDifference(currentDirection(), angle)) > undershoot){
wait1Msec(1);
}
motor[left] = 0;
motor[right] = 0;
}
void turnToDirection(int turnDirection) {
adjustAngle();
int currentDir;
currentDir = currentDirection();
double target = (currentDir + turnDirection) * M_PI / 2;
while(fabs(currentHeading() - target) > turnThreshold) {
if(fabs(currentHeading() - target) < slowThreshold) {
if(currentHeading() > target) {
set_motors(-5,5);
}
if(currentHeading() < target) {
set_motors(5,-5);
}
} else {
if(currentHeading() > target) {
set_motors(-15,15);
}
if(currentHeading() < target) {
set_motors(15,-15);
}
}
calcPos();
}
set_motors(0,0);
adjustAngle();
usleep(10000);
}
void CountProducerWidget::on_directionCombo_activated(int /*index*/)
{
if (m_producer) {
m_producer->set("direction", currentDirection().toLatin1().constData());
m_producer->set(kShotcutDetailProperty, detail().toUtf8().constData());
emit producerChanged(producer());
}
}
/**
* Turns around until black is found
*/
void turnToLine(void) {
spinInDirection();
while(!isDark()) {
abortTimeslice();
}
int startDir = currentDirection();
while(isDark()) {
abortTimeslice();
}
motor[left] = 0;
motor[right] = 0;
int centre = startDir + (angleDifference(currentDirection(), startDir)/2);
nxtDisplayCenteredTextLine(6, "cen: %i", centre);
turnToAngle(centre, 0);
}
Mlt::Properties CountProducerWidget::getPreset() const
{
Mlt::Properties p;
p.set("direction", currentDirection().toLatin1().constData());
p.set("style", currentStyle().toLatin1().constData());
p.set("sound", currentSound().toLatin1().constData());
p.set("background", currentBackground().toLatin1().constData());
p.set("drop", ui->dropCheckBox->isChecked());
setLength(&p, ui->durationSpinBox->value());
return p;
}
Mlt::Producer* CountProducerWidget::newProducer(Mlt::Profile& profile)
{
Mlt::Producer* p = new Mlt::Producer(profile, "count:");
p->set("direction", currentDirection().toLatin1().constData());
p->set("style", currentStyle().toLatin1().constData());
p->set("sound", currentSound().toLatin1().constData());
p->set("background", currentBackground().toLatin1().constData());
p->set("drop", ui->dropCheckBox->isChecked());
setLength(p, ui->durationSpinBox->value());
p->set(kShotcutCaptionProperty, ui->nameLabel->text().toUtf8().constData());
p->set(kShotcutDetailProperty, detail().toUtf8().constData());
return p;
}
int gatherForwardsData(void) {
wait10Msec(100);
PlaySound(soundBlip);
time100[T2] = 0;
int timeToWait = minDistance*10/(2*sweepSpeed) - 10;
int count = 0;
int total = 0;
int forwards = 0;
while(time100[T2] < timeToWait) {
//forwards = forwards + (angleDifference(currentDirection(), forwards))/++count;
count+= 2;
total += count;
//This formula give more weight to recent samples
forwards = forwards + (count * (angleDifference(currentDirection(), forwards))/total);
nxtDisplayCenteredTextLine(5, "%i", forwards);
wait10Msec(5);
}
PlaySound(soundBlip);
PlaySound(soundBlip);
return forwards;
}
void Straight()
{
adjustAngle();
// Going Up
int i;
int motorSpeed;
for (i = 0; i < 11; i++) {
set_motors(3 * i, 3 * i);
usleep(100000);
}
if(currentDirection() == 0 || currentDirection() == 4) {
idealY += 60;
printf("UP TargetY: %f\n",idealY);
while(fabs(currentY()-idealY) > 1) {
if(fabs(currentY()-idealY) < 10) {
if(currentY() < idealY) {
correctToStraight(2 * fabs(currentY()-idealY));
// set_motors(motorSpeed, motorSpeed);
} else {
correctToStraight(-2 * fabs(currentY()-idealY));
// set_motors(-5 -2 * fabs(currentY()-idealY),-5 -2 * fabs(currentY()-idealY));
}
} else if(currentY() < idealY) {
set_motors(40,40);
} else if(currentY() > idealY) {
set_motors(-40,-40);
}
calcPos();
//printf("X:%f Y:%f\n", currentX(),currentY());
}
set_motors(0,0);
bCoord.y += 1;
}
// Going Down
if(currentDirection() == 2) {
idealY -= 60;
printf("DOWN TargetY: %f\n",idealY);
while(fabs(currentY()-idealY) > 1) {
if(fabs(currentY()-idealY) < 10) {
if(currentY() > idealY) {
correctToStraight(2 * fabs(currentY()-idealY));
// set_motors(5 + 2 * fabs(currentY()-idealY), 5 + 2 * fabs(currentY()-idealY));
} else {
correctToStraight(-2 * fabs(currentY()-idealY));
// set_motors(-5 -2 * fabs(currentY()-idealY),-5 -2 * fabs(currentY()-idealY));
}
} else if(currentY() < idealY) {
set_motors(-40,-40);
} else if(currentY() > idealY) {
set_motors(40,40);
}
calcPos();
//printf("X:%f Y:%f\n", currentX(),currentY());
}
set_motors(0,0);
bCoord.y -= 1;
}
// Going Right
if(currentDirection() == 1) {
idealX += 60;
printf("RIGHT TargetX: %f\n",idealX);
while(fabs(currentX()-idealX) > 1) {
if(fabs(currentX()-idealX) < 10) {
if(currentX() < idealX) {
correctToStraight(2 * fabs(currentX()-idealX));
// set_motors(5 + 2 * fabs(currentX()-idealX),5 + 2 * fabs(currentX()-idealX));
} else {
correctToStraight(-2 * fabs(currentX()-idealX));
// set_motors(-5 -2 * fabs(currentX()-idealX),-5 -2 * fabs(currentX()-idealX));
}
} else if(currentX() < idealX) {
set_motors(40,40);
} else if(currentX() > idealX) {
set_motors(-40,-40);
}
calcPos();
//printf("X:%f Y:%f\n", currentX(),currentY());
}
set_motors(0,0);
bCoord.x += 1;
}
// Going Left
if(currentDirection() == 3) {
idealX -= 60;
printf("LEFT TargetX: %f\n",idealX);
while(fabs(currentX()-idealX) > 1) {
if(fabs(currentX()-idealX) < 10) {
if(currentX() > idealX) {
correctToStraight(2 * fabs(currentX()-idealX));
// set_motors(5 + 2 * fabs(currentX()-idealX),5 + 2 * fabs(currentX()-idealX));
} else {
correctToStraight(-2 * fabs(currentX()-idealX));
// set_motors(-5 -2 * fabs(currentX()-idealX),-5 -2 * fabs(currentX()-idealX));
}
} else if(currentX() < idealX) {
set_motors(-40,-40);
} else if(currentX() > idealX) {
set_motors(40,40);
}
calcPos();
//printf("X:%f Y:%f\n", currentX(),currentY());
}
set_motors(0,0);
bCoord.x -= 1;
}
}
bool detectNode(int forwards) {
return abs(angleDifference(currentDirection(), forwards)) > ANGLE_THRESHOLD;
}
