아두이노 로봇 팔 레코드 & 플레이 소스
https://www.instructables.com/id/How-to-Make-Record-and-Play-Servo-Based-Robotic-Ar/
https://www.robotshop.com/community/robots/show/micro-servo-robot
https://www.robotshop.com/community/robots/show/mini-robotic-arm
이 프로젝트에서는 4 개의 미니 서보와 4 개의 전위차계를 사용했습니다.
디지털 핀 4, 6 입력 설정
디지털 핀 13 입력 설정
디지털 핀 13 값을 HIGH로 전달하여 led를 켠다.
디지털 핀 3, 10, 5, 11 서보모터 4대 조종할 출력 핀으로 설정
아날로그 핀 0, 1, 2, 3 저항센서 입력 설정
/* serial config: Board: Arduiono Pro / Pro Mini Port: tty.usbseriala400eMNr Programmer: USBtinyISP */
// Definitionen #include <Servo.h> // servo treiber
Servo servo_0; Servo servo_1; Servo servo_2; Servo servo_3;
int sensorPin0 = A0; // Schulter int sensorPin1 = A1; // Handfind int sensorPin2 = A2; // Ellbogen int sensorPin3 = A3; // Zange int count0, arrayStep, arrayMax, countverz, Taster, stepsMax, steps, time = 1000, del = 1000, temp; //arraystep = memory what pos in the array //arrayMax = max steps we safed to array //countverz = seems to be something to calculate the delay between complete moves //Taster = Button //stepsMax = longest way a servo have to travel //steps = single steps for a move between stored positions unsigned int verz = 0;
long previousMillis1 = 0; long previousMillis2 = 0; long previousMillis3 = 0; long previousMillis4 = 0; long previousMicros = 0; unsigned long currentMillis = millis(); unsigned long currentMicros = micros();
// arrays int Delay[7] = {0,0,1,1,1,1,1}; // array to map gripper pot to delay in seconds int SensVal[4]; // sensor value float dif[4], ist[4], sol[4], dir[4]; // difference between stored position and momentary position int joint0[180];// array for servo(s) int joint1[180]; int joint2[180]; int joint3[180]; int top = 179; // we should not write over the end from a array // status boolean playmode = false, Step = false;
void setup() { pinMode(4, INPUT); // sets the digital pin 4 as input pinMode(6, INPUT); pinMode(13, OUTPUT); // sets the digital pin 13 as outtput digitalWrite(13, HIGH); // sets the LED on servo_0.attach(3); // attaches the servo servo_1.attach(10); servo_2.attach(5); servo_3.attach(11); Serial.begin(115200); // Baudrate have to be same on the IDE Serial.println("mini robot ready..."); //delay(1000); digitalWrite(13, LOW); }
void loop() // here we go! { currentMillis = millis(); // all is about timing currentMicros = micros();
// read the button Button();
if(!playmode) // manualy modus { if(currentMillis - previousMillis1 > 25) // 25miliseconds until next manual mode update { if (arrayStep < top) { previousMillis1 = currentMillis; //reset readPot(); // get the value from potentiometers mapping(); // map to milliseconds for servos move_servo(); // setz newservo position //record(); } // end counter < max } // end step check } // ende manualy move
else if(playmode) // play { if (Step) // next step read from array { digitalWrite(13, HIGH); //LED if (arrayStep < arrayMax) // we not reach the end from stored data { arrayStep += 1; // next array pos Read(); // from the arrays calculate(); // find biggest travel distance and calculate the other 3 servos (the have to do smaler steps to be finished at same time!) Step = 0; digitalWrite(13, LOW); } else // array read finished > start over { arrayStep = 0; // calc_pause(); // delay between moves read from potentiometer countverz = 0; // used for the delay while(countverz < verz) // verz = time getting from calc_pause(); { // here we do loop and wait until next start over countverz += 1; calc_pause(); digitalWrite(13, HIGH); delay(25); digitalWrite(13, LOW); delay(975); } } //Serial.println(arrayStep); } else // do the servos! { if (currentMicros - previousMicros > time) // here we do a single micro step { // previousMicros = currentMicros; play_servo(); } } }// ende playmode
// ---------------------------------------------------------------------------------Hardware pause switch PIN 6 while (digitalRead(4) == false) { digitalWrite(13, HIGH); delay(500); digitalWrite(13, LOW); delay(500); } // ---------------------------------------------------------------------------------- Textout serial // serial ausgabe 1 sek /*if(currentMillis - previousMillis2 > 5000) { previousMillis2 = currentMillis; /*count0 = 0; while(count0 < 4) { int val = SensVal[count0]; // val = map(val, 142, 888, 0, 180); Serial.println(val); //Serial.println("test"); count0 += 1; } Serial.println(playmode); Serial.println(arrayStep); Serial.println(arrayMax); Serial.println(" "); }*/ }
// ---------------------------------------------------------------------------------------- sub routinen void calc_pause() // read pot and map to usable delay time after a complete move is done { readPot(); temp = SensVal[3]; if (temp < 0) temp = 0; temp = map(temp, 0, 680, 0 ,5); verz = Delay[temp]; // verz = delay in second Serial.print(temp); Serial.print(" "); Serial.print(verz); Serial.print(" "); Serial.println(countverz); }
void readPot() // read analog inputs and add some offsets (mechanical corrections) { SensVal[0] = analogRead(sensorPin0); //SensVal[0] += -10; // rotate SensVal[1] = analogRead(sensorPin1); //SensVal[1] += 280; // Shoulder SensVal[2] = analogRead(sensorPin2); //SensVal[2] += -50; // hand SensVal[3] = analogRead(sensorPin3); // SensVal[3] += 0;// gripper Serial.print(SensVal[2]);Serial.print(" "); // CHECK } void mapping() // we need microsecond for the servos instead potentiometer values { ist[0] = map(SensVal[0], 150, 900, 600, 2400);// drehen ist[1] = map(SensVal[1], 1000, 100, 550, 2400);// Schulter ist[2] = map(SensVal[2], 120, 860, 400, 2500);// Hand ist[3] = map(SensVal[3], 1023, 0, 500, 2500);// Zange Serial.println(ist[2]); // CHECK } void record() { joint0[arrayStep] = ist[0]; // write positions in servo array joint1[arrayStep] = ist[1]; joint2[arrayStep] = ist[2]; joint3[arrayStep] = ist[3]; } void Read() { sol[0] = joint0[arrayStep]; // read from the array sol[1] = joint1[arrayStep]; sol[2] = joint2[arrayStep]; sol[3] = joint3[arrayStep]; } void move_servo() { servo_0.writeMicroseconds(ist[3]); // send milissecond values to servos servo_1.writeMicroseconds(ist[2]); servo_2.writeMicroseconds(ist[0]); servo_3.writeMicroseconds(ist[1]); }
// ------------------------------------------------------------ single steps calculating void calculate() { // travel distance for each servo dif[0] = abs(ist[0]-sol[0]); dif[1] = abs(ist[1]-sol[1]); dif[2] = abs(ist[2]-sol[2]); dif[3] = abs(ist[3]-sol[3]);
// biggest travel way from all 4 servos stepsMax = max(dif[0],dif[1]); stepsMax = max(stepsMax,dif[2]); stepsMax = max(stepsMax,dif[3]); // stepsMax is the biggest distance a servo have to do beween momentary position and new pos read from the array
//Serial.println(stepsMax);
if (stepsMax < 500) // del(ay) between a single step is bigger is move is smaler. just looks cool del = 1200; else del = 600;
// calculating single (micro) step for each servo // need that to do move all servos in a loop (stepsMax times done) with different values. // This makes all servos have done the traveling distance at same time if (sol[0] < ist[0]) dir[0] = 0-dif[0]/stepsMax; else dir[0] = dif[0]/stepsMax; if (sol[1] < ist[1]) dir[1] = 0-dif[1]/stepsMax; else dir[1] = dif[1]/stepsMax; if (sol[2] < ist[2]) dir[2] = 0-dif[2]/stepsMax; else dir[2] = dif[2]/stepsMax; if (sol[3] < ist[3]) dir[3] = 0-dif[3]/stepsMax; else dir[3] = dif[3]/stepsMax; //Serial.println(dir4);
} void play_servo() { steps += 1; if (steps < stepsMax) // sure we not reach the end from a move { //time = del*5;// anfahr rampe if(steps == 20) time = del*4; // ramp up else if(steps == 40) time = del*3; // time is the delay in microsecns we wait in the mainloop until else if(steps == 80) time = del*2; // a micro step will be done else if(steps == 100) time = del-1; // cannot explain here is not del*1
if(steps == stepsMax-200) time = del*2; // stop ramp down (200 microsteps before end time will be increased else if(steps == stepsMax-80) time = del*3; else if(steps == stepsMax-40) time = del*4; else if(steps == stepsMax-20) time = del*5;
ist[0] += dir[0]; // set new pos ist[1] += dir[1]; ist[2] += dir[2]; ist[3] += dir[3];
servo_0.writeMicroseconds(ist[3]); // Zange //anschlüsse gemappt! servo_1.writeMicroseconds(ist[2]); // Hand servo_2.writeMicroseconds(ist[0]); // Schulter servo_3.writeMicroseconds(ist[1]); // Ellbogen } else { Step = 1; // next step aus array lesen steps = 0; // servo zwischenschritte } }
void data_out() // just to write the recorded data to serial { int i = 0; while(i < arrayMax) { digitalWrite(13, HIGH); i += 1; Serial.print(joint0[i]); Serial.print(", "); } Serial.println("Joint0"); i = 0; while(i < arrayMax) { digitalWrite(13, HIGH); i += 1; Serial.print(joint1[i]); Serial.print(", "); } Serial.println("Joint1"); i = 0; while(i < arrayMax) { digitalWrite(13, HIGH); i += 1; Serial.print(joint2[i]); Serial.print(", "); } Serial.println("Joint2"); i = 0; while(i < arrayMax) { digitalWrite(13, HIGH); i += 1; Serial.print(joint3[i]); Serial.print(", "); } Serial.println("Joint3"); }
void Button() // check buttons for single and doubleclick { if (digitalRead(6) == false) { delay(1); if (digitalRead(6) == true) // taster losgelassen { if (Taster == 0) { Taster = 1; previousMillis3 = currentMillis; //Serial.print("Status Record "); Serial.println(Taster); } else if ((Taster == 1) && (currentMillis - previousMillis3 < 250)) { Taster = 2; //Serial.println(Taster); } /*else if ((Taster == 2) && (currentMillis - previousMillis3 < 500)) { Taster = 3; Serial.println(Taster); }*/ } }
if ((Taster == 1) && (currentMillis - previousMillis3 > 1000)) // write to array { arrayStep += 1; arrayMax = arrayStep; record(); Taster = 0; playmode = false; Serial.print("Record Step: "); Serial.println(arrayStep); digitalWrite(13, HIGH); delay(100); digitalWrite(13, LOW); } else if (Taster == 2) { arrayStep = 0; playmode = true; Taster = 0; Step = 1; Serial.println("playmode "); data_out(); delay(250); digitalWrite(13, LOW); } /*if (Taster == 3) { // ++ arrayStep // playmode = 1; Taster = 0; Serial.println("Clear "); }*/ if (currentMillis - previousMillis3 > 2000) // button Status clear { Taster = 0; //Serial.println("restart "); } }
|
댓글 0
번호 | 제목 | 글쓴이 | 날짜 | 조회 수 |
---|---|---|---|---|
공지 | 계획 | Progress | 2015.07.02 | 134901 |
9 | 고추말뚝박기 망치 | nanumi | 2019.04.30 | 149307 |
8 | 아두이노 함수 및 명령어 설명 | nanumi | 2018.11.12 | 168509 |
7 | 아두이노 로봇팔(robot arm) 소스 및 문제점 정리 | nanumi | 2018.11.12 | 137124 |
6 | fritzing 응답없음 현상(멈춤 현상) 해결하기 | nanumi | 2018.11.11 | 188090 |
» | 아두이노 로봇 팔 레코드 & 플레이 소스 | nanumi | 2018.11.11 | 137692 |