使用电位计进行Arduino Braccio录制

Question

我目前正在使用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;


}
}