Components and supplies
5 mm LED: Red
Analog joystick (Generic)
Micro servo SG90
Resistor 10k ohm
Resistor 680 ohm
Arduino UNO
Apps and platforms
Arduino IDE
Project description
Code
Arduiono Joystick controller for MeArm ® robot with recording of coordinates. (version 1.3.1)
arduino
Use Arduino IDE for programming Arduiono UNO base board via USB port.
1/* meArm analog joysticks version 1.3.1 - UtilStudio.com Dec 2018 2 Uses two analogue joysticks and four servos. 3 4 In version 1.3 was improved recording of coordinates. 5 Some bugs was removed. 6 7 First joystick moves gripper forwards, backwards, left and right, 8 button start/stop recording positions. 9 10 Second joystick moves gripper up, down, and closes and opens, 11 button start/stop playing recorded positions. 12 Press button for 2 seconds to autoplay. 13 14 Pins: 15 Arduino Stick1 Stick2 Base Shoulder Elbow Gripper Record/ 16 GND GND GND Brown Brown Brown Brown Auto play 17 5V VCC VCC Red Red Red Red LED 18 A0 HOR 19 A1 VER 20 PD2 BUTT 21 A2 HOR 22 A3 VER 23 PD3 BUTT 24 11 Yellow 25 10 Yellow 26 9 Yellow 27 6 Yellow 28 PD4 X 29*/ 30#include <Servo.h> 31 32bool repeatePlaying = false; /* Repeatedly is running recorded cycle */ 33int delayBetweenCycles = 2000; /* Delay between cycles */ 34 35int basePin = 11; /* Base servo */ 36int shoulderPin = 10; /* Shoulder servo */ 37int elbowPin = 9; /* Elbow servo */ 38int gripperPin = 6; /* Gripper servo */ 39 40int xdirPin = 0; /* Base - joystick1*/ 41int ydirPin = 1; /* Shoulder - joystick1 */ 42int zdirPin = 3; /* Elbow - joystick2 */ 43int gdirPin = 2; /* Gripper - joystick2 */ 44 45//int pinRecord = A4; /* Button record - backward compatibility */ 46//int pinPlay = A5; /* Button play - backward compatibility */ 47int pinRecord = PD2; /* Button record - recommended (A4 is deprecated, will by used for additional joystick) */ 48int pinPlay = PD3; /* Button play - recommended (A5 is deprecated, will by used for additional joystick) */ 49int pinLedRecord = PD4; /* LED - indicates recording (light) or auto play mode (blink one) */ 50 51bool useInternalPullUpResistors = false; 52 53const int buffSize = 512; /* Size of recording buffer */ 54 55int startBase = 90; 56int startShoulder = 90; 57int startElbow = 90; 58int startGripper = 0; 59 60int posBase = 90; 61int posShoulder = 90; 62int posElbow = 90; 63int posGripper = 0; 64 65int lastBase = 90; 66int lastShoulder = 90; 67int lastElbow = 90; 68int lastGripper = 90; 69 70int minBase = 0; 71int maxBase = 150; 72int minShoulder = 0; 73int maxShoulder = 150; 74int minElbow = 0; 75int maxElbow = 150; 76int minGripper = 0; 77int maxGripper = 150; 78 79const int countServo = 4; 80int buff[buffSize]; 81int buffAdd[countServo]; 82int recPos = 0; 83int playPos = 0; 84 85int buttonRecord = HIGH; 86int buttonPlay = HIGH; 87 88int buttonRecordLast = LOW; 89int buttonPlayLast = LOW; 90 91bool record = false; 92bool play = false; 93bool debug = false; 94 95String command = "Manual"; 96int printPos = 0; 97 98int buttonPlayDelay = 20; 99int buttonPlayCount = 0; 100 101bool ledLight = false; 102 103Servo servoBase; 104Servo servoShoulder; 105Servo servoElbow; 106Servo servoGripper; 107 108void setup() { 109 Serial.begin(9600); 110 111 if (useInternalPullUpResistors) { 112 pinMode(pinRecord, INPUT_PULLUP); 113 pinMode(pinPlay, INPUT_PULLUP); 114 } 115 else 116 { 117 pinMode(pinRecord, INPUT); 118 pinMode(pinPlay, INPUT); 119 } 120 121 pinMode(xdirPin, INPUT); 122 pinMode(ydirPin, INPUT); 123 pinMode(zdirPin, INPUT); 124 pinMode(gdirPin, INPUT); 125 126 pinMode(pinLedRecord, OUTPUT); 127 128 servoBase.attach(basePin); 129 servoShoulder.attach(shoulderPin); 130 servoElbow.attach(elbowPin); 131 servoGripper.attach(gripperPin); 132 133 StartPosition(); 134 135 digitalWrite(pinLedRecord, HIGH); 136 delay(1000); 137 digitalWrite(pinLedRecord, LOW); 138} 139 140void loop() { 141 142 buttonRecord = digitalRead(pinRecord); 143 buttonPlay = digitalRead(pinPlay); 144 145 // Serial.print(buttonRecord); 146 // Serial.print("\ "); 147 // Serial.println(buttonPlay); 148 // for testing purposes 149 150 if (buttonPlay == LOW) 151 { 152 buttonPlayCount++; 153 154 if (buttonPlayCount >= buttonPlayDelay) 155 { 156 repeatePlaying = true; 157 } 158 } 159 else buttonPlayCount = 0; 160 161 if (buttonPlay != buttonPlayLast) 162 { 163 if (record) 164 { 165 record = false; 166 } 167 168 if (buttonPlay == LOW) 169 { 170 play = !play; 171 repeatePlaying = false; 172 173 if (play) 174 { 175 StartPosition(); 176 } 177 } 178 } 179 180 if (buttonRecord != buttonRecordLast) 181 { 182 if (buttonRecord == LOW) 183 { 184 record = !record; 185 186 if (record) 187 { 188 play = false; 189 repeatePlaying = false; 190 recPos = 0; 191 } 192 else 193 { 194 if (debug) PrintBuffer(); 195 } 196 } 197 } 198 199 buttonPlayLast = buttonPlay; 200 buttonRecordLast = buttonRecord; 201 202 float dx = map(analogRead(xdirPin), 0, 1023, -5.0, 5.0); 203 float dy = map(analogRead(ydirPin), 0, 1023, 5.0, -5.0); 204 float dz = map(analogRead(zdirPin), 0, 1023, 5.0, -5.0); 205 float dg = map(analogRead(gdirPin), 0, 1023, 5.0, -5.0); 206 207 if (abs(dx) < 1.5) dx = 0; 208 if (abs(dy) < 1.5) dy = 0; 209 if (abs(dz) < 1.5) dz = 0; 210 if (abs(dg) < 1.5) dg = 0; 211 212 posBase += dx; 213 posShoulder += dy; 214 posElbow += dz; 215 posGripper += dg; 216 217 if (play) 218 { 219 if (playPos >= recPos) { 220 playPos = 0; 221 222 if (repeatePlaying) 223 { 224 delay(delayBetweenCycles); 225 StartPosition(); 226 } 227 else 228 { 229 play = false; 230 } 231 } 232 233 bool endOfData = false; 234 235 while (!endOfData) 236 { 237 if (playPos >= buffSize - 1) break; 238 if (playPos >= recPos) break; 239 240 int data = buff[playPos]; 241 int angle = data & 0xFFF; 242 int servoNumber = data & 0x3000; 243 endOfData = data & 0x4000; 244 245 switch (servoNumber) 246 { 247 case 0x0000: 248 posBase = angle; 249 break; 250 251 case 0x1000: 252 posShoulder = angle; 253 break; 254 255 case 0x2000: 256 posElbow = angle; 257 break; 258 259 case 0x3000: 260 posGripper = angle; 261 dg = posGripper - lastGripper; 262 break; 263 } 264 265 playPos++; 266 } 267 } 268 269 if (posBase > maxBase) posBase = maxBase; 270 if (posShoulder > maxShoulder) posShoulder = maxShoulder; 271 if (posElbow > maxElbow) posElbow = maxElbow; 272 if (posGripper > maxGripper) posGripper = maxGripper; 273 274 if (posBase < minBase) posBase = minBase; 275 if (posShoulder < minShoulder) posShoulder = minShoulder; 276 if (posElbow < minElbow) posElbow = minElbow; 277 if (posGripper < minGripper) posGripper = minGripper; 278 279 servoBase.write(posBase); 280 servoShoulder.write(posShoulder); 281 servoElbow.write(posElbow); 282 283 bool waitGripper = false; 284 if (dg < 0) { 285 posGripper = minGripper; 286 waitGripper = true; 287 } 288 else if (dg > 0) { 289 posGripper = maxGripper; 290 waitGripper = true; 291 } 292 293 servoGripper.write(posGripper); 294 if (play && waitGripper) 295 { 296 delay(1000); 297 } 298 299 if ((lastBase != posBase) | (lastShoulder != posShoulder) | (lastElbow != posElbow) | (lastGripper != posGripper)) 300 { 301 if (record) 302 { 303 if (recPos < buffSize - countServo) 304 { 305 int buffPos = 0; 306 307 if (lastBase != posBase) 308 { 309 buffAdd[buffPos] = posBase; 310 buffPos++; 311 } 312 313 if (lastShoulder != posShoulder) 314 { 315 buffAdd[buffPos] = posShoulder | 0x1000; 316 buffPos++; 317 } 318 319 if (lastElbow != posElbow) 320 { 321 buffAdd[buffPos] = posElbow | 0x2000; 322 buffPos++; 323 } 324 325 if (lastGripper != posGripper) 326 { 327 buffAdd[buffPos] = posGripper | 0x3000; 328 buffPos++; 329 } 330 331 buffAdd[buffPos - 1] = buffAdd[buffPos - 1] | 0x4000; 332 333 for (int i = 0; i < buffPos; i++) 334 { 335 buff[recPos + i] = buffAdd[i]; 336 } 337 338 recPos += buffPos; 339 } 340 } 341 342 command = "Manual"; 343 printPos = 0; 344 345 if (play) 346 { 347 command = "Play"; 348 printPos = playPos; 349 } 350 else if (record) 351 { 352 command = "Record"; 353 printPos = recPos; 354 } 355 356 Serial.print(command); 357 Serial.print("\ "); 358 Serial.print(printPos); 359 Serial.print("\ "); 360 Serial.print(posBase); 361 Serial.print("\ "); 362 Serial.print(posShoulder); 363 Serial.print("\ "); 364 Serial.print(posElbow); 365 Serial.print("\ "); 366 Serial.print(posGripper); 367 Serial.print("\ "); 368 Serial.print(record); 369 Serial.print("\ "); 370 Serial.print(play); 371 Serial.println(); 372 } 373 374 lastBase = posBase; 375 lastShoulder = posShoulder; 376 lastElbow = posElbow; 377 lastGripper = posGripper; 378 379 if ( repeatePlaying) 380 { 381 ledLight = !ledLight; 382 } 383 else 384 { 385 if (ledLight) 386 { 387 ledLight = false; 388 } 389 390 if (record) 391 { 392 ledLight = true; 393 } 394 }; 395 396 digitalWrite(pinLedRecord, ledLight); 397 delay(50); 398} 399 400void PrintBuffer() 401{ 402 for (int i = 0; i < recPos; i++) 403 { 404 int data = buff[i]; 405 int angle = data & 0xFFF; 406 int servoNumber = data & 0x3000; 407 bool endOfData = data & 0x4000; 408 409 Serial.print("Servo="); 410 Serial.print(servoNumber); 411 Serial.print("\ Angle="); 412 Serial.print(angle); 413 Serial.print("\ End="); 414 Serial.print(endOfData); 415 Serial.print("\ Data="); 416 Serial.print(data, BIN); 417 Serial.println(); 418 } 419} 420 421void StartPosition() 422{ 423 int angleBase = servoBase.read(); 424 int angleShoulder = servoShoulder.read(); 425 int angleElbow = servoElbow.read(); 426 int angleGripper = servoGripper.read(); 427 428 Serial.print(angleBase); 429 Serial.print("\ "); 430 Serial.print(angleShoulder); 431 Serial.print("\ "); 432 Serial.print(angleElbow); 433 Serial.print("\ "); 434 Serial.print(angleGripper); 435 Serial.println("\ "); 436 437 posBase = startBase; 438 posShoulder = startShoulder; 439 posElbow = startElbow; 440 posGripper = startGripper; 441 442 servoBase.write(posBase); 443 servoShoulder.write(posShoulder); 444 servoElbow.write(posElbow); 445 servoGripper.write(posGripper); 446}
Downloadable files
MeArm robot - joystick servo recorder - test board
Testing board with only two servos connected. See schematic diagram to connect 4 servo motors.
MeArm robot - joystick servo recorder - test board
Joystick controller for MeArm ® robot - shematic diagram (version 1.3)
Joystick controller for MeArm ® robot with recording of coordinates - shematic diagram
Joystick controller for MeArm ® robot - shematic diagram (version 1.3)
Joystick controller for MeArm ® robot - shematic diagram (version 1.3)
Joystick controller for MeArm ® robot with recording of coordinates - shematic diagram
Joystick controller for MeArm ® robot - shematic diagram (version 1.3)
MeArm robot - joystick servo recorder - test board
Testing board with only two servos connected. See schematic diagram to connect 4 servo motors.
MeArm robot - joystick servo recorder - test board
Comments
Only logged in users can leave comments