Uarm librerias

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 :


*******************************************************************************

* 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 :


*******************************************************************************/

#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);


{

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;


{

delay(20);

// buzzer alert

alert(1, 20, 0);

delay(500); //SG-> Added to delay for user to remove hand

// buzzer alert



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


{

alert(2, 200, 100);

while(!digitalRead(BTN_D4));

delay(50);

servoL.detach();

servoR.detach();

servoRot.detach();

servoHandRot.detach();

while(1)

{

// D4 button - recording


{

recordFlag = true;

alert(1, 50, 0);

lstTime = millis();


{


{

recordFlag = false;

writeEEPROM();

alert(1, 300, 0);


}

}

delay(20);

if(recordFlag)

record(BTN_D4, BTN_D7);

}

// D7 button - play


{

playFlag = false;

alert(1, 100, 0);

if(firstFlag)

{

readEEPROM();

firstFlag = false;

}

lstTime = millis();


{


{

playFlag = true;

alert(1, 300, 0);


}

}

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;


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


}

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()

{


}

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;


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



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

Uarm librerias

Documents

Transcript of Uarm librerias