uarm librerias
DESCRIPTION
Librerias de UarmTRANSCRIPT
Standard.ino
/************************************************************************
* File Name : Standard
* Author : Evan
* Updated : Evan
* Version : V0.0.2
* Date : 15 June, 2014
* Description : Mouse Control or Leap Motion Control(Processing),
Calibration Mode & Rrecording Mode.
* License :
* Copyright(C) 2014 UFactory Team. All right reserved.
* *************************************************************************
* Updated : Alex
* Date : 04 Mar, 2015
* Version : V0.0.2
* Description : CtrlData 0x80 for RESET
*************************************************************************/
#include
#include
int heightTemp = 0, stretchTemp = 0, rotationTemp = 0, handRotTemp = 0;
char stateMachine = 0, counter = 0;
char dataBuf[9] = {0};
UF_uArm uarm; // initialize the uArm library
void setup()
{
Serial.begin(9600); // start serial port at 9600 bps
// while(!Serial); // wait for serial port to connect. Needed for Leonardo only
uarmReset();
}
void loop()
{
uarm.calibration(); // if corrected, you could remove it, no harm though
uarm.recordingMode(50); // sampling time: 50ms, Recording time: 17 seconds.
if(Serial.available())
{
byte rxBuf = Serial.read();
if(stateMachine == 0)
{
stateMachine = rxBuf == 0xFF? 1:0;
}
else if(stateMachine == 1)
{
stateMachine = rxBuf == 0xAA? 2:0;
}
else if(stateMachine == 2)
{
dataBuf[counter++] = rxBuf;
if(counter > 8) // receive 9 byte data
{
stateMachine = 0;
counter=0;
if(dataBuf[8] & RESET)
{
uarmReset();
}
else{
*((char *)(&rotationTemp) ) = dataBuf[1]; // recevive 1byte
*((char *)(&rotationTemp)+1) = dataBuf[0];
*((char *)(&stretchTemp ) ) = dataBuf[3];
*((char *)(&stretchTemp )+1) = dataBuf[2];
*((char *)(&heightTemp ) ) = dataBuf[5];
*((char *)(&heightTemp )+1) = dataBuf[4];
*((char *)(&handRotTemp ) ) = dataBuf[7];
*((char *)(&handRotTemp )+1) = dataBuf[6];
uarm.setPosition(stretchTemp, heightTemp, rotationTemp, handRotTemp);
/* pump action, Valve Stop. */
if(dataBuf[8] & CATCH) uarm.gripperCatch();
/* pump stop, Valve action.
Note: The air relief valve can not work for a long time,
should be less than ten minutes. */
if(dataBuf[8] & RELEASE) uarm.gripperRelease();
}
}
}
}
/* delay release valve, this function must be in the main loop */
uarm.gripperDetach();
}
void uarmReset(){
uarm.init(); // initialize the uArm position
uarm.setServoSpeed(SERVO_R, 0); // 0=full speed, 1-255 slower to faster
uarm.setServoSpeed(SERVO_L, 0); // 0=full speed, 1-255 slower to faster
uarm.setServoSpeed(SERVO_ROT, 50); // 0=full speed, 1-255 slower to faster
}
UF_uArm.h
/******************************************************************************
* File Name : UF_uArm.h
* Author : Evan
* Updated : Evan
* Version : V0.0.3 (BATE)
* Created Date : 12 Dec, 2014
* Modified Date : 17 Jan, 2015
* Description :
* License :
* Copyright(C) 2014 UFactory Team. All right reserved.
*******************************************************************************
* Updated : Alex
* Date : 04 Mar, 2015
* Version : V0.0.2
* Description : CtrlData 0x80 for RESET
*************************************************************************/
#include
#include
#include "VarSpeedServo.h"
#ifndef UF_uArm_Metal_h
#define UF_uArm_Metal_h
/**************** Macro definitions ****************/
#define ARM_A 148 // upper arm
#define ARM_B 160 // lower arm
#define ARM_2AB 47360 // 2*A*B
#define ARM_A2 21904 // A^2
#define ARM_B2 25600 // B^2
#define ARM_A2B2 47504 // A^2 + B^2
#define ARM_STRETCH_MIN 0
#define ARM_STRETCH_MAX 195
#define ARM_HEIGHT_MIN -150
#define ARM_HEIGHT_MAX 160
#define ARM_ROTATION_MIN -90
#define ARM_ROTATION_MAX 90
#define HAND_ROTATION_MIN -90
#define HAND_ROTATION_MAX 90
#define HAND_ANGLE_OPEN 25
#define HAND_ANGLE_CLOSE 70
#define FIXED_OFFSET_L 25
#define FIXED_OFFSET_R 42
#define D090M_SERVO_MIN_PUL 500
#define D090M_SERVO_MAX_PUL 2500
#define D009A_SERVO_MIN_PUL 600
#define D009A_SERVO_MAX_PUL 2550
#define SAMPLING_DEADZONE 2
#define INIT_POS_L 145//37 // the angle of calibration position (initial angle)
#define INIT_POS_R 68//25 // the angle of calibration position (initial angle)
#define BTN_TIMEOUT_1000 1000
#define BTN_TIMEOUT_3000 3000
#define CATCH 0x01
#define RELEASE 0x02
#define RESET 0x80
#define CALIBRATION_FLAG 0xEE
#define SERVO_MAX 635
#define SERVO_MIN 72
#define MEMORY_SERVO_PER 335 // eeprom: (1024 - 3 - 14)/3=335
#define DATA_FLAG 255
#define BUFFER_OUTPUT 5
/***************** Port definitions *****************/
#define BTN_D4 4 //
#define BTN_D7 7 //
#define BUZZER 3 //
#define LIMIT_SW 2 // Limit Switch
#define PUMP_EN 6 //
#define VALVE_EN 5 //
#define SERVO_HAND 9 //
#define SERVO_HAND_ROT 10 //
#define SERVO_ROT 11 //
#define SERVO_R 12 //
#define SERVO_L 13 //
class UF_uArm
{
public:
UF_uArm();
void init(); // initialize the uArm position
void calibration(); //
void recordingMode(unsigned char _sampleDelay = 50);
void setPosition(double _stretch, double _height, int _armRot, int _handRot); //
void setServoSpeed(char _servoNum, unsigned char _servoSpeed); // 0=full speed, 1-255 slower to faster
int readAngle(char _servoNum);
void gripperCatch(); //
void gripperRelease(); //
void gripperDetach(); //
void gripperDirectDetach(); //
void pumpOn(); // pump enable
void pumpOff(); // pump disnable
void valveOn(); // valve enable, decompression
void valveOff(); // valve disnable
void detachServo(char _servoNum);
void sendData(byte _dataAdd, int _dataIn); //
void alert(int _times, int _runTime, int _stopTime);
void writeEEPROM();
void readEEPROM();
void play(unsigned char buttonPin);
void record(unsigned char buttonPin, unsigned char buttonPinC);
void servoBufOutL(unsigned char _lastDt, unsigned char _dt);
void servoBufOutR(unsigned char _lastDt, unsigned char _dt);
void servoBufOutRot(unsigned char _lastDt, unsigned char _dt);
private:
/******************* Servo offset *******************/
char offsetL;
char offsetR;
/***************** Define variables *****************/
int heightLst;
int height;
int stretch;
int rotation;
int handRot;
boolean playFlag;
boolean recordFlag;
boolean firstFlag;
boolean gripperRst;
unsigned char sampleDelay;
unsigned char servoSpdR;
unsigned char servoSpdL;
unsigned char servoSpdRot;
unsigned char servoSpdHand;
unsigned char servoSpdHandRot;
unsigned char leftServoLast;
unsigned char rightServoLast;
unsigned char rotServoLast;
unsigned char griperState[14];
unsigned char data[3][MEMORY_SERVO_PER+1]; // 0: L 1: R 2: Rotation
unsigned long delay_loop;
unsigned long lstTime; //limit: 50days
/*************** Create servo objects ***************/
VarSpeedServo servoR;
VarSpeedServo servoL;
VarSpeedServo servoRot;
VarSpeedServo servoHand;
VarSpeedServo servoHandRot;
};
#endif
/******************************************************************************
* File Name : UF_uArm.cpp
* Author : Evan
* Updated : Evan
* Version : V0.0.2 (BATE)
* Date : 12 Dec, 2014
* Modified Date : 17 Jan, 2015
* Description :
* License :
* Copyright(C) 2014 UFactory Team. All right reserved.
*******************************************************************************/
#include "UF_uArm_Metal.h"
UF_uArm::UF_uArm()
{
heightLst= 0;
height= 0;
stretch= 0;
rotation= 0;
handRot= 0;
lstTime= 0;
delay_loop= 0;
servoSpdR= 0;
servoSpdL= 0;
servoSpdRot= 100;
servoSpdHand= 0;
servoSpdHandRot= 0;
leftServoLast= 110;
rightServoLast= 100;
rotServoLast= 90;
sampleDelay= 50;
playFlag= false;
recordFlag= true;
firstFlag= true;
gripperRst= true;
griperState[14]= 0;
data[3][MEMORY_SERVO_PER+1] = 0; // 0: L 1: R 2: Rotation
}
void UF_uArm::init()
{
// read offset data
offsetL = EEPROM.read(1);
offsetR = EEPROM.read(2);
// initialization the pin
pinMode(LIMIT_SW, INPUT); digitalWrite(LIMIT_SW, HIGH);
pinMode(BTN_D4, INPUT); digitalWrite(BTN_D4, HIGH);
pinMode(BTN_D7, INPUT); digitalWrite(BTN_D7, HIGH);
pinMode(BUZZER, OUTPUT); digitalWrite(BUZZER, LOW);
pinMode(PUMP_EN, OUTPUT); digitalWrite(PUMP_EN, LOW);
pinMode(VALVE_EN, OUTPUT); digitalWrite(VALVE_EN, LOW);
if (EEPROM.read(0) == CALIBRATION_FLAG) // read of offset flag
{
// attaches the servo on pin to the servo object
servoL.attach(SERVO_L, D090M_SERVO_MIN_PUL, D090M_SERVO_MAX_PUL);
servoR.attach(SERVO_R, D090M_SERVO_MIN_PUL, D090M_SERVO_MAX_PUL);
servoRot.attach(SERVO_ROT, D090M_SERVO_MIN_PUL, D090M_SERVO_MAX_PUL);
servoHand.attach(SERVO_HAND, D009A_SERVO_MIN_PUL, D009A_SERVO_MAX_PUL);
servoHandRot.attach(SERVO_HAND_ROT, D009A_SERVO_MIN_PUL, D009A_SERVO_MAX_PUL);
servoHand.write(HAND_ANGLE_OPEN, 0, true);
servoHand.detach();
servoL.write(map(readAngle(SERVO_L), SERVO_MIN, SERVO_MAX, 0, 180));
servoR.write(map(readAngle(SERVO_R), SERVO_MIN, SERVO_MAX, 0, 180));
servoRot.write(map(readAngle(SERVO_ROT), SERVO_MIN, SERVO_MAX, 0, 180));
// initialization postion
setServoSpeed(SERVO_R, 15); // 0=full speed, 1-255 slower to faster
setServoSpeed(SERVO_L, 15); // 0=full speed, 1-255 slower to faster
setServoSpeed(SERVO_ROT, 10); // 0=full speed, 1-255 slower to faster
setPosition(stretch, height, rotation, handRot);
}
else
{// buzzer alert if calibration needed
alert(3, 200, 200);
}
}
void UF_uArm::calibration()
{
int initPosL = INIT_POS_L - 35; // Added 30 degrees here to start at reasonable point
int initPosR = INIT_POS_R - 35; // Added 30 degrees here to start at reasonable point
if(!digitalRead(BTN_D7))
{
delay(20);
// buzzer alert
alert(1, 20, 0);
}
lstTime = millis();
while(!digitalRead(BTN_D7))
{
if(millis() - lstTime > BTN_TIMEOUT_3000)
{
// buzzer alert
alert(2, 50, 100);
while(!digitalRead(BTN_D7))
{
servoL.detach();
servoR.detach();
}
while(1)
{
// SG-> Commentary: While user adjusts for minimum angles, keep track of angle and add
// margin of analog reading value of 12, which is about 3 degrees.
int minAngle_L = readAngle(SERVO_L) - 12;
int minAngle_R = readAngle(SERVO_R) - 12;
if(!digitalRead(BTN_D7))
{
delay(20);
// buzzer alert
alert(1, 20, 0);
delay(500); //SG-> Added to delay for user to remove hand
// buzzer alert
servoL.attach(SERVO_L, D090M_SERVO_MIN_PUL, D090M_SERVO_MAX_PUL);
servoR.attach(SERVO_R, D090M_SERVO_MIN_PUL, D090M_SERVO_MAX_PUL);
servoL.write(map(readAngle(SERVO_L), SERVO_MIN, SERVO_MAX, 0, 180));
servoR.write(map(readAngle(SERVO_R), SERVO_MIN, SERVO_MAX, 0, 180));
servoR.write(initPosR, 10, true); // 0=full speed, 1-255 slower to faster
servoL.write(initPosL, 10, true); // 0=full speed, 1-255 slower to faster
delay(200);
while(readAngle(SERVO_R) < minAngle_R - SAMPLING_DEADZONE)
{
servoR.write(++initPosR);
delay(50);
}
while(readAngle(SERVO_R) > minAngle_R + SAMPLING_DEADZONE)
{
servoR.write(--initPosR);
delay(50);
}
while(readAngle(SERVO_L) < minAngle_L - SAMPLING_DEADZONE)
{
servoL.write(++initPosL);
delay(50);
}
while(readAngle(SERVO_L) > minAngle_L + SAMPLING_DEADZONE)
{
servoL.write(--initPosL);
delay(50);
}
offsetL = initPosL - INIT_POS_L;
offsetR = initPosR - INIT_POS_R;
EEPROM.write(0, CALIBRATION_FLAG); // Added flag to know if offset is done
EEPROM.write(1, offsetL);// offsetL
EEPROM.write(2, offsetR);// offsetR
// buzzer alert
alert(1, 500, 0);
// reset postion
init();
break;
}
}
}
}
}
void UF_uArm::recordingMode(unsigned char _sampleDelay)
{
sampleDelay = _sampleDelay;
// D4 button - Recording mode
if(!digitalRead(BTN_D4))
{
alert(2, 200, 100);
while(!digitalRead(BTN_D4));
delay(50);
servoL.detach();
servoR.detach();
servoRot.detach();
servoHandRot.detach();
while(1)
{
// D4 button - recording
if(!digitalRead(BTN_D4))
{
recordFlag = true;
alert(1, 50, 0);
lstTime = millis();
while(!digitalRead(BTN_D4))
{
if(millis() - lstTime > BTN_TIMEOUT_1000)
{
recordFlag = false;
writeEEPROM();
alert(1, 300, 0);
while(!digitalRead(BTN_D4));
}
}
delay(20);
if(recordFlag)
record(BTN_D4, BTN_D7);
}
// D7 button - play
if(!digitalRead(BTN_D7))
{
playFlag = false;
alert(1, 100, 0);
if(firstFlag)
{
readEEPROM();
firstFlag = false;
}
lstTime = millis();
while(!digitalRead(BTN_D7))
{
if(millis() - lstTime > BTN_TIMEOUT_1000)
{
playFlag = true;
alert(1, 300, 0);
while(!digitalRead(BTN_D7));
}
}
delay(20);
play(BTN_D7);
}
}
}
}
void UF_uArm::setPosition(double _stretch, double _height, int _armRot, int _handRot)
{
_armRot = -_armRot;
if(!digitalRead(LIMIT_SW) && _height < heightLst) //limit switch protection
_height = heightLst;
// input limit
_stretch = constrain(_stretch, ARM_STRETCH_MIN, ARM_STRETCH_MAX) + 68;// +68, set zero -stretch
_height = constrain(_height, ARM_HEIGHT_MIN, ARM_HEIGHT_MAX);
_armRot = constrain(_armRot, ARM_ROTATION_MIN, ARM_ROTATION_MAX) + 90;// +90, change -90~90 to 0~180
_handRot = constrain(_handRot, HAND_ROTATION_MIN, HAND_ROTATION_MAX) + 90;// +90, change -90~90 to 0~180
// angle calculation
double stretch2height2 = _stretch * _stretch + _height * _height; //
double angleA = (acos( (ARM_A2B2 - stretch2height2) / ARM_2AB )) * RAD_TO_DEG; // angle between the upper and the lower
double angleB = (atan(_height/_stretch)) * RAD_TO_DEG; //
double angleC = (acos((ARM_A2 + stretch2height2 -ARM_B2)/(2 * ARM_A * sqrt(stretch2height2)))) * RAD_TO_DEG; //
int angleR = 180 - angleA - angleB - angleC + FIXED_OFFSET_R + offsetR; //
int angleL = angleB + angleC + FIXED_OFFSET_L + offsetL; //
// angle limit
angleL = constrain(angleL, 15 + offsetL, 180 + offsetL);
//angleR = constrain(angleR, 0 + offsetR, 180 + offsetR);
//angleR = constrain(angleR, angleL - 110 + offsetR, angleR);// behind -120+30 = -90
//if(angleL < 25+offsetL)
//angleR = constrain(angleR, 80 + offsetR, angleR);// front down
// set servo position
servoR.write(angleR, servoSpdR, false);
servoL.write(angleL, servoSpdL, false);
servoRot.write(_armRot, servoSpdRot, false);
servoHandRot.write(_handRot, servoSpdHandRot, false);
heightLst = _height;
}
void UF_uArm::setServoSpeed(char _servoNum, unsigned char _servoSpeed) // 0=full speed, 1-255 slower to faster
{
switch(_servoNum)
{
case SERVO_L:
servoSpdR = _servoSpeed;
break;
case SERVO_R:
servoSpdL = _servoSpeed;
break;
case SERVO_ROT:
servoSpdRot = _servoSpeed;
break;
case SERVO_HAND_ROT:
servoSpdHand = _servoSpeed;
break;
case SERVO_HAND:
servoSpdHandRot = _servoSpeed;
break;
default: break;
}
}
int UF_uArm::readAngle(char _servoNum)
{
int portAd;
switch(_servoNum)
{
case SERVO_L:
portAd = A0;
break;
case SERVO_R:
portAd = A1;
break;
case SERVO_ROT:
portAd = A2;
break;
case SERVO_HAND_ROT:
portAd = A3;
break;
case SERVO_HAND:
portAd = A4;
break;
default: return 0; break;
}
int adAdd = 0;
for(char i=0; i 300000) // delay release valve
{
servoHand.detach();
digitalWrite(VALVE_EN, LOW); // valve disnable
delay_loop=0;
}
}
void UF_uArm::gripperDirectDetach()
{
servoHand.detach();
digitalWrite(PUMP_EN, LOW); // pump disnable
digitalWrite(VALVE_EN, LOW); // valve disnable
}
void UF_uArm::pumpOn()
{
digitalWrite(PUMP_EN, HIGH); // pump enable
}
void UF_uArm::pumpOff()
{
digitalWrite(PUMP_EN, LOW); // pump disnable
}
void UF_uArm::valveOn()
{
digitalWrite(VALVE_EN, HIGH); // valve enable, decompression
}
void UF_uArm::valveOff()
{
digitalWrite(VALVE_EN, LOW); // valve disnable
}
void UF_uArm::detachServo(char _servoNum)
{
switch(_servoNum)
{
case SERVO_L:
servoL.detach();
break;
case SERVO_R:
servoR.detach();
break;
case SERVO_ROT:
servoRot.detach();
break;
case SERVO_HAND_ROT:
servoHandRot.detach();
break;
case SERVO_HAND:
servoHand.detach();
break;
default: break;
}
}
void UF_uArm::sendData(byte _dataAdd, int _dataIn)
{
Serial.write(0xFF); Serial.write(0xAA); // send data head
Serial.write(_dataAdd);
Serial.write(*((char *)(&_dataIn) + 1));
Serial.write(*((char *)(&_dataIn)));
}
void UF_uArm::alert(int _times, int _runTime, int _stopTime)
{
for(int _ct=0; _ct < _times; _ct++)
{
delay(_stopTime);
digitalWrite(BUZZER, HIGH);
delay(_runTime);
digitalWrite(BUZZER, LOW);
}
}
void UF_uArm::play(unsigned char buttonPin)
{
unsigned int addr = 0;
unsigned char addrC = 0;
// Serial.println("Playing");
// attaches the servo on pin to the servo object
servoL.attach(SERVO_L, D090M_SERVO_MIN_PUL, D090M_SERVO_MAX_PUL);
servoR.attach(SERVO_R, D090M_SERVO_MIN_PUL, D090M_SERVO_MAX_PUL);
servoRot.attach(SERVO_ROT, D090M_SERVO_MIN_PUL, D090M_SERVO_MAX_PUL);
while(digitalRead(buttonPin))
{
if(data[2][addr] == DATA_FLAG)
{
if(playFlag){ addr = 0; addrC = 0;}
else break;
}
unsigned char leftServo = data[0][addr];
unsigned char rightServo = data[1][addr];
unsigned char rotServo = data[2][addr];
if(addr%3==0)
{
if((griperState[addrC / 8] >> (addrC % 8)) & 1)
gripperCatch();
else
gripperRelease();
addrC++;
}
// Serial.print("Read Memory-> left: "); Serial.print(leftServo);
// Serial.print("\tright: "); Serial.print(rightServo);
// Serial.print("\trot: "); Serial.println(rotServo);
servoBufOutL(leftServoLast, leftServo);
servoBufOutR(rightServoLast, rightServo);
servoBufOutRot(rotServoLast, rotServo);
leftServoLast = leftServo;
rightServoLast = rightServo;
rotServoLast = rotServo;
delay(sampleDelay);
addr++;
}
// Serial.println("Done");
servoL.detach();
servoR.detach();
servoRot.detach();
gripperDirectDetach();
delay(250);
}
void UF_uArm::record(unsigned char buttonPin, unsigned char buttonPinC)
{
unsigned int addr = 0;
unsigned char addrC = 0;
boolean btnSt = false;
memset(griperState,0,14); // reset griperState array
gripperDirectDetach();
// Serial.println("Recording");
while(digitalRead(buttonPin))
{
unsigned int leftServo = map(constrain(readAngle(SERVO_L), SERVO_MIN, SERVO_MAX), SERVO_MIN, SERVO_MAX, 0, 180);
unsigned int rightServo = map(constrain(readAngle(SERVO_R), SERVO_MIN, SERVO_MAX), SERVO_MIN, SERVO_MAX, 0, 180);
unsigned int rotServo = map(constrain(readAngle(SERVO_ROT), SERVO_MIN, SERVO_MAX), SERVO_MIN, SERVO_MAX, 0, 180);
data[0][addr] = leftServo;
data[1][addr] = rightServo;
data[2][addr] = rotServo;
//Serial.print(addr);Serial.print("\t");Serial.print("REC data-> left: "); Serial.print(leftServo);
//Serial.print("\tright: "); Serial.print(rightServo);
//Serial.print("\trot: "); Serial.println(rotServo);
if(!digitalRead(buttonPinC))
{
while(!digitalRead(buttonPinC));
delay(10);
if(btnSt)
{
gripperRelease();
alert(2, 50, 50);
}
else
{
gripperCatch();
alert(1, 50, 0);
}
btnSt = !btnSt;
}
if(addr%3==0) // 3 steps, save griper state to array
{
if(btnSt)
griperState[addrC / 8] |= (1