void drawTau(Servo *leftWheel, Servo *rightWheel, Servo *penServo){
//DRAWING THE SERIF MARKS AND TOP PART OF TAU
penDown(leftWheel, rightWheel, penServo); //Put pen down to start drawing
backward(leftWheel, rightWheel, DISTANCE*.09); //drawing left serif
forward(leftWheel, rightWheel, DISTANCE*.09); //Move to initial point
turnRight(leftWheel, rightWheel, RIGHT_TURN_90_DEGREES); //Turn to draw top of T
forward(leftWheel, rightWheel, DISTANCE*.293 + 2*(DISTANCE*.207)); //drawing top part of T
turnRight(leftWheel, rightWheel, RIGHT_TURN_90_DEGREES);
forward(leftWheel, rightWheel, DISTANCE*.09); //drawing right serif
backward(leftWheel, rightWheel, DISTANCE*.09);
//Now we want to move it to the center of T to continue drawing middle part of Tau
//penUp(leftWheel, rightWheel, penServo); //penUp so it doesn't leave a trail
turnRight(leftWheel, rightWheel, RIGHT_TURN_90_DEGREES);
penUp(leftWheel, rightWheel, penServo); //penUp so it doesn't leave a trail
forward(leftWheel, rightWheel, (DISTANCE*.293 + 2*(DISTANCE*.207)) / 2); // Move to the top center of Tau
penDown(leftWheel, rightWheel, penServo); //put pen down to draw again
turnLeft(leftWheel, rightWheel, LEFT_TURN_90_DEGREES - 70); //turn 90 degrees to get ready to draw bottom Tau
forward(leftWheel, rightWheel, DISTANCE - 80);
//DRAW BOTTOM PART OF TAU
turnRight(leftWheel, rightWheel, RIGHT_TURN_90_DEGREES);
forward(leftWheel, rightWheel, DISTANCE*.073);
backward(leftWheel, rightWheel, DISTANCE*.135);
penUp(leftWheel, rightWheel, penServo); //completed drawing Tau, pen up.
stopBot(leftWheel, rightWheel, DISTANCE); //finished drawing, freeze for ten seconds.
}
void Robot::drawLine(int points[4], BioloidController bioloid)
{
InverseKinematics(points);
bioloid.poseSize = 2; // load two poses in, one for each vertex
bioloid.readPose();//find where the servos are currently
penUp();
bioloid.setNextPose(1, points[0]); //set the coordinates for the vertex to which the arm is moving first
bioloid.setNextPose(2, points[1]);
bioloid.interpolateSetup(5000); // setup for interpolation from current->next
while(bioloid.interpolating > 0)
{
//keep moving until next pose is reached
bioloid.interpolateStep();
delay(3);
}
penDown();
//bioloid.readPose();//find where the servos are currently
bioloid.setNextPose(1, points[2]);
bioloid.setNextPose(2, points[3]);
bioloid.interpolateSetup(1000); // setup for interpolation from current->next over 1/2 a second
while(bioloid.interpolating > 0)
{
//keep moving until next pose is reached
bioloid.interpolateStep();
delay(3);
}relaxArm(bioloid);
}
bool Mapper::penDownCallback(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) {
penDown();
firstDraw_ = true; // The next instance will be the first time drawing on the map
res.success = true;
res.message = "Pen is down. Map will be marked.";
return true;
}
/**
* Tablet events are handled here
* @param event event info
*/
bool EditorView::viewportEvent(QEvent *event)
{
if(event->type() == QEvent::TabletMove) {
// Stylus moved
QTabletEvent *tabev = static_cast<QTabletEvent*>(event);
tabev->accept();
const dpcore::Point point(mapToScene(tabev->pos()), tabev->pressure());
if(!prevpoint_.intSame(point)) {
if(isdragging_)
moveDrag(tabev->x(), tabev->y());
else {
if(pendown_) {
if(point.pressure()==0) {
// Missed a release event
pendown_ = NOTDOWN;
emit penUp();
} else {
emit penMove(point);
}
}
updateOutline(point);
}
prevpoint_ = point;
}
} else if(event->type() == QEvent::TabletPress) {
// Stylus touches the tablet surface
QTabletEvent *tabev = static_cast<QTabletEvent*>(event);
tabev->accept();
if(dragbtndown_) {
startDrag(tabev->x(), tabev->y(), dragbtndown_);
} else {
if(pendown_ == NOTDOWN) {
const dpcore::Point point(mapToScene(tabev->pos()), tabev->pressure());
pendown_ = TABLETDOWN;
emit penDown(point);
updateOutline(point);
prevpoint_ = point;
}
}
} else if(event->type() == QEvent::TabletRelease) {
// Stylus lifted
QTabletEvent *tabev = static_cast<QTabletEvent*>(event);
tabev->accept();
if(isdragging_) {
stopDrag();
} else if(pendown_ == TABLETDOWN) {
dpcore::Point point(mapToScene(tabev->pos()), 0);
updateOutline(point);
prevpoint_ = point;
pendown_ = NOTDOWN;
emit penUp();
}
} else {
return QGraphicsView::viewportEvent(event);
}
return true;
}
Turtle::Turtle(Composite* owner){
double pts_body[4][2] = {{0, 15}, {20, 0}, {0, -15}, {0,15}};
setColor(COLOR("red"), false);
setFill();
penDown();
reset(canvas_width()/2, canvas_height()/2,pts_body, 4, owner);
}
void drawTheta(Servo *leftWheel, Servo *rightWheel, Servo *penServo){
///// DRAW THE OUTER SHELL OF THE THETA /////
//Change DISTANCE in constants.h to change size of the Theta
forward(leftWheel, rightWheel, DISTANCE*.293);
turnRight(leftWheel, rightWheel, THETA_RIGHT_TURN); //Draws top right corner
forward(leftWheel, rightWheel, DISTANCE*.293);
turnRight(leftWheel, rightWheel, THETA_RIGHT_TURN); //Draws the right edge
forward(leftWheel, rightWheel, DISTANCE*.587);
turnRight(leftWheel, rightWheel, THETA_RIGHT_TURN); //Draws lower right corner
forward(leftWheel, rightWheel, DISTANCE*.293);
turnRight(leftWheel, rightWheel, THETA_RIGHT_TURN); //Draws bottom of Theta
forward(leftWheel, rightWheel, DISTANCE*.293);
turnRight(leftWheel, rightWheel, THETA_RIGHT_TURN); //Draws bottom left corner
forward(leftWheel, rightWheel, DISTANCE*.293);
turnRight(leftWheel, rightWheel, THETA_RIGHT_TURN - 10);
forward(leftWheel, rightWheel, DISTANCE*.587); //Draws the left Theta edge
turnRight(leftWheel, rightWheel, THETA_RIGHT_TURN); //Draws top left corner
forward(leftWheel, rightWheel, DISTANCE*.293);
turnRight(leftWheel, rightWheel, THETA_RIGHT_TURN);
///// DRAW THE MIDDLE PART OF THE THETA /////
//First, position the robot.
penUp(leftWheel, rightWheel, penServo); //Raise pen up
turnRight(leftWheel, rightWheel, RIGHT_TURN_90_DEGREES); // Turn right
forward(leftWheel, rightWheel, DISTANCE*.65); // Go towards the center of circle
// Now, let's actually draw the "lightning bolt", or center.
turnLeft(leftWheel, rightWheel, LEFT_TURN_90_DEGREES * 1.5); // Turn to get ready for drawing
penDown(leftWheel, rightWheel, penServo); // Put the pen down.
// This part will actually draw the center.
forward(leftWheel, rightWheel, DISTANCE*.146);
turnRight(leftWheel, rightWheel, RIGHT_TURN_90_DEGREES / 2);
forward(leftWheel, rightWheel, DISTANCE*.146);
turnLeft(leftWheel, rightWheel, LEFT_TURN_90_DEGREES / 2);
forward(leftWheel, rightWheel, DISTANCE*.146);
// Stop the robot and lift the pen.
// Freeze for ten seconds.
penUp(leftWheel, rightWheel, penServo);
stopBot(leftWheel, rightWheel, DISTANCE);
}
//! Handle mouse press events
void EditorView::mousePressEvent(QMouseEvent *event)
{
/** @todo why do we sometimes get mouse events for tablet strokes? */
if(pendown_ != NOTDOWN)
return;
if(event->button() == Qt::MidButton || dragbtndown_) {
startDrag(event->x(), event->y(), dragbtndown_!=ROTATE?TRANSLATE:ROTATE);
} else if(event->button() == Qt::LeftButton && isdragging_==NOTRANSFORM) {
pendown_ = MOUSEDOWN;
emit penDown(
dpcore::Point(mapToScene(event->pos()), 1.0)
);
}
}
bool LogoParser :: penDown(ItCurr &, ItEnd &)
{
emit penDown();
return true;
}
void Executer::executePenDown(TreeNode* node) {
// //qDebug() << "called";
if (!checkParameterQuantity(node, 0, 20000+Token::PenDown*100+90)) return;
emit penDown();
}
