firmware
This commit is contained in:
parent
5d17370cc2
commit
cf65aa0c8c
277
firmware/armCtl/armCtl.ino
Normal file
277
firmware/armCtl/armCtl.ino
Normal file
@ -0,0 +1,277 @@
|
||||
#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);
|
||||
}
|
Loading…
Reference in New Issue
Block a user