From aea72d68ed3445b71bab208e1497f6e547409b89 Mon Sep 17 00:00:00 2001 From: Thomas Date: Mon, 2 May 2022 10:45:08 -0400 Subject: [PATCH] Initial Upload --- baseline_dev/baseline_dev.ino | 557 ++++++++++++++++++++++++++++++++++ 1 file changed, 557 insertions(+) create mode 100644 baseline_dev/baseline_dev.ino diff --git a/baseline_dev/baseline_dev.ino b/baseline_dev/baseline_dev.ino new file mode 100644 index 0000000..a08d493 --- /dev/null +++ b/baseline_dev/baseline_dev.ino @@ -0,0 +1,557 @@ +#include +#include + +#include + +// 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; + } +}