Let's Make Robots! | RobotShop

Micro Servo Robot

 
AttachmentSize
MiniRobot_1_3.ino_.zip2.73 KB
MiniRobot_1_3EN.ino_.zip3.52 KB
Micro_Servo_Robot_Flowchart.pdf84.04 KB
Micro_Servo_Robot_Diagramm.pdf74.13 KB

4 axis robot arm with a small gripper and teach function. (In fact: it is a 3 axis robot)

This robot can follow. This robot can learn. And this robot can repeat endles. And he lost all his memory if the batterys are low. This makes it new all the time he is powered again. - The idea for this project is taken from https://www.robotshop.com/letsmakerobots/node/21633 - Stoerpeak made me do it! -

Arduino mini pro 5V/16MHz

4 micro servos 180degrees

4 22k potentiometer

NmH Battery 4.8V

Wires, Button, Switch

Balsa wood, Metal, Plastic

Zip Ties and Glue

 

Teach mode: After a reset the robot arm follows the teach in arm while simple mapping the analog inputs every 25ms to the servo motors. Pressing the button stores each servo position in a array

Play mode: Double press the button switch to play mode. The sketch reads the array step by step and and moves the robot arm. For cool loocking movements i added a routine calculates different micro steps for each servo to have moving start and end sync on all axis. Also added a ramp for soft increase/decrease velocity. Shorter travel distances the robot does slow, longer distances with faster speed. Its all about timing so my thoughts in this moment

Play Mode version 1.1 The gripper input is used to set the delay (0,1,3,15,60,300 seconds) after a loop is done. The switch (it was left from the project start) pauses the robot

08/2015 i added the same code 1.3 but i try add english comments

10/2015 flowchart for the code and electrical diagram. (make sure you use a pullUp resistor at input 4,6 / change the code for input 4. This should fix the 'slow running problem' often happend)

 

Hey people!!: If u share my code please keep decency and give me credits. And dont forget Stoerpeak!

Comment viewing options

Select your preferred way to display the comments and click "Save settings" to activate your changes.

Hello. I tried to change the code for 6 servos but i cant move the arm . After i load the code, the led(13) remain opened and the arm blocked . Can you help me with the code? Main code:

#include <Servo.h> Servo servo_0; Servo servo_1; Servo servo_2; Servo servo_3; Servo servo_4; int count0, arrayStep, arrayMax, countverz, Taster, stepsMax, steps, time = 1000, del = 1000, temp; unsigned int verz = 0; long previousMillis1 = 0; long previousMillis2 = 0; long previousMillis3 = 0; long previousMillis4 = 0; long previousMillis5 = 0; long previousMillis6 = 0; long previousMicros = 0; unsigned long currentMillis = millis(); unsigned long currentMicros = micros(); int Delay[7] = {0,0,1,3,15,60,300}; int SensVal[6]; float dif[6], ist[6], sol[6], dir[6]; int joint0[180]; int joint1[180]; int joint2[180]; int joint3[180]; int joint4[180]; int joint5[180]; int top = 179; boolean playmode = false, Step = false; void setup() { pinMode(7, INPUT); pinMode(12, INPUT); pinMode(13, OUTPUT); digitalWrite(13, HIGH); servo_0.attach(3); servo_1.attach(6); servo_2.attach(10); servo_3.attach(5); servo_4.attach(11); servo_5.attach(9); Serial.begin(9600); Serial.println("mini robot ready..."); 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) { if (arrayStep < top) { previousMillis1 = currentMillis; readPot(); mapping(); move_servo(); //record(); } } } else if(playmode) { if (Step) { digitalWrite(13, HIGH); if (arrayStep < arrayMax) { arrayStep += 1; Read(); calculate(); Step = 0; digitalWrite(13, LOW); } else // array read finished > start over { arrayStep = 0; calc_pause(); countverz = 0; while(countverz < verz) { 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) { // previousMicros = currentMicros; play_servo(); } } } while (digitalRead(7) == true) { digitalWrite(13, HIGH); delay(500); digitalWrite(13, LOW); delay(500); } } void calc_pause() { readPot(); temp = SensVal[6];/ if (temp < 0) temp = 0; temp = map(temp, 0, 680, 0 ,6); verz = Delay[temp]; // verz = delay in second /*Serial.print(temp); Serial.print(" "); Serial.print(verz); Serial.print(" "); Serial.println(countverz);*/ } void readPot() { SensVal[0] = analogRead(sensorPin0); SensVal[0] += 0; SensVal[1] = analogRead(sensorPin1); SensVal[1] += 0; SensVal[2] = analogRead(sensorPin2); SensVal[2] += 0; SensVal[3] = analogRead(sensorPin3); SensVal[3] += 0; SensVal[4] = analogRead(sensorPin4); SensVal[4] += 0; SensVal[5] = analogRead(sensorPin5); SensVal[5] += 0; //Serial.print(SensVal[2]);Serial.print(" "); // CHECK } void mapping() { ist[0] = map(SensVal[0], 0, 1023,600, 2400);// rotire ist[1] = map(SensVal[1], 0, 1023, 550, 2400);// cot ist[2] = map(SensVal[2], 120, 860, 400, 2500);// umar ist[3] = map(SensVal[3], 0, 1023, 500, 2500);// baza ist[4] = map(SensVal[4], 120, 860, 400, 2500);// incheietura ist[5] = map(SensVal[5], 0, 1023, 500, 2500);// gheara //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]; joint4[arrayStep] = ist[4]; joint5[arrayStep] = ist[5]; } void Read() { sol[0] = joint0[arrayStep]; // read from the array sol[1] = joint1[arrayStep]; sol[2] = joint2[arrayStep]; sol[3] = joint3[arrayStep]; sol[4] = joint4[arrayStep]; sol[5] = joint5[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]); servo_4.writeMicroseconds(ist[4]);/////? servo_5.writeMicroseconds(ist[5]);//////? } 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]); dif[4] = abs(ist[4]-sol[4]); dif[5] = abs(ist[5]-sol[5]); // biggest travel way from all 4 servos stepsMax = max(dif[0],dif[1]); stepsMax = max(stepsMax,dif[2]); stepsMax = max(stepsMax,dif[3]); stepsMax = max(stepsMax,dif[4]); stepsMax = max(stepsMax,dif[5]); stepsMax /= 12; if (stepsMax < 500) del = 650; else del = 301; 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; if (sol[4] < ist[4]) dir[4] = 0-dif[4]/stepsMax; else dir[4] = dif[4]/stepsMax; if (sol[5] < ist[5]) dir[5] = 0-dif[5]/stepsMax; else dir[5] = dif[5]/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; else if(steps == 40) time = del*3; else if(steps == 80) time = del*2; else if(steps == 100) time = del-1; // cannot explain here is not del*1 if(steps == stepsMax-200) time = del*2; 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]; ist[4] += dir[4]; ist[5] += dir[5]; servo_0.writeMicroseconds(ist[3]); // Zange servo_1.writeMicroseconds(ist[2]); // Hand servo_2.writeMicroseconds(ist[0]); // Schulter servo_3.writeMicroseconds(ist[1]); // Ellbogen servo_4.writeMicroseconds(ist[4]); // Schulter servo_5.writeMicroseconds(ist[5]); // Ellbogen } else { Step = 1; steps = 0; } } 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(12) == false) { delay(20);/ if (digitalRead(12) == true) { 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) { Taster = 0; //Serial.println("restart "); } }

if(steps == 20) time = del*4;    
else if(steps == 40) time = del*3;
else if(steps == 80) time = del*2;
else if(steps == 100) time = del-1;

hello Pinaut, i a newbie  , i don't know the word "time" had highlight orange. what function ? and 4th line, why not time = del*1 ? tks you so much

Hi, do you have any suggestion for me? my servos are too wild and shaky. They are totally not following the servos. Is it because I used 10k potentiometers? And I wanted it to saved in EEPROM or any storage, do you have any reference you can share with me? thank you :)

hi, any suggestion? my servos are too wild and shaky, Is it because I used 10k ptentiometers? And I wanted it to save it in EEPROM, but i don't have any idea about it. So I was thinking if you have any refrence you could share to me? thank you :)

hi, any suggestion? my servos are too wild and shaky, Is it because I used 10k ptentiometers? And I wanted it to save it in EEPROM, but i don't have any idea about it. So I was thinking if you have any refrence you could share to me? thank you :)

Hello What modifications do I need to do so that I can use this code in an arduino nano instead of a mini pro?

 @pinaut May I know your contact information? i have some questions want to ask you

Good day! Sir, can I use this project as an entry for our website. Rest assured that I am to credit the entry to you.