#include "TicTacToeArm.h" TicTacToeArm::TicTacToeArm(int pin_base, int pin_shoulder, int pin_elbow) { this->base.attach(pin_base); this->shoulder.attach(pin_shoulder); this->elbow.attach(pin_elbow); } void TicTacToeArm::home(unsigned long step_delay) { float tb, ts, te; float cb, cs, ce; float db, ds, de; this->penState = 1; this->getAngleMap(80, 90, tb, ts, te); cb = this->base.readMicroseconds(); cs = this->shoulder.readMicroseconds(); ce = this->elbow.readMicroseconds(); db = (tb - cb) / STEP_MAX; ds = (ts - cs) / STEP_MAX; de = (te - ce) / STEP_MAX; if(db != 0 || ds !=0 || de !=0) { for(int i = 0; i < STEP_MAX; i++) { cb += db; cs += ds; ce += de; this->setServoMicroseconds(cb, cs, ce); delay(step_delay); } } this->pen_X = 0; this->pen_Y = 80; } void TicTacToeArm::penUp() { if(this->penState == 0) { float cb, cs, ce; float tb, ts, te; float db, ds, de; this->penState = 1; this->getAngleMap(this->_toRadius(this->pen_X, this->pen_Y), this->_toAngle(this->pen_X, this->pen_Y), tb, ts, te); cb = this->base.readMicroseconds(); cs = this->shoulder.readMicroseconds(); ce = this->elbow.readMicroseconds(); db = (tb - cb) / STEP_MAX; ds = (ts - cs) / STEP_MAX; de = (te - ce) / STEP_MAX; if(db != 0 || ds !=0 || de !=0) { for(int i = 0; i < STEP_MAX; i++) { cb += db; cs += ds; ce += de; this->setServoMicroseconds(cb, cs, ce); delay(DEFAULT_STEP_MS); } } } } void TicTacToeArm::penDown() { if(this->penState == 1) { float cb, cs, ce; float tb, ts, te; float db, ds, de; this->penState = 0; this->getAngleMap(this->_toRadius(this->pen_X, this->pen_Y), this->_toAngle(this->pen_X, this->pen_Y), tb, ts, te); cb = this->base.readMicroseconds(); cs = this->shoulder.readMicroseconds(); ce = this->elbow.readMicroseconds(); db = (tb - cb) / STEP_MAX; ds = (ts - cs) / STEP_MAX; de = (te - ce) / STEP_MAX; if(db != 0 || ds !=0 || de !=0) { for(int i = 0; i < STEP_MAX; i++) { cb += db; cs += ds; ce += de; this->setServoMicroseconds(cb, cs, ce); delay(DEFAULT_STEP_MS); } } } } void TicTacToeArm::lineTo(float x, float y) { float dx, dy; float cx, cy; float tb, ts, te; cx = this->pen_X; cy = this->pen_Y; DEBUG << "Current (x, y): (" << cx << ", " << cy << ")"; DEBUG << "Target (x, y): (" << x << ", " << y << ")"; dx = (x - cx) / STEP_MAX; dy = (y - cy) / STEP_MAX; DEBUG << "Step (dx, dy): (" << dx << ", " << dy << ")"; if(dx != 0 || dy != 0) { for (int i = 0; i < STEP_MAX; i++ ) { cx += dx; cy += dy; this->getAngleMap(this->_toRadius(cx, cy), this->_toAngle(cx, cy), tb, ts, te); this->setServoMicroseconds(tb, ts, te); delay(DEFAULT_STEP_MS); } this->pen_X = x; this->pen_Y = y; } } void TicTacToeArm::getAngleMap(float r, float a, float& b, float& s, float& e) { if(this->penState == 1) { b = map(a, 60.0, 120.0, 1087, 1697); s = map(r, 88.0, 158.0, 1786, 2036); e = map(r, 88.0, 158.0, 1225, 1675); } else { b = map(a, 60.0, 120.0, 1087, 1697); s = map(r, 88.0, 158.0, 1936, 2156); e = map(r, 88.0, 158.0, 1105, 1655); } } void TicTacToeArm::setServoMicroseconds(float b, float s, float e) { DEBUG << "Servo Microseconds (b, s, e): (" << b << ", " << s << ", " << e << ")"; this->base.writeMicroseconds(b); this->shoulder.writeMicroseconds(s); this->elbow.writeMicroseconds(e); } float TicTacToeArm::_toAngle(float x, float y) { return (atan2(y, x)*180.0)/PI; } float TicTacToeArm::_toRadius(float x, float y) { return sqrt(x*x + y*y); }