278 lines
5.0 KiB
C++
278 lines
5.0 KiB
C++
#include <Servo.h>
|
|
|
|
// HARDWARE PINS
|
|
#define motBase 2
|
|
#define motBoom 3
|
|
#define motJib 4
|
|
#define motRJib 5
|
|
#define motWrPan 6
|
|
#define motWrTlt 7
|
|
|
|
// SERIAL SLOTS
|
|
#define axiBase 0
|
|
#define axiBoom 1
|
|
#define axiJib 2
|
|
#define axiRJib 3
|
|
#define axiWrPan 4
|
|
#define axiWrTlt 5
|
|
|
|
// array of motors
|
|
const int motors[] = { motBase , motBoom, motJib, motRJib, motWrPan, motWrTlt };
|
|
// length of motor array;
|
|
const int motLength = 6;
|
|
|
|
|
|
//
|
|
// Axis Class, a mechanical joint
|
|
//
|
|
class Axis
|
|
{
|
|
public:
|
|
Axis() {
|
|
m_servo = Servo();
|
|
maneuver( 90 ); // home
|
|
m_lastUpdate = millis();
|
|
}
|
|
|
|
int angle() { return m_angle; }
|
|
void attach(int pin) { m_servo.attach(pin); }
|
|
|
|
void update() {
|
|
if((millis() - m_lastUpdate) > m_interval) // time to update
|
|
{
|
|
m_lastUpdate = millis();
|
|
m_angle = m_servo.read();
|
|
|
|
if (m_angle < m_target) {
|
|
m_angle += m_increment;
|
|
if (m_angle > m_target) { // overshoot
|
|
m_angle = m_target;
|
|
}
|
|
}
|
|
else if (m_angle > m_target) {
|
|
m_angle -= m_increment;
|
|
if (m_angle < m_target) {
|
|
m_angle = m_target; // overshoot
|
|
}
|
|
}
|
|
m_servo.write(m_angle); // write the new angle
|
|
}
|
|
}
|
|
|
|
void maneuver(int target, int interval = 15, int increment = 1) {
|
|
m_target = constrain(target, 0, 180);
|
|
m_interval = max(interval, 1);
|
|
m_increment = constrain(increment, 1, 180);
|
|
}
|
|
|
|
private:
|
|
Servo m_servo; // the servo
|
|
int m_angle {0}; // current servo position
|
|
int m_target {0}; // target servo position
|
|
int m_increment {0}; // increment to move for each interval
|
|
int m_interval {0}; // interval between updates
|
|
unsigned long m_lastUpdate; // last update of position
|
|
}; // Axis
|
|
|
|
|
|
//
|
|
// Robot Class, a mechanical unit
|
|
//
|
|
class Robot
|
|
{
|
|
public:
|
|
Robot() {
|
|
for (int i = 0; i < motLength; i++) {
|
|
m_axis[i] = new Axis();
|
|
m_axis[i]->update();
|
|
}
|
|
};
|
|
|
|
Axis *axis(int n) { return m_axis[n]; }
|
|
|
|
void sequence() {
|
|
for (int i = 0; i < motLength; i++) {
|
|
m_axis[i]->update();
|
|
m_position[i] = m_axis[i]->angle();
|
|
}
|
|
} // sequence
|
|
|
|
String position() {
|
|
String s = "{";
|
|
for (int i = 0; i < motLength; i++) {
|
|
String v = String((uint8_t)m_axis[i]->angle(), HEX);
|
|
if (v.length() < 2) {
|
|
v = '0' + v;
|
|
}
|
|
s = String(s + v);
|
|
if (i < motLength-1) {
|
|
s = String(s + ",");
|
|
}
|
|
}
|
|
return String(s + "}");
|
|
} // position
|
|
|
|
private:
|
|
Axis *m_axis[motLength];
|
|
int m_position[motLength] {};
|
|
|
|
}; // Robot
|
|
Robot r;
|
|
|
|
|
|
//
|
|
//
|
|
//
|
|
int nibble2number(int n) {
|
|
switch (n) {
|
|
case '0':
|
|
return 0;
|
|
case '1':
|
|
return 1;
|
|
case '2':
|
|
return 2;
|
|
case '3':
|
|
return 3;
|
|
case '4':
|
|
return 4;
|
|
case '5':
|
|
return 5;
|
|
case '6':
|
|
return 6;
|
|
case '7':
|
|
return 7;
|
|
case '8':
|
|
return 8;
|
|
case '9':
|
|
return 9;
|
|
case 'a':
|
|
return 10;
|
|
case 'A':
|
|
return 10;
|
|
case 'b':
|
|
return 11;
|
|
case 'B':
|
|
return 11;
|
|
case 'c':
|
|
return 12;
|
|
case 'C':
|
|
return 12;
|
|
case 'd':
|
|
return 13;
|
|
case 'D':
|
|
return 13;
|
|
case 'e':
|
|
return 14;
|
|
case 'E':
|
|
return 14;
|
|
case 'f':
|
|
return 15;
|
|
case 'F':
|
|
return 15;
|
|
default:
|
|
return -1;
|
|
}
|
|
}
|
|
|
|
|
|
//
|
|
// read command from serial port
|
|
//
|
|
void readCommand() {
|
|
bool inMessage = false;
|
|
int i = 0;
|
|
while (Serial.available()) {
|
|
delay(10); // small delay to allow input buffer to fill
|
|
|
|
int c = Serial.read();
|
|
if ( c = -1 ) { // no bytes read
|
|
break;
|
|
}
|
|
if (!isAscii(c)) {
|
|
break; // only ready ASCII
|
|
}
|
|
|
|
// start a message
|
|
if (!inMessage) {
|
|
if (c == '{') { // message start delimiter
|
|
inMessage = true;
|
|
}
|
|
continue;
|
|
}
|
|
|
|
// in a message
|
|
if (c == '}') { // message end delimiter
|
|
inMessage = false;
|
|
break;
|
|
}
|
|
if (c == ',') { // value seperator
|
|
i++;
|
|
if (i >= motLength) {
|
|
inMessage = false;
|
|
break;
|
|
}
|
|
continue;
|
|
}
|
|
|
|
// read a hex value
|
|
int Msb = c;
|
|
if (!isHexadecimalDigit(Msb)) {
|
|
inMessage = false;
|
|
break;
|
|
}
|
|
while(!Serial.available()) { // Lsb isn't in buffer yer
|
|
delay(3); // todo: make this less bad
|
|
}
|
|
int Lsb = Serial.read();
|
|
if (!isHexadecimalDigit(Lsb)) {
|
|
inMessage = false;
|
|
break;
|
|
}
|
|
|
|
// move the axis
|
|
int v = ( Msb << 4 ) | Lsb;
|
|
r.axis(i)->maneuver(v);
|
|
|
|
// next
|
|
continue;
|
|
}
|
|
|
|
} // readCommand
|
|
|
|
|
|
//
|
|
// ARDUINO Setup
|
|
//
|
|
void setup() {
|
|
// init hardware
|
|
for (int i = 0; i < motLength; i++) {
|
|
r.axis(i)->attach(motors[i]);
|
|
}
|
|
|
|
// home isnt' default of 90
|
|
r.axis(axiBoom)->maneuver(140);
|
|
r.axis(axiJib)->maneuver(10);
|
|
r.axis(axiRJib)->maneuver(95);
|
|
|
|
// turn on the serial port
|
|
Serial.begin(19200);
|
|
}
|
|
|
|
|
|
//
|
|
// ARDUINO Loop
|
|
//
|
|
void loop() {
|
|
// read from serial port
|
|
readCommand();
|
|
|
|
// sequence the arm.
|
|
r.sequence();
|
|
|
|
// write current position info to serial port
|
|
if (Serial) {
|
|
Serial.println(r.position());
|
|
}
|
|
delay(15);
|
|
}
|