/** ****************************************************************************** * @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: [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 \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 < 0) { return OUT_OF_RANGE_NO_SOLUTION; } else if (speed < 100) { unsigned char dutyCycle = map(speed, 0, 99, 0, 255); controller.setServoSpeed(dutyCycle); 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 { controller.setServoSpeed(255); 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); debugPrint("target B=%f, L=%f, R=%f\r\n", curRot, curLeft, curRight); 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); 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)); 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) { 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); } 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(); } }