From 69b1ff0bdd9f4264d53b6f8d48ab800df85f8eb8 Mon Sep 17 00:00:00 2001 From: yuntian1019 Date: Tue, 13 Dec 2016 10:05:25 +0800 Subject: [PATCH 1/5] v2.2.2 refactor code. add some examples show how to use the code. --- examples/Demos/button/button.ino | 106 ++++ examples/Demos/move/move.ino | 90 +++ examples/Demos/timer/timer.ino | 96 +++ examples/Metal/Metal.ino | 85 ++- src/UFServo.cpp | 9 + src/UFServo.h | 1 + src/uArm.cpp | 722 ---------------------- src/uArm.h | 149 +---- src/uArmAPI.cpp | 993 ++++++++++++++++++++++++++++++ src/uArmAPI.h | 255 ++++++++ src/uArmButton.cpp | 2 +- src/uArmButton.h | 2 +- src/uArmBuzzer.cpp | 2 +- src/uArmBuzzer.h | 2 +- src/uArmComm.cpp | 976 +++++++++++------------------ src/uArmComm.h | 98 +-- src/uArmConfig.cpp | 155 ----- src/uArmConfig.h | 33 +- src/uArmController.cpp | 223 +------ src/uArmController.h | 35 +- src/uArmDebug.cpp | 191 ++++++ src/uArmDebug.h | 33 + src/uArmHWConfig.cpp | 10 - src/uArmIIC.cpp | 4 +- src/uArmIIC.h | 2 +- src/{uArmHWConfig.h => uArmPin.h} | 8 +- src/uArmRecorder.cpp | 4 +- src/uArmRecorder.h | 4 +- src/uArmRingBuffer.cpp | 65 ++ src/uArmRingBuffer.h | 36 ++ src/uArmService.cpp | 356 +++++++++++ src/uArmService.h | 119 ++++ src/uArmTypes.h | 46 ++ 33 files changed, 2913 insertions(+), 1999 deletions(-) create mode 100644 examples/Demos/button/button.ino create mode 100644 examples/Demos/move/move.ino create mode 100644 examples/Demos/timer/timer.ino delete mode 100644 src/uArm.cpp create mode 100644 src/uArmAPI.cpp create mode 100644 src/uArmAPI.h delete mode 100644 src/uArmConfig.cpp create mode 100644 src/uArmDebug.cpp create mode 100644 src/uArmDebug.h delete mode 100644 src/uArmHWConfig.cpp rename src/{uArmHWConfig.h => uArmPin.h} (94%) create mode 100644 src/uArmRingBuffer.cpp create mode 100644 src/uArmRingBuffer.h create mode 100644 src/uArmService.cpp create mode 100644 src/uArmService.h create mode 100644 src/uArmTypes.h diff --git a/examples/Demos/button/button.ino b/examples/Demos/button/button.ino new file mode 100644 index 0000000..1fed54d --- /dev/null +++ b/examples/Demos/button/button.ino @@ -0,0 +1,106 @@ +// Demo button +#include "uArm.h" + +#define USE_SERIAL_CMD 0 // 1: use serial for control 0: just use arduino to control(release ROM and RAM space) + +unsigned long tickStartTime = millis(); // get timestamp; + +void setup() +{ + Serial.begin(115200); + Init(); // Don't remove + + debugPrint("debug start"); // uncomment DEBUG in uArmConfig.h to use debug function + + // TODO + service.setButtonService(false); // disable build in button service + +} + +void loop() +{ + run(); // Don't remove + + // TODO + if (buttonMenu.longPressed()) + { + buttonMenu.clearEvent(); + Serial.println("menu button long pressed event"); + } + else if (buttonMenu.clicked()) + { + buttonMenu.clearEvent(); // manually clear event + + Serial.println("menu button click event"); + } + + if (buttonPlay.longPressed()) + { + buttonPlay.clearEvent(); + Serial.println("play button long pressed event"); + } + else if (buttonPlay.clicked()) + { + buttonPlay.clearEvent(); // manually clear event + + Serial.println("play button click event"); + } +} + +// time out every TICK_INTERVAL(50 ms default) +void tickTimeOut() +{ + +} + +//////////////////////////////////////////////////////////// +// DO NOT EDIT +void Init() +{ + uArmInit(); // Don't remove + service.init(); + + #if USE_SERIAL_CMD == 1 + serialCmdInit(); + + + #endif +} + +void run() +{ + #if USE_SERIAL_CMD == 1 + handleSerialCmd(); + #endif + + manage_inactivity(); // Don't remove +} + +void tickTaskRun() +{ + tickTimeOut(); + + buttonPlay.tick(); + buttonMenu.tick(); +#ifdef MKII + ledRed.tick(); + service.btDetect(); +#endif +} + +void manage_inactivity(void) +{ +#if USE_SERIAL_CMD == 1 + getSerialCmd(); // for serial communication +#endif + service.run(); // for led, button, bt etc. + + // because there is no other hardware timer available in UNO, so use a soft timer + // it's necessary for button,led, bt + // so Don't remove it if you need them + if(millis() - tickStartTime >= TICK_INTERVAL) + { + tickStartTime = millis(); + tickTaskRun(); + } +} diff --git a/examples/Demos/move/move.ino b/examples/Demos/move/move.ino new file mode 100644 index 0000000..bcd8e6b --- /dev/null +++ b/examples/Demos/move/move.ino @@ -0,0 +1,90 @@ +// Demo +#include "uArm.h" + +#define USE_SERIAL_CMD 0 // 1: use serial for control 0: just use arduino to control(release ROM and RAM space) + +unsigned long tickStartTime = millis(); // get timestamp; + +void setup() +{ + Serial.begin(115200); + Init(); // Don't remove + + debugPrint("debug start"); // uncomment DEBUG in uArmConfig.h to use debug function + + // TODO + service.setButtonService(false); // disable build in button service + moveTo(0, 150, 150); + +} + +void loop() +{ + run(); // Don't remove + + // TODO + moveTo(0, 150, 150); + moveTo(100, 150, 150); + pumpOn(); + moveTo(-100, 200, 150); + pumpOff(); + +} + +// time out every TICK_INTERVAL(50 ms default) +void tickTimeOut() +{ + +} + +//////////////////////////////////////////////////////////// +// DO NOT EDIT +void Init() +{ + uArmInit(); // Don't remove + service.init(); + + #if USE_SERIAL_CMD == 1 + serialCmdInit(); + + + #endif +} + +void run() +{ + #if USE_SERIAL_CMD == 1 + handleSerialCmd(); + #endif + + manage_inactivity(); // Don't remove +} + +void tickTaskRun() +{ + tickTimeOut(); + + buttonPlay.tick(); + buttonMenu.tick(); +#ifdef MKII + ledRed.tick(); + service.btDetect(); +#endif +} + +void manage_inactivity(void) +{ +#if USE_SERIAL_CMD == 1 + getSerialCmd(); // for serial communication +#endif + service.run(); // for led, button, bt etc. + + // because there is no other hardware timer available in UNO, so use a soft timer + // it's necessary for button,led, bt + // so Don't remove it if you need them + if(millis() - tickStartTime >= TICK_INTERVAL) + { + tickStartTime = millis(); + tickTaskRun(); + } +} diff --git a/examples/Demos/timer/timer.ino b/examples/Demos/timer/timer.ino new file mode 100644 index 0000000..0e8f2c1 --- /dev/null +++ b/examples/Demos/timer/timer.ino @@ -0,0 +1,96 @@ + +#include "uArm.h" + +#define USE_SERIAL_CMD 1 // 1: use serial for control 0: just use arduino to control(release ROM and RAM space) + +unsigned long tickStartTime = millis(); // get timestamp; + +unsigned int timeOutCount = 0; +unsigned int second = 0; + +void setup() +{ + Serial.begin(115200); + Init(); // Don't remove + + debugPrint("debug start"); // uncomment DEBUG in uArmConfig.h to use debug function + + // TODO + + +} + +void loop() +{ + run(); // Don't remove + + // TODO + +} + +// time out every TICK_INTERVAL(50 ms default) +void tickTimeOut() +{ + timeOutCount++; + + // 1s time out + if (timeOutCount > (1000 / TICK_INTERVAL)) + { + second++; + timeOutCount = 0; + char buf[60]; + msprintf(buf, "%d seconds elapsed\r\n", second); + Serial.print(buf); + } +} + +//////////////////////////////////////////////////////////// +// DO NOT EDIT +void Init() +{ + uArmInit(); // Don't remove + service.init(); + + #if USE_SERIAL_CMD == 1 + serialCmdInit(); + + #endif +} + +void run() +{ + #if USE_SERIAL_CMD == 1 + handleSerialCmd(); + #endif + + manage_inactivity(); // Don't remove +} + +void tickTaskRun() +{ + tickTimeOut(); + + buttonPlay.tick(); + buttonMenu.tick(); +#ifdef MKII + ledRed.tick(); + service.btDetect(); +#endif +} + +void manage_inactivity(void) +{ +#if USE_SERIAL_CMD == 1 + getSerialCmd(); // for serial communication +#endif + service.run(); // for led, button, bt etc. + + // because there is no other hardware timer available in UNO, so use a soft timer + // it's necessary for button,led, bt + // so Don't remove it if you need them + if(millis() - tickStartTime >= TICK_INTERVAL) + { + tickStartTime = millis(); + tickTaskRun(); + } +} diff --git a/examples/Metal/Metal.ino b/examples/Metal/Metal.ino index 334e0fe..4ce89f2 100644 --- a/examples/Metal/Metal.ino +++ b/examples/Metal/Metal.ino @@ -1,12 +1,87 @@ + #include "uArm.h" -void setup() { - Serial.begin(115200); // start serial port at 115200 bps - uArm.setup(); +#define USE_SERIAL_CMD 1 // 1: use serial for control 0: just use arduino to control(release ROM and RAM space) + +unsigned long tickStartTime = millis(); // get timestamp; +static void Init(); + +void setup() +{ + Serial.begin(115200); + Init(); // Don't remove + + debugPrint("debug start"); // uncomment DEBUG in uArmConfig.h to use debug function + + // TODO + moveTo(0, 150, 150); + Serial.println("@1"); // report ready } -void loop() { - uArm.run(); +void loop() +{ + run(); // Don't remove + + // TODO + +} + +// time out every TICK_INTERVAL(50 ms default) +void tickTimeOut() +{ + +} + +//////////////////////////////////////////////////////////// +// DO NOT EDIT +static void Init() +{ + uArmInit(); // Don't remove + service.init(); + + #if USE_SERIAL_CMD == 1 + serialCmdInit(); + + + #endif +} + +void run() +{ + #if USE_SERIAL_CMD == 1 + handleSerialCmd(); + #endif + + manage_inactivity(); // Don't remove +} + +void tickTaskRun() +{ + tickTimeOut(); + + buttonPlay.tick(); + buttonMenu.tick(); +#ifdef MKII + ledRed.tick(); + service.btDetect(); +#endif +} + +void manage_inactivity(void) +{ +#if USE_SERIAL_CMD == 1 + getSerialCmd(); // for serial communication +#endif + service.run(); // for led, button, bt etc. + + // because there is no other hardware timer available in UNO, so use a soft timer + // it's necessary for button,led, bt + // so Don't remove it if you need them + if(millis() - tickStartTime >= TICK_INTERVAL) + { + tickStartTime = millis(); + tickTaskRun(); + } } diff --git a/src/UFServo.cpp b/src/UFServo.cpp index 7ec1c68..c1c7578 100644 --- a/src/UFServo.cpp +++ b/src/UFServo.cpp @@ -352,6 +352,15 @@ void Servo::write(float value) } } +void Servo::setSpeed(unsigned char speed) +{ + byte channel = this->servoIndex; + uint8_t oldSREG = SREG; + cli(); + servos[channel].speed = speed; + SREG = oldSREG; +} + void Servo::writeMicroseconds(int value) { // calculate and store the values for the given channel diff --git a/src/UFServo.h b/src/UFServo.h index b834828..817561b 100644 --- a/src/UFServo.h +++ b/src/UFServo.h @@ -107,6 +107,7 @@ class Servo int readMicroseconds(); // returns current pulse width in microseconds for this servo (was read_us() in first release) bool attached(); // return true if this servo is attached, otherwise false void setPulseWidthRange(int min, int max); + void setSpeed(unsigned char speed); private: uint8_t servoIndex; // index into the channel data for this servo int8_t min; // minimum is this value times 4 added to MIN_PULSE_WIDTH diff --git a/src/uArm.cpp b/src/uArm.cpp deleted file mode 100644 index e77d287..0000000 --- a/src/uArm.cpp +++ /dev/null @@ -1,722 +0,0 @@ -/** - ****************************************************************************** - * @file uArmClass.cpp - * @author David.Long - * @email xiaokun.long@ufactory.cc - * @date 2016-09-28 - * @license GNU - * copyright(c) 2016 UFactory Team. All right reserved - ****************************************************************************** - */ - -#include "uArm.h" - -#include "uArmComm.h" - -uArmClass uArm; -extern uArmComm gComm; - -uArmClass::uArmClass() -{ - mCurStep = -1; - mTotalSteps = -1; - mRecordAddr = 0; - - mReportInterval = 0; - - mReportStartTime = millis(); - mTickStartTime = millis(); - mTickRecorderTime = millis(); -} - - - -void uArmClass::initHardware() -{ - pinMode(BTN_D4, INPUT_PULLUP); //special mode for calibration - pinMode(BUZZER, OUTPUT); - pinMode(LIMIT_SW, INPUT_PULLUP); - pinMode(BTN_D7, INPUT_PULLUP); - pinMode(PUMP_EN, OUTPUT); - pinMode(GRIPPER, OUTPUT); - - #ifdef MKII - pinMode(SYS_LED, OUTPUT); - - digitalWrite(PUMP_EN, HIGH);//keep the pump off - #endif - - #ifdef METAL - pinMode(VALVE_EN, OUTPUT); - #endif - - - -} - -void uArmClass::setReportInterval(unsigned int interval) -{ - mReportInterval = interval; -} - -void uArmClass::setup() -{ - - initHardware(); - mController.init(); - - mButtonD4.setPin(BTN_D4); - mButtonD7.setPin(BTN_D7); - - gBuzzer.setPin(BUZZER); - - #ifdef MKII - mLed.setPin(SYS_LED); - #endif - - mCurStep = -1; - mTotalSteps = -1; - - - uArm.moveTo(0, 150, 150); - Serial.println("@1"); -} - - -void uArmClass::controllerRun() -{ - - - if (mCurStep >= 0 && mCurStep < mTotalSteps) - { - - if((millis() - mStartTime) >= (mCurStep * mTimePerStep)) - { - - // ignore the point if cannot reach - if (mController.limitRange(mPathX[mCurStep], mPathY[mCurStep], mPathZ[mCurStep]) != OUT_OF_RANGE_NO_SOLUTION) - { - //debugPrint("curStep:%d, %s, %s, %s", mCurStep, D(mPathX[mCurStep]), D(mPathY[mCurStep]), D(mPathZ[mCurStep])); - /*if (mCurStep == (mTotalSteps - 1)) - { - double angles[3]; - angles[0] = mController.getReverseServoAngle(0, mPathX[mCurStep]); - angles[1] = mController.getReverseServoAngle(1, mPathY[mCurStep]); - angles[2] = mController.getReverseServoAngle(2, mPathZ[mCurStep]); - debugPrint("curStep:%d, %s, %s, %s", mCurStep, D(angles[0]), D(angles[1]), D(angles[2])); - mController.writeServoAngle(angles[0], angles[1], angles[2]); - //mController.writeServoAngle(mPathX[mCurStep], mPathY[mCurStep], mPathZ[mCurStep]); - } - else - */ - { - mController.writeServoAngle(mPathX[mCurStep], mPathY[mCurStep], mPathZ[mCurStep]); - } - } - - mCurStep++; - - - if (mCurStep >= mTotalSteps) - { - mCurStep = -1; - - } - } - } - else - { - mCurStep = -1; - - } -} - -void uArmClass::recorderTick() -{ - //sys led function detec every 0.05s----------------------------------------------------------------- - - switch(mSysStatus)//every 0.125s per point - { - case SINGLE_PLAY_MODE: - if(play() == false) - { - mSysStatus = NORMAL_MODE; - mRecordAddr = 0; - } - break; - - case LOOP_PLAY_MODE: - - if(play() == false) - { - mRecordAddr = 0; - } - break; - - case LEARNING_MODE: - case LEARNING_MODE_STOP: - if(record() == false) - { - mSysStatus = NORMAL_MODE; - mRecordAddr = 0; - - mController.attachAllServo(); - - } - break; - - default: - break; - } - -} - - - -void uArmClass::systemRun() -{ -//check the button4 status------------------------------------------------------------------------ - - if (mButtonD4.clicked()) - { - //debugPrint("btnD4 down"); - mButtonD4.clearEvent(); - switch (mSysStatus) - { - case NORMAL_MODE: - case NORMAL_BT_CONNECTED_MODE: - mSysStatus = LEARNING_MODE; - mRecordAddr = 0;//recording/playing address - mController.detachAllServo(); - break; - - case LEARNING_MODE: - //LEARNING_MODE_STOP is just used to notificate record() function to stop, once record() get it then change the sys_status to normal_mode - mSysStatus = LEARNING_MODE_STOP;//do not detec if BT is connected here, will do it seperatly - - mController.pumpOff(); - - break; - - default: break; - } - } - - - - //check the button7 status------------------------------------------------------------------------- - if (mButtonD7.longPressed()) - { - mButtonD7.clearEvent(); - switch(mSysStatus) - { - case NORMAL_MODE: - case NORMAL_BT_CONNECTED_MODE: - mRecordAddr = 0; - mSysStatus = LOOP_PLAY_MODE; - break; - - case SINGLE_PLAY_MODE: - case LOOP_PLAY_MODE: - break; - - case LEARNING_MODE: - break; - } - } - else if (mButtonD7.clicked()) - { - mButtonD7.clearEvent(); - - //debugPrint("btnD7 down"); - - switch(mSysStatus) - { - case NORMAL_MODE: - case NORMAL_BT_CONNECTED_MODE: - mRecordAddr = 0;//recording/playing address - mSysStatus = SINGLE_PLAY_MODE; // or play just one time - - break; - - case SINGLE_PLAY_MODE: - case LOOP_PLAY_MODE: - mController.pumpOff(); - mSysStatus = NORMAL_MODE; - break; - - case LEARNING_MODE: - - if (mController.pumpStatus()) - { - mController.pumpOff(); - } - else - { - mController.pumpOn(); - } - break; - } - } - - - if (mReportInterval > 0) - { - if(millis() - mReportStartTime >= mReportInterval) - { - mReportStartTime = millis(); - gComm.reportPos(); - } - - } - -} - -void uArmClass::btDetect() -{ -#ifdef MKII - if (!gBuzzer.on() && ((mSysStatus == NORMAL_MODE) || (mSysStatus == NORMAL_BT_CONNECTED_MODE))) - { - pinMode(BT_DETECT_PIN, INPUT); - digitalWrite(BT_DETECT_PIN,HIGH); - - if (digitalRead(BT_DETECT_PIN) == HIGH)//do it here - { - mLed.on(); - mSysStatus = NORMAL_BT_CONNECTED_MODE; - } - else - { - mLed.off(); - mSysStatus = NORMAL_MODE; - } - - //pinMode(BT_DETECT_PIN, OUTPUT); - } -#endif -} - -void uArmClass::tickTaskRun() -{ - //recorderTick(); - mButtonD7.tick(); - mButtonD4.tick(); -#ifdef MKII - mLed.tick(); - btDetect(); -#endif -} - -void uArmClass::run() -{ - gComm.run(); - controllerRun(); - systemRun(); - - // if(mTime50ms != millis() % TICK_INTERVAL) - // { - // mTime50ms = millis() % TICK_INTERVAL; - // if(mTime50ms == 0) - // { - // tickTaskRun(); - // } - // } - - if(millis() - mTickStartTime >= TICK_INTERVAL) - { - mTickStartTime = millis(); - tickTaskRun(); - } - - - if (millis() - mTickRecorderTime >= 50) - { - mTickRecorderTime= millis(); - recorderTick(); - } -} - -void uArmClass::stopMove() -{ - mCurStep = -1; -} - -bool uArmClass::isMoving() -{ - return (mCurStep != -1); -} - - -bool uArmClass::play() -{ - - unsigned char data[5]; // 0: L 1: R 2: Rotation 3: hand rotation 4:gripper - - - mRecorder.read(mRecordAddr, data, 5); - debugPrint("mRecordAddr = %d, data=%d, %d, %d", mRecordAddr, data[0], data[1], data[2]); - - if(data[0] != 255) - { - //double x, y, z; - //mController.getXYZFromAngle(x, y, z, (double)data[2], (double)data[0], (double)data[1]); - moveToAngle((double)data[2], (double)data[0], (double)data[1]); - //mController.writeServoAngle((double)data[2], (double)data[0], (double)data[1]); - mController.writeServoAngle(SERVO_HAND_ROT_NUM, (double)data[3]); - unsigned char pumpStatus = mController.pumpStatus() > 0 ? 1 : 0; - if (pumpStatus != data[4]) - { - if (data[4]) - { - mController.pumpOn(); - } - else - { - mController.pumpOff(); - } - } - } - else - { - - mController.pumpOff(); - - return false; - } - - mRecordAddr += 5; - - return true; -} - -bool uArmClass::record() -{ - debugPrint("mRecordAddr = %d", mRecordAddr); - - if(mRecordAddr <= 65530) - { - unsigned char data[5]; // 0: L 1: R 2: Rotation 3: hand rotation 4:gripper - if((mRecordAddr != 65530) && (mSysStatus != LEARNING_MODE_STOP)) - { - double rot, left, right; - //mController.updateAllServoAngle(); - mController.readServoAngles(rot, left, right); - data[0] = (unsigned char)left; - data[1] = (unsigned char)right; - data[2] = (unsigned char)rot; - data[3] = (unsigned char)mController.readServoAngle(SERVO_HAND_ROT_NUM); - data[4] = mController.pumpStatus() > 0 ? 1 : 0; - - debugPrint("l=%d, r=%d, r= %d", data[0], data[1], data[2]); - } - else - { - data[0] = 255;//255 is the ending flag - mRecorder.write(mRecordAddr, data, 5); - - return false; - } - - mRecorder.write(mRecordAddr, data, 5); - mRecordAddr += 5; - - return true; - } - else - { - return false; - } - -} - - -void uArmClass::interpolate(double startVal, double endVal, double *interpVals, int steps, byte easeType) -{ - - startVal = startVal / 10.0; - endVal = endVal / 10.0; - - double delta = endVal - startVal; - for (byte i = 1; i <= steps; i++) - { - float t = (float)i / steps; - //*(interp_vals+f) = 10.0*(start_val + (3 * delta) * (t * t) + (-2 * delta) * (t * t * t)); - *(interpVals + i - 1) = 10.0 * (startVal + t * t * delta * (3 + (-2) * t)); - } -} - -unsigned char uArmClass::moveTo(double x, double y, double z, double speed) -{ - - double angleRot = 0, angleLeft = 0, angleRight = 0; - double curRot = 0, curLeft = 0, curRight = 0; - double targetRot = 0; - double targetLeft = 0; - double targetRight = 0; - double curX = 0; - double curY = 0; - double curZ = 0; - int i = 0; - int totalSteps = 0; - unsigned int timePerStep; - - unsigned char status = 0; - - status = mController.coordianteToAngle(x, y, z, targetRot, targetLeft, targetRight); - - debugPrint("moveTo status: %d, angle: %s, %s, %s\n", status, D(targetRot), D(targetLeft), D(targetRight)); - - if (status == OUT_OF_RANGE_NO_SOLUTION) - { - return OUT_OF_RANGE_NO_SOLUTION; - } - - if (speed == 0) - { - mCurStep = -1; - mController.writeServoAngle(targetRot, targetLeft, targetRight); - return IN_RANGE; - } - - // get current angles - mController.getServoAngles(curRot, curLeft, curRight); - // get current xyz - mController.getCurrentXYZ(curX, curY, curZ); - - // calculate max steps - totalSteps = max(abs(targetRot - curRot), abs(targetLeft - curLeft)); - totalSteps = max(totalSteps, abs(targetRight - curRight)); - - if (totalSteps <= 0) - return OUT_OF_RANGE_NO_SOLUTION; - - totalSteps = totalSteps < STEP_MAX ? totalSteps : STEP_MAX; - - // calculate step time - double distance = sqrt((x-curX) * (x-curX) + (y-curY) * (y-curY) + (z-curZ) * (z-curZ)); - speed = constrain(speed, 100, 1000); - timePerStep = distance / speed * 1000.0 / totalSteps; - - - // keep timePerStep <= STEP_MAX_TIME - if (timePerStep > STEP_MAX_TIME) - { - double ratio = double(timePerStep) / STEP_MAX_TIME; - - if (totalSteps * ratio < STEP_MAX) - { - totalSteps *= ratio; - timePerStep = STEP_MAX_TIME; - } - else - { - totalSteps = STEP_MAX; - timePerStep = STEP_MAX_TIME; - } - } - - - totalSteps = totalSteps < STEP_MAX ? totalSteps : STEP_MAX; - - //debugPrint("totalSteps= %d\n", totalSteps); - - // trajectory planning - interpolate(curX, x, mPathX, totalSteps, INTERP_EASE_INOUT_CUBIC); - interpolate(curY, y, mPathY, totalSteps, INTERP_EASE_INOUT_CUBIC); - interpolate(curZ, z, mPathZ, totalSteps, INTERP_EASE_INOUT_CUBIC); - - for (i = 0; i < totalSteps; i++) - { - status = mController.coordianteToAngle(mPathX[i], mPathY[i], mPathZ[i], angleRot, angleLeft, angleRight); - - if (status != IN_RANGE) - { - break; - } - else - { - mPathX[i] = angleRot; - mPathY[i] = angleLeft; - mPathZ[i] = angleRight; - } - } - - if (i < totalSteps) - { - interpolate(curRot, targetRot, mPathX, totalSteps, INTERP_EASE_INOUT_CUBIC); - interpolate(curLeft, targetLeft, mPathY, totalSteps, INTERP_EASE_INOUT_CUBIC); - interpolate(curRight, targetRight, mPathZ, totalSteps, INTERP_EASE_INOUT_CUBIC); - } - - mPathX[totalSteps - 1] = targetRot; - mPathY[totalSteps - 1] = targetLeft; - mPathZ[totalSteps - 1] = targetRight; - - -#ifdef DEBUG - { - int i = 0; - - for (i = 0; i < totalSteps; i++) - { - debugPrint("step%d, x=%s, y=%s, z=%s\n", i, D(mPathX[i]), D(mPathY[i]), D(mPathZ[i])); - } - } -#endif - - mTimePerStep = timePerStep; - mTotalSteps = totalSteps; - mCurStep = 0; - mStartTime = millis(); - - return IN_RANGE; -} - -void uArmClass::interpolateEven(double startVal, double endVal, double *interpVals, int steps, byte easeType) -{ - - startVal = startVal / 10.0; - endVal = endVal / 10.0; - - double delta = endVal - startVal; - float t = (float)delta / steps; - for (byte i = 1; i <= steps; i++) - { - - //*(interp_vals+f) = 10.0*(start_val + (3 * delta) * (t * t) + (-2 * delta) * (t * t * t)); - *(interpVals + i - 1) = 10.0 * (startVal + t * i); - } -} - - -unsigned char uArmClass::moveToAngle(double targetRot, double targetLeft, double targetRight) -{ - - - double angleRot = 0, angleLeft = 0, angleRight = 0; - double curRot = 0, curLeft = 0, curRight = 0; - double x = 0; - double y = 0; - double z = 0; - double curX = 0; - double curY = 0; - double curZ = 0; - int i = 0; - int totalSteps = 0; - unsigned int timePerStep; - - unsigned char status = 0; - -// status = mController.coordianteToAngle(x, y, z, targetRot, targetLeft, targetRight); - status = mController.getXYZFromAngle(x, y, z, targetRot, targetLeft, targetRight); - - debugPrint("moveTo status: %d, angle: %s, %s, %s\n", status, D(targetRot), D(targetLeft), D(targetRight)); - - if (status == OUT_OF_RANGE_NO_SOLUTION) - { - return OUT_OF_RANGE_NO_SOLUTION; - } - - // get current angles - mController.getServoAngles(curRot, curLeft, curRight); - // get current xyz - mController.getCurrentXYZ(curX, curY, curZ); - - // calculate max steps - totalSteps = max(abs(targetRot - curRot), abs(targetLeft - curLeft)); - totalSteps = max(totalSteps, abs(targetRight - curRight)); - - if (totalSteps <= 0) - return OUT_OF_RANGE_NO_SOLUTION; - - totalSteps = totalSteps < STEP_MAX ? totalSteps : STEP_MAX; - - // calculate step time - //double distance = sqrt((x-curX) * (x-curX) + (y-curY) * (y-curY) + (z-curZ) * (z-curZ)); - //speed = constrain(speed, 100, 1000); - timePerStep = (TICK_INTERVAL) / totalSteps; - - - //keep timePerStep <= STEP_MAX_TIME - // if (timePerStep > STEP_MAX_TIME) - // { - // double ratio = double(timePerStep) / STEP_MAX_TIME; - - // if (totalSteps * ratio < STEP_MAX) - // { - // totalSteps *= ratio; - // timePerStep = STEP_MAX_TIME; - // } - // else - // { - // totalSteps = STEP_MAX; - // timePerStep = STEP_MAX_TIME; - // } - // } - - - totalSteps = totalSteps < STEP_MAX ? totalSteps : STEP_MAX; - - //debugPrint("totalSteps= %d\n", totalSteps); - - // trajectory planning - interpolateEven(curX, x, mPathX, totalSteps, INTERP_EASE_INOUT_CUBIC); - interpolateEven(curY, y, mPathY, totalSteps, INTERP_EASE_INOUT_CUBIC); - interpolateEven(curZ, z, mPathZ, totalSteps, INTERP_EASE_INOUT_CUBIC); - - for (i = 0; i < totalSteps; i++) - { - status = mController.coordianteToAngle(mPathX[i], mPathY[i], mPathZ[i], angleRot, angleLeft, angleRight); - - if (status != IN_RANGE) - { - break; - } - else - { - mPathX[i] = angleRot; - mPathY[i] = angleLeft; - mPathZ[i] = angleRight; - } - } - - if (i < totalSteps) - { - interpolateEven(curRot, targetRot, mPathX, totalSteps, INTERP_EASE_INOUT_CUBIC); - interpolateEven(curLeft, targetLeft, mPathY, totalSteps, INTERP_EASE_INOUT_CUBIC); - interpolateEven(curRight, targetRight, mPathZ, totalSteps, INTERP_EASE_INOUT_CUBIC); - } - - mPathX[totalSteps - 1] = targetRot; - mPathY[totalSteps - 1] = targetLeft; - mPathZ[totalSteps - 1] = targetRight; - - -#ifdef DEBUG - { - int i = 0; - - for (i = 0; i < totalSteps; i++) - { - debugPrint("step%d, x=%s, y=%s, z=%s\n", i, D(mPathX[i]), D(mPathY[i]), D(mPathZ[i])); - } - } -#endif - - mTimePerStep = timePerStep; - mTotalSteps = totalSteps; - mCurStep = 0; - mStartTime = millis(); - - return IN_RANGE; -} - -#ifdef MKII -bool uArmClass::isPowerPlugIn() -{ - if (analogRead(POWER_DETECT) > 400) - return true; - else - return false; -} -#endif \ No newline at end of file diff --git a/src/uArm.h b/src/uArm.h index 3b91f90..0080616 100644 --- a/src/uArm.h +++ b/src/uArm.h @@ -1,14 +1,3 @@ -/** - ****************************************************************************** - * @file uArm.h - * @author David.Long - * @email xiaokun.long@ufactory.cc - * @date 2016-09-28 - * @license GNU - * copyright(c) 2016 UFactory Team. All right reserved - ****************************************************************************** - */ - #ifndef _UARM_H_ #define _UARM_H_ @@ -17,142 +6,18 @@ #include #include "UFServo.h" #include "uArmConfig.h" -#include "uArmHWConfig.h" +#include "uArmTypes.h" +#include "uArmPin.h" #include "uArmController.h" #include "uArmBuzzer.h" #include "uArmRecorder.h" #include "uArmButton.h" #include "uArmLed.h" - -#define STEP_MAX 60 - -#define INTERP_EASE_INOUT_CUBIC 0 // original cubic ease in/out -#define INTERP_LINEAR 1 -#define INTERP_EASE_INOUT 2 // quadratic easing methods -#define INTERP_EASE_IN 3 -#define INTERP_EASE_OUT 4 - -#define NORMAL_MODE 0 -#define NORMAL_BT_CONNECTED_MODE 1 -#define LEARNING_MODE 2 -#define SINGLE_PLAY_MODE 3 -#define LOOP_PLAY_MODE 4 -#define LEARNING_MODE_STOP 5 - -#define STEP_MAX_TIME 20 // ms - - - -#define OK 0 -#define ERR1 1 -#define ERR2 2 - -#define SS "[S]" -#define S0 "[S0]" -#define S1 "[S1]" -#define S2 "[S2]" -#define FF "[F]" -#define F0 "[F0]" -#define F1 "[F1]" - -// Calibration Flag & OFFSET EEPROM ADDRESS -#define CALIBRATION_FLAG 10 -#define CALIBRATION_LINEAR_FLAG 11 -#define CALIBRATION_MANUAL_FLAG 12 -#define CALIBRATION_STRETCH_FLAG 13 - - - -//#define TIME_PER_STEP 10 - -#define SERVO_9G_MAX 460 -#define SERVO_9G_MIN 98 - -#define CONFIRM_FLAG 0x80 - - -#define SUCCESS 1 -#define FAILED -1 - -#define DATA_TYPE_BYTE 1 -#define DATA_TYPE_INTEGER 2 -#define DATA_TYPE_FLOAT 4 - -#define EXTERNAL_EEPROM_SYS_ADDRESS 0xA2 -#define EXTERNAL_EEPROM_USER_ADDRESS 0xA0 - -class uArmClass -{ -public: - uArmClass(); - - void setup(); - void run(); - - unsigned char moveTo(double x, double y, double z, double speed = 100); - unsigned char moveToAngle(double x, double y, double z); - - bool isMoving(); - void stopMove(); - void setReportInterval(unsigned int interval); - void reportPos(); - -#ifdef MKII - bool isPowerPlugIn(); -#endif - -public: - uArmController mController; - uArmRecorder mRecorder; - -private: - void initHardware(); - char parseParam(String cmnd, const char *parameters, int parameterCount, double valueArray[]); - void interpolate(double startVal, double endVal, double *interpVals, int steps, byte easeType); - void interpolateEven(double startVal, double endVal, double *interpVals, int steps, byte easeType); - - void controllerRun(); - void systemRun(); - bool play(); - bool record(); - void tickTaskRun(); - void recorderTick(); - - void btDetect(); - - - -private: - int mCurStep; - int mTotalSteps; - unsigned int mTimePerStep; - unsigned long mStartTime; - - double mPathX[STEP_MAX]; - double mPathY[STEP_MAX]; - double mPathZ[STEP_MAX]; - - unsigned char mSysStatus = NORMAL_MODE; - unsigned int mRecordAddr = 0; - - unsigned char mTime50ms; - unsigned long mTickStartTime; - - uArmButton mButtonD4; - uArmButton mButtonD7; - - unsigned int mReportInterval; // ms. 0 means no report - unsigned int mTimeInterval; - unsigned long mReportStartTime; - - unsigned long mTickRecorderTime; -#ifdef MKII - uArmLed mLed; -#endif -}; -extern uArmClass uArm; - +#include "uArmService.h" +#include "uArmDebug.h" +#include "uArmComm.h" +#include "uArmAPI.h" -#endif // _UARM_H_ +#endif // _UARM_H_ \ No newline at end of file diff --git a/src/uArmAPI.cpp b/src/uArmAPI.cpp new file mode 100644 index 0000000..d963ba6 --- /dev/null +++ b/src/uArmAPI.cpp @@ -0,0 +1,993 @@ +/** + ****************************************************************************** + * @file uArmAPI.cpp + * @author David.Long + * @email xiaokun.long@ufactory.cc + * @date 2016-11-28 + ****************************************************************************** + */ + +#include "uArmAPI.h" + +uArmButton buttonMenu; +uArmButton buttonPlay; + +#ifdef MKII + uArmLed ledRed; +#endif + +#ifdef DEBUG +#define STEP_MAX 30 // no enough ram for debug. so reduce the size to enable debug +#else +#define STEP_MAX 60 +#endif + +#define INTERP_EASE_INOUT_CUBIC 0 // original cubic ease in/out +#define INTERP_LINEAR 1 +#define INTERP_EASE_INOUT 2 // quadratic easing methods +#define INTERP_EASE_IN 3 +#define INTERP_EASE_OUT 4 + +#define STEP_MAX_TIME 20 // ms + +#define EXTERNAL_EEPROM_SYS_ADDRESS 0xA2 +#define EXTERNAL_EEPROM_USER_ADDRESS 0xA0 + +#define PUMP_GRABBING_CURRENT 55 + +static int mCurStep; +static int mTotalSteps; +static unsigned int mTimePerStep; +static unsigned long mStartTime; +static double mPathX[STEP_MAX]; +static double mPathY[STEP_MAX]; +static double mPathZ[STEP_MAX]; + + +static unsigned char _moveTo(double x, double y, double z, double speed); +static void _sort(unsigned int array[], unsigned int len); +static void _controllerRun(); + +void initHardware() +{ + + pinMode(BTN_D4, INPUT_PULLUP); //special mode for calibration + pinMode(BUZZER, OUTPUT); + pinMode(LIMIT_SW, INPUT_PULLUP); + pinMode(BTN_D7, INPUT_PULLUP); + pinMode(PUMP_EN, OUTPUT); + pinMode(GRIPPER, OUTPUT); + + #ifdef MKII + pinMode(SYS_LED, OUTPUT); + + digitalWrite(PUMP_EN, HIGH);//keep the pump off + #endif + + #ifdef METAL + pinMode(VALVE_EN, OUTPUT); + #endif + + + buttonMenu.setPin(BTN_D4); + buttonPlay.setPin(BTN_D7); + + buzzer.setPin(BUZZER); + + #ifdef MKII + ledRed.setPin(SYS_LED); + #endif + + + + +} + +/*! + \brief init components + */ +void uArmInit() +{ + + initHardware(); + controller.init(); + + mCurStep = -1; + mTotalSteps = -1; +} + +/*! + \brief move to pos(x, y, z) + \param x, y, z in mm + \param speed in mm/min + \return IN_RANGE if everything is OK + \return OUT_OF_RANGE_NO_SOLUTION if cannot reach + \return OUT_OF_RANGE will move to the closest pos + \return NO_NEED_TO_MOVE if it is already there + */ +unsigned char moveTo(double x, double y, double z, double speed) +{ + unsigned char result = IN_RANGE; + + debugPrint("moveTo: x=%f, y=%f, z=%f, speed=%f", x, y, z, speed); + + // when speed less than 100 mm/min, move to destination directly + if (speed < 100) + { + // convert to angle and move to target angle directly + double angleB, angleL, angleR; + result = controller.xyzToAngle(x, y, z, angleB, angleL, angleR); + if (result != OUT_OF_RANGE_NO_SOLUTION) + { + controller.writeServoAngle(angleB, angleL, angleR); + } + + return result; + } + else + { + result = _moveTo(x, y, z, speed); + + if(result != OUT_OF_RANGE_NO_SOLUTION) + { + _controllerRun(); + + } + + return result; + } +} + +/*! + \brief move to pos(x, y, z) according to current pos + \param x, y, z (mm) + \param speed (mm/min) + \return IN_RANGE if everything is OK + \return OUT_OF_RANGE_NO_SOLUTION if cannot reach + \return OUT_OF_RANGE will move to the closest pos + \return NO_NEED_TO_MOVE if it is already there + */ +unsigned char relativeMove(double x, double y, double z, double speed) +{ + double x1, y1, z1; + // get cur xyz + controller.getCurrentXYZ(x1, y1, z1); + + x1 += x; + y1 += y; + z1 += z; + + return moveTo(x1, y1, z1, speed); +} + +/*! + \brief move to pos of polor coordinates(s, r, h) + \param s: stretch(mm) + \param r: angle (0~180) + \param h: height(mm) + \param speed (mm/min) + \return IN_RANGE if everything is OK + \return OUT_OF_RANGE_NO_SOLUTION if cannot reach + \return OUT_OF_RANGE will move to the closest pos + \return NO_NEED_TO_MOVE if it is already there + */ +unsigned char moveToPol(double s, double r, double h, double speed) +{ + double x, y, z; + + polToXYZ(s, r, h, x, y, z); + return moveTo(x, y, z, speed); +} + +/*! + \brief attach servo(0~3) + \param servoNumber: SERVO_ROT_NUM, SERVO_LEFT_NUM, SERVO_RIGHT_NUM, SERVO_HAND_ROT_NUM + \return true or false + */ +bool attachServo(unsigned char servoNumber) +{ + if (servoNumber < SERVO_COUNT) + { + controller.attachServo(servoNumber); + return true; + } + + return false; +} + +/*! + \brief detach servo(0~3) + \param servoNumber: SERVO_ROT_NUM, SERVO_LEFT_NUM, SERVO_RIGHT_NUM, SERVO_HAND_ROT_NUM + \return true or false + */ +bool detachServo(unsigned char servoNumber) +{ + if (servoNumber < SERVO_COUNT) + { + controller.detachServo(servoNumber); + return true; + } + + return false; +} + +/*! + \brief set servo angle + \param servoNumber(0~3) + \param angle (0~180) + \return OK if everything is OK + \return ERR_SERVO_INDEX_EXCEED_LIMIT if servoNumber not in range(0~3) + \return ERR_ANGLE_OUT_OF_RANGE if angle not in range(0~180) + */ +unsigned char setServoAngle(unsigned char servoNumber, double angle) +{ + if (servoNumber >= SERVO_COUNT) + return ERR_SERVO_INDEX_EXCEED_LIMIT; + + if (angle > 180 || angle < 0) + return ERR_ANGLE_OUT_OF_RANGE; + + controller.writeServoAngle(servoNumber, angle); + + return OK; +} + +/*! + \brief get servo angle + \param servoNumber(0~3) + \return value of angle + \return -1 if servoNumber not in range(0~3) + */ +double getServoAngle(unsigned char servoNumber) +{ + if (servoNumber >= SERVO_COUNT) + return -1; + + return controller.readServoAngle(servoNumber); +} + + +/*! + \brief gripper work + */ +void gripperCatch() +{ + digitalWrite(GRIPPER, LOW); +} + +/*! + \brief gripper stop + */ +void gripperRelease() +{ + digitalWrite(GRIPPER, HIGH); +} + +/*! + \brief get gripper status + \return STOP if gripper is not working + \return WORKING if gripper is working but not catched sth + \return GRABBING if gripper got sth + */ +unsigned char getGripperStatus() +{ +#ifdef MKII + //Serial.println(getAnalogData(GRIPPER_FEEDBACK)); + if (digitalRead(GRIPPER) == HIGH) + { + return STOP;//NOT WORKING + } + else + { + + if (getAnalogPinValue(GRIPPER_FEEDBACK) > 600) + { + return WORKING; + } + else + { + return GRABBING; + } + } +#elif defined(METAL) + if(digitalRead(GRIPPER) == HIGH) + { + return STOP; + } + else + { + if(getAnalogPinValue(GRIPPER_FEEDBACK) > 600) + { + return WORKING; + } + else + { + return GRABBING; + } + } +#endif +} + +/*! + \brief get pump status + \return STOP if pump is not working + \return WORKING if pump is working but not catched sth + \return GRABBING if pump got sth + */ +unsigned char getPumpStatus() +{ +#ifdef MKII + if (digitalRead(PUMP_EN) == HIGH) + { + return STOP; + } + else + { + //Serial.println(getAnalogData(PUMP_FEEDBACK)); + if (getAnalogPinValue(PUMP_FEEDBACK) <= PUMP_GRABBING_CURRENT) + { + return GRABBING; + } + else + { + return WORKING; + } + } +#elif defined (METAL) + if (digitalRead(PUMP_EN) == HIGH) + { + return 1; + } + else + { + return 0; + } +#endif +} + +/*! + \brief pump working + */ +void pumpOn() +{ + #ifdef MKII + digitalWrite(PUMP_EN, LOW); + #elif defined(METAL) + digitalWrite(PUMP_EN, HIGH); + digitalWrite(VALVE_EN, LOW); + #endif +} + +/*! + \brief pump stop + */ +void pumpOff() +{ + + + #ifdef MKII + digitalWrite(PUMP_EN, HIGH); + #elif defined(METAL) + digitalWrite(PUMP_EN, LOW); + digitalWrite(VALVE_EN, HIGH); + #endif +} + + +/*! + \brief get tip status + \return true if limit switch hit + */ +bool getTip() +{ + if (digitalRead(LIMIT_SW)) + { + return true; + } + else + { + return false; + } +} + +/*! + \brief get pin value + \param pin of arduino + \return HIGH or LOW + */ +int getDigitalPinValue(unsigned int pin) +{ + return digitalRead(pin); +} + +/*! + \brief set pin value + \param pin of arduino + \param value: HIGH or LOW + */ +void setDigitalPinValue(unsigned int pin, unsigned char value) +{ + if (value) + { + digitalWrite(pin, HIGH); + } + else + { + digitalWrite(pin, LOW); + } +} + +/*! + \brief get analog value of pin + \param pin of arduino + \return value of analog data + */ +int getAnalogPinValue(unsigned int pin) +{ + unsigned int dat[8]; + + + for(int i = 0; i < 8; i++) + { + dat[i] = analogRead(pin); + } + + _sort(dat, 8); + + unsigned int result = (dat[2]+dat[3]+dat[4]+dat[5])/4; + + return result; +} + +/*! + \brief convert polor coordinates to Cartesian coordinate + \param s(mm), r(0~180), h(mm) + \output x, y, z(mm) + */ +void polToXYZ(double s, double r, double h, double& x, double& y, double& z) +{ + z = h; + x = s * cos(r / MATH_TRANS); + y = s * sin(r / MATH_TRANS); +} + +/*! + \brief check pos reachable + \return IN_RANGE if everything is OK + \return OUT_OF_RANGE_NO_SOLUTION if cannot reach + \return OUT_OF_RANGE can move to the closest pos + */ +unsigned char validatePos(double x, double y, double z) +{ + double angleRot, angleLeft, angleRight; + return controller.xyzToAngle(x, y, z, angleRot, angleLeft, angleRight, false); +} + +/*! + \brief get current pos + \output x, y, z(mm) + */ +void getCurrentXYZ(double& x, double& y, double& z) +{ + controller.updateAllServoAngle(); + controller.getCurrentXYZ(x, y, z); +} + +/*! + \brief get current pos of polor coordinates + \output s(mm), r(0~180), h(mm) + */ +void getCurrentPosPol(double& s, double& r, double& h) +{ + + double angleRot, angleLeft, angleRight; + double x, y, z; + + controller.updateAllServoAngle(); + controller.getCurrentXYZ(x, y, z); + controller.getServoAngles(angleRot, angleLeft, angleRight); + double stretch; + stretch = sqrt(x * x + y * y); + + s = stretch; + r = angleRot; + h = z; +} + +/*! + \brief get servo angles from pos(x, y, z) + \param x, y, z(mm) + \output angles of servo(0~180) + \return IN_RANGE if everything is OK + \return OUT_OF_RANGE_NO_SOLUTION if cannot reach + \return OUT_OF_RANGE can move to the closest pos + */ +unsigned char xyzToAngle(double x, double y, double z, double& angleRot, double& angleLeft, double& angleRight) +{ + return controller.xyzToAngle(x, y, z, angleRot, angleLeft, angleRight); +} + +/*! + \brief get pos(x, y, z) from servo angles + \param angles of servo(0~180) + \output x, y, z(mm) + \return IN_RANGE if everything is OK + \return OUT_OF_RANGE_NO_SOLUTION if cannot reach + \return OUT_OF_RANGE can move to the closest pos + */ +unsigned char angleToXYZ(double angleRot, double angleLeft, double angleRight, double& x, double& y, double& z) +{ + return controller.getXYZFromAngle(x, y, z, angleRot, angleLeft, angleRight); +} + +/*! + \brief get e2prom data + \param device: EEPROM_ON_CHIP, EEPROM_EXTERN_USER, EEPROM_EXTERN_SYSTEM + \param addr: 0~2047(EEPROM_ON_CHIP), 0~65535(EEPROM_EXTERN_USER, EEPROM_EXTERN_SYSTEM) + \param type: DATA_TYPE_BYTE, DATA_TYPE_INTEGER, DATA_TYPE_FLOAT + */ +double getE2PROMData(unsigned char device, unsigned int addr, unsigned char type) +{ + double result = 0; + + uint8_t deviceAddr; + + + union { + float fdata; + uint8_t data[4]; + } FData; + + + switch(device) + { + + case 0: + + switch(type) + { + case DATA_TYPE_BYTE: + { + int val = EEPROM.read(addr); + result = val; + break; + } + case DATA_TYPE_INTEGER: + { + int i_val = 0; + EEPROM.get(addr, i_val); + result = i_val; + break; + } + case DATA_TYPE_FLOAT: + { + double f_val = 0.0f; + EEPROM.get(addr,f_val); + result = f_val; + break; + } + } + + break; + + case 1: + deviceAddr = EXTERNAL_EEPROM_USER_ADDRESS; + break; + + case 2: + deviceAddr = EXTERNAL_EEPROM_SYS_ADDRESS; + break; + + default: + return ADDRESS_ERROR; + } + + + if (device == 1 || device == 2) + { + int num = 0; + switch(type) + { + case DATA_TYPE_BYTE: + { + num = 1; + break; + } + case DATA_TYPE_INTEGER: + { + num = 2; + break; + } + case DATA_TYPE_FLOAT: + { + num = 4; + break; + } + default: + return PARAMETER_ERROR; + } + + unsigned char i=0; + i = (addr % 128); + // Since the eeprom's sector is 128 byte, if we want to write 5 bytes per cycle we need to care about when there's less than 5 bytes left + if (i >= (129-num)) + { + i = 128 - i; + iic_readbuf(FData.data, deviceAddr, addr, i);// write data + delay(5); + iic_readbuf(FData.data + i, deviceAddr, addr + i, num - i);// write data + } + //if the left bytes are greater than 5, just do it + else + { + iic_readbuf(FData.data, deviceAddr, addr, num);// write data + } + + + switch(type) + { + case DATA_TYPE_BYTE: + { + result = FData.data[0]; + break; + } + case DATA_TYPE_INTEGER: + { + result = (FData.data[0] << 8) + FData.data[1]; + break; + } + case DATA_TYPE_FLOAT: + { + result = FData.fdata; + break; + } + } + + + } + + return result; + +} + +/*! + \brief set e2prom data + \param device: EEPROM_ON_CHIP, EEPROM_EXTERN_USER, EEPROM_EXTERN_SYSTEM + \param addr: 0~2047(EEPROM_ON_CHIP), 0~65535(EEPROM_EXTERN_USER, EEPROM_EXTERN_SYSTEM) + \param type: DATA_TYPE_BYTE, DATA_TYPE_INTEGER, DATA_TYPE_FLOAT + \param value: value to write + */ +double setE2PROMData(unsigned char device, unsigned int addr, unsigned char type, double value) +{ + uint8_t deviceAddr; + + union { + float fdata; + uint8_t data[4]; + } FData; + + switch(device) + { + + case 0: + switch(type) + { + case DATA_TYPE_BYTE: + { + byte b_val; + b_val = byte(value); + EEPROM.write(addr, b_val); + break; + } + case DATA_TYPE_INTEGER: + { + int i_val = 0; + i_val = int(value); + EEPROM.put(addr, i_val); + break; + } + case DATA_TYPE_FLOAT: + { + float f_val = 0.0f; + f_val = float(value); + EEPROM.put(addr,f_val); + // Serial.println(f_val); + break; + } + } + break; + case 1: + deviceAddr = EXTERNAL_EEPROM_USER_ADDRESS; + break; + + case 2: + deviceAddr = EXTERNAL_EEPROM_SYS_ADDRESS; + break; + + default: + return ADDRESS_ERROR; + } + + + if (device == 1 || device == 2) + { + int num = 0; + switch(type) + { + case DATA_TYPE_BYTE: + { + FData.data[0] = byte(value); + num = 1; + break; + } + case DATA_TYPE_INTEGER: + { + int i_val = 0; + i_val = int(value); + FData.data[0] = (i_val & 0xff00) >> 8; + FData.data[1] = i_val & 0xff; + num = 2; + break; + } + case DATA_TYPE_FLOAT: + { + FData.fdata = float(value); + num = 4; + break; + } + default: + return PARAMETER_ERROR; + } + + unsigned char i=0; + i = (addr % 128); + // Since the eeprom's sector is 128 byte, if we want to write 5 bytes per cycle we need to care about when there's less than 5 bytes left + if (i >= (129-num)) + { + i = 128 - i; + iic_writebuf(FData.data, deviceAddr, addr, i);// write data + delay(5); + iic_writebuf(FData.data + i, deviceAddr, addr + i, num - i);// write data + } + //if the left bytes are greater than 5, just do it + else + { + iic_writebuf(FData.data, deviceAddr, addr, num);// write data + } + + + + + } + +} + +#ifdef MKII +/*! + \brief stop move immediately + */ +void stopMove() +{ + mCurStep = -1; +} + +/*! + \brief is moving now + */ +bool isMoving() +{ + return (mCurStep != -1); +} + +/*! + \brief check if power plug in + */ +bool isPowerPlugIn() +{ + if (analogRead(POWER_DETECT) > 400) + return true; + else + return false; +} +#endif + +//////////////////////////////////////////////////////////////////////////// +// private functions + +static void _sort(unsigned int array[], unsigned int len) +{ + unsigned char i=0,j=0; + unsigned int temp = 0; + + for(i = 0; i < len; i++) + { + for(j = 0; i+j < (len-1); j++) + { + if(array[j] > array[j+1]) + { + temp = array[j]; + array[j] = array[j+1]; + array[j+1] = temp; + } + } + } +} + +static void _interpolate(double startVal, double endVal, double *interpVals, int steps, byte easeType) +{ + + startVal = startVal / 10.0; + endVal = endVal / 10.0; + + double delta = endVal - startVal; + for (byte i = 1; i <= steps; i++) + { + float t = (float)i / steps; + //*(interp_vals+f) = 10.0*(start_val + (3 * delta) * (t * t) + (-2 * delta) * (t * t * t)); + *(interpVals + i - 1) = 10.0 * (startVal + t * t * delta * (3 + (-2) * t)); + } +} + +static unsigned char _moveTo(double x, double y, double z, double speed) +{ + + double angleRot = 0, angleLeft = 0, angleRight = 0; + double curRot = 0, curLeft = 0, curRight = 0; + double targetRot = 0; + double targetLeft = 0; + double targetRight = 0; + double curX = 0; + double curY = 0; + double curZ = 0; + int i = 0; + int totalSteps = 0; + unsigned int timePerStep; + + unsigned char status = 0; + + status = controller.xyzToAngle(x, y, z, targetRot, targetLeft, targetRight); + + + if (status == OUT_OF_RANGE_NO_SOLUTION) + { + return OUT_OF_RANGE_NO_SOLUTION; + } + + if (speed == 0) + { + mCurStep = -1; + controller.writeServoAngle(targetRot, targetLeft, targetRight); + return IN_RANGE; + } + + // get current angles + controller.getServoAngles(curRot, curLeft, curRight); + // get current xyz + controller.getCurrentXYZ(curX, curY, curZ); + + // calculate max steps + totalSteps = max(abs(targetRot - curRot), abs(targetLeft - curLeft)); + totalSteps = max(totalSteps, abs(targetRight - curRight)); + + if (totalSteps <= 0) + return NO_NEED_TO_MOVE; + + totalSteps = totalSteps < STEP_MAX ? totalSteps : STEP_MAX; + + // calculate step time + double distance = sqrt((x-curX) * (x-curX) + (y-curY) * (y-curY) + (z-curZ) * (z-curZ)); + speed = constrain(speed, 100, 1000); + timePerStep = distance / speed * 1000.0 / totalSteps; + + + // keep timePerStep <= STEP_MAX_TIME + if (timePerStep > STEP_MAX_TIME) + { + double ratio = double(timePerStep) / STEP_MAX_TIME; + + if (totalSteps * ratio < STEP_MAX) + { + totalSteps *= ratio; + timePerStep = STEP_MAX_TIME; + } + else + { + totalSteps = STEP_MAX; + timePerStep = STEP_MAX_TIME; + } + } + + + totalSteps = totalSteps < STEP_MAX ? totalSteps : STEP_MAX; + + //debugPrint("totalSteps= %d\n", totalSteps); + + // trajectory planning + _interpolate(curX, x, mPathX, totalSteps, INTERP_EASE_INOUT_CUBIC); + _interpolate(curY, y, mPathY, totalSteps, INTERP_EASE_INOUT_CUBIC); + _interpolate(curZ, z, mPathZ, totalSteps, INTERP_EASE_INOUT_CUBIC); + + for (i = 0; i < totalSteps; i++) + { + status = controller.xyzToAngle(mPathX[i], mPathY[i], mPathZ[i], angleRot, angleLeft, angleRight); + + if (status != IN_RANGE) + { + break; + } + else + { + mPathX[i] = angleRot; + mPathY[i] = angleLeft; + mPathZ[i] = angleRight; + } + } + + if (i < totalSteps) + { + _interpolate(curRot, targetRot, mPathX, totalSteps, INTERP_EASE_INOUT_CUBIC); + _interpolate(curLeft, targetLeft, mPathY, totalSteps, INTERP_EASE_INOUT_CUBIC); + _interpolate(curRight, targetRight, mPathZ, totalSteps, INTERP_EASE_INOUT_CUBIC); + } + + mPathX[totalSteps - 1] = targetRot; + mPathY[totalSteps - 1] = targetLeft; + mPathZ[totalSteps - 1] = targetRight; + + + mTimePerStep = timePerStep; + mTotalSteps = totalSteps; + mCurStep = 0; + mStartTime = millis(); + + return IN_RANGE; +} + + + +static void _controllerRun() +{ + + + while (mCurStep >= 0 && mCurStep < mTotalSteps) + { + + if((millis() - mStartTime) >= (mCurStep * mTimePerStep)) + { + + // ignore the point if cannot reach + if (controller.limitRange(mPathX[mCurStep], mPathY[mCurStep], mPathZ[mCurStep]) != OUT_OF_RANGE_NO_SOLUTION) + { + //debugPrint("curStep:%d, %f, %f, %f", mCurStep, mPathX[mCurStep], mPathY[mCurStep], mPathZ[mCurStep]); + if (mCurStep == (mTotalSteps - 1)) + { + double angles[3]; + angles[0] = controller.getReverseServoAngle(0, mPathX[mCurStep]); + angles[1] = controller.getReverseServoAngle(1, mPathY[mCurStep]); + angles[2] = controller.getReverseServoAngle(2, mPathZ[mCurStep]); + //debugPrint("curStep:%d, %f, %f, %f", mCurStep, angles[0], angles[1], angles[2]); + controller.writeServoAngle(angles[0], angles[1], angles[2]); + //controller.writeServoAngle(mPathX[mCurStep], mPathY[mCurStep], mPathZ[mCurStep]); + } + else + + { + controller.writeServoAngle(mPathX[mCurStep], mPathY[mCurStep], mPathZ[mCurStep]); + } + } + + mCurStep++; + + + if (mCurStep >= mTotalSteps) + { + mCurStep = -1; + + + } + } + + manage_inactivity(); + } + +} \ No newline at end of file diff --git a/src/uArmAPI.h b/src/uArmAPI.h new file mode 100644 index 0000000..6eacf29 --- /dev/null +++ b/src/uArmAPI.h @@ -0,0 +1,255 @@ +/*! + \file uArmAPI.h + \brief uArm API for Arduino + \author David Long + \license GNU + \copyright(c) 2016 UFactory Team. All right reserved + */ + +#ifndef _UARMAPI_H_ +#define _UARMAPI_H_ + +#include +#include +#include +#include "UFServo.h" +#include "uArmConfig.h" +#include "uArmPin.h" +#include "uArmController.h" +#include "uArmBuzzer.h" +#include "uArmRecorder.h" +#include "uArmButton.h" +#include "uArmLed.h" +#include "uArmService.h" +#include "uArmTypes.h" + + +extern uArmButton buttonMenu; // D4 in Metal +extern uArmButton buttonPlay; // D7 in Metal + +#ifdef MKII + extern uArmLed ledRed; // red led in MKII +#endif + +extern void manage_inactivity(void); + +/*! + \brief init components + */ +void uArmInit(); + +/*! + \brief move to pos(x, y, z) + \param x, y, z in mm + \param speed in mm/min + \return IN_RANGE if everything is OK + \return OUT_OF_RANGE_NO_SOLUTION if cannot reach + \return OUT_OF_RANGE will move to the closest pos + \return NO_NEED_TO_MOVE if it is already there + */ +unsigned char moveTo(double x, double y, double z, double speed = 100); + +/*! + \brief move to pos of polor coordinates(s, r, h) + \param s: stretch(mm) + \param r: angle (0~180) + \param h: height(mm) + \param speed (mm/min) + \return IN_RANGE if everything is OK + \return OUT_OF_RANGE_NO_SOLUTION if cannot reach + \return OUT_OF_RANGE will move to the closest pos + \return NO_NEED_TO_MOVE if it is already there + */ +unsigned char moveToPol(double s, double r, double h, double speed); + +/*! + \brief move to pos(x, y, z) according to current pos + \param x, y, z (mm) + \param speed (mm/min) + \return IN_RANGE if everything is OK + \return OUT_OF_RANGE_NO_SOLUTION if cannot reach + \return OUT_OF_RANGE will move to the closest pos + \return NO_NEED_TO_MOVE if it is already there + */ +unsigned char relativeMove(double x, double y, double z, double speed); + +/*! + \brief attach servo(0~3) + \param servoNumber: SERVO_ROT_NUM, SERVO_LEFT_NUM, SERVO_RIGHT_NUM, SERVO_HAND_ROT_NUM + \return true or false + */ +bool attachServo(unsigned char servoNumber); + +/*! + \brief detach servo(0~3) + \param servoNumber: SERVO_ROT_NUM, SERVO_LEFT_NUM, SERVO_RIGHT_NUM, SERVO_HAND_ROT_NUM + \return true or false + */ +bool detachServo(unsigned char servoNumber); + +/*! + \brief set servo angle + \param servoNumber(0~3) + \param angle (0~180) + \return OK if everything is OK + \return ERR_SERVO_INDEX_EXCEED_LIMIT if servoNumber not in range(0~3) + \return ERR_ANGLE_OUT_OF_RANGE if angle not in range(0~180) + */ +unsigned char setServoAngle(unsigned char servoNumber, double angle); + +/*! + \brief get servo angle + \param servoNumber(0~3) + \return value of angle + \return -1 if servoNumber not in range(0~3) + */ +double getServoAngle(unsigned char servoNumber); + +/*! + \brief gripper work + */ +void gripperCatch(); + +/*! + \brief gripper stop + */ +void gripperRelease(); + +/*! + \brief get gripper status + \return STOP if gripper is not working + \return WORKING if gripper is working but not catched sth + \return GRABBING if gripper got sth + */ +unsigned char getGripperStatus(); + +/*! + \brief pump working + */ +void pumpOn(); + +/*! + \brief pump stop + */ +void pumpOff(); + +/*! + \brief get pump status + \return STOP if pump is not working + \return WORKING if pump is working but not catched sth + \return GRABBING if pump got sth + */ +unsigned char getPumpStatus(); + +/*! + \brief get tip status + \return true if limit switch hit + */ +bool getTip(); + + +/*! + \brief check pos reachable + \return IN_RANGE if everything is OK + \return OUT_OF_RANGE_NO_SOLUTION if cannot reach + \return OUT_OF_RANGE can move to the closest pos + */ +unsigned char validatePos(double x, double y, double z); + +/*! + \brief convert polor coordinates to Cartesian coordinate + \param s(mm), r(0~180), h(mm) + \output x, y, z(mm) + */ +void polToXYZ(double s, double r, double h, double& x, double& y, double& z); + +/*! + \brief get current pos + \output x, y, z(mm) + */ +void getCurrentXYZ(double& x, double& y, double& z); + +/*! + \brief get current pos of polor coordinates + \output s(mm), r(0~180), h(mm) + */ +void getCurrentPosPol(double& s, double& r, double& h); + +/*! + \brief get servo angles from pos(x, y, z) + \param x, y, z(mm) + \output angles of servo(0~180) + \return IN_RANGE if everything is OK + \return OUT_OF_RANGE_NO_SOLUTION if cannot reach + \return OUT_OF_RANGE can move to the closest pos + */ +unsigned char xyzToAngle(double x, double y, double z, double& angleRot, double& angleLeft, double& angleRight); + +/*! + \brief get pos(x, y, z) from servo angles + \param angles of servo(0~180) + \output x, y, z(mm) + \return IN_RANGE if everything is OK + \return OUT_OF_RANGE_NO_SOLUTION if cannot reach + \return OUT_OF_RANGE can move to the closest pos + */ +unsigned char angleToXYZ(double angleRot, double angleLeft, double angleRight, double& x, double& y, double& z); + +/*! + \brief get pin value + \param pin of arduino + \return HIGH or LOW + */ +int getDigitalPinValue(unsigned int pin); + +/*! + \brief set pin value + \param pin of arduino + \param value: HIGH or LOW + */ +void setDigitalPinValue(unsigned int pin, unsigned char value); + +/*! + \brief get analog value of pin + \param pin of arduino + \return value of analog data + */ +int getAnalogPinValue(unsigned int pin); + + +/*! + \brief get e2prom data + \param device: EEPROM_ON_CHIP, EEPROM_EXTERN_USER, EEPROM_EXTERN_SYSTEM + \param addr: 0~2047(EEPROM_ON_CHIP), 0~65535(EEPROM_EXTERN_USER, EEPROM_EXTERN_SYSTEM) + \param type: DATA_TYPE_BYTE, DATA_TYPE_INTEGER, DATA_TYPE_FLOAT + */ +double getE2PROMData(unsigned char device, unsigned int addr, unsigned char type); + +/*! + \brief set e2prom data + \param device: EEPROM_ON_CHIP, EEPROM_EXTERN_USER, EEPROM_EXTERN_SYSTEM + \param addr: 0~2047(EEPROM_ON_CHIP), 0~65535(EEPROM_EXTERN_USER, EEPROM_EXTERN_SYSTEM) + \param type: DATA_TYPE_BYTE, DATA_TYPE_INTEGER, DATA_TYPE_FLOAT + \param value: value to write + */ +double setE2PROMData(unsigned char device, unsigned int addr, unsigned char type, double value); + +#ifdef MKII +/*! + \brief stop move immediately + */ +void stopMove(); + +/*! + \brief is moving now + */ +bool isMoving(); + + +/*! + \brief check if power plug in + */ + bool isPowerPlugIn(); +#endif + +#endif // _UARMAPI_H_ diff --git a/src/uArmButton.cpp b/src/uArmButton.cpp index 2c8b315..50449ef 100644 --- a/src/uArmButton.cpp +++ b/src/uArmButton.cpp @@ -62,7 +62,7 @@ void uArmButton::tick() case HALF_PRESSED: if (isPressed()) { - gBuzzer.buzz(4000, 100); + buzzer.buzz(4000, 100); mState = PRESSED; } else diff --git a/src/uArmButton.h b/src/uArmButton.h index c517d9a..0660f33 100644 --- a/src/uArmButton.h +++ b/src/uArmButton.h @@ -13,7 +13,7 @@ #include #include "uArmConfig.h" -#include "uArmHWConfig.h" +#include "uArmPin.h" #include "uArmBuzzer.h" #define EVENT_NONE 0 diff --git a/src/uArmBuzzer.cpp b/src/uArmBuzzer.cpp index 6759d08..eed9d5f 100644 --- a/src/uArmBuzzer.cpp +++ b/src/uArmBuzzer.cpp @@ -11,7 +11,7 @@ #include "uArmBuzzer.h" -uArmBuzzer gBuzzer; +uArmBuzzer buzzer; diff --git a/src/uArmBuzzer.h b/src/uArmBuzzer.h index 1dd4b85..4f1652e 100644 --- a/src/uArmBuzzer.h +++ b/src/uArmBuzzer.h @@ -37,6 +37,6 @@ class uArmBuzzer unsigned long mDuration; }; -extern uArmBuzzer gBuzzer; +extern uArmBuzzer buzzer; #endif // _UARMBUZZER_H_ diff --git a/src/uArmComm.cpp b/src/uArmComm.cpp index 00538b4..ed2256d 100644 --- a/src/uArmComm.cpp +++ b/src/uArmComm.cpp @@ -10,117 +10,71 @@ */ #include "uArmComm.h" +#include "uArmRingBuffer.h" -uArmComm gComm; -// unsigned char uArmComm::cmdIndex = 0; -// CommState uArmComm::mState = IDLE; -// unsigned char uArmComm::cmdReceived[COM_LEN_MAX]; +static CommState commState = IDLE; +static unsigned char cmdReceived[COM_LEN_MAX]; +static unsigned char cmdIndex = 0; -/* -struct Command -{ - char cmd[5]; - int parametersCount; - char parameters[4]; - void (*execute)(double value[4]); -}; - -const Command command[] PROGMEM= { - {"sMov", 4, {'X', 'Y', 'Z', 'V'}, &gComm.cmdMove}, // - {"sPol", 4, {'S', 'R', 'H', 'V'}, &gComm.cmdMovePol}, // - {"sAtt", 1, {'N'}, &gComm.cmdSetAttachServo}, // - {"sDet", 1, {'N'}, &gComm.cmdSetDetachServo}, // - {"sSer", 2, {'N', 'V'}, &gComm.cmdSetServoAngle}, - - {"sAng", 2, {'N', 'V'}, &gComm.cmdSetServoAngleWithOffset}, // - {"sPum", 1, {'V'}, &gComm.cmdSetPump}, // - {"sGri", 1, {'V'}, &gComm.cmdSetGripper}, // - {"sBuz", 2, {'F', 'T'}, &gComm.cmdSetBuzz}, // - {"sStp", 0, {}, &gComm.cmdStopMove}, // - - {"gVer", 0, {}, &gComm.cmdGetVersion}, // - {"gSim", 3, {'X', 'Y', 'Z'}, &gComm.cmdSimulatePos}, // - {"gCrd", 0, {}, &gComm.cmdGetCurrentXYZ}, // - {"gPol", 0, {}, &gComm.cmdGetCurrentPosPol}, // - {"gAng", 0, {}, &gComm.cmdGetCurrentAngle}, // - - {"gSer", 0, {}, &gComm.cmdGetServoAngle}, - {"gIKX", 3, {'X', 'Y', 'Z'}, &gComm.cmdCoordinateToAngle}, // - {"gFKT", 3, {'T', 'L', 'R'}, &gComm.cmdAngleToXYZ}, // - {"gMov", 0, {}, &gComm.cmdIsMoving}, // - {"gTip", 0, {}, &gComm.cmdGetTip}, // - - {"gDig", 1, {'N'}, &gComm.cmdGetDigitValue}, // - {"sDig", 2, {'N', 'V'}, &gComm.cmdSetDigitValue}, // - {"gAna", 1, {'N'}, &gComm.cmdGetAnalogValue}, // - {"gEEP", 2, {'A', 'T'}, &gComm.cmdGetE2PROMData}, // - {"sEEP", 3, {'A', 'T', 'V'}, &gComm.cmdSetE2PROMData}, // - - - {"gGri", 0, {}, &gComm.cmdGetGripperStatus}, // - {"gPum", 0, {}, &gComm.cmdGetPumpStatus}, // +static uArmRingBuffer ringBuffer; -#ifdef MKII - {"gPow", 0, {}, &gComm.cmdGetPowerStatus}, // -#endif +#define RESULT_BUFFER_SIZE 50 - {"gSAD", 1, {'N'}, &gComm.cmdGetServoAnalogData} - - - -}; +#define RING_BUFFER_SIZE 48 +uint8_t bufData[RING_BUFFER_SIZE]; -*/ -uArmComm::uArmComm() +static void replyError(int serialNum, unsigned int errorCode) { - cmdIndex = 0; - mState = IDLE; -} - - + if (serialNum > 0) + { + Serial.print("$"); + Serial.print(serialNum); + Serial.print(" "); + } -void uArmComm::replyError(int serialNum, unsigned int errorCode) -{ - Serial.print("$"); - Serial.print(serialNum); - Serial.print(" E"); + Serial.print("E"); Serial.println(errorCode); } -void uArmComm::replyOK(int serialNum) +static void replyOK(int serialNum) { - Serial.print("$"); - Serial.print(serialNum); - Serial.print(" "); + if (serialNum > 0) + { + Serial.print("$"); + Serial.print(serialNum); + Serial.print(" "); + } Serial.println("OK"); } -void uArmComm::replyResult(int serialNum, String result) +static void replyResult(int serialNum, String result) { - Serial.print("$"); - Serial.print(serialNum); - Serial.print(" OK "); + if (serialNum > 0) + { + Serial.print("$"); + Serial.print(serialNum); + Serial.print(" "); + } + Serial.print("OK "); Serial.println(result); } -void uArmComm::reportResult(int reportCode, String result) +static void reportResult(int reportCode, String result) { + Serial.print("@"); Serial.print(reportCode); Serial.print(" "); Serial.println(result); } -unsigned char uArmComm::cmdMove(int serialNum, int parameterCount, double value[4]) +static unsigned char cmdMove(int serialNum, int parameterCount, double value[4]) { if (parameterCount != 4) return PARAMETER_ERROR; - debugPrint("cmdMove x:%s, y:%s, z:%s, v:%s\n\n", D(value[0]), D(value[1]), D(value[2]), D(value[3])); - - - if (uArm.moveTo(value[0], value[1], value[2], value[3]) != OUT_OF_RANGE_NO_SOLUTION) + if (moveTo(value[0], value[1], value[2], value[3]) != OUT_OF_RANGE_NO_SOLUTION) { replyOK(serialNum); } @@ -132,80 +86,102 @@ unsigned char uArmComm::cmdMove(int serialNum, int parameterCount, double value[ return 0; } -unsigned char uArmComm::cmdMovePol(int serialNum, int parameterCount, double value[4]) +static unsigned char cmdMovePol(int serialNum, int parameterCount, double value[4]) { - double x, y, z; - if (parameterCount != 4) return PARAMETER_ERROR; - - uArm.mController.getXYZFromPolar(x, y, z, value[0], value[1], value[2]); - uArm.moveTo(x, y, z, value[3]); - - replyOK(serialNum); + if (moveToPol(value[0], value[1], value[2], value[3]) != OUT_OF_RANGE_NO_SOLUTION) + { + replyOK(serialNum); + } + else + { + return OUT_OF_RANGE; + } + return 0; } -unsigned char uArmComm::cmdSetAttachServo(int serialNum, int parameterCount, double value[4]) +static unsigned char cmdSetAttachServo(int serialNum, int parameterCount, double value[4]) { if (parameterCount != 1) return PARAMETER_ERROR; - uArm.mController.attachServo(value[0]); - - replyOK(serialNum); - return 0; + if (attachServo(value[0])) + { + replyOK(serialNum); + return 0; + } + else + { + return OUT_OF_RANGE; + } + } -unsigned char uArmComm::cmdSetDetachServo(int serialNum, int parameterCount, double value[4]) +static unsigned char cmdSetDetachServo(int serialNum, int parameterCount, double value[4]) { if (parameterCount != 1) return PARAMETER_ERROR; - uArm.mController.detachServo(value[0]); - replyOK(serialNum); + if (detachServo(value[0])) + { + replyOK(serialNum); + return 0; + } + else + { + return OUT_OF_RANGE; + } return 0; } -unsigned char uArmComm::cmdSetServoAngle(int serialNum, int parameterCount, double value[4]) -{ - if (parameterCount != 2) - return PARAMETER_ERROR; +// static unsigned char cmdSetServoAngle(int serialNum, int parameterCount, double value[4]) +// { +// if (parameterCount != 2) +// return PARAMETER_ERROR; - uArm.mController.writeServoAngle(byte(value[0]), value[1], false); - replyOK(serialNum); +// uArm.mController.writeServoAngle(byte(value[0]), value[1], false); +// replyOK(serialNum); - return 0; -} +// return 0; +// } -unsigned char uArmComm::cmdSetServoAngleWithOffset(int serialNum, int parameterCount, double value[4]) +static unsigned char cmdSetServoAngleWithOffset(int serialNum, int parameterCount, double value[4]) { if (parameterCount != 2) return PARAMETER_ERROR; - uArm.mController.writeServoAngle(byte(value[0]), value[1], true); - replyOK(serialNum); + if (setServoAngle(byte(value[0]), value[1]) == OK) + { + replyOK(serialNum); + return 0; + } + else + { + return PARAMETER_ERROR; + } - return 0; + } -unsigned char uArmComm::cmdSetPump(int serialNum, int parameterCount, double value[4]) +static unsigned char cmdSetPump(int serialNum, int parameterCount, double value[4]) { if (parameterCount != 1) return PARAMETER_ERROR; if (value[0] == 0)//off { - uArm.mController.pumpOff(); + pumpOff(); } else//on { - uArm.mController.pumpOn(); + pumpOn(); } replyOK(serialNum); @@ -213,7 +189,7 @@ unsigned char uArmComm::cmdSetPump(int serialNum, int parameterCount, double val return 0; } -unsigned char uArmComm::cmdSetGripper(int serialNum, int parameterCount, double value[4]) +static unsigned char cmdSetGripper(int serialNum, int parameterCount, double value[4]) { if (parameterCount != 1) @@ -221,11 +197,11 @@ unsigned char uArmComm::cmdSetGripper(int serialNum, int parameterCount, double if (value[0]==0)//release { - uArm.mController.gripperRelease(); + gripperRelease(); } else//catch { - uArm.mController.gripperCatch(); + gripperCatch(); } replyOK(serialNum); @@ -233,37 +209,39 @@ unsigned char uArmComm::cmdSetGripper(int serialNum, int parameterCount, double return 0; } -unsigned char uArmComm::cmdSetBuzz(int serialNum, int parameterCount, double value[4]) +static unsigned char cmdSetBuzz(int serialNum, int parameterCount, double value[4]) { if (parameterCount != 2) return PARAMETER_ERROR; - gBuzzer.buzz(value[0], value[1]*1000); // convert to ms + buzzer.buzz(value[0], value[1]*1000); // convert to ms replyOK(serialNum); return 0; } -unsigned char uArmComm::cmdStopMove(int serialNum, int parameterCount, double value[4]) +#ifdef MKII +static unsigned char cmdStopMove(int serialNum, int parameterCount, double value[4]) { if (parameterCount != 0) return PARAMETER_ERROR; - uArm.stopMove(); + stopMove(); replyOK(serialNum); return 0; } +#endif -unsigned char uArmComm::cmdGetHWVersion(int serialNum, int parameterCount, double value[4]) +static unsigned char cmdGetHWVersion(int serialNum, int parameterCount, double value[4]) { if (parameterCount != 0) return PARAMETER_ERROR; - char result[128]; + char result[RESULT_BUFFER_SIZE]; - ardprintf(result, "V%s", HW_VER); + msprintf(result, "V%s", HW_VER); replyResult(serialNum, result); @@ -271,14 +249,14 @@ unsigned char uArmComm::cmdGetHWVersion(int serialNum, int parameterCount, doubl return 0; } -unsigned char uArmComm::cmdGetSWVersion(int serialNum, int parameterCount, double value[4]) +static unsigned char cmdGetSWVersion(int serialNum, int parameterCount, double value[4]) { if (parameterCount != 0) return PARAMETER_ERROR; - char result[128]; + char result[RESULT_BUFFER_SIZE]; - ardprintf(result, "V%s", SW_VER); + msprintf(result, "V%s", SW_VER); replyResult(serialNum, result); @@ -286,9 +264,9 @@ unsigned char uArmComm::cmdGetSWVersion(int serialNum, int parameterCount, doubl return 0; } -unsigned char uArmComm::cmdSimulatePos(int serialNum, int parameterCount, double value[4]) +static unsigned char cmdSimulatePos(int serialNum, int parameterCount, double value[4]) { - double angleRot, angleLeft, angleRight; + if (parameterCount != 4) return PARAMETER_ERROR; @@ -299,13 +277,13 @@ unsigned char uArmComm::cmdSimulatePos(int serialNum, int parameterCount, double double r = value[1]; double h = value[2]; - uArm.mController.getXYZFromPolar(value[0], value[1], value[2], s, r, h); + polToXYZ(s, r, h, value[0], value[1], value[2]); } - unsigned char status = uArm.mController.coordianteToAngle(value[0], value[1], value[2], angleRot, angleLeft, angleRight, false); + unsigned char status = validatePos(value[0], value[1], value[2]); - char result[128]; + char result[RESULT_BUFFER_SIZE]; switch(status) { case IN_RANGE: @@ -325,111 +303,69 @@ unsigned char uArmComm::cmdSimulatePos(int serialNum, int parameterCount, double return 0; } -unsigned char uArmComm::cmdGetCurrentXYZ(int serialNum, int parameterCount, double value[4]) +static unsigned char cmdGetCurrentXYZ(int serialNum, int parameterCount, double value[4]) { - //char letters[3] = {'X','Y','Z'}; if (parameterCount != 0) return PARAMETER_ERROR; - //debugPrint("cmdGetCurrentXYZ"); - uArm.mController.updateAllServoAngle(); - uArm.mController.getCurrentXYZ(value[0], value[1], value[2]); + getCurrentXYZ(value[0], value[1], value[2]); - char result[128]; - ardprintf(result, "X%f Y%f Z%f", value[0], value[1], value[2]); + char result[RESULT_BUFFER_SIZE]; + msprintf(result, "X%f Y%f Z%f", value[0], value[1], value[2]); replyResult(serialNum, result); return 0; } -unsigned char uArmComm::cmdGetCurrentPosPol(int serialNum, int parameterCount, double value[4]) -{ - double angleRot, angleLeft, angleRight; - double x, y, z; - - if (parameterCount != 0) - return PARAMETER_ERROR; - - - uArm.mController.updateAllServoAngle(); - uArm.mController.getCurrentXYZ(x, y, z); - uArm.mController.getServoAngles(angleRot, angleLeft, angleRight); - double stretch; - stretch = sqrt(x * x + y * y); - //char letters[3] = {'S','R','H'}; - value[0] = stretch; - value[1] = angleRot; - value[2] = z; - //printf(true, value, letters, 3); - - char result[128]; - ardprintf(result, "S%f R%f H%f", value[0], value[1], value[2]); - - replyResult(serialNum, result); - - return 0; -} - -unsigned char uArmComm::cmdGetCurrentAngle(int serialNum, int parameterCount, double value[4]) +static unsigned char cmdGetCurrentPosPol(int serialNum, int parameterCount, double value[4]) { - //char letters[4] = {'B','L','R','H'}; - if (parameterCount != 0) return PARAMETER_ERROR; - value[0] = uArm.mController.readServoAngle(SERVO_ROT_NUM, true); - value[1] = uArm.mController.readServoAngle(SERVO_LEFT_NUM, true); - value[2] = uArm.mController.readServoAngle(SERVO_RIGHT_NUM, true); - value[3] = uArm.mController.readServoAngle(SERVO_HAND_ROT_NUM, true); - //printf(true, value, letters, 4); + getCurrentPosPol(value[0], value[1], value[2]); - char result[128]; - ardprintf(result, "B%f L%f R%f H%f", value[0], value[1], value[2], value[3]); + char result[RESULT_BUFFER_SIZE]; + msprintf(result, "S%f R%f H%f", value[0], value[1], value[2]); replyResult(serialNum, result); return 0; } -unsigned char uArmComm::cmdGetServoAngle(int serialNum, int parameterCount, double value[4]) +static unsigned char cmdGetCurrentAngle(int serialNum, int parameterCount, double value[4]) { - //char letters[4] = {'B','L','R','H'}; - if (parameterCount != 0) return PARAMETER_ERROR; - value[0] = uArm.mController.readServoAngle(SERVO_ROT_NUM, false); - value[1] = uArm.mController.readServoAngle(SERVO_LEFT_NUM, false); - value[2] = uArm.mController.readServoAngle(SERVO_RIGHT_NUM, false); - value[3] = uArm.mController.readServoAngle(SERVO_HAND_ROT_NUM, false); - //printf(true, value, letters, 4); + value[0] = getServoAngle(SERVO_ROT_NUM); + value[1] = getServoAngle(SERVO_LEFT_NUM); + value[2] = getServoAngle(SERVO_RIGHT_NUM); + value[3] = getServoAngle(SERVO_HAND_ROT_NUM); - char result[128]; - ardprintf(result, "B%f L%f R%f H%f", value[0], value[1], value[2], value[3]); + char result[RESULT_BUFFER_SIZE]; + msprintf(result, "B%f L%f R%f H%f", value[0], value[1], value[2], value[3]); replyResult(serialNum, result); return 0; } -unsigned char uArmComm::cmdCoordinateToAngle(int serialNum, int parameterCount, double value[4]) +static unsigned char cmdCoordinateToAngle(int serialNum, int parameterCount, double value[4]) { double rot, left, right; - //bool success; if (parameterCount != 3) return PARAMETER_ERROR; - uArm.mController.coordianteToAngle(value[0], value[1], value[2], rot, left, right); - //char letters[3] = {'T','L','R'}; + xyzToAngle(value[0], value[1], value[2], rot, left, right); + value[0] = rot; value[1] = left; value[2] = right; - //success = true; - //printf(success,value,letters,3); - char result[128]; - ardprintf(result, "B%f L%f R%f", value[0], value[1], value[2]); + + char result[RESULT_BUFFER_SIZE]; + msprintf(result, "B%f L%f R%f", value[0], value[1], value[2]); replyResult(serialNum, result); @@ -437,7 +373,7 @@ unsigned char uArmComm::cmdCoordinateToAngle(int serialNum, int parameterCount, return 0; } -unsigned char uArmComm::cmdAngleToXYZ(int serialNum, int parameterCount, double value[4]) +static unsigned char cmdAngleToXYZ(int serialNum, int parameterCount, double value[4]) { double x, y, z; bool success; @@ -445,7 +381,7 @@ unsigned char uArmComm::cmdAngleToXYZ(int serialNum, int parameterCount, double if (parameterCount != 3) return PARAMETER_ERROR; - if(uArm.mController.getXYZFromAngle(x, y, z, value[0], value[1], value[2]) == OUT_OF_RANGE) + if(angleToXYZ(value[0], value[1], value[2], x, y, z) == OUT_OF_RANGE) { success = false; } @@ -454,26 +390,25 @@ unsigned char uArmComm::cmdAngleToXYZ(int serialNum, int parameterCount, double success = true; } - //char letters[3] = {'X','Y','Z'}; value[0] = x; value[1] = y; value[2] = z; - //printf(success,value,letters,3); - char result[128]; - ardprintf(result, "X%f Y%f Z%f", value[0], value[1], value[2]); + char result[RESULT_BUFFER_SIZE]; + msprintf(result, "X%f Y%f Z%f", value[0], value[1], value[2]); replyResult(serialNum, result); return 0; } -unsigned char uArmComm::cmdIsMoving(int serialNum, int parameterCount, double value[4]) +#ifdef MKII +static unsigned char cmdIsMoving(int serialNum, int parameterCount, double value[4]) { if (parameterCount != 0) return PARAMETER_ERROR; - char result[128]; - if(uArm.isMoving()) + char result[RESULT_BUFFER_SIZE]; + if(isMoving()) { strcpy(result, "V1"); } @@ -486,14 +421,17 @@ unsigned char uArmComm::cmdIsMoving(int serialNum, int parameterCount, double va return 0; } +#endif -unsigned char uArmComm::cmdGetTip(int serialNum, int parameterCount, double value[4]) +static unsigned char cmdGetTip(int serialNum, int parameterCount, double value[4]) { if (parameterCount != 0) return PARAMETER_ERROR; - char result[128]; - if(digitalRead(LIMIT_SW)) + char result[RESULT_BUFFER_SIZE]; + + + if(getTip()) { strcpy(result, "V1"); } @@ -507,175 +445,82 @@ unsigned char uArmComm::cmdGetTip(int serialNum, int parameterCount, double valu return 0; } -unsigned char uArmComm::cmdGetDigitValue(int serialNum, int parameterCount, double value[4]) +static unsigned char cmdGetDigitValue(int serialNum, int parameterCount, double value[4]) { if (parameterCount != 1) return PARAMETER_ERROR; - int val = digitalRead(value[0]); - - //printf(true, val); - char result[128]; - ardprintf(result, "V%d", val); + int val = getDigitalPinValue(value[0]); + + char result[RESULT_BUFFER_SIZE]; + msprintf(result, "V%d", val); replyResult(serialNum, result); return 0; } -unsigned char uArmComm::cmdSetDigitValue(int serialNum, int parameterCount, double value[4]) +static unsigned char cmdSetDigitValue(int serialNum, int parameterCount, double value[4]) { if (parameterCount != 2) return PARAMETER_ERROR; - //Serial.println(SS);// successful feedback send it immediately - // write the digit value - value[1] == 1 ? digitalWrite(value[0], HIGH) : digitalWrite(value[0], LOW); + setDigitalPinValue(value[0], value[1]); replyOK(serialNum); return 0; } -unsigned char uArmComm::cmdGetAnalogValue(int serialNum, int parameterCount, double value[4]) +static unsigned char cmdGetAnalogValue(int serialNum, int parameterCount, double value[4]) { if (parameterCount != 1) return PARAMETER_ERROR; - int val = analogRead(value[0]); + int val = getAnalogPinValue(value[0]); - //printf(true, val); - char result[128]; - ardprintf(result, "V%d", val); + char result[RESULT_BUFFER_SIZE]; + msprintf(result, "V%d", val); replyResult(serialNum, result); return 0; } -unsigned char uArmComm::cmdGetE2PROMData(int serialNum, int parameterCount, double value[4]) +static unsigned char cmdGetE2PROMData(int serialNum, int parameterCount, double value[4]) { if (parameterCount != 3) return PARAMETER_ERROR; - char result[128]; - uint8_t deviceAddr; - uint32_t addr = value[1]; + char result[RESULT_BUFFER_SIZE]; - union { - float fdata; - uint8_t data[4]; - } FData; int device = int(value[0]); int type = value[2]; + uint32_t addr = value[1]; - switch(device) - { - - case 0: - - switch(type) - { - case DATA_TYPE_BYTE: - { - int val = EEPROM.read(addr); - ardprintf(result, "V%d", val); - break; - } - case DATA_TYPE_INTEGER: - { - int i_val = 0; - EEPROM.get(addr, i_val); - ardprintf(result, "V%d", i_val); - //Serial.println("S" + String(i_val) + ""); - break; - } - case DATA_TYPE_FLOAT: - { - double f_val = 0.0f; - EEPROM.get(addr,f_val); - ardprintf(result, "V%f", f_val); - //Serial.println("S" + String(f_val) + ""); - break; - } - } - - break; - - case 1: - deviceAddr = EXTERNAL_EEPROM_USER_ADDRESS; - break; - - case 2: - deviceAddr = EXTERNAL_EEPROM_SYS_ADDRESS; - break; - - default: - return ADDRESS_ERROR; - } + double resultVal = getE2PROMData(device, addr, type); - - if (device == 1 || device == 2) + switch(type) { - int num = 0; - switch(type) + case DATA_TYPE_BYTE: { - case DATA_TYPE_BYTE: - { - num = 1; - break; - } - case DATA_TYPE_INTEGER: - { - num = 2; - break; - } - case DATA_TYPE_FLOAT: - { - num = 4; - break; - } - default: - return PARAMETER_ERROR; + int val = resultVal; + msprintf(result, "V%d", val); + break; } - - unsigned char i=0; - i = (addr % 128); - // Since the eeprom's sector is 128 byte, if we want to write 5 bytes per cycle we need to care about when there's less than 5 bytes left - if (i >= (129-num)) + case DATA_TYPE_INTEGER: { - i = 128 - i; - iic_readbuf(FData.data, deviceAddr, addr, i);// write data - delay(5); - iic_readbuf(FData.data + i, deviceAddr, addr + i, num - i);// write data + int i_val = resultVal; + msprintf(result, "V%d", i_val); + break; } - //if the left bytes are greater than 5, just do it - else + case DATA_TYPE_FLOAT: { - iic_readbuf(FData.data, deviceAddr, addr, num);// write data - } - - switch(type) - { - case DATA_TYPE_BYTE: - { - ardprintf(result, "V%d", FData.data[0]); - break; - } - case DATA_TYPE_INTEGER: - { - ardprintf(result, "V%d", (FData.data[0] << 8) + FData.data[1]); - break; - } - case DATA_TYPE_FLOAT: - { - ardprintf(result, "V%f", FData.fdata); - break; - } + double f_val = resultVal; + msprintf(result, "V%f", f_val); + break; } - - } replyResult(serialNum, result); @@ -684,161 +529,78 @@ unsigned char uArmComm::cmdGetE2PROMData(int serialNum, int parameterCount, doub } -unsigned char uArmComm::cmdSetE2PROMData(int serialNum, int parameterCount, double value[4]) +static unsigned char cmdSetE2PROMData(int serialNum, int parameterCount, double value[4]) { if (parameterCount != 4) return PARAMETER_ERROR; - uint8_t deviceAddr; - uint32_t addr = value[1]; - - union { - float fdata; - uint8_t data[4]; - } FData; int type = value[2]; int device = int(value[0]); + uint32_t addr = value[1]; - switch(device) - { + setE2PROMData(device, addr, type, value[3]); - case 0: - switch(type) - { - case DATA_TYPE_BYTE: - { - byte b_val; - b_val = byte(value[3]); - EEPROM.write(addr, b_val); - break; - } - case DATA_TYPE_INTEGER: - { - int i_val = 0; - i_val = int(value[3]); - EEPROM.put(addr, i_val); - break; - } - case DATA_TYPE_FLOAT: - { - float f_val = 0.0f; - f_val = float(value[3]); - EEPROM.put(addr,f_val); - // Serial.println(f_val); - break; - } - } - break; - case 1: - deviceAddr = EXTERNAL_EEPROM_USER_ADDRESS; - break; + replyOK(serialNum); - case 2: - deviceAddr = EXTERNAL_EEPROM_SYS_ADDRESS; - break; + return 0; - default: - return ADDRESS_ERROR; - } +} +static unsigned char cmdSetButtonService(int serialNum, int parameterCount, double value[4]) +{ + if (parameterCount != 1) + return PARAMETER_ERROR; - if (device == 1 || device == 2) + if (value[0]) { - int num = 0; - switch(type) - { - case DATA_TYPE_BYTE: - { - FData.data[0] = byte(value[3]); - num = 1; - break; - } - case DATA_TYPE_INTEGER: - { - int i_val = 0; - i_val = int(value[3]); - FData.data[0] = (i_val & 0xff00) >> 8; - FData.data[1] = i_val & 0xff; - num = 2; - break; - } - case DATA_TYPE_FLOAT: - { - FData.fdata = float(value[3]); - num = 4; - break; - } - default: - return PARAMETER_ERROR; - } - - unsigned char i=0; - i = (addr % 128); - // Since the eeprom's sector is 128 byte, if we want to write 5 bytes per cycle we need to care about when there's less than 5 bytes left - if (i >= (129-num)) - { - i = 128 - i; - iic_writebuf(FData.data, deviceAddr, addr, i);// write data - delay(5); - iic_writebuf(FData.data + i, deviceAddr, addr + i, num - i);// write data - } - //if the left bytes are greater than 5, just do it - else - { - iic_writebuf(FData.data, deviceAddr, addr, num);// write data - } - - - - + service.setButtonService(true); + } + else + { + service.setButtonService(false); } replyOK(serialNum); - return 0; - } -unsigned char uArmComm::cmdGetGripperStatus(int serialNum, int parameterCount, double value[4]) +static unsigned char cmdGetGripperStatus(int serialNum, int parameterCount, double value[4]) { if (parameterCount != 0) return PARAMETER_ERROR; - unsigned char status = uArm.mController.gripperStatus(); + unsigned char status = getGripperStatus(); - //String ret = "[S" + String(status) + "]"; - - //Serial.println(ret); - char result[128]; - ardprintf(result, "V%d", status); + char result[RESULT_BUFFER_SIZE]; + msprintf(result, "V%d", status); replyResult(serialNum, result); return 0; } -unsigned char uArmComm::cmdGetPumpStatus(int serialNum, int parameterCount, double value[4]) + + + +static unsigned char cmdGetPumpStatus(int serialNum, int parameterCount, double value[4]) { if (parameterCount != 0) return PARAMETER_ERROR; - char result[128]; -#ifdef MKII - + char result[RESULT_BUFFER_SIZE]; - unsigned char status = uArm.mController.pumpStatus(); - - //String ret = "[S" + String(status) + "]"; + unsigned char status = getPumpStatus(); - //Serial.println(ret); - ardprintf(result, "V%d", status); +#ifdef MKII + + msprintf(result, "V%d", status); #elif defined(METAL) - if (uArm.mController.pumpStatus()) + if (status) strcpy(result, "V1"); else strcpy(result, "V0"); @@ -852,14 +614,14 @@ unsigned char uArmComm::cmdGetPumpStatus(int serialNum, int parameterCount, doub #ifdef MKII -unsigned char uArmComm::cmdGetPowerStatus(int serialNum, int parameterCount, double value[4]) +static unsigned char cmdGetPowerStatus(int serialNum, int parameterCount, double value[4]) { if (parameterCount != 0) return PARAMETER_ERROR; - char result[128]; + char result[RESULT_BUFFER_SIZE]; - if (uArm.isPowerPlugIn()) + if (isPowerPlugIn()) strcpy(result, "V1"); else strcpy(result, "V0"); @@ -870,40 +632,30 @@ unsigned char uArmComm::cmdGetPowerStatus(int serialNum, int parameterCount, dou } #endif -unsigned char uArmComm::cmdGetServoAnalogData(int serialNum, int parameterCount, double value[4]) -{ - if (parameterCount != 1) - return PARAMETER_ERROR; +// static unsigned char cmdGetServoAnalogData(int serialNum, int parameterCount, double value[4]) +// { +// if (parameterCount != 1) +// return PARAMETER_ERROR; - unsigned int data = uArm.mController.getServoAnalogData(value[0]); - //Serial.println(data); - char result[128]; +// unsigned int data = uArm.mController.getServoAnalogData(value[0]); +// //Serial.println(data); +// char result[RESULT_BUFFER_SIZE]; - ardprintf(result, "V%d", result); +// msprintf(result, "V%d", result); - replyResult(serialNum, result); +// replyResult(serialNum, result); - return 0; -} +// return 0; +// } -unsigned char uArmComm::cmdRelativeMove(int serialNum, int parameterCount, double value[4]) +static unsigned char cmdRelativeMove(int serialNum, int parameterCount, double value[4]) { if (parameterCount != 4) return PARAMETER_ERROR; - debugPrint("cmdRelativeMove x:%s, y:%s, z:%s, v:%s\n\n", D(value[0]), D(value[1]), D(value[2]), D(value[3])); - - double x, y, z; - // get cur xyz - uArm.mController.getCurrentXYZ(x, y, z); - - value[0] += x; - value[1] += y; - value[2] += z; - - if (uArm.moveTo(value[0], value[1], value[2], value[3]) != OUT_OF_RANGE_NO_SOLUTION) + if (relativeMove(value[0], value[1], value[2], value[3]) != OUT_OF_RANGE_NO_SOLUTION) { replyOK(serialNum); } @@ -916,13 +668,13 @@ unsigned char uArmComm::cmdRelativeMove(int serialNum, int parameterCount, doubl } -unsigned char uArmComm::cmdSetReportInterval(int serialNum, int parameterCount, double value[4]) +static unsigned char cmdSetReportInterval(int serialNum, int parameterCount, double value[4]) { if (parameterCount != 1) return PARAMETER_ERROR; - uArm.setReportInterval(value[0]*1000); + service.setReportInterval(value[0]*1000); replyOK(serialNum); @@ -930,14 +682,14 @@ unsigned char uArmComm::cmdSetReportInterval(int serialNum, int parameterCount, return 0; } -unsigned char uArmComm::cmdGetDeviceName(int serialNum, int parameterCount, double value[4]) +static unsigned char cmdGetDeviceName(int serialNum, int parameterCount, double value[4]) { if (parameterCount != 0) return PARAMETER_ERROR; - char result[128]; + char result[RESULT_BUFFER_SIZE]; - ardprintf(result, "V%s", DEVICE_NAME); + msprintf(result, "V%s", DEVICE_NAME); replyResult(serialNum, result); @@ -945,14 +697,14 @@ unsigned char uArmComm::cmdGetDeviceName(int serialNum, int parameterCount, doub return 0; } -unsigned char uArmComm::cmdGetAPIVersion(int serialNum, int parameterCount, double value[4]) +static unsigned char cmdGetAPIVersion(int serialNum, int parameterCount, double value[4]) { if (parameterCount != 0) return PARAMETER_ERROR; - char result[128]; + char result[RESULT_BUFFER_SIZE]; - ardprintf(result, "V%s", SW_VER); + msprintf(result, "V%s", SW_VER); replyResult(serialNum, result); @@ -960,13 +712,13 @@ unsigned char uArmComm::cmdGetAPIVersion(int serialNum, int parameterCount, doub return 0; } -unsigned char uArmComm::cmdGetDeviceUUID(int serialNum, int parameterCount, double value[4]) +static unsigned char cmdGetDeviceUUID(int serialNum, int parameterCount, double value[4]) { if (parameterCount != 0) return PARAMETER_ERROR; - char result[128]; + char result[RESULT_BUFFER_SIZE]; strcpy(result, "V1234567890"); @@ -975,104 +727,30 @@ unsigned char uArmComm::cmdGetDeviceUUID(int serialNum, int parameterCount, doub return 0; } - -void uArmComm::reportPos() +void reportButtonEvent(unsigned char buttonId, unsigned char event) { - double x, y, z; - - uArm.mController.updateAllServoAngle(); - uArm.mController.getCurrentXYZ(x, y, z); - - char result[128]; - ardprintf(result, "X%f Y%f Z%f", x, y, z); - - reportResult(REPORT_POS, result); - -} - -/* -char uArmComm::parseParam(String cmnd, const char *parameters, int parameterCount, double valueArray[]) -{ - for (byte i = 0; i < parameterCount; i++) - { - char startIndex = cmnd.indexOf(parameters[i]) + 1; - //debugPrint("startIndex = %d", startIndex); - if (startIndex == -1) - { - //Serial.println(F("[F1]")); - return ERR1; - } - - char endIndex = 0; - if (i != parameterCount-1) - { - endIndex = cmnd.indexOf(parameters[i+1]); - if (endIndex == -1) - { - //Serial.println(F("[F1]")); - return ERR1; - } - } - else - { - endIndex = cmnd.length(); - } - - valueArray[i] = cmnd.substring(startIndex, endIndex).toFloat(); - - } - - - - return OK; + char result[RESULT_BUFFER_SIZE]; + msprintf(result, "B%d V%d", buttonId, event); + reportResult(REPORT_BUTTON, result); } -void uArmComm::runCommand(String message) +void reportPos() { - - String cmdStr = message.substring(0, 4); - double value[4]; - int i = 0; + double x, y, z, frontEndAngle; - Command iCmd; + getCurrentXYZ(x, y, z); + frontEndAngle = getServoAngle(SERVO_HAND_ROT_NUM); - int len = sizeof(command)/sizeof(command[0]); + debugPrint("angle = %f", frontEndAngle); + char result[RESULT_BUFFER_SIZE]; + msprintf(result, "X%f Y%f Z%f R%f", x, y, z, frontEndAngle); - for (i = 0; i < len; i++) - { - memcpy_P(&iCmd, &command[i], sizeof(Command)); - - if (cmdStr == iCmd.cmd) - { - - if (iCmd.parametersCount == 0) - { - iCmd.execute(value); - } - else if (parseParam(message, iCmd.parameters, iCmd.parametersCount, value) == OK) - { - iCmd.execute(value); - } - else - { - Serial.println("[ERR1]"); - } - break; - } - - } + reportResult(REPORT_POS, result); - if (i == len) - { - Serial.println("[ERR3]"); - } - } -*/ - -void uArmComm::HandleMoveCmd(int cmdCode, int serialNum, int parameterCount, double value[4]) +static void HandleMoveCmd(int cmdCode, int serialNum, int parameterCount, double value[4]) { unsigned char result = false; @@ -1090,9 +768,11 @@ void uArmComm::HandleMoveCmd(int cmdCode, int serialNum, int parameterCount, dou result = cmdSetServoAngleWithOffset(serialNum, parameterCount, value); break; +#ifdef MKII case 203: result = cmdStopMove(serialNum, parameterCount, value); break; +#endif case 204: result = cmdRelativeMove(serialNum, parameterCount, value); @@ -1113,7 +793,7 @@ void uArmComm::HandleMoveCmd(int cmdCode, int serialNum, int parameterCount, dou } } -void uArmComm::HandleSettingCmd(int cmdCode, int serialNum, int parameterCount, double value[4]) +static void HandleSettingCmd(int cmdCode, int serialNum, int parameterCount, double value[4]) { unsigned char result = false; @@ -1123,9 +803,11 @@ void uArmComm::HandleSettingCmd(int cmdCode, int serialNum, int parameterCount, result = cmdSetReportInterval(serialNum, parameterCount, value); break; +#ifdef MKII case 200: result = cmdIsMoving(serialNum, parameterCount, value); break; +#endif case 201: result = cmdSetAttachServo(serialNum, parameterCount, value); @@ -1137,7 +819,7 @@ void uArmComm::HandleSettingCmd(int cmdCode, int serialNum, int parameterCount, case 210: result = cmdSetBuzz(serialNum, parameterCount, value); - break; + break; case 211: result = cmdGetE2PROMData(serialNum, parameterCount, value); @@ -1147,6 +829,10 @@ void uArmComm::HandleSettingCmd(int cmdCode, int serialNum, int parameterCount, result = cmdSetE2PROMData(serialNum, parameterCount, value); break; + case 213: + result = cmdSetButtonService(serialNum, parameterCount, value); + break; + case 220: result = cmdCoordinateToAngle(serialNum, parameterCount, value); break; @@ -1188,7 +874,7 @@ void uArmComm::HandleSettingCmd(int cmdCode, int serialNum, int parameterCount, } } -void uArmComm::HandleQueryCmd(int cmdCode, int serialNum, int parameterCount, double value[4]) +static void HandleQueryCmd(int cmdCode, int serialNum, int parameterCount, double value[4]) { unsigned char result = false; @@ -1257,12 +943,12 @@ void uArmComm::HandleQueryCmd(int cmdCode, int serialNum, int parameterCount, do } } -bool uArmComm::parseCommand(char *message) +static bool parseCommand(char *message) { double value[6]; int index = 0; - - // Serial.println(message); + bool hasSerialNum = false; + debugPrint("message=%s", message); int len = strlen(message); @@ -1270,6 +956,7 @@ bool uArmComm::parseCommand(char *message) char *pch; char cmdType; + // skip white space while (len > 0) { if (isspace(message[len-1])) @@ -1289,20 +976,34 @@ bool uArmComm::parseCommand(char *message) + + + if (message[0] == '#') + { + hasSerialNum = true; + } + pch = strtok(message, " "); while (pch != NULL && index < 6) { - // Serial.println(pch); + //debugPrint("pch=%s", pch); switch (index) { case 0: - value[index] = atof(pch); + if (!hasSerialNum) + { + cmdType = *(pch); + } + value[index] = atof(pch+1); break; case 1: - cmdType = *(pch); - // Serial.println(cmdType); + if (hasSerialNum) + { + cmdType = *(pch); + } + //debugPrint("cmdType=%d", cmdType); value[index] = atof(pch+1); break; @@ -1312,8 +1013,7 @@ bool uArmComm::parseCommand(char *message) } - // Serial.print("value="); - // Serial.println(value[index]); + //debugPrint("value=%f", value[index]); pch = strtok(NULL, " "); @@ -1322,41 +1022,66 @@ bool uArmComm::parseCommand(char *message) } - int serialNum = value[0]; - int cmdCode = value[1]; - int parameterCount = index - 2; + int serialNum = 0; + int cmdCode = 0; + int parameterCount = 0; + int valueStartIndex = 0; + + if (hasSerialNum) + { + serialNum = value[0]; + cmdCode = value[1]; + parameterCount = index - 2; + valueStartIndex = 2; + } + else + { + serialNum = 0; + cmdCode = value[0]; + parameterCount = index - 1; + valueStartIndex = 1; + } switch (cmdType) { case 'G': - HandleMoveCmd(cmdCode, serialNum, parameterCount, value+2); + HandleMoveCmd(cmdCode, serialNum, parameterCount, value + valueStartIndex); break; case 'M': - HandleSettingCmd(cmdCode, serialNum, parameterCount, value+2); + HandleSettingCmd(cmdCode, serialNum, parameterCount, value + valueStartIndex); break; case 'P': - HandleQueryCmd(cmdCode, serialNum, parameterCount, value+2); + HandleQueryCmd(cmdCode, serialNum, parameterCount, value + valueStartIndex); break; } - - - } -void uArmComm::handleSerialData(char data) +static void handleSerialData(char data) { - switch (mState) + static unsigned char cmdCount = 0; + + + switch (commState) { case IDLE: - if (data == '#') + if (data == '#' || data == 'G' || data == 'M' || data == 'P') { - mState = START; + commState = START; cmdIndex = 0; + if (data != '#') + { + cmdCount = 1; // get cmd code + } + else + { + cmdCount = 0; + } + cmdReceived[cmdIndex++] = data; } break; @@ -1365,12 +1090,26 @@ void uArmComm::handleSerialData(char data) if (cmdIndex >= COM_LEN_MAX) { - mState = IDLE; + commState = IDLE; } - else if (data == '#') + else if (data == '#') // restart { - cmdIndex = 0; + cmdCount = 0; + cmdReceived[cmdIndex++] = data; + } + else if (data == 'G' || data == 'M' || data == 'P') + { + if (cmdCount >= 1) // restart + { + cmdIndex = 0; + cmdReceived[cmdIndex++] = data; + } + else + { + cmdCount++; + cmdReceived[cmdIndex++] = data; + } } else if (data == '\n') { @@ -1378,7 +1117,7 @@ void uArmComm::handleSerialData(char data) cmdReceived[cmdIndex] = '\0'; parseCommand((char*)cmdReceived); - mState = IDLE; + commState = IDLE; } else { @@ -1388,13 +1127,13 @@ void uArmComm::handleSerialData(char data) break; default: - mState = IDLE; + commState = IDLE; break; } } -void uArmComm::SerialCmdRun() +void serialCmdRun() { char data = -1; @@ -1414,48 +1153,33 @@ void uArmComm::SerialCmdRun() } } -void uArmComm::run() +void getSerialCmd() { - SerialCmdRun(); -} + int data = -1; -/* -void uArmComm::printf(bool success, double *dat, char *letters, unsigned char num) -{ - if(success == true) - Serial.print(F("[S")); - else - Serial.print(F("[F")); - //print the parameter - for(unsigned char i = 0; i < num; i++) + while (Serial.available()) { - Serial.print(letters[i]); - Serial.print(dat[i]); - } - Serial.println(F("]")); + data = Serial.read(); + if (data != -1) + { + ringBuffer.put((uint8_t)data); + } + } } -void uArmComm::printf(bool success, double dat) +void handleSerialCmd() { - if(success == true) - Serial.print(F("[S")); - else - Serial.print(F("[F")); - - Serial.print(dat); - Serial.println(F("]")); + uint8_t data = 0; + while (ringBuffer.get(&data)) + { + handleSerialData(data); + } } -void uArmComm::printf(bool success, int dat) +void serialCmdInit() { - if(success == true) - Serial.print(F("[S")); - else - Serial.print(F("[F")); - - Serial.print(dat); - Serial.println(F("]")); + ringBuffer.init(bufData, RING_BUFFER_SIZE); } -*/ + diff --git a/src/uArmComm.h b/src/uArmComm.h index 6f6452f..25aaba6 100644 --- a/src/uArmComm.h +++ b/src/uArmComm.h @@ -16,7 +16,12 @@ #include "uArm.h" #include "uArmBuzzer.h" -#define COM_LEN_MAX 60 +#define COM_LEN_MAX 48 + +#define OUT_OF_RANGE 10 +#define NO_SUCH_CMD 20 +#define PARAMETER_ERROR 21 +#define ADDRESS_ERROR 22 enum CommState { @@ -29,95 +34,18 @@ enum CommState }; -#define OUT_OF_RANGE 10 -#define NO_SUCH_CMD 20 -#define PARAMETER_ERROR 21 -#define ADDRESS_ERROR 22 - -#define REPORT_POS 3 - -class uArmComm -{ - - - -public: - uArmComm(); - - unsigned char cmdMove(int serialNum, int parameterCount, double value[4]); - unsigned char cmdMovePol(int serialNum, int parameterCount, double value[4]); - unsigned char cmdSetAttachServo(int serialNum, int parameterCount, double value[4]); - unsigned char cmdSetDetachServo(int serialNum, int parameterCount, double value[4]); - unsigned char cmdSetServoAngle(int serialNum, int parameterCount, double value[4]); - - unsigned char cmdSetServoAngleWithOffset(int serialNum, int parameterCount, double value[4]); - unsigned char cmdSetPump(int serialNum, int parameterCount, double value[4]); - unsigned char cmdSetGripper(int serialNum, int parameterCount, double value[4]); - unsigned char cmdSetBuzz(int serialNum, int parameterCount, double value[4]); - unsigned char cmdStopMove(int serialNum, int parameterCount, double value[4]); - - unsigned char cmdGetHWVersion(int serialNum, int parameterCount, double value[4]); - unsigned char cmdGetSWVersion(int serialNum, int parameterCount, double value[4]); - unsigned char cmdSimulatePos(int serialNum, int parameterCount, double value[4]); - unsigned char cmdGetCurrentXYZ(int serialNum, int parameterCount, double value[4]); - unsigned char cmdGetCurrentPosPol(int serialNum, int parameterCount, double value[4]); - unsigned char cmdGetCurrentAngle(int serialNum, int parameterCount, double value[4]); - - unsigned char cmdGetServoAngle(int serialNum, int parameterCount, double value[4]); - unsigned char cmdCoordinateToAngle(int serialNum, int parameterCount, double value[4]); - unsigned char cmdAngleToXYZ(int serialNum, int parameterCount, double value[4]); - unsigned char cmdIsMoving(int serialNum, int parameterCount, double value[4]); - unsigned char cmdGetTip(int serialNum, int parameterCount, double value[4]); - - unsigned char cmdGetDigitValue(int serialNum, int parameterCount, double value[4]); - unsigned char cmdSetDigitValue(int serialNum, int parameterCount, double value[4]); - unsigned char cmdGetAnalogValue(int serialNum, int parameterCount, double value[4]); - unsigned char cmdGetE2PROMData(int serialNum, int parameterCount, double value[4]); - unsigned char cmdSetE2PROMData(int serialNum, int parameterCount, double value[4]); +#define REPORT_POS 3 +#define REPORT_BUTTON 4 - unsigned char cmdGetGripperStatus(int serialNum, int parameterCount, double value[4]); +void reportPos(); - - unsigned char cmdGetPumpStatus(int serialNum, int parameterCount, double value[4]); -#ifdef MKII - unsigned char cmdGetPowerStatus(int serialNum, int parameterCount, double value[4]); -#endif +void reportButtonEvent(unsigned char buttonId, unsigned char event); - unsigned char cmdGetServoAnalogData(int serialNum, int parameterCount, double value[4]); +void serialCmdInit(); - unsigned char cmdRelativeMove(int serialNum, int parameterCount, double value[4]); +void getSerialCmd(); - unsigned char cmdSetReportInterval(int serialNum, int parameterCount, double value[4]); - unsigned char cmdGetDeviceName(int serialNum, int parameterCount, double value[4]); - unsigned char cmdGetAPIVersion(int serialNum, int parameterCount, double value[4]); - unsigned char cmdGetDeviceUUID(int serialNum, int parameterCount, double value[4]); +void handleSerialCmd(); - void reportPos(); - - void run(); - - bool parseCommand(char *message); - char parseParam(String cmnd, const char *parameters, int parameterCount, double valueArray[]); - void runCommand(String message); - void SerialCmdRun(); - void handleSerialData(char data); - - void printf(bool success, double *dat, char *letters, unsigned char num); - void printf(bool success, double dat); - void printf(bool success, int dat); - - void HandleMoveCmd(int cmdCode, int serialNum, int parameterCount, double value[4]); - void HandleSettingCmd(int cmdCode, int serialNum, int parameterCount, double value[4]); - void HandleQueryCmd(int cmdCode, int serialNum, int parameterCount, double value[4]); - - void replyOK(int serialNum); - void replyError(int serialNum, unsigned int errorCode); - void replyResult(int serialNum, String result); - void reportResult(int reportCode, String result); -private: - CommState mState; - unsigned char cmdReceived[COM_LEN_MAX]; - unsigned char cmdIndex; -}; #endif // _UARMCOMM_H_ diff --git a/src/uArmConfig.cpp b/src/uArmConfig.cpp deleted file mode 100644 index f85bfe3..0000000 --- a/src/uArmConfig.cpp +++ /dev/null @@ -1,155 +0,0 @@ -/** - ****************************************************************************** - * @file uArmConfig.cpp - * @author David.Long - * @email xiaokun.long@ufactory.cc - * @date 2016-09-28 - * @license GNU - * copyright(c) 2016 UFactory Team. All right reserved - ****************************************************************************** - */ - -#include "uArmConfig.h" -#include - -#define ARDBUFFER 50 - - - - -int ardprintf(char *result, char *str, ...) -{ - int i, count=0, j=0, flag=0; - char temp[ARDBUFFER+1]; - char num[ARDBUFFER+1]; - for(i=0; str[i]!='\0';i++) if(str[i]=='%') count++; - - result[0] = '\0'; - va_list argv; - va_start(argv, count); - for(i=0,j=0; str[i]!='\0';i++) - { - if(str[i]=='%') - { - temp[j] = '\0'; - strcat(result, temp); - j=0; - temp[0] = '\0'; - - switch(str[++i]) - { - case 'd': - { - - itoa(va_arg(argv, int), num, 10); - strcat(result, num); - break; - } - - case 'l': ltoa(va_arg(argv, long), num, 10); - strcat(result, num); - break; - - case 'f': - { - char d_str[10]; - dtostrf(va_arg(argv, double), 4, 2, d_str); - strcat(result, d_str); - break; - } - - case 'c': num[0] = (char)va_arg(argv, int); - num[1] = '\0'; - strcat(result, num); - break; - case 's': strcat(result, va_arg(argv, char *)); - break; - default: ; - }; - } - else - { - temp[j] = str[i]; - j = (j+1)%ARDBUFFER; - if(j==0) - { - temp[ARDBUFFER] = '\0'; - strcat(result, temp); - temp[0]='\0'; - } - } - }; - - return count + 1; -} - - - -#ifdef DEBUG - - - -#define PRINT_BUF 128 - - - -// convert double value to string -char* D(double value) -{ - - static char d_str[5][7] = {0}; - - static unsigned char d_index = 0; - - d_index++; - if (d_index >= 5) - d_index = 0; - - dtostrf(value, 4, 2, d_str[d_index]); - return d_str[d_index]; -} - - - -void dprint(char *fmt, ...) -{ - char buf[PRINT_BUF]; - - va_list args; - va_start(args, fmt); - vsnprintf(buf, PRINT_BUF, fmt, args); - va_end(args); - Serial.println(buf); -} - - -#ifdef F - -void dprint(const __FlashStringHelper *fmt, ...) -{ - char buf[PRINT_BUF]; - - va_list args; - va_start(args, fmt); -#ifdef __AVR__ - vsnprintf_P(buf, sizeof(buf), (const char *)fmt, args); // progmem for AVR -#else - vsnprintf(buf, sizeof(buf), (const char *)fmt, args); // for the rest of the world -#endif - va_end(args); - Serial.println(buf); -} - -#endif // F - - -#else - -char* D(double value) -{ - - - return NULL; -} - -#endif // DEBUG \ No newline at end of file diff --git a/src/uArmConfig.h b/src/uArmConfig.h index 96d2780..bd0f754 100644 --- a/src/uArmConfig.h +++ b/src/uArmConfig.h @@ -14,28 +14,26 @@ #include -int ardprintf(char *result, char *str, ...); -// #define MKII + +//#define MKII #define METAL -//#define DEBUG +//#define DEBUG // uncomment if you want to print debug info +#define DebugSerial Serail // usr serial0 as debug port +//#define METAL_MOTOR // use servos made of metal -//#define METAL_MOTOR -#ifdef DEBUG - #define debugPrint dprint -#else - #define debugPrint -#endif #ifdef MKII #define HW_VER "3.1" - #define SW_VER "2.2.1" + #define SW_VER "2.2.2" #elif defined(METAL) #define HW_VER "2.1" - #define SW_VER "2.2.1" + #define SW_VER "2.2.2" +#else + #error "NO machine model defined(METAL, MKII)" #endif #ifdef METAL @@ -49,20 +47,7 @@ int ardprintf(char *result, char *str, ...); #define TICK_INTERVAL 50 // ms -// conver double value to string -char* D(double value); - -#ifdef DEBUG - -void dprint(char *fmt, ...); - -#ifdef F -void dprint(const __FlashStringHelper *fmt, ...); -#endif - -#else -#endif // DEBUG #endif // _UARMCONFIG_H_ diff --git a/src/uArmController.cpp b/src/uArmController.cpp index 9efe1fc..d04bacf 100644 --- a/src/uArmController.cpp +++ b/src/uArmController.cpp @@ -12,10 +12,13 @@ #include "uArmController.h" #include "uArmIIC.h" #include "uArmRecorder.h" +#include "uArmAPI.h" + +uArmController controller; uArmController::uArmController() { - + mServoSpeed = 255; } void uArmController::init() @@ -33,7 +36,7 @@ void uArmController::init() } mServoAngleOffset[i] = servoOffset; - debugPrint("offset[%d]: %s\n", i, D(mServoAngleOffset[i])); + } // mServoAngleOffset[0] = 6; @@ -112,7 +115,7 @@ void uArmController::writeServoAngle(double servoRotAngle, double servoLeftAngle double uArmController::getReverseServoAngle(byte servoNum, double servoAngle) { #ifdef MKII - debugPrint("s=%d, angleIn=%s", servoNum, D(servoAngle)); + if (servoAngle < mCurAngle[servoNum]) { unsigned char data[2]; @@ -122,11 +125,10 @@ double uArmController::getReverseServoAngle(byte servoNum, double servoAngle) addr &= 0xfffe; - //gRecorder.read(addr, data, 2); iic_readbuf(data, EXTERNAL_EEPROM_SYS_ADDRESS, addr, 2); i_val = (data[0] << 8) + data[1]; - debugPrint("addr=%d,i_val=%d", addr, i_val); + servoAngle = servoAngle - (i_val) - 1; } #endif @@ -144,7 +146,6 @@ void uArmController::writeServoAngle(byte servoNum, double servoAngle, boolean w mCurAngle[servoNum] = servoAngle; servoAngle = writeWithOffset ? (servoAngle + mServoAngleOffset[servoNum]) : servoAngle; - debugPrint("serveAngle1=%s", D(servoAngle)); #ifdef MKII servoAngle = getReverseServoAngle(servoNum, servoAngle); @@ -165,9 +166,9 @@ void uArmController::writeServoAngle(byte servoNum, double servoAngle, boolean w } #endif - debugPrint("serveAngle2=%s", D(servoAngle)); - mServo[servoNum].write(servoAngle); + + mServo[servoNum].write(servoAngle, mServoSpeed); } @@ -199,13 +200,6 @@ void uArmController::readServoCalibrationData(unsigned int address, double& angl min_data_calibration_address = (((unsigned int)angle - (DATA_LENGTH >> 2)) * 2); } - // Serial.print("input"); - // Serial.println(angle); - // Serial.print("min_data_calibration_address:"); - // Serial.println(min_data_calibration_address); - - //if (min_data_calibration_address < 0) - // min_data_calibration_address = 0; unsigned char dataLen = DATA_LENGTH; if (min_data_calibration_address + dataLen > 360) @@ -213,8 +207,7 @@ void uArmController::readServoCalibrationData(unsigned int address, double& angl dataLen = 360 - min_data_calibration_address; } - // Serial.print("dataLen:"); - // Serial.println(dataLen); + iic_readbuf(calibration_data, EXTERNAL_EEPROM_SYS_ADDRESS, address + min_data_calibration_address, dataLen); @@ -237,8 +230,7 @@ void uArmController::readServoCalibrationData(unsigned int address, double& angl { another_closest_data = ((calibration_data[i_min+i_min+2]<<8) + calibration_data[3+i_min+i_min])/10.0;//bigger than closest - // Serial.print("another_closest_data1:"); - // Serial.println(another_closest_data); + if(abs(another_closest_data - closest_data) < 0.001) { @@ -259,8 +251,7 @@ void uArmController::readServoCalibrationData(unsigned int address, double& angl if (i_min > 0) { another_closest_data = ((calibration_data[i_min+i_min-2]<<8) + calibration_data[i_min+i_min-1])/10.0;//smaller than closest - // Serial.print("another_closest_data2:"); - // Serial.println(another_closest_data); + if(abs(another_closest_data - closest_data) < 0.001) { @@ -277,9 +268,7 @@ void uArmController::readServoCalibrationData(unsigned int address, double& angl } } - //angle += 1.0; - // Serial.print("output angle:"); - // Serial.println(angle); + } @@ -342,110 +331,6 @@ void uArmController::updateAllServoAngle(boolean withOffset) } -void uArmController::gripperCatch() -{ - digitalWrite(GRIPPER, LOW); -} - -void uArmController::gripperRelease() -{ - digitalWrite(GRIPPER, HIGH); -} - -unsigned char uArmController::gripperStatus() -{ -#ifdef MKII - //Serial.println(getAnalogData(GRIPPER_FEEDBACK)); - if (digitalRead(GRIPPER) == HIGH) - { - return STOP;//NOT WORKING - } - else - { - - if (getAnalogData(GRIPPER_FEEDBACK) > 600) - { - return WORKING; - } - else - { - return GRABBING; - } - } -#elif defined(METAL) - if(digitalRead(GRIPPER) == HIGH) - { - return STOP; - } - else - { - if(getAnalogData(GRIPPER_FEEDBACK) > 600) - { - return WORKING; - } - else - { - return GRABBING; - } - } -#endif -} - - -unsigned char uArmController::pumpStatus() -{ -#ifdef MKII - if (digitalRead(PUMP_EN) == HIGH) - { - return STOP; - } - else - { - //Serial.println(getAnalogData(PUMP_FEEDBACK)); - if (getAnalogData(PUMP_FEEDBACK) <= PUMP_GRABBING_CURRENT) - { - return GRABBING; - } - else - { - return WORKING; - } - } -#elif defined (METAL) - if (digitalRead(PUMP_EN) == HIGH) - { - return 1; - } - else - { - return 0; - } -#endif -} - - -void uArmController::pumpOn() -{ - #ifdef MKII - digitalWrite(PUMP_EN, LOW); - #elif defined(METAL) - digitalWrite(PUMP_EN, HIGH); - digitalWrite(VALVE_EN, LOW); - #endif -} - -void uArmController::pumpOff() -{ - - - #ifdef MKII - digitalWrite(PUMP_EN, HIGH); - #elif defined(METAL) - digitalWrite(PUMP_EN, LOW); - digitalWrite(VALVE_EN, HIGH); - #endif -} - unsigned char uArmController::getCurrentXYZ(double& x, double& y, double& z) { @@ -461,14 +346,14 @@ unsigned char uArmController::getCurrentXYZ(double& x, double& y, double& z) return IN_RANGE; } -unsigned char uArmController::getXYZFromPolar(double& x, double& y, double& z, double s, double r, double h) -{ - double stretch = s; +// unsigned char uArmController::getXYZFromPolar(double& x, double& y, double& z, double s, double r, double h) +// { +// double stretch = s; - z = h; - x = s * cos(r / MATH_TRANS); - y = s * sin(r / MATH_TRANS); -} +// z = h; +// x = s * cos(r / MATH_TRANS); +// y = s * sin(r / MATH_TRANS); +// } unsigned char uArmController::getXYZFromAngle(double& x, double& y, double& z, double rot, double left, double right) @@ -485,7 +370,7 @@ unsigned char uArmController::getXYZFromAngle(double& x, double& y, double& z, d return IN_RANGE; } -unsigned char uArmController::coordianteToAngle(double x, double y, double z, double& angleRot, double& angleLeft, double& angleRight, boolean allowApproximate) +unsigned char uArmController::xyzToAngle(double x, double y, double z, double& angleRot, double& angleLeft, double& angleRight, boolean allowApproximate) { double xIn = 0.0; double zIn = 0.0; @@ -551,7 +436,7 @@ unsigned char uArmController::coordianteToAngle(double x, double y, double z, do angleRight = angleRight - phi; - //debugPrint("angle r=%s, l=%s, r=%s", D(angleRot), D(angleLeft), D(angleRight)); + //determine if the angle can be reached return limitRange(angleRot, angleLeft, angleRight); } @@ -686,15 +571,11 @@ double uArmController::analogToAngle(byte servoNum, int inputAnalog) angleValue = (inputAnalog - min) * (angleMax - angleMin) / (max - min) + angleMin ; - // Serial.print("angleValue="); - // Serial.println(angleValue); + return angleValue; - // els - // { - // return 0; - // } + /* unsigned char adc_calibration_data[DATA_LENGTH],data[4]; //get the calibration data around the data input @@ -792,7 +673,7 @@ double uArmController::analogToAngle(byte servoNum, int inputAnalog) angle = (angle_range_min + angle_range_max) / 2.0;//angle from 1-180 but the address from 0-179 } - //debugPrint("angle=%s", D(angle)); + return angle; @@ -814,66 +695,18 @@ double uArmController::analogToAngle(byte servoNum, int inputAnalog) double angle = intercept + slope * inputAnalog; - //debugPrint("analogToAngle: inter:%s, slo:%s, inpu:%d, angle:%s", D(intercept), D(slope), inputAnalog, D(angle)); return angle; } #endif -void uArmController::sort(unsigned int array[], unsigned int len) -{ - unsigned char i=0,j=0; - unsigned int temp = 0; - - for(i = 0; i < len; i++) - { - for(j = 0; i+j < (len-1); j++) - { - if(array[j] > array[j+1]) - { - temp = array[j]; - array[j] = array[j+1]; - array[j+1] = temp; - } - } - } -} unsigned int uArmController::getServoAnalogData(byte servoNum) { - // unsigned int dat[8]; - - - // for(int i = 0; i < 8; i++) - // { - // dat[i] = analogRead(SERVO_ANALOG_PIN[servoNum]); - // } - - // sort(dat, 8); - - // unsigned int result = (dat[2]+dat[3]+dat[4]+dat[5])/4; - - // return result; - return getAnalogData(SERVO_ANALOG_PIN[servoNum]); + return getAnalogPinValue(SERVO_ANALOG_PIN[servoNum]); } -unsigned int uArmController::getAnalogData(byte pin) -{ - unsigned int dat[8]; - - - for(int i = 0; i < 8; i++) - { - dat[i] = analogRead(pin); - } - - sort(dat, 8); - - unsigned int result = (dat[2]+dat[3]+dat[4]+dat[5])/4; - - return result; -} double uArmController::readServoAngleOffset(byte servoNum) { @@ -884,3 +717,7 @@ double uArmController::readServoAngleOffset(byte servoNum) return manualServoOffset; } +unsigned char uArmController::setServoSpeed(byte servoNum, unsigned char speed) +{ + mServoSpeed = speed; +} \ No newline at end of file diff --git a/src/uArmController.h b/src/uArmController.h index e87a71b..173ab77 100644 --- a/src/uArmController.h +++ b/src/uArmController.h @@ -17,22 +17,15 @@ #include #include "UFServo.h" #include "uArmConfig.h" -#include "uArmHWConfig.h" +#include "uArmPin.h" +#include "uArmTypes.h" -#define SERVO_ROT_NUM 0 -#define SERVO_LEFT_NUM 1 -#define SERVO_RIGHT_NUM 2 -#define SERVO_HAND_ROT_NUM 3 -#define SERVO_COUNT 4 #define DEFAULT_ANGLE 60 -#define GRABBING 2 -#define WORKING 1 -#define STOP 0 -#define PUMP_GRABBING_CURRENT 55 + #ifdef MKII @@ -58,9 +51,7 @@ #endif -#define IN_RANGE 1 -#define OUT_OF_RANGE_NO_SOLUTION 2 -#define OUT_OF_RANGE 3 + #define LOWER_ARM_MAX_ANGLE 120 @@ -108,25 +99,17 @@ class uArmController double getServoAngles(double& servoRotAngle, double& servoLeftAngle, double& servoRightAngle); double getServeAngle(byte servoNum); - void gripperCatch(); - void gripperRelease(); - unsigned char gripperStatus(); - void pumpOn(); - void pumpOff(); - - - unsigned char pumpStatus(); #ifdef MKII void readServoCalibrationData(unsigned int address, double& angle); #endif unsigned char getCurrentXYZ(double& x, double& y, double& z); - unsigned char getXYZFromPolar(double& x, double& y, double& z, double s, double r, double h); + //unsigned char getXYZFromPolar(double& x, double& y, double& z, double s, double r, double h); unsigned char getXYZFromAngle(double& x, double& y, double& z, double rot, double left, double right); - unsigned int getAnalogData(byte pin); + unsigned char setServoSpeed(byte servoNum, unsigned char speed); unsigned int getServoAnalogData(byte servoNum); - unsigned char coordianteToAngle(double x, double y, double z, double& angleRot, double& angleLeft, double& angleRight, boolean allowApproximate = true); + unsigned char xyzToAngle(double x, double y, double z, double& angleRot, double& angleLeft, double& angleRight, boolean allowApproximate = true); unsigned char limitRange(double& angleRot, double& angleLeft, double& angleRight); double analogToAngle(byte servoNum, int inputAnalog); @@ -141,6 +124,7 @@ class uArmController protected: Servo mServo[SERVO_COUNT]; + unsigned char mServoSpeed = 255; double mCurAngle[SERVO_COUNT] = {90, 90, 0, 90}; unsigned int mMaxAdcPos[SERVO_COUNT] = {180}; @@ -152,4 +136,7 @@ class uArmController }; + +extern uArmController controller; + #endif // _UARMCONTROLLER_H_ diff --git a/src/uArmDebug.cpp b/src/uArmDebug.cpp new file mode 100644 index 0000000..dc4c850 --- /dev/null +++ b/src/uArmDebug.cpp @@ -0,0 +1,191 @@ +/** + ****************************************************************************** + * @file uArmDebug.cpp + * @author David.Long + * @email xiaokun.long@ufactory.cc + * @date 2016-12-02 + ****************************************************************************** + */ + +#include "uArmDebug.h" +#include + + +#define ARDBUFFER 50 + +int msprintf(char *result, char *str, ...) +{ + int i, count=0, j=0, flag=0; + char temp[ARDBUFFER+1]; + char num[ARDBUFFER+1]; + for(i=0; str[i]!='\0';i++) if(str[i]=='%') count++; + + result[0] = '\0'; + + if (count == 0) + { + for (i = 0; str[i] != '\0'; i++) + { + result[i] = str[i]; + } + result[i] = '\0'; + } + else + { + + va_list argv; + va_start(argv, count); + for(i=0,j=0; str[i]!='\0';i++) + { + if(str[i]=='%') + { + temp[j] = '\0'; + strcat(result, temp); + j=0; + temp[0] = '\0'; + + switch(str[++i]) + { + case 'd': + { + + itoa(va_arg(argv, int), num, 10); + strcat(result, num); + break; + } + + case 'l': ltoa(va_arg(argv, long), num, 10); + strcat(result, num); + break; + + case 'f': + { + char d_str[10]; + dtostrf(va_arg(argv, double), 4, 2, d_str); + strcat(result, d_str); + break; + } + + case 'c': num[0] = (char)va_arg(argv, int); + num[1] = '\0'; + strcat(result, num); + break; + case 's': strcat(result, va_arg(argv, char *)); + break; + default: ; + }; + } + else + { + temp[j] = str[i]; + j = (j+1)%ARDBUFFER; + if(j==0) + { + temp[ARDBUFFER] = '\0'; + strcat(result, temp); + temp[0]='\0'; + } + } + } + } + temp[j] = '\0'; + strcat(result, temp); + + return count + 1; +} + + + + + + + + +void mprint(char *fmt, ...) +{ + char buf[PRINT_BUF]; + + int i, count=0, j=0, flag=0; + char temp[ARDBUFFER+1]; + char num[ARDBUFFER+1]; + for(i=0; fmt[i]!='\0';i++) + { + if(fmt[i]=='%') + count++; + } + + if (count == 0) + { + for (i = 0; fmt[i] != '\0'; i++) + { + buf[i] = fmt[i]; + } + buf[i] = '\0'; + } + else + { + buf[0] = '\0'; + va_list argv; + va_start(argv, count); + for(i=0,j=0; fmt[i]!='\0';i++) + { + if(fmt[i]=='%') + { + temp[j] = '\0'; + strcat(buf, temp); + j=0; + temp[0] = '\0'; + + switch(fmt[++i]) + { + case 'd': + { + + itoa(va_arg(argv, int), num, 10); + strcat(buf, num); + break; + } + + case 'l': ltoa(va_arg(argv, long), num, 10); + strcat(buf, num); + break; + + case 'f': + { + char d_str[10]; + dtostrf(va_arg(argv, double), 4, 2, d_str); + strcat(buf, d_str); + break; + } + + case 'c': num[0] = (char)va_arg(argv, int); + num[1] = '\0'; + strcat(buf, num); + break; + case 's': strcat(buf, va_arg(argv, char *)); + break; + default: ; + }; + } + else + { + temp[j] = fmt[i]; + j = (j+1)%ARDBUFFER; + if(j==0) + { + temp[ARDBUFFER] = '\0'; + strcat(buf, temp); + temp[0]='\0'; + } + } + } + } + + + temp[j] = '\0'; + strcat(buf, temp); + + PrintSerial.println(buf); +} + + diff --git a/src/uArmDebug.h b/src/uArmDebug.h new file mode 100644 index 0000000..946a2d7 --- /dev/null +++ b/src/uArmDebug.h @@ -0,0 +1,33 @@ +/** + ****************************************************************************** + * @file uArmDebug.h + * @author David.Long + * @email xiaokun.long@ufactory.cc + * @date 2016-12-02 + ****************************************************************************** + */ + +#ifndef _UARMDEBUG_H_ +#define _UARMDEBUG_H_ + +#include +#include "uArmConfig.h" + + + +#ifdef DEBUG + #define debugPrint mprint +#else + #define debugPrint +#endif + +#define PrintSerial Serial + +#define PRINT_BUF 128 + + +void mprint(char *fmt, ...); + +int msprintf(char *result, char *str, ...); + +#endif // _UARMDEBUG_H_ diff --git a/src/uArmHWConfig.cpp b/src/uArmHWConfig.cpp deleted file mode 100644 index bc94cf6..0000000 --- a/src/uArmHWConfig.cpp +++ /dev/null @@ -1,10 +0,0 @@ -/** - ****************************************************************************** - * @file uArmHWConfig.cpp - * @author David.Long - * @email xiaokun.long@ufactory.cc - * @date 2016-10-17 - ****************************************************************************** - */ - -#include "uArmHWConfig.h" diff --git a/src/uArmIIC.cpp b/src/uArmIIC.cpp index d6ed045..4222ead 100644 --- a/src/uArmIIC.cpp +++ b/src/uArmIIC.cpp @@ -53,6 +53,7 @@ unsigned char read_ack() { SCL_CLEAR;// SCL=0 iic_stop(); + PORT_DDR = old_state; return 1; } else{ @@ -112,9 +113,10 @@ unsigned char iic_receivebyte() byte |= 0x01; delay_us(); SCL_CLEAR;// SCL=0 - PORT_DDR = old_state; + delay_us(); } + PORT_DDR = old_state; return byte; } diff --git a/src/uArmIIC.h b/src/uArmIIC.h index 8b12429..1aa43d8 100644 --- a/src/uArmIIC.h +++ b/src/uArmIIC.h @@ -12,7 +12,7 @@ #include #include "uArmConfig.h" -#include "uArmHWConfig.h" +#include "uArmPin.h" #ifdef MKII diff --git a/src/uArmHWConfig.h b/src/uArmPin.h similarity index 94% rename from src/uArmHWConfig.h rename to src/uArmPin.h index b031dbc..fe6240f 100644 --- a/src/uArmHWConfig.h +++ b/src/uArmPin.h @@ -1,14 +1,14 @@ /** ****************************************************************************** - * @file uArmHWConfig.h + * @file uArmPin.h * @author David.Long * @email xiaokun.long@ufactory.cc * @date 2016-10-17 ****************************************************************************** */ -#ifndef _UARMHWCONFIG_H_ -#define _UARMHWCONFIG_H_ +#ifndef _UARMPIN_H_ +#define _UARMPIN_H_ #include "uArmConfig.h" @@ -69,4 +69,4 @@ #endif // MKII -#endif // _UARMHWCONFIG_H_ +#endif // _UARMPIN_H_ diff --git a/src/uArmRecorder.cpp b/src/uArmRecorder.cpp index 392fa8d..f4c8e8f 100644 --- a/src/uArmRecorder.cpp +++ b/src/uArmRecorder.cpp @@ -11,7 +11,9 @@ #include "uArmRecorder.h" -uArmRecorder gRecorder; +uArmRecorder recorder; + +extern void reportPos(); uArmRecorder::uArmRecorder() { diff --git a/src/uArmRecorder.h b/src/uArmRecorder.h index 633009a..7ce0bc6 100644 --- a/src/uArmRecorder.h +++ b/src/uArmRecorder.h @@ -14,7 +14,7 @@ #include #include "uArmConfig.h" -#include "uArmHWConfig.h" +#include "uArmPin.h" #include "uArmIIC.h" @@ -32,7 +32,7 @@ class uArmRecorder }; -extern uArmRecorder gRecorder; +extern uArmRecorder recorder; #endif // _UARMRECORDER_H_ diff --git a/src/uArmRingBuffer.cpp b/src/uArmRingBuffer.cpp new file mode 100644 index 0000000..f52bd26 --- /dev/null +++ b/src/uArmRingBuffer.cpp @@ -0,0 +1,65 @@ +/** + ****************************************************************************** + * @file uArmRingBuffer.cpp + * @author David.Long + * @email xiaokun.long@ufactory.cc + * @date 2016-12-08 + ****************************************************************************** + */ + +#include "uArmRingBuffer.h" + +uArmRingBuffer::uArmRingBuffer() +{ + +} + +void uArmRingBuffer::init(uint8_t *data_buf, uint32_t buf_size) +{ + head = 0; + tail = 0; + data = data_buf; + buffer_size = buf_size; +} + + +uint32_t uArmRingBuffer::put(uint8_t value) +{ + + if (isFull()) + { + return 0; + } + + data[tail] = value; + + tail = (tail + 1) % buffer_size; + + return 1; + +} + +uint32_t uArmRingBuffer::get(uint8_t *value) +{ + + if (isEmpty()) + return 0; + + *value = data[head]; + + head = (head + 1) % buffer_size; + + + return 1; + +} + +uint32_t uArmRingBuffer::isFull() +{ + return ((tail + 1) % buffer_size) == head ? 1 : 0; +} + +uint32_t uArmRingBuffer::isEmpty() +{ + return head == tail ? 1 : 0; +} \ No newline at end of file diff --git a/src/uArmRingBuffer.h b/src/uArmRingBuffer.h new file mode 100644 index 0000000..6b8f34a --- /dev/null +++ b/src/uArmRingBuffer.h @@ -0,0 +1,36 @@ +/** + ****************************************************************************** + * @file uArmRingBuffer.h + * @author David.Long + * @email xiaokun.long@ufactory.cc + * @date 2016-12-08 + ****************************************************************************** + */ + +#ifndef _UARMRINGBUFFER_H_ +#define _UARMRINGBUFFER_H_ + +#include + +class uArmRingBuffer +{ + +public: + uArmRingBuffer(); + void init(uint8_t *data_buf, uint32_t buf_size); + uint32_t put(uint8_t value); + uint32_t get(uint8_t *value); + uint32_t isFull(); + uint32_t isEmpty(); + +private: + uint32_t head; + uint32_t tail; + uint8_t *data; + uint32_t buffer_size; + + +}; + + +#endif // _UARMRINGBUFFER_H_ diff --git a/src/uArmService.cpp b/src/uArmService.cpp new file mode 100644 index 0000000..26554fd --- /dev/null +++ b/src/uArmService.cpp @@ -0,0 +1,356 @@ +/** + ****************************************************************************** + * @file uArmService.cpp + * @author David.Long + * @email xiaokun.long@ufactory.cc + * @date 2016-09-28 + * @license GNU + * copyright(c) 2016 UFactory Team. All right reserved + ****************************************************************************** + */ + +#include "uArm.h" + +#include "uArmComm.h" + +uArmService service; + + + +uArmService::uArmService() +{ + + mRecordAddr = 0; + + mReportInterval = 0; + + mButtonServiceDisable = false; + + mReportStartTime = millis(); + + mTickRecorderTime = millis(); +} + +void uArmService::setButtonService(bool on) +{ + if (on) + { + mButtonServiceDisable = false; + } + else + { + mButtonServiceDisable = true; + } +} + + + +void uArmService::setReportInterval(unsigned int interval) +{ + mReportInterval = interval; +} + +void uArmService::init() +{ + +} + + + + +void uArmService::recorderTick() +{ + //sys led function detec every 0.05s----------------------------------------------------------------- + + switch(mSysStatus)//every 0.125s per point + { + case SINGLE_PLAY_MODE: + if(play() == false) + { + mSysStatus = NORMAL_MODE; + mRecordAddr = 0; + } + break; + + case LOOP_PLAY_MODE: + + if(play() == false) + { + mRecordAddr = 0; + } + break; + + case LEARNING_MODE: + case LEARNING_MODE_STOP: + if(record() == false) + { + mSysStatus = NORMAL_MODE; + mRecordAddr = 0; + + controller.attachAllServo(); + + } + break; + + default: + break; + } + +} + + + +void uArmService::systemRun() +{ +//check the button4 status------------------------------------------------------------------------ + if (mButtonServiceDisable) + { + if (buttonMenu.longPressed()) + { + buttonMenu.clearEvent(); + reportButtonEvent(0, EVENT_LONG_PRESS); + } + else if (buttonMenu.clicked()) + { + //debugPrint("btnD4 down"); + buttonMenu.clearEvent(); + reportButtonEvent(0, EVENT_CLICK); + } + + + + //check the button7 status------------------------------------------------------------------------- + if (buttonPlay.longPressed()) + { + buttonPlay.clearEvent(); + reportButtonEvent(1, EVENT_LONG_PRESS); + } + else if (buttonPlay.clicked()) + { + buttonPlay.clearEvent(); + reportButtonEvent(1, EVENT_CLICK); + } + } + else + { + if (buttonMenu.clicked()) + { + //debugPrint("btnD4 down"); + buttonMenu.clearEvent(); + switch (mSysStatus) + { + case NORMAL_MODE: + case NORMAL_BT_CONNECTED_MODE: + mSysStatus = LEARNING_MODE; + mRecordAddr = 0;//recording/playing address + controller.detachAllServo(); + break; + + case LEARNING_MODE: + //LEARNING_MODE_STOP is just used to notificate record() function to stop, once record() get it then change the sys_status to normal_mode + mSysStatus = LEARNING_MODE_STOP;//do not detec if BT is connected here, will do it seperatly + + pumpOff(); + + break; + + default: break; + } + } + + + + //check the button7 status------------------------------------------------------------------------- + if (buttonPlay.longPressed()) + { + buttonPlay.clearEvent(); + switch(mSysStatus) + { + case NORMAL_MODE: + case NORMAL_BT_CONNECTED_MODE: + mRecordAddr = 0; + mSysStatus = LOOP_PLAY_MODE; + break; + + case SINGLE_PLAY_MODE: + case LOOP_PLAY_MODE: + break; + + case LEARNING_MODE: + break; + } + } + else if (buttonPlay.clicked()) + { + buttonPlay.clearEvent(); + + //debugPrint("btnD7 down"); + + switch(mSysStatus) + { + case NORMAL_MODE: + case NORMAL_BT_CONNECTED_MODE: + mRecordAddr = 0;//recording/playing address + mSysStatus = SINGLE_PLAY_MODE; // or play just one time + + break; + + case SINGLE_PLAY_MODE: + case LOOP_PLAY_MODE: + pumpOff(); + mSysStatus = NORMAL_MODE; + break; + + case LEARNING_MODE: + + if (getPumpStatus()) + { + pumpOff(); + } + else + { + pumpOn(); + } + break; + } + } + } + + + if (mReportInterval > 0) + { + if(millis() - mReportStartTime >= mReportInterval) + { + mReportStartTime = millis(); + reportPos(); + } + + } + +} + +void uArmService::btDetect() +{ +#ifdef MKII + if (!buzzer.on() && ((mSysStatus == NORMAL_MODE) || (mSysStatus == NORMAL_BT_CONNECTED_MODE))) + { + pinMode(BT_DETECT_PIN, INPUT); + digitalWrite(BT_DETECT_PIN,HIGH); + + if (digitalRead(BT_DETECT_PIN) == HIGH)//do it here + { + ledRed.on(); + mSysStatus = NORMAL_BT_CONNECTED_MODE; + } + else + { + ledRed.off(); + mSysStatus = NORMAL_MODE; + } + + //pinMode(BT_DETECT_PIN, OUTPUT); + } +#endif +} + + +void uArmService::run() +{ + + systemRun(); + + if (millis() - mTickRecorderTime >= 50) + { + mTickRecorderTime= millis(); + recorderTick(); + } +} + + + + +bool uArmService::play() +{ + + unsigned char data[5]; // 0: L 1: R 2: Rotation 3: hand rotation 4:gripper + + + recorder.read(mRecordAddr, data, 5); + debugPrint("mRecordAddr = %d, data=%d, %d, %d", mRecordAddr, data[0], data[1], data[2]); + + if(data[0] != 255) + { + //double x, y, z; + //controller.getXYZFromAngle(x, y, z, (double)data[2], (double)data[0], (double)data[1]); + //moveToAngle((double)data[2], (double)data[0], (double)data[1]); + controller.writeServoAngle((double)data[2], (double)data[0], (double)data[1]); + controller.writeServoAngle(SERVO_HAND_ROT_NUM, (double)data[3]); + unsigned char pumpStatus = getPumpStatus() > 0 ? 1 : 0; + if (pumpStatus != data[4]) + { + if (data[4]) + { + pumpOn(); + } + else + { + pumpOff(); + } + } + } + else + { + + pumpOff(); + + return false; + } + + mRecordAddr += 5; + + return true; +} + +bool uArmService::record() +{ + debugPrint("mRecordAddr = %d", mRecordAddr); + + if(mRecordAddr <= 65530) + { + unsigned char data[5]; // 0: L 1: R 2: Rotation 3: hand rotation 4:gripper + if((mRecordAddr != 65530) && (mSysStatus != LEARNING_MODE_STOP)) + { + double rot, left, right; + //controller.updateAllServoAngle(); + controller.readServoAngles(rot, left, right); + data[0] = (unsigned char)left; + data[1] = (unsigned char)right; + data[2] = (unsigned char)rot; + data[3] = (unsigned char)controller.readServoAngle(SERVO_HAND_ROT_NUM); + data[4] = getPumpStatus() > 0 ? 1 : 0; + + debugPrint("l=%d, r=%d, r= %d", data[0], data[1], data[2]); + } + else + { + data[0] = 255;//255 is the ending flag + recorder.write(mRecordAddr, data, 5); + + return false; + } + + recorder.write(mRecordAddr, data, 5); + mRecordAddr += 5; + + return true; + } + else + { + return false; + } + +} + + + + diff --git a/src/uArmService.h b/src/uArmService.h new file mode 100644 index 0000000..0afc048 --- /dev/null +++ b/src/uArmService.h @@ -0,0 +1,119 @@ +/** + ****************************************************************************** + * @file uArmSystem.h + * @author David.Long + * @email xiaokun.long@ufactory.cc + * @date 2016-09-28 + * @license GNU + * copyright(c) 2016 UFactory Team. All right reserved + ****************************************************************************** + */ + +#ifndef _UARMSERVICE_H_ +#define _UARMSERVICE_H_ + +#include +#include +#include +#include "UFServo.h" +#include "uArmConfig.h" +#include "uArmPin.h" +#include "uArmController.h" +#include "uArmBuzzer.h" +#include "uArmRecorder.h" +#include "uArmButton.h" +#include "uArmLed.h" +#include "uArmAPI.h" +#include "uArmComm.h" + + + + +#define NORMAL_MODE 0 +#define NORMAL_BT_CONNECTED_MODE 1 +#define LEARNING_MODE 2 +#define SINGLE_PLAY_MODE 3 +#define LOOP_PLAY_MODE 4 +#define LEARNING_MODE_STOP 5 + + + + + +#define OK 0 +#define ERR1 1 +#define ERR2 2 + +#define SS "[S]" +#define S0 "[S0]" +#define S1 "[S1]" +#define S2 "[S2]" +#define FF "[F]" +#define F0 "[F0]" +#define F1 "[F1]" + +// Calibration Flag & OFFSET EEPROM ADDRESS +#define CALIBRATION_FLAG 10 +#define CALIBRATION_LINEAR_FLAG 11 +#define CALIBRATION_MANUAL_FLAG 12 +#define CALIBRATION_STRETCH_FLAG 13 + + + +//#define TIME_PER_STEP 10 + +#define SERVO_9G_MAX 460 +#define SERVO_9G_MIN 98 + +#define CONFIRM_FLAG 0x80 + + +#define SUCCESS 1 +#define FAILED -1 + + + + + + + + +class uArmService +{ +public: + uArmService(); + + void init(); + void run(); + + void btDetect(); + void setReportInterval(unsigned int interval); + + void setButtonService(bool on); + +private: + + void systemRun(); + bool play(); + bool record(); + void tickTaskRun(); + void recorderTick(); + +private: + unsigned char mButtonServiceDisable = false; + unsigned char mSysStatus = NORMAL_MODE; + unsigned int mRecordAddr = 0; + + unsigned int mReportInterval; // ms. 0 means no report + + unsigned long mReportStartTime; + + unsigned long mTickRecorderTime; + +}; + +extern uArmService service; + + + +#endif // _UARMSERVICE_H_ diff --git a/src/uArmTypes.h b/src/uArmTypes.h new file mode 100644 index 0000000..a4f5853 --- /dev/null +++ b/src/uArmTypes.h @@ -0,0 +1,46 @@ +/** + ****************************************************************************** + * @file uArmTypes.h + * @author David.Long + * @email xiaokun.long@ufactory.cc + * @date 2016-12-12 + ****************************************************************************** + */ + +#ifndef _UARMTYPES_H_ +#define _UARMTYPES_H_ + +// gripper or pump status +#define STOP 0 +#define WORKING 1 +#define GRABBING 2 + +//return values +#define OK 0 + +#define IN_RANGE 1 +#define OUT_OF_RANGE_NO_SOLUTION 2 +#define OUT_OF_RANGE 3 +#define NO_NEED_TO_MOVE 4 + +#define ERR_SERVO_INDEX_EXCEED_LIMIT 5 +#define ERR_ANGLE_OUT_OF_RANGE 6 + +// e2prom device +#define EEPROM_ON_CHIP 0 +#define EEPROM_EXTERN_USER 1 +#define EEPROM_EXTERN_SYSTEM 2 + +// e2prom data type +#define DATA_TYPE_BYTE 1 +#define DATA_TYPE_INTEGER 2 +#define DATA_TYPE_FLOAT 4 + +// servo define +#define SERVO_ROT_NUM 0 +#define SERVO_LEFT_NUM 1 +#define SERVO_RIGHT_NUM 2 +#define SERVO_HAND_ROT_NUM 3 +#define SERVO_COUNT 4 + +#endif // _UARMTYPES_H_ From 8e63a7b1511278bb9d75501ab29105895984d532 Mon Sep 17 00:00:00 2001 From: yuntian1019 Date: Wed, 14 Dec 2016 17:02:54 +0800 Subject: [PATCH 2/5] v2.2.3 moveTo: set servo duty cycle when speed in range(1~99) --- examples/Demos/button/button.ino | 2 +- examples/Demos/timer/timer.ino | 2 +- src/README.md | 1 - src/uArmAPI.cpp | 28 ++++++++++++++++------ src/uArmAPI.h | 5 +++- src/uArmConfig.h | 4 ++-- src/uArmController.cpp | 40 +++++++++++++++++++++++++------- src/uArmController.h | 7 ++++-- 8 files changed, 65 insertions(+), 24 deletions(-) delete mode 100644 src/README.md diff --git a/examples/Demos/button/button.ino b/examples/Demos/button/button.ino index 1fed54d..109a8b7 100644 --- a/examples/Demos/button/button.ino +++ b/examples/Demos/button/button.ino @@ -14,7 +14,7 @@ void setup() // TODO service.setButtonService(false); // disable build in button service - + moveTo(0, 150, 150); // initial pos } void loop() diff --git a/examples/Demos/timer/timer.ino b/examples/Demos/timer/timer.ino index 0e8f2c1..57d646b 100644 --- a/examples/Demos/timer/timer.ino +++ b/examples/Demos/timer/timer.ino @@ -16,7 +16,7 @@ void setup() debugPrint("debug start"); // uncomment DEBUG in uArmConfig.h to use debug function // TODO - + moveTo(0, 150, 150); // initial pos } diff --git a/src/README.md b/src/README.md deleted file mode 100644 index 1f525a3..0000000 --- a/src/README.md +++ /dev/null @@ -1 +0,0 @@ -#MK diff --git a/src/uArmAPI.cpp b/src/uArmAPI.cpp index d963ba6..3302b19 100644 --- a/src/uArmAPI.cpp +++ b/src/uArmAPI.cpp @@ -99,7 +99,10 @@ void uArmInit() /*! \brief move to pos(x, y, z) \param x, y, z in mm - \param speed in mm/min + \param speed: + [0]: move to destination directly + [1~99]: change the dutycycle of servo (1~99%) + [100~1000]: mm/min, will do interpolation to control the speed and block process util move done \return IN_RANGE if everything is OK \return OUT_OF_RANGE_NO_SOLUTION if cannot reach \return OUT_OF_RANGE will move to the closest pos @@ -112,9 +115,16 @@ unsigned char moveTo(double x, double y, double z, double speed) debugPrint("moveTo: x=%f, y=%f, z=%f, speed=%f", x, y, z, speed); // when speed less than 100 mm/min, move to destination directly - if (speed < 100) + if (speed < 0) { - // convert to angle and move to target angle directly + return OUT_OF_RANGE_NO_SOLUTION; + } + else if (speed < 100) + { + unsigned char dutyCycle = map(speed, 0, 99, 0, 255); + Serial.print(dutyCycle); + controller.setServoSpeed(dutyCycle); + double angleB, angleL, angleR; result = controller.xyzToAngle(x, y, z, angleB, angleL, angleR); if (result != OUT_OF_RANGE_NO_SOLUTION) @@ -122,10 +132,11 @@ unsigned char moveTo(double x, double y, double z, double speed) controller.writeServoAngle(angleB, angleL, angleR); } - return result; + return result; } else { + controller.setServoSpeed(255); result = _moveTo(x, y, z, speed); if(result != OUT_OF_RANGE_NO_SOLUTION) @@ -846,7 +857,7 @@ static unsigned char _moveTo(double x, double y, double z, double speed) unsigned char status = 0; status = controller.xyzToAngle(x, y, z, targetRot, targetLeft, targetRight); - + debugPrint("target B=%f, L=%f, R=%f\r\n", curRot, curLeft, curRight); if (status == OUT_OF_RANGE_NO_SOLUTION) { @@ -865,6 +876,8 @@ static unsigned char _moveTo(double x, double y, double z, double speed) // get current xyz controller.getCurrentXYZ(curX, curY, curZ); + debugPrint("B=%f, L=%f, R=%f\r\n", curRot, curLeft, curRight); + // calculate max steps totalSteps = max(abs(targetRot - curRot), abs(targetLeft - curLeft)); totalSteps = max(totalSteps, abs(targetRight - curRight)); @@ -900,7 +913,7 @@ static unsigned char _moveTo(double x, double y, double z, double speed) totalSteps = totalSteps < STEP_MAX ? totalSteps : STEP_MAX; - //debugPrint("totalSteps= %d\n", totalSteps); + debugPrint("totalSteps= %d\n", totalSteps); // trajectory planning _interpolate(curX, x, mPathX, totalSteps, INTERP_EASE_INOUT_CUBIC); @@ -925,6 +938,7 @@ static unsigned char _moveTo(double x, double y, double z, double speed) if (i < totalSteps) { + debugPrint("i < totalSteps\r\n"); _interpolate(curRot, targetRot, mPathX, totalSteps, INTERP_EASE_INOUT_CUBIC); _interpolate(curLeft, targetLeft, mPathY, totalSteps, INTERP_EASE_INOUT_CUBIC); _interpolate(curRight, targetRight, mPathZ, totalSteps, INTERP_EASE_INOUT_CUBIC); @@ -958,7 +972,7 @@ static void _controllerRun() // ignore the point if cannot reach if (controller.limitRange(mPathX[mCurStep], mPathY[mCurStep], mPathZ[mCurStep]) != OUT_OF_RANGE_NO_SOLUTION) { - //debugPrint("curStep:%d, %f, %f, %f", mCurStep, mPathX[mCurStep], mPathY[mCurStep], mPathZ[mCurStep]); + debugPrint("curStep:%d, %f, %f, %f", mCurStep, mPathX[mCurStep], mPathY[mCurStep], mPathZ[mCurStep]); if (mCurStep == (mTotalSteps - 1)) { double angles[3]; diff --git a/src/uArmAPI.h b/src/uArmAPI.h index 6eacf29..d86dfe9 100644 --- a/src/uArmAPI.h +++ b/src/uArmAPI.h @@ -41,7 +41,10 @@ void uArmInit(); /*! \brief move to pos(x, y, z) \param x, y, z in mm - \param speed in mm/min + \param speed: + [0]: move to destination directly + [1~99]: change the dutycycle of servo (1~99%) + [100~1000]: mm/min, will do interpolation to control the speed and block process util move done \return IN_RANGE if everything is OK \return OUT_OF_RANGE_NO_SOLUTION if cannot reach \return OUT_OF_RANGE will move to the closest pos diff --git a/src/uArmConfig.h b/src/uArmConfig.h index bd0f754..dadb1a6 100644 --- a/src/uArmConfig.h +++ b/src/uArmConfig.h @@ -28,10 +28,10 @@ #ifdef MKII #define HW_VER "3.1" - #define SW_VER "2.2.2" + #define SW_VER "2.2.3" #elif defined(METAL) #define HW_VER "2.1" - #define SW_VER "2.2.2" + #define SW_VER "2.2.3" #else #error "NO machine model defined(METAL, MKII)" #endif diff --git a/src/uArmController.cpp b/src/uArmController.cpp index d04bacf..aef389a 100644 --- a/src/uArmController.cpp +++ b/src/uArmController.cpp @@ -141,7 +141,7 @@ void uArmController::writeServoAngle(byte servoNum, double servoAngle, boolean w - + servoAngle = constrain(servoAngle, 0.00, 180.00); mCurAngle[servoNum] = servoAngle; servoAngle = writeWithOffset ? (servoAngle + mServoAngleOffset[servoNum]) : servoAngle; @@ -182,6 +182,7 @@ void uArmController::readServoCalibrationData(unsigned int address, double& angl deltaA = 0xffff; deltaB = 0; + if (abs(angle - 0.0) < 0.001) { return; @@ -385,6 +386,12 @@ unsigned char uArmController::xyzToAngle(double x, double y, double z, double& a y = (double)((int)(y*10)/10.0); z = (double)((int)(z*10)/10.0); + + if (z > MAX_Z || z < MIN_Z) + { + return OUT_OF_RANGE_NO_SOLUTION; + } + zIn = (z - MATH_L1) / MATH_LOWER_ARM; if(!allowApproximate)//if need the move to closest point we have to jump over the return function @@ -443,25 +450,32 @@ unsigned char uArmController::xyzToAngle(double x, double y, double z, double& a unsigned char uArmController::limitRange(double& angleRot, double& angleLeft, double& angleRight) { + unsigned char result = IN_RANGE; + //determine if the angle can be reached if(isnan(angleRot)||isnan(angleLeft)||isnan(angleRight)) { - return OUT_OF_RANGE_NO_SOLUTION; + result = OUT_OF_RANGE_NO_SOLUTION; } - if(((angleLeft + mServoAngleOffset[SERVO_LEFT_NUM]) < LOWER_ARM_MIN_ANGLE)||((angleLeft + mServoAngleOffset[SERVO_LEFT_NUM]) > LOWER_ARM_MAX_ANGLE))//check the right in range + else if(((angleLeft + mServoAngleOffset[SERVO_LEFT_NUM]) < LOWER_ARM_MIN_ANGLE)||((angleLeft + mServoAngleOffset[SERVO_LEFT_NUM]) > LOWER_ARM_MAX_ANGLE))//check the right in range { - return OUT_OF_RANGE; + result = OUT_OF_RANGE; } - if(((angleRight + mServoAngleOffset[SERVO_RIGHT_NUM]) < UPPER_ARM_MIN_ANGLE)||((angleRight + mServoAngleOffset[SERVO_RIGHT_NUM]) > UPPER_ARM_MAX_ANGLE))//check the left in range + else if(((angleRight + mServoAngleOffset[SERVO_RIGHT_NUM]) < UPPER_ARM_MIN_ANGLE)||((angleRight + mServoAngleOffset[SERVO_RIGHT_NUM]) > UPPER_ARM_MAX_ANGLE))//check the left in range { - return OUT_OF_RANGE; + result = OUT_OF_RANGE; } - if(((180 - angleLeft - angleRight)>LOWER_UPPER_MAX_ANGLE)||((180 - angleLeft - angleRight)LOWER_UPPER_MAX_ANGLE)||((180 - angleLeft - angleRight) Date: Wed, 14 Dec 2016 17:42:53 +0800 Subject: [PATCH 3/5] delete print code --- src/uArmAPI.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/uArmAPI.cpp b/src/uArmAPI.cpp index 3302b19..14a47ef 100644 --- a/src/uArmAPI.cpp +++ b/src/uArmAPI.cpp @@ -122,7 +122,7 @@ unsigned char moveTo(double x, double y, double z, double speed) else if (speed < 100) { unsigned char dutyCycle = map(speed, 0, 99, 0, 255); - Serial.print(dutyCycle); + controller.setServoSpeed(dutyCycle); double angleB, angleL, angleR; From 77d0680022bb3006ac1effdba63eafe0b3fb0322 Mon Sep 17 00:00:00 2001 From: Alex Date: Fri, 10 Feb 2017 14:46:20 +0800 Subject: [PATCH 4/5] add missing linreg.h --- src/linreg.cpp | 55 ++++++++++++++++++++++++++++++++++++ src/linreg.h | 77 ++++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 132 insertions(+) create mode 100644 src/linreg.cpp create mode 100644 src/linreg.h diff --git a/src/linreg.cpp b/src/linreg.cpp new file mode 100644 index 0000000..26b2e5d --- /dev/null +++ b/src/linreg.cpp @@ -0,0 +1,55 @@ +/* +file linreg.cpp +*/ +#include +#include +#include "linreg.h" + + +LinearRegression::LinearRegression(double *x, double *y, long size) +{ + long i; + a = b = sumX = sumY = sumXsquared = sumYsquared = sumXY = 0.0; + n = 0L; + + if (size > 0L) // if size greater than zero there are data arrays + for (n = 0, i = 0L; i < size; i++) + addXY(x[i], y[i]); +} + +void LinearRegression::addXY(const double& x, const double& y) +{ + n++; + sumX += x; + sumY += y; + sumXsquared += x * x; + sumYsquared += y * y; + sumXY += x * y; + Calculate(); +} + +void LinearRegression::Calculate() +{ + if (haveData()) + { + if (fabs(double(n) * sumXsquared - sumX * sumX) > DBL_EPSILON) + { + b = (double(n) * sumXY - sumY * sumX) / + (double(n) * sumXsquared - sumX * sumX); + a = (sumY - b * sumX) / double(n); + + double sx = b * (sumXY - sumX * sumY / double(n)); + double sy2 = sumYsquared - sumY * sumY / double(n); + double sy = sy2 - sx; + + coefD = sx / sy2; + coefC = sqrt(coefD); + stdError = sqrt(sy / double(n - 2)); + } + else + { + a = b = coefD = coefC = stdError = 0.0; + } + } +} + diff --git a/src/linreg.h b/src/linreg.h new file mode 100644 index 0000000..8e5b8f8 --- /dev/null +++ b/src/linreg.h @@ -0,0 +1,77 @@ +#pragma once +/* linreg.h +Linear Regression calculation class + +by: David C. Swaim II, Ph.D. + +This class implements a standard linear regression on +experimental data using a least squares fit to a straight +line graph. Calculates coefficients a and b of the equation: + +y = a + b * x + +for data points of x and y. Also calculates the coefficient of +determination, the coefficient of correlation, and standard +error of estimate. + +The value n (number of points) must be greater than 2 to +calculate the regression. This is primarily because the +standard error has a (N-2) in the denominator. + +Check haveData() to see if there is enough data in +LinearRegression to get values. + +You can think of the x,y pairs as 2 dimensional points. +The class Point2D is included to allow pairing x and y +values together to represent a point on a plane. + +*/ + +#ifndef _LINREG_H_ +#define _LINREG_H_ + + + + +class LinearRegression +{ + + +public: + // Constructor using an array of Point2D objects + // This is also the default constructor + + // Constructor using arrays of x values and y values + LinearRegression(double *x, double *y, long size = 0); + + virtual void addXY(const double& x, const double& y); + + // Must have at least 3 points to calculate + // standard error of estimate. Do we have enough data? + int haveData() const { return (n > 2 ? 1 : 0); } + long items() const { return n; } + + virtual double getA() const { return a; } + virtual double getB() const { return b; } + + double getCoefDeterm() const { return coefD; } + double getCoefCorrel() const { return coefC; } + double getStdErrorEst() const { return stdError; } + virtual double estimateY(double x) const { return (a + b * x); } + +protected: + long n; // number of data points input so far + double sumX, sumY; // sums of x and y + double sumXsquared, // sum of x squares + sumYsquared; // sum y squares + double sumXY; // sum of x*y + + double a, b; // coefficients of f(x) = a + b*x + double coefD, // coefficient of determination + coefC, // coefficient of correlation + stdError; // standard error of estimate + + void Calculate(); // calculate coefficients +}; + +#endif // end of linreg.h \ No newline at end of file From aad00adfcfd918ec996960e64143222fefbc214f Mon Sep 17 00:00:00 2001 From: Duke Fong Date: Tue, 8 Aug 2017 19:56:31 +0800 Subject: [PATCH 5/5] update readme --- README.md | 24 ++++-------------------- 1 file changed, 4 insertions(+), 20 deletions(-) diff --git a/README.md b/README.md index 432a707..b7ff62a 100644 --- a/README.md +++ b/README.md @@ -10,23 +10,10 @@ Specification Please read Documentation Center /API ## Installation -### Library Manager -*Recommend* - -- Download [Arduino IDE][622f1188] -- Import the uArm Library (search uArmLibrary) using Library Manager. - If you don't know how to use Library Manager. Please Refer this [Import Library Using Library Manager][4b323740] -- Upload the example to your uArm - If you don't know how to upload Arduino sketch, We recommend you read this [Arduino Getting Started on Windows][397d20eb] or [Arduino Getting Started on MacOS][2d8a8b7a] -- You need to recalibrate when you are new to this library. we recommend the GUI Calibration tool. - - - -### Manual Installation - -- Download [Arduino IDE][622f1188] -- Clone the source code into your computer. -- Open Arduino IDE, import the zip as library. Please refer this [Import a .zip Library][8cd4af00] +- Download and install [Arduino IDE][622f1188] +- Clone the source code under ```~/Arduino/libraries/``` +- Open Arduino IDE, choose one ```ino``` file under ```examples/``` +- Select board as ```Genuino Uno```, and port as ```/dev/ttyUSB0```(i.e. for linux) - Upload the example to your uArm If you don't know how to upload Arduino sketch, We recommend you read this [Arduino Getting Started on Windows][397d20eb] or [Arduino Getting Started on MacOS][2d8a8b7a] - You need to recalibrate when you are new to this library. we recommend the GUI Calibration tool. @@ -36,9 +23,6 @@ Specification Please read Documentation Center /API please reference Arduino [GettingStarted][GettingStarted] Sketch - - [8cd4af00]: https://www.arduino.cc/en/Guide/Libraries#toc4 "Import a .zip Library" - [4b323740]: https://www.arduino.cc/en/Guide/Libraries#toc3 "Import Library Using Library Manager" [3]: http://developer.ufactory.cc/quickstart/arduino/ [622f1188]: https://www.arduino.cc/en/Main/Software "Arduino IDE" [397d20eb]: https://www.arduino.cc/en/Guide/Windows "Arduino Getting Start on Windows"