我目前正在使用Braccio机器人进行项目开发,我正在尝试让该机器人复制电位器以记录它们,然后使用数组重播它们。该机器人目前确实复制了电位计的动作,但是没有将其正确记录到数组中,也无法从数组中正确获取值。
#include <Servo.h> //Servo header file
#include <Braccio.h>
//Declare object for 5 Servo Motors
Servo base;
Servo shoulder;
Servo elbow;
Servo wrist_ver;
Servo wrist_rot;
Servo gripper;
//Global Variable Declaration
int S0_pos, S1_pos, S2_pos, S3_pos, S4_pos;
int POT_0,POT_1,POT_2,POT_3,POT_4;
// Variable used to set memory time limit of 100s for robot movments
int x = 0;
// An array for recording robot movements and an array for calling the recorded movements
int RecordingArray[]={};
int PlayArray0,PlayArray1,PlayArray2,PlayArray3,PlayArray4;
// array variables for record to save the movements for each limb of the robot
int RAV0 = 0;
int RAV1 = 1;
int RAV2 = 2;
int RAV3 = 3;
int RAV4 = 4;
// array variables to call the recorded movements for each limb
int PAV0 = 0;
int PAV1 = 1;
int PAV2 = 2;
int PAV3 = 3;
int PAV4 = 4;
const int RecordButtonPin = 2;
int HallEffectValue = 0;
const int HallEffectPin = 13;
const int startPoint = 11;
const int endPoint = 12;
int carstate = 0;
const int start=3;
int recorded = 0;
const int Forward_motion = 8;
const int Backward_motion = 4;
int PlayButtonReading = 0;
int PlayCounter = 0;
int RecordButtonState;
int moveit;
int tit = 0;
int y;
int z;
int add = 5;
// Limit of the memory for the play function
int I;
// Tenporary variable to call the play function untill the motor code is complete
int counter = 1;
void setup()
{
Braccio.begin();
Serial.begin(9600);
//Serial.println(F("'R' to Record 'P' to play")); //Instruct the user
pinMode(RecordButtonPin, INPUT);
pinMode(HallEffectPin, INPUT_PULLUP);
pinMode(endPoint, INPUT_PULLUP);
pinMode(startPoint, INPUT_PULLUP);
pinMode(start, INPUT);
pinMode(Forward_motion, OUTPUT);
pinMode(Backward_motion, OUTPUT);
}
void loop(){
int moveit = digitalRead(start);
if (moveit== HIGH)
{
record_Function();
}
if (moveit== LOW)
{
playFunction();
tit = 1;
}
}
void record_Function(){
for (;z< 500;z = z+1){
POT_0 = analogRead(A0); // Read the pot values from the microcontroller
POT_1 = analogRead(A1);
POT_2 = analogRead(A2);
POT_3 = analogRead(A3);
POT_4 = analogRead(A4);
S0_pos = map(POT_0,0,614,1,179); //Map it for 1st Servo
S1_pos = map(POT_1,0,614,1,179); //Map it for 2nd Servo
S2_pos = map(POT_2,0,614,1,179); //Map it for 3rd Servo
S3_pos = map(POT_3,0,614,1,179); //Map it for 4th Servo
S4_pos = map(POT_4,0,614,1,179); //Map it for 5th Servo
delay (10);
Braccio.ServoMovement(10 , S0_pos, S1_pos, S2_pos, S3_pos, S4_pos, 0);
//using pot values as the input for the robot servo motor values
RecordingArray[RAV0] = S0_pos; //Recording the postion of the pots in the record array every 10 ms
RecordingArray[RAV1] = S1_pos;
RecordingArray[RAV2] = S2_pos;
RecordingArray[RAV3] = S3_pos;
RecordingArray[RAV4] = S4_pos;
RAV0 = RAV0+5;
RAV0 = RAV1+5;
RAV0 = RAV2+5;
RAV0 = RAV3+5;
RAV0 = RAV4+5;
}
}
void playFunction(){
for (;y,500;y++){
PlayArray0 = RecordingArray[PAV0];
PlayArray1 = RecordingArray[PAV1];
PlayArray2 = RecordingArray[PAV2];
PlayArray3 = RecordingArray[PAV3];
PlayArray4 = RecordingArray[PAV4];
Braccio.ServoMovement(10 , PlayArray0,PlayArray1, PlayArray2, PlayArray3, PlayArray4, 0);
PAV0 = PAV0 + 5;
PAV1 = PAV1 + 5;
PAV2 = PAV2 + 5;
PAV3 = PAV3 + 5;
PAV4 = PAV4 + 5;
}
}