Arduino的控制（⼀）：Arduino步进电机六轴机械⼿（油管搬）/*
⽤简单的脚本移动6⾃由度机械⼿臂
*/
#include<math.h>
#define PI 3.1415926535897932384626433832795
//轴1的驱动程序
#define PUL1_PIN 39
#define DIR1_PIN 37
//轴2的驱动程序
#define PUL2_PIN 43
#define DIR2_PIN 41
/
/轴3的驱动程序
#define PUL3_PIN 47
#define DIR3_PIN 45
//轴4的驱动程序
#define PUL4_PIN 46
#define DIR4_PIN 48
//轴5的驱动程序
#define PUL5_PIN A6
#define DIR5_PIN A7
//轴6的驱动程序
#define PUL6_PIN A0
#define DIR6_PIN A1
//轴3,2和1的使能引脚
#define EN321_PIN 32
#define EN4_PIN A8
#define EN5_PIN A2
#define EN6_PIN 38
double curPos1 =0.0;
double curPos2 =0.0;
double curPos3 =0.0;
double curPos4 =0.0;
double curPos5 =0.0;
double curPos6 =0.0;
boolean PULstat1 =0;
boolean PULstat2 =0;
boolean PULstat3 =0;
boolean PULstat4 =0;
boolean PULstat5 =0;
boolean PULstat6 =0;
//机器⼈⼏何模型
const double dl1 =360.0/200.0/32.0/4.8;
const double dl2 =360.0/200.0/32.0/4.0;
const double dl3 =360.0/200.0/32.0/5.0;
const double dl4 =360.0/200.0/32.0/2.8;
const double dl5 =360.0/200.0/32.0/2.1;
const double dl6 =360.0/200.0/32.0/1.0;
const double r1 =47.0;
const double r2 =110.0;
const double r3 =26.0;
const double d1 =133.0;
const double d3 =0.0;
const double d4 =117.50;
const double d6 =28.0;
void tup()
fitu
{
pinMode(PUL1_PIN, OUTPUT);
pinMode(DIR1_PIN, OUTPUT);
pinMode(PUL2_PIN, OUTPUT);
pinMode(DIR2_PIN, OUTPUT);
pinMode(PUL3_PIN, OUTPUT);
pinMode(DIR3_PIN, OUTPUT);
pinMode(PUL4_PIN, OUTPUT);
pinMode(DIR4_PIN, OUTPUT);
pinMode(PUL5_PIN, OUTPUT);
pinMode(DIR5_PIN, OUTPUT);
pinMode(PUL6_PIN, OUTPUT);
pinMode(DIR6_PIN, OUTPUT);
pinMode(EN321_PIN, OUTPUT);
pinMode(EN4_PIN, OUTPUT);
pinMode(EN5_PIN, OUTPUT);
pinMode(EN6_PIN, OUTPUT);
digitalWrite(PUL1_PIN, LOW);// 齿轮速⽐= 96/20 = 4.8 digitalWrite(DIR1_PIN, LOW);//LOW为负向
packages是什么意思
digitalWrite(PUL2_PIN, LOW);// 齿数⽐为4 digitalWrite(DIR2_PIN, LOW);//LOW为负
digitalWrite(PUL3_PIN, LOW);//齿速⽐为5 digitalWrite(DIR3_PIN, LOW);//LOW = 反⽅向digitalWrite(PUL4_PIN, LOW);// 齿数⽐为2.8 digitalWrite(DIR4_PIN, LOW);//LOW = positive direction
digitalWrite(PUL5_PIN, LOW);// gear ratio = 42/20 = 2.1 digitalWrite(DIR5_PIN, LOW);//LOW = positi
ve directionfrench kiss
digitalWrite(PUL6_PIN, LOW);// gear ratio = 1 digitalWrite(DIR6_PIN, LOW);//LOW = positive direction
// 所有关节关闭
digitalWrite(EN321_PIN, HIGH);
digitalWrite(EN4_PIN, HIGH);
修图培训digitalWrite(EN5_PIN, HIGH);
digitalWrite(EN6_PIN, HIGH);
purple是什么意思//Serial.begin(9600);
}
void loop()
{
// 打开所有关节
delay(10000);
digitalWrite(EN321_PIN, LOW);
digitalWrite(EN4_PIN, LOW);
digitalWrite(EN5_PIN, LOW);
digitalWrite(EN6_PIN, LOW);
delay(1000);
// 回到坐标位置(所有关节等于0)
// joint #2
digitalWrite(DIR2_PIN, HIGH);
int delValue=4000;
int incValue =7;
int accRate=530;
int totSteps=2791*2;
for(int i=0; i < totSteps; i++)
{
equalsif(totSteps >(2*accRate +1))//#1
{
if(i < accRate)
{
//加速
delValue = delValue - incValue;//4000-7 = 3993 }
el if(i >(totSteps - accRate))
{
//减速
delValue = delValue + incValue;
}
}
信仰的英文el//#1
{
//⽆适当的加速度/减速空间
if(i <((totSteps -(totSteps %2))/2))
{
/
/加速
delValue = delValue - incValue;
}
el if(i >((totSteps +(totSteps %2))/2))
{
//减速
delValue = delValue + incValue;
}
}
digitalWrite(PUL2_PIN, HIGH); delayMicroconds(delValue);//delValue = 3997 digitalWrite(PUL2_PIN, LOW); delayMicroconds(delValue);
}
// joint #3
digitalWrite(DIR3_PIN, HIGH);
delValue=4000;
incValue=7;
accRate=530;
totSteps=6569;
for(int i=0; i < totSteps; i++)
{
if(totSteps >(2*accRate +1)){
if(i < accRate){
//acceleration
delValue = delValue - incValue;
}el if(i >(totSteps - accRate)){
//decceleration
delValue = delValue + incValue;
}
}el{
//no space for proper acceleration/decceleration if(i <((totSteps -(totSteps %2))/2)){
//acceleration
delValue = delValue - incValue;
}el if(i >((totSteps +(totSteps %2))/2)){
//decceleration
delValue = delValue + incValue;
}
}
digitalWrite(PUL3_PIN, HIGH); delayMicroconds(delValue);
digitalWrite(PUL3_PIN, LOW); delayMicroconds(delValue);
}
// joint #5
digitalWrite(DIR5_PIN, HIGH);
delValue=4000;
incValue=7;
totSteps=90/dl5;
for(int i=0; i < totSteps; i++)
{
if(totSteps >(2*accRate +1)){
if(i < accRate){
//acceleration
delValue = delValue - incValue;
}el if(i >(totSteps - accRate)){
//decceleration
delValue = delValue + incValue;
}
}el{
/
/no space for proper acceleration/decceleration
if(i <((totSteps -(totSteps %2))/2)){
//acceleration
delValue = delValue - incValue;
}el if(i >((totSteps +(totSteps %2))/2)){
//decceleration
delValue = delValue + incValue;
}
}
digitalWrite(PUL5_PIN, HIGH);
delayMicroconds(delValue);
digitalWrite(PUL5_PIN, LOW);
delayMicroconds(delValue);
}
//--------------------------------------------------------GoGoGo-------------------
curPos1=0.0;
curPos2=0.0;
curPos3=0.0;
curPos4=0.0;
curPos5=90.0;
curPos6=0.0;
float Xhome[6]={164.5,0.0,241.0,90.0,180.0,-90.0};//{x, y, z, ZYZ 欧拉⾓}
float X1[6]={164.5,0.0,141.0,90.0,180.0,-90.0};
英语早教机构float X11[6]={164.5+14.7,35.4,141.0,90.0,180.0,-90.0};
float X12[6]={164.5+50.0,50.0,141.0,90.0,180.0,-90.0};
float X13[6]={164.5+85.3,35.4,141.0,90.0,180.0,-90.0};
float X14[6]={164.5+100.0,0.0,141.0,90.0,180.0,-90.0};
float X15[6]={164.5+85.3,-35.4,141.0,90.0,180.0,-90.0};
float X16[6]={164.5+50.0,-50.0,141.0,90.0,180.0,-90.0};
float X17[6]={164.5+14.7,-35.4,141.0,90.0,180.0,-90.0};
float X18[6]={164.5+50.0,0.0,141.0,90.0,180.0,-90.0};
float X2[6]={264.5,0.0,141.0,0.0,90.0,0.0};
float X3[6]={164.5,100.0,141.0,90.0,90.0,0.0};
float X4[6]={164.5,-100.0,141.0,90.0,-90.0,0.0};
float Jhome[6], J1[6], J11[6], J12[6], J13[6], J14[6], J15[6], J16[6], J17[6], J18[6], J2[6], J3[6], J4[6]; InverK(Xhome, Jhome);
InverK(X1, J1);
InverK(X11, J11);
InverK(X12, J12);
InverK(X13, J13);
InverK(X14, J14);
InverK(X15, J15);
InverK(X16, J16);
InverK(X17, J17);一切顺利英文
InverK(X18, J18);
InverK(X2, J2);
InverK(X3, J3);
goStrightLine(Jhome, J1,0.25e-4,0.75e-10,0.0,0.0);
float velG=0.25e-4;
goStrightLine(J1, J11,0.25e-4,0.75e-10,0.0,0.5*velG); goStrightLine(J11, J12,0.25e-4,0.75e-10,0.5*velG,0.5*velG); goStrightLine(J12, J13,0.25e-4,0.75e-10,0.5*velG,0.5*velG); goStrightLine(J13, J14,0.25e-4,0.75e-10,0.5*velG,0.5*velG); goStrightLine(J14, J15,0.25e-4,0.75e-10,0.5*velG,0.5*velG); goStrightLine(J15, J16,0.25e-4,0.75e-10,0.5*velG,0.5*velG); goStrightLine(J16, J17,0.25e-4,0.75e-10,0.5*velG,0.5*velG); goStrightLine(J17, J1,0.25e-4,0.75e-10,0.5*velG,0.0);
goStrightLine(J1, J18,0.25e-4,0.75e-10,0.0,0.8*velG); goStrightLine(J18, J14,0.25e-4,0.75e-10,0.8*velG,0.0); goStrightLine(J14, J1,0.25e-4,0.75e-10,0.0,0.0);
goStrightLine(J1, J2,0.25e-4,0.75e-10,0.0,0.0);
goStrightLine(J2, J1,0.25e-4,0.75e-10,0.0,0.0);
goStrightLine(J1, J3,0.25e-4,0.75e-10,0.0,0.0);
goStrightLine(J3, J1,0.25e-4,0.75e-10,0.0,0.0);
goStrightLine(J1, J4,0.25e-4,0.75e-10,0.0,0.0);jasmine
goStrightLine(J4, J1,0.25e-4,0.75e-10,0.0,0.0);
goStrightLine(J1, Jhome,0.25e-4,0.75e-10,0.0,0.0);
//--------------------------------------------------------GoGoGoBack---------------
// 从原来的位置回到折叠的位置
// joint #5
digitalWrite(DIR5_PIN, LOW);
delValue=4000;
incValue=7;
accRate=530;
totSteps=90/dl5;
for(int i=0; i < totSteps; i++)
{
if(totSteps >(2*accRate +1)){
if(i < accRate){
//acceleration
delValue = delValue - incValue;
}el if(i >(totSteps - accRate)){
//decceleration
delValue = delValue + incValue;
}
}el{
//no space for proper acceleration/decceleration
if(i <((totSteps -(totSteps %2))/2)){
//acceleration
delValue = delValue - incValue;
}el if(i >((totSteps +(totSteps %2))/2)){
//decceleration
delValue = delValue + incValue;
}
}
digitalWrite(PUL5_PIN, HIGH);
delayMicroconds(delValue);
digitalWrite(PUL5_PIN, LOW);
delayMicroconds(delValue);
}
// joint #3
digitalWrite(DIR3_PIN, LOW);
delValue=4000;
