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