Initial Upload
This commit is contained in:
commit
aea72d68ed
1 changed files with 557 additions and 0 deletions
557
baseline_dev/baseline_dev.ino
Normal file
557
baseline_dev/baseline_dev.ino
Normal file
|
|
@ -0,0 +1,557 @@
|
|||
#include <Encoder.h>
|
||||
#include <avr/pgmspace.h>
|
||||
|
||||
#include <AccelStepper.h>
|
||||
|
||||
// Firmware version
|
||||
const char* VERSION = "0.0.1";
|
||||
|
||||
const int NUM_JOINTS = 6;
|
||||
|
||||
/*
|
||||
MOTOR DIRECTION - motor directions can be changed on the caibration page in the software but can also
|
||||
be changed here: example using DM542T driver(CW) set to 1 - if using ST6600 or DM320T driver(CCW) set to 0
|
||||
DEFAULT = 111011 */
|
||||
|
||||
const int J1rotdir = 1;
|
||||
const int J2rotdir = 1;
|
||||
const int J3rotdir = 1;
|
||||
const int J4rotdir = 0;
|
||||
const int J5rotdir = 1;
|
||||
const int J6rotdir = 1;
|
||||
const int ROT_DIRS[] = { J1rotdir, J2rotdir, J3rotdir, J4rotdir, J5rotdir, J6rotdir };
|
||||
|
||||
// approx encoder counts at rest position, 0 degree joint angle
|
||||
// DEFAULTS 38681, 11264, 0, 36652, 5839, 15671
|
||||
// J2 and J6 joint enc { 38681, 902, 0, 36652, 2198, 15671 }
|
||||
const int REST_ENC_POSITIONS[] = { 38681, 11264, 0, 36652, 5839, 15671 };
|
||||
|
||||
const int J1stepPin = 0;
|
||||
const int J1dirPin = 1;
|
||||
const int J2stepPin = 2;
|
||||
const int J2dirPin = 3;
|
||||
const int J3stepPin = 4;
|
||||
const int J3dirPin = 5;
|
||||
const int J4stepPin = 6;
|
||||
const int J4dirPin = 7;
|
||||
const int J5stepPin = 8;
|
||||
const int J5dirPin = 9;
|
||||
const int J6stepPin = 10;
|
||||
const int J6dirPin = 11;
|
||||
const int TRstepPin = 12;
|
||||
const int TRdirPin = 13;
|
||||
const int STEP_PINS[] = { J1stepPin, J2stepPin, J3stepPin, J4stepPin, J5stepPin, J6stepPin };
|
||||
const int DIR_PINS[] = { J1dirPin, J2dirPin, J3dirPin, J4dirPin, J5dirPin, J6dirPin };
|
||||
|
||||
// set encoder pins
|
||||
Encoder J1encPos(14, 15);
|
||||
Encoder J2encPos(16, 17);
|
||||
Encoder J3encPos(18, 19);
|
||||
Encoder J4encPos(33, 34);
|
||||
Encoder J5encPos(35, 36);
|
||||
Encoder J6encPos(37, 38);
|
||||
Encoder JOINT_ENCODERS[] = { J1encPos, J2encPos, J3encPos, J4encPos, J5encPos, J6encPos };
|
||||
// DEFAULTS { 1, 1, 1, 1, 1, 1 }
|
||||
// J2 joint enc { 1, -1, 1, 1, 1, 1 }
|
||||
int ENC_DIR[] = { 1, 1, 1, 1, 1, 1 }; // +1 if encoder direction matches motor direction
|
||||
|
||||
// set calibration limit switch pins
|
||||
const int J1calPin = 27;
|
||||
const int J2calPin = 28;
|
||||
const int J3calPin = 29;
|
||||
const int J4calPin = 30;
|
||||
const int J5calPin = 31;
|
||||
const int J6calPin = 32;
|
||||
const int CAL_PINS[] = { J1calPin, J2calPin, J3calPin, J4calPin, J5calPin, J6calPin };
|
||||
const int LIMIT_SWITCH_HIGH[] = { 1, 1, 1, 1, 1, 1 }; // to account for both NC and NO limit switches
|
||||
const int CAL_DIR[] = { -1, -1, 1, -1, -1, 1 }; // joint rotation direction to limit switch
|
||||
const int CAL_SPEED = 600; // motor steps per second
|
||||
// DEFAULT { 1, 1, 1, 2, 1, 1 }
|
||||
// J2 joint enc { 1, 10, 1, 2, 1, 1 }
|
||||
const int CAL_SPEED_MULT[] = { 1, 1, 1, 2, 1, 1 }; // multiplier to account for transmission ratios
|
||||
|
||||
// motor and encoder steps per revolution
|
||||
// DEFAULTS { 400, 400, 400, 400, 800, 400 }
|
||||
// J2 motor 4000 steps per rev { 400, 4000, 400, 400, 800, 400 }
|
||||
// const int MOTOR_STEPS_PER_REV[] = { 200, 200, 200, 200, 200, 200 };
|
||||
const int MOTOR_STEPS_PER_REV[] = { 400, 400, 400, 400, 800, 400 };
|
||||
|
||||
// num of steps in range of motion of joint, may vary depending on your limit switch
|
||||
// DEFAULT { 77363, 36854, 40878, 71967, 23347, 32358 }
|
||||
// J2 joint enc { 77363, 2949, 40878, 71967, 4740, 32358 }
|
||||
const int ENC_RANGE_STEPS[] = { 77363, 36854, 40878, 71967, 23347, 32358 };
|
||||
|
||||
//set encoder multiplier, encoder steps per motor step
|
||||
// DEFAULT { 5.12, 5.12, 5.12, 5.12, 2.56, 5.12 }
|
||||
// J2 and J5 joint enc { 5.12, 0.04096, 5.12, 5.12, 0.5197, 5.12 }
|
||||
const float ENC_MULT[] = { 5.12, 5.12, 5.12, 5.12, 5.12, 5.12 };
|
||||
// const float ENC_MULT[] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 };
|
||||
// DEFAULT { 227.5555555555556, 284.4444444444444, 284.4444444444444, 223.0044444444444, 56.04224675948152, 108.0888888888889 }
|
||||
// J2 and J5 joint enc { 227.5555555555556, 22.75555555555556, 284.4444444444444, 223.0044444444444, 22.75555555555556, 108.0888888888889 }
|
||||
const float ENC_STEPS_PER_DEG[] = { 227.5555555555556, 284.4444444444444, 284.4444444444444, 223.0044444444444, 56.04224675948152, 108.0888888888889 };
|
||||
|
||||
// speed and acceleration settings
|
||||
float JOINT_MAX_SPEED[] = { 2.0, 2.0, 2.0, 2.0, 2.0, 2.0 }; // deg/s
|
||||
float JOINT_MAX_ACCEL[] = { 5.0, 5.0, 5.0, 5.0, 5.0, 5.0 }; // deg/s^2
|
||||
// DEFAULT { 1500, 1500, 1500, 2000, 1500, 1500 }
|
||||
// J2 4000 steps/rev { 1500, 15000, 1500, 2000, 1500, 1500 }
|
||||
int MOTOR_MAX_SPEED[] = { 5500, 1500, 1500, 1500, 1500, 1500 }; // motor steps per sec
|
||||
// DEFAULT { 250, 250, 250, 250, 250, 250 }
|
||||
// J2 4000 steps/rev { 250, 2500, 250, 250, 250, 250 }
|
||||
int MOTOR_MAX_ACCEL[] = { 25000, 25000, 25000, 25000, 25000, 25000 }; // motor steps per sec^2
|
||||
|
||||
// played around with this a little but I don't see a need to use this at the moment
|
||||
float MOTOR_SPEED_MULT[] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 }; // for tuning position control
|
||||
float MOTOR_ACCEL_MULT[] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 }; // for tuning position control
|
||||
|
||||
AccelStepper stepperJoints[NUM_JOINTS];
|
||||
|
||||
enum SM { STATE_INIT, STATE_TRAJ, STATE_ERR };
|
||||
SM STATE = STATE_INIT;
|
||||
|
||||
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
//MAIN
|
||||
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
void setup() {
|
||||
// run once:
|
||||
Serial.begin(115200);
|
||||
|
||||
for (int i = 0; i < NUM_JOINTS; ++i)
|
||||
{
|
||||
JOINT_ENCODERS[i].write(REST_ENC_POSITIONS[i]);
|
||||
}
|
||||
|
||||
pinMode(TRstepPin, OUTPUT);
|
||||
pinMode(TRdirPin, OUTPUT);
|
||||
pinMode(J1stepPin, OUTPUT);
|
||||
pinMode(J1dirPin, OUTPUT);
|
||||
pinMode(J2stepPin, OUTPUT);
|
||||
pinMode(J2dirPin, OUTPUT);
|
||||
pinMode(J3stepPin, OUTPUT);
|
||||
pinMode(J3dirPin, OUTPUT);
|
||||
pinMode(J4stepPin, OUTPUT);
|
||||
pinMode(J4dirPin, OUTPUT);
|
||||
pinMode(J5stepPin, OUTPUT);
|
||||
pinMode(J5dirPin, OUTPUT);
|
||||
pinMode(J6stepPin, OUTPUT);
|
||||
pinMode(J6dirPin, OUTPUT);
|
||||
|
||||
pinMode(J1calPin, INPUT_PULLUP);
|
||||
pinMode(J2calPin, INPUT_PULLUP);
|
||||
pinMode(J3calPin, INPUT_PULLUP);
|
||||
pinMode(J4calPin, INPUT_PULLUP);
|
||||
pinMode(J5calPin, INPUT_PULLUP);
|
||||
pinMode(J6calPin, INPUT_PULLUP);
|
||||
|
||||
digitalWrite(TRstepPin, HIGH);
|
||||
digitalWrite(J1stepPin, HIGH);
|
||||
digitalWrite(J2stepPin, HIGH);
|
||||
digitalWrite(J3stepPin, HIGH);
|
||||
digitalWrite(J4stepPin, HIGH);
|
||||
digitalWrite(J5stepPin, HIGH);
|
||||
digitalWrite(J6stepPin, HIGH);
|
||||
|
||||
}
|
||||
|
||||
bool initStateTraj(String inData)
|
||||
{
|
||||
// parse initialisation message
|
||||
int idxVersion = inData.indexOf('A');
|
||||
String softwareVersion = inData.substring(idxVersion + 1, inData.length() - 1);
|
||||
int versionMatches = (softwareVersion == VERSION);
|
||||
|
||||
// return acknowledgement with result
|
||||
String msg = String("ST") + String("A") + String(versionMatches) + String("B") + String(VERSION) + String("\n");
|
||||
Serial.print(msg);
|
||||
|
||||
return versionMatches ? true : false;
|
||||
}
|
||||
|
||||
void readEncPos(int* encPos)
|
||||
{
|
||||
/*
|
||||
encPos[0] = J1encPos.read() * ENC_DIR[0];
|
||||
encPos[1] = J2encPos.read() * ENC_DIR[1];
|
||||
encPos[2] = J3encPos.read() * ENC_DIR[2];
|
||||
encPos[3] = J4encPos.read() * ENC_DIR[3];
|
||||
encPos[4] = J5encPos.read() * ENC_DIR[4];
|
||||
encPos[5] = J6encPos.read() * ENC_DIR[5];
|
||||
*/
|
||||
encPos[0] = stepperJoints[0].currentPosition();
|
||||
encPos[1] = stepperJoints[1].currentPosition();
|
||||
encPos[2] = stepperJoints[2].currentPosition();
|
||||
encPos[3] = stepperJoints[3].currentPosition();
|
||||
encPos[4] = stepperJoints[4].currentPosition();
|
||||
encPos[5] = stepperJoints[5].currentPosition();
|
||||
}
|
||||
|
||||
void updateStepperSpeed(String inData)
|
||||
{
|
||||
int idxSpeedJ1 = inData.indexOf('A');
|
||||
int idxAccelJ1 = inData.indexOf('B');
|
||||
int idxSpeedJ2 = inData.indexOf('C');
|
||||
int idxAccelJ2 = inData.indexOf('D');
|
||||
int idxSpeedJ3 = inData.indexOf('E');
|
||||
int idxAccelJ3 = inData.indexOf('F');
|
||||
int idxSpeedJ4 = inData.indexOf('G');
|
||||
int idxAccelJ4 = inData.indexOf('H');
|
||||
int idxSpeedJ5 = inData.indexOf('I');
|
||||
int idxAccelJ5 = inData.indexOf('J');
|
||||
int idxSpeedJ6 = inData.indexOf('K');
|
||||
int idxAccelJ6 = inData.indexOf('L');
|
||||
|
||||
JOINT_MAX_SPEED[0] = inData.substring(idxSpeedJ1 + 1, idxAccelJ1).toFloat();
|
||||
JOINT_MAX_ACCEL[0] = inData.substring(idxAccelJ1 + 1, idxSpeedJ2).toFloat();
|
||||
JOINT_MAX_SPEED[1] = inData.substring(idxSpeedJ2 + 1, idxAccelJ2).toFloat();
|
||||
JOINT_MAX_ACCEL[1] = inData.substring(idxAccelJ2 + 1, idxSpeedJ3).toFloat();
|
||||
JOINT_MAX_SPEED[2] = inData.substring(idxSpeedJ3 + 1, idxAccelJ3).toFloat();
|
||||
JOINT_MAX_ACCEL[2] = inData.substring(idxAccelJ3 + 1, idxSpeedJ4).toFloat();
|
||||
JOINT_MAX_SPEED[3] = inData.substring(idxSpeedJ4 + 1, idxAccelJ4).toFloat();
|
||||
JOINT_MAX_ACCEL[3] = inData.substring(idxAccelJ4 + 1, idxSpeedJ5).toFloat();
|
||||
JOINT_MAX_SPEED[4] = inData.substring(idxSpeedJ5 + 1, idxAccelJ5).toFloat();
|
||||
JOINT_MAX_ACCEL[4] = inData.substring(idxAccelJ5 + 1, idxSpeedJ6).toFloat();
|
||||
JOINT_MAX_SPEED[5] = inData.substring(idxSpeedJ6 + 1, idxAccelJ6).toFloat();
|
||||
JOINT_MAX_ACCEL[5] = inData.substring(idxAccelJ6 + 1).toFloat();
|
||||
}
|
||||
|
||||
void calibrateJoints(int* calJoints)
|
||||
{
|
||||
// check which joints to calibrate
|
||||
bool calAllDone = false;
|
||||
bool calJointsDone[NUM_JOINTS];
|
||||
for (int i = 0; i < NUM_JOINTS; ++i)
|
||||
{
|
||||
calJointsDone[i] = !calJoints[i];
|
||||
}
|
||||
|
||||
// first pass of calibration, fast speed
|
||||
for (int i = 0; i < NUM_JOINTS; i++)
|
||||
{
|
||||
stepperJoints[i].setSpeed(CAL_SPEED * CAL_SPEED_MULT[i] * CAL_DIR[i]);
|
||||
}
|
||||
while (!calAllDone)
|
||||
{
|
||||
calAllDone = true;
|
||||
for (int i = 0; i < NUM_JOINTS; ++i)
|
||||
{
|
||||
// if joint is not calibrated yet
|
||||
if (!calJointsDone[i])
|
||||
{
|
||||
// check limit switches
|
||||
if (!reachedLimitSwitch(i))
|
||||
{
|
||||
// limit switch not reached, continue moving
|
||||
stepperJoints[i].runSpeed();
|
||||
calAllDone = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
// limit switch reached
|
||||
stepperJoints[i].setSpeed(0); // redundancy
|
||||
calJointsDone[i] = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
delay(2000);
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
bool reachedLimitSwitch(int joint)
|
||||
{
|
||||
int pin = CAL_PINS[joint];
|
||||
// check multiple times to deal with noise
|
||||
// possibly EMI from motor cables?
|
||||
if (digitalRead(pin) == LIMIT_SWITCH_HIGH[joint])
|
||||
{
|
||||
if (digitalRead(pin) == LIMIT_SWITCH_HIGH[joint])
|
||||
{
|
||||
if (digitalRead(pin) == LIMIT_SWITCH_HIGH[joint])
|
||||
{
|
||||
if (digitalRead(pin) == LIMIT_SWITCH_HIGH[joint])
|
||||
{
|
||||
if (digitalRead(pin) == LIMIT_SWITCH_HIGH[joint])
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void stateTRAJ()
|
||||
{
|
||||
// initialise joint steps
|
||||
int curEncSteps[NUM_JOINTS];
|
||||
readEncPos(curEncSteps);
|
||||
|
||||
int cmdEncSteps[NUM_JOINTS];
|
||||
for (int i = 0; i < NUM_JOINTS; ++i)
|
||||
{
|
||||
cmdEncSteps[i] = curEncSteps[i];
|
||||
}
|
||||
|
||||
// initialise AccelStepper instance
|
||||
for (int i = 0; i < NUM_JOINTS; ++i)
|
||||
{
|
||||
stepperJoints[i] = AccelStepper(1, STEP_PINS[i], DIR_PINS[i]);
|
||||
stepperJoints[i].setPinsInverted(true, false, false); // DM542T CW
|
||||
stepperJoints[i].setAcceleration(MOTOR_MAX_ACCEL[i]);
|
||||
stepperJoints[i].setMaxSpeed(MOTOR_MAX_SPEED[i]);
|
||||
}
|
||||
stepperJoints[3].setPinsInverted(false, false, false); // J4 DM320T CCW
|
||||
stepperJoints[3].setMinPulseWidth(7.5);
|
||||
|
||||
// message
|
||||
String inData = "";
|
||||
|
||||
// start loop
|
||||
while (STATE == STATE_TRAJ)
|
||||
{
|
||||
char received = '\0';
|
||||
// check for message from host
|
||||
if (Serial.available())
|
||||
{
|
||||
received = Serial.read();
|
||||
inData += received;
|
||||
}
|
||||
|
||||
// process message when new line character is received
|
||||
if (received == '\n')
|
||||
{
|
||||
String function = inData.substring(0, 2);
|
||||
// update trajectory information
|
||||
if (function == "MT")
|
||||
{
|
||||
// read current joint positions
|
||||
readEncPos(curEncSteps);
|
||||
|
||||
// update host with joint positions
|
||||
String msg = String("JP") + String("A") + String(curEncSteps[0]) + String("B") + String(curEncSteps[1]) + String("C") + String(curEncSteps[2])
|
||||
+ String("D") + String(curEncSteps[3]) + String("E") + String(curEncSteps[4]) + String("F") + String(curEncSteps[5]) + String("\n");
|
||||
Serial.print(msg);
|
||||
|
||||
// get new position commands
|
||||
int msgIdxJ1 = inData.indexOf('A');
|
||||
int msgIdxJ2 = inData.indexOf('B');
|
||||
int msgIdxJ3 = inData.indexOf('C');
|
||||
int msgIdxJ4 = inData.indexOf('D');
|
||||
int msgIdxJ5 = inData.indexOf('E');
|
||||
int msgIdxJ6 = inData.indexOf('F');
|
||||
cmdEncSteps[0] = inData.substring(msgIdxJ1 + 1, msgIdxJ2).toInt();
|
||||
cmdEncSteps[1] = inData.substring(msgIdxJ2 + 1, msgIdxJ3).toInt();
|
||||
cmdEncSteps[2] = inData.substring(msgIdxJ3 + 1, msgIdxJ4).toInt();
|
||||
cmdEncSteps[3] = inData.substring(msgIdxJ4 + 1, msgIdxJ5).toInt();
|
||||
cmdEncSteps[4] = inData.substring(msgIdxJ5 + 1, msgIdxJ6).toInt();
|
||||
cmdEncSteps[5] = inData.substring(msgIdxJ6 + 1).toInt();
|
||||
|
||||
// update target joint positions
|
||||
readEncPos(curEncSteps);
|
||||
for (int i = 0; i < NUM_JOINTS; ++i)
|
||||
{
|
||||
//curEncSteps[1] = J2encPos.read() * ENC_DIR[1];
|
||||
int diffEncSteps = cmdEncSteps[i] - curEncSteps[i];
|
||||
//if (abs(diffEncSteps) > 2)
|
||||
//{
|
||||
int diffMotSteps = diffEncSteps / ENC_MULT[i];
|
||||
/*
|
||||
if (diffMotSteps < MOTOR_STEPS_PER_REV[i])
|
||||
{
|
||||
// for the last rev of motor, introduce artificial decceleration
|
||||
// to help prevent overshoot
|
||||
diffMotSteps = diffMotSteps / 2;
|
||||
}
|
||||
*/
|
||||
//stepperJoints[i].move(diffMotSteps);
|
||||
stepperJoints[i].moveTo(cmdEncSteps[i]);
|
||||
stepperJoints[i].run();
|
||||
//}
|
||||
}
|
||||
}
|
||||
else if (function == "JC")
|
||||
{
|
||||
// calibrate joint 6
|
||||
int calJoint6[] = { 0, 0, 0, 0, 0, 1 }; // 000001
|
||||
calibrateJoints(calJoint6);
|
||||
|
||||
// record encoder steps
|
||||
int calStepJ6 = J6encPos.read() * ENC_DIR[5];
|
||||
|
||||
// calibrate joints 1 to 5
|
||||
int calJoints[] = { 1, 1, 1, 1, 1, 0 }; // 111110
|
||||
calibrateJoints(calJoints);
|
||||
|
||||
// record encoder steps
|
||||
int calStepJ1 = J1encPos.read() * ENC_DIR[0];
|
||||
int calStepJ2 = J2encPos.read() * ENC_DIR[1];
|
||||
int calStepJ3 = J3encPos.read() * ENC_DIR[2];
|
||||
int calStepJ4 = J4encPos.read() * ENC_DIR[3];
|
||||
int calStepJ5 = J5encPos.read() * ENC_DIR[4];
|
||||
|
||||
// if limit switch at lower end, set encoder to 0
|
||||
// otherwise set to encoder upper limit
|
||||
J1encPos.write(0);
|
||||
J2encPos.write(0);
|
||||
J3encPos.write(ENC_RANGE_STEPS[2]);
|
||||
J4encPos.write(0);
|
||||
J5encPos.write(0);
|
||||
J6encPos.write(ENC_RANGE_STEPS[5]);
|
||||
|
||||
// read current joint positions
|
||||
readEncPos(curEncSteps);
|
||||
|
||||
// return to original position
|
||||
for (int i = 0; i < NUM_JOINTS; ++i)
|
||||
{
|
||||
stepperJoints[i].setAcceleration(MOTOR_MAX_ACCEL[i]);
|
||||
stepperJoints[i].setMaxSpeed(MOTOR_MAX_SPEED[i]);
|
||||
cmdEncSteps[i] = REST_ENC_POSITIONS[i];
|
||||
stepperJoints[i].move((REST_ENC_POSITIONS[i] - curEncSteps[i]) / ENC_MULT[i]);
|
||||
}
|
||||
|
||||
bool restPosReached = false;
|
||||
while (!restPosReached)
|
||||
{
|
||||
restPosReached = true;
|
||||
// read current joint positions
|
||||
readEncPos(curEncSteps);
|
||||
for (int i = 0; i < NUM_JOINTS; ++i)
|
||||
{
|
||||
if (abs(REST_ENC_POSITIONS[i] - curEncSteps[i]) > 5)
|
||||
{
|
||||
restPosReached = false;
|
||||
stepperJoints[i].move((REST_ENC_POSITIONS[i] - curEncSteps[i]) / ENC_MULT[i]);
|
||||
stepperJoints[i].run();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// calibration done, send calibration values
|
||||
String msg = String("JC") + String("A") + String(calStepJ1) + String("B") + String(calStepJ2) + String("C") + String(calStepJ3)
|
||||
+ String("D") + String(calStepJ4) + String("E") + String(calStepJ5) + String("F") + String(calStepJ6) + String("\n");
|
||||
Serial.print(msg);
|
||||
|
||||
for (int i = 0; i < NUM_JOINTS; ++i)
|
||||
{
|
||||
stepperJoints[i].setAcceleration(MOTOR_MAX_ACCEL[i]);
|
||||
stepperJoints[i].setMaxSpeed(MOTOR_MAX_SPEED[i]);
|
||||
}
|
||||
}
|
||||
else if (function == "JP")
|
||||
{
|
||||
// read current joint positions
|
||||
readEncPos(curEncSteps);
|
||||
|
||||
// update host with joint positions
|
||||
String msg = String("JP") + String("A") + String(curEncSteps[0]) + String("B") + String(curEncSteps[1]) + String("C") + String(curEncSteps[2])
|
||||
+ String("D") + String(curEncSteps[3]) + String("E") + String(curEncSteps[4]) + String("F") + String(curEncSteps[5]) + String("\n");
|
||||
Serial.print(msg);
|
||||
}
|
||||
else if (function == "SS")
|
||||
{
|
||||
updateStepperSpeed(inData);
|
||||
// set motor speed and acceleration
|
||||
for (int i = 0; i < NUM_JOINTS; ++i)
|
||||
{
|
||||
MOTOR_MAX_SPEED[i] = JOINT_MAX_SPEED[i] * ENC_STEPS_PER_DEG[i] / ENC_MULT[i];
|
||||
MOTOR_MAX_ACCEL[i] = JOINT_MAX_ACCEL[i] * ENC_STEPS_PER_DEG[i] / ENC_MULT[i];
|
||||
stepperJoints[i].setAcceleration(MOTOR_MAX_ACCEL[i] * MOTOR_ACCEL_MULT[i]);
|
||||
stepperJoints[i].setMaxSpeed(MOTOR_MAX_SPEED[i] * MOTOR_SPEED_MULT[i]);
|
||||
}
|
||||
// read current joint positions
|
||||
readEncPos(curEncSteps);
|
||||
|
||||
// update host with joint positions
|
||||
String msg = String("JP") + String("A") + String(curEncSteps[0]) + String("B") + String(curEncSteps[1]) + String("C") + String(curEncSteps[2])
|
||||
+ String("D") + String(curEncSteps[3]) + String("E") + String(curEncSteps[4]) + String("F") + String(curEncSteps[5]) + String("\n");
|
||||
Serial.print(msg);
|
||||
}
|
||||
else if (function == "ST")
|
||||
{
|
||||
if (!initStateTraj(inData))
|
||||
{
|
||||
STATE = STATE_INIT;
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// clear message
|
||||
inData = "";
|
||||
}
|
||||
// execute motor commands
|
||||
for (int i = 0; i < NUM_JOINTS; ++i)
|
||||
{
|
||||
// target joint positions are already updated, just call run()
|
||||
stepperJoints[i].run();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void stateINIT()
|
||||
{
|
||||
// message
|
||||
String inData = "";
|
||||
|
||||
// start loop
|
||||
while (STATE == STATE_INIT)
|
||||
{
|
||||
char received = '\0';
|
||||
// check for message from host
|
||||
if (Serial.available())
|
||||
{
|
||||
received = Serial.read();
|
||||
inData += received;
|
||||
}
|
||||
|
||||
// process message when new line character is received
|
||||
if (received == '\n')
|
||||
{
|
||||
String function = inData.substring(0, 2);
|
||||
// update trajectory information
|
||||
if (function == "ST")
|
||||
{
|
||||
if (initStateTraj(inData))
|
||||
{
|
||||
STATE = STATE_TRAJ;
|
||||
return;
|
||||
}
|
||||
}
|
||||
inData = "";
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void stateERR()
|
||||
{
|
||||
// enter holding state
|
||||
digitalWrite(J1stepPin, LOW);
|
||||
digitalWrite(J2stepPin, LOW);
|
||||
digitalWrite(J3stepPin, LOW);
|
||||
digitalWrite(J4stepPin, LOW);
|
||||
digitalWrite(J5stepPin, LOW);
|
||||
digitalWrite(J6stepPin, LOW);
|
||||
|
||||
// do recovery @TODO
|
||||
while (STATE == STATE_ERR) {}
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
// state control
|
||||
switch (STATE)
|
||||
{
|
||||
case STATE_TRAJ:
|
||||
stateTRAJ();
|
||||
break;
|
||||
case STATE_ERR:
|
||||
stateERR();
|
||||
break;
|
||||
default:
|
||||
stateINIT();
|
||||
break;
|
||||
}
|
||||
}
|
||||
Loading…
Reference in a new issue