Arduino红外传感器控制机械臂程序程序有错误请大家来讨论一下

ID:226815 · 发表于 2018-4-13 14:22

用红外避障传感器控制机械臂抓取物体。
当红外避障传感器检测到物体时，输入低电平。然后调用函数机械臂抓取物体。
但是程序有错误，找了半天也没找出来。请大家来讨论一下。

Arduino源程序如下

#include <Servo.h> //使用servo库
const int Aarm = 0;
const int Barm = 1;
const int Carm = 2;
const int Darm = 3;
const int Earm = 4;
const int Farm = 5;
const int maxServos = 6;
Servo servos[maxServos];
int DSD = 15; //Default Servo Delay (默认电机运动延迟时间)
//此变量用于控制电机运行速度.增大此变量数值将
//降低电机运行速度从而控制机械臂动作速度。
int val_left=0;
int val_centre=0;
int val_Right=0;
void setup(){
pinMode(A0,INPUT);
pinMode(A1,INPUT);
pinMode(A2,INPUT);
for(int i=0;i<=5;i++){
servos[i].attach(i);
servos[i].write(90);
delay(10);
}
}
void loop(){
val_left = digitalRead(A0);
val_centre= digitalRead(A0);
val_Right = digitalRead(A0);
if (val_left == LOW) { //例如:b67三个字节
servos[0].write(); //底盘向左旋转
Servo_Stretch(); //舵机抓取物体
servos[0].write(); //舵机转回中位
armIniPos(); //舵机收缩
}
else if(val_centre == LOW){
Servo_Stretch(); //舵机抓取物体
armIniPos(); //舵机收缩
}
else if(val_Right == LOW){
servos[0].write(); //底盘向左旋转
Servo_Stretch(); //舵机抓取物体
servos[0].write(); //舵机转回中位
armIniPos(); //舵机收缩
}
}
void armIniPos(){ //舵机收缩回起始位置
int robotIniPos[5][3] = { //每个舵机角度不同，需要修改
{Barm, 90, DSD},
{Carm, 90, DSD},
{Darm, 90, DSD},
{Earm, 90, DSD},
{Farm, 90, DSD}
};
for (int i = 0; i < 5; i++){
servoCmd(robotIniPos[i][0], robotIniPos[i][1], robotIniPos[i][2]); //舵机编号，目标角度，延迟.此函数控制舵机的速度
}
}
void servoCmd(int j,int toPos, int servoDelay){ //舵机编号，目标角度，延迟.此函数控制舵机的速度
int fromPos;
fromPos = servos(j).read();
if (fromPos <= toPos){ //如果“起始角度值”小于“目标角度值”
for (int i=fromPos; i<=toPos; i++){
servos(j).write(i);
delay (servoDelay);
}
} else { //否则“起始角度值”大于“目标角度值”
for (int i=fromPos; i>=toPos; i--){
servos(j).write(i);
delay (servoDelay);
}
}
}
void Servo_Stretch(){
int diceMove1[5][3] = {
{Barm, 30, DSD},
{Carm, 90, DSD},
{Darm, 43, DSD},
{Earm, 126, DSD},
{Farm, 90, DSD}
};
for (int i = 0; i < 5; i++){
servoCmd(diceMove1[i][0], diceMove1[i][1], diceMove1[i][2]);
delay(200);
}

复制代码

ID:309562 · 发表于 2018-4-16 20:45

C:\Users\Administrator\Desktop
红框中的分号“；”应该去掉，大括号后不需要“；”

帐号		自动登录	找回密码
密码			立即注册

Arduino红外传感器控制机械臂程序 程序有错误请大家来讨论一下

Arduino红外传感器控制机械臂程序程序有错误请大家来讨论一下