Components and supplies
1
PVC Pipe 40mm Diameter
1
Arduino Leonardo
1
Arduino Due
2
SM-4303 Standard Servo Motor
100
Jumper wires (generic)
1
Hand Tools (Screwdrivers, Cutters, etc.)
192
Header Pins
1
CAD Application
1
Packaging Rubber Bands
2
PCB 8x2 cm
1
PVC Tube, 6mm diameter
1
PVC Pipe 25mm Diameter
1
Glass Nails
1
Paper Clips
14
SG90 Micro-servo motor
1
Staple Pins
1
Plastic Cable Ties
75
Balsa Wood parts
1
Crafting Tools (Drills and Accessories)
14
Flat Washer 1/4" Screw Size
1
Fishing Ropes
Apps and platforms
1
Arduino IDE
Project description
Code
Application Program - Arduino Leonardo
arduino
This one shall be loaded to Leonardo
1#include <Servo.h> 2 3/* Serial Comm.Baudrate Setting: ARDUINO LEONARDO */ 4 const int _BAUDRATE = 4800; 5/* End of Serial Comm.Baudrate Setting: ARDUINO LEONARDO */ 6 7/* Tag-Channel(Pin) Assignments and IO_Channel() Function: ARDUINO LEONARDO */ 8 const int _11FRZL = 0; // 11: Front Arm Radius ZL 9 const int _11FRZH = 1; // 11: Front Arm Radius ZH 10 const int _11FRTX = A2; // 11: Front Arm Radius Actual Pos 11 const int _11FRCM = 9; // 11: Front Arm Radius CMD 12 13 const int _11FUZL = 2; // 11: Front Arm Ulna ZL 14 const int _11FUZH = 3; // 11: Front Arm Ulna ZH 15 const int _11FUTX = A3; // 11: Front Arm Ulna Actual Pos 16 const int _11FUCM = 10; // 11: Front Arm Ulna CMD 17 18 const int _21UAZL = 4; // 21: Upper Arm ZL 19 const int _21UAZH = 5; // 21: Upper Arm ZH 20 const int _21UATX = A4; // 21: Upper Arm Actual Pos 21 const int _21UACM = 11; // 21: Upper Arm CMD 22 23 const int _31SHZL = 6; // 31: Shoulder ZL 24 const int _31SHZH = 7; // 31: Shoulder ZH 25 const int _31SHTX = A5; // 31: Shoulder Actual Pos 26 const int _31SHCM = 13; // 21: Shoulder CMD 27 28 const int _00XXXC = 8; // Execute Command (0: Stop/Idle, 1: Execute) 29 const int _00XXSP = A0; // Setpoint Register 30 const int _00XXCM = A1; // Command Selector (*1) 31 const int _00XXOK = 12; // Command Executed by Arduino Leonardo (*2-IMPORTANT) 32 33 int IO_Channel(String Tag) { /* VERIFIED FUNCTION: ARDUINO LEONARDO */ 34 /* This function is used to validate IO Channel for DI/DO/AO/PWM-O. 35 * If given tag is valid, respective channel number will be returned. 36 * Otherwise -1 will be returned as "error". 37 */ 38 if (Tag=="_11FRZL") return(_11FRZL); 39 if (Tag=="_11FRZH") return(_11FRZH); 40 if (Tag=="_11FRTX") return(_11FRTX); 41 if (Tag=="_11FRCM") return(_11FRCM); 42 if (Tag=="_11FUZL") return(_11FUZL); 43 if (Tag=="_11FUZH") return(_11FUZH); 44 if (Tag=="_11FUTX") return(_11FUTX); 45 if (Tag=="_11FUCM") return(_11FUCM); 46 if (Tag=="_21UAZL") return(_21UAZL); 47 if (Tag=="_21UAZH") return(_21UAZH); 48 if (Tag=="_21UATX") return(_21UATX); 49 if (Tag=="_21UACM") return(_21UACM); 50 if (Tag=="_31SHZL") return(_31SHZL); 51 if (Tag=="_31SHZH") return(_31SHZH); 52 if (Tag=="_31SHTX") return(_31SHTX); 53 if (Tag=="_31SHCM") return(_31SHCM); 54 if (Tag=="_00XXXC") return(_00XXXC); 55 if (Tag=="_00XXSP") return(_00XXSP); 56 if (Tag=="_00XXCM") return(_00XXCM); 57 if (Tag=="_00XXOK") return(_00XXOK); 58 return(-1); 59} /* End of "IO_Channel(String Tag)": ARDUINO LEONARDO */ 60/* End of Tag-Channel(Pin) Assignments and IO_Channel() Function: ARDUINO LEONARDO */ 61 62 63/* Servo _Definitions + CommandToServo() function: ARDUINO LEONARDO */ 64 Servo _11FR_Servo; 65 Servo _11FU_Servo; 66 Servo _21UA_Servo; 67 Servo _31SH_Servo; 68 69 void CommandToServo(String Tag, int Value) { /* VERIFIED FUNCTION: ARDUINO LEONARDO */ 70 /* This function is used to actuate PWM outputs to Servo Motors. */ 71 if (Tag=="_11FRCM") _11FR_Servo.write(Value); 72 if (Tag=="_11FUCM") _11FU_Servo.write(Value); 73 if (Tag=="_21UACM") _21UA_Servo.write(Value); 74 if (Tag=="_31SHCM") _31SH_Servo.write(Value); 75 } /* End of "CommandToServo()": ARDUINO LEONARDO */ 76/* End of Servo _Definitions + CommandToServo() function: ARDUINO LEONARDO */ 77 78 79/* Constant Parameters: ARDUINO LEONARDO */ 80 const int _DBPercent = 1; // % Deadband 81 const int _CCW = 1; // Servo Direction for Counter Clockwise 82 const int _CW = -1; // Servo Direction for Counter Clockwise 83/* End of Constant Parameters: ARDUINO LEONARDO */ 84 85 86/* Constant Parameters for Intercommunication: ARDUINO LEONARDO */ 87 const int _FA_Radius = 310; // Front Arm Radius Section 88 const int _FA_Ulna = 425; // Front Arm Ulna Section 89 const int _UpperArm = 540; // Upper Arm Section 90 const int _Shoulder = 655; // Front Arm Radius Section 91 const int _ICDB = 40; // Front Arm Radius Section 92 /* Since the signals cannot generate exact integer values, Section 93 * shall be identified by [ (SectionValue) +/- _ICDB ] reading. 94 */ 95/* End of Constant Parameters for Intercommunication: ARDUINO LEONARDO */ 96 97 98/* "void setup()": ARDUINO LEONARDO */ 99void setup() { 100 // TEST CODES: 101 102 // VERIFIED CODES: 103 pinMode(_11FRZL, INPUT_PULLUP); 104 pinMode(_11FRZH, INPUT_PULLUP); 105 pinMode(_11FUZL, INPUT_PULLUP); 106 pinMode(_11FUZH, INPUT_PULLUP); 107 pinMode(_21UAZL, INPUT_PULLUP); 108 pinMode(_21UAZH, INPUT_PULLUP); 109 pinMode(_31SHZL, INPUT_PULLUP); 110 pinMode(_31SHZH, INPUT_PULLUP); 111 pinMode(_00XXXC, INPUT_PULLUP); 112 pinMode(_00XXOK, OUTPUT); 113 114 _11FR_Servo.attach(_11FRCM); 115 _11FU_Servo.attach(_11FUCM); 116 _21UA_Servo.attach(_21UACM); 117 _31SH_Servo.attach(_31SHCM); 118 119 /* This part is used to send initial "neutral position/stop" 120 * to all PWM outputs assigned to Servo Motors. 121 */ 122 _11FR_Servo.write(90); 123 _11FU_Servo.write(90); 124 _21UA_Servo.write(90); 125 _31SH_Servo.write(90); 126 127 /* Initial Feedback to Due */ 128 digitalWrite(_00XXOK, LOW); 129 130 /* analogReadResolution(10); Analog Reading Scale: 0-1023 for ARDUINO LEONARDO*/ 131 132 /* analogWriteResolution(10); Analog Writing Scale: 0-1023 for ARDUINO LEONARDO*/ 133 134 Serial.begin(_BAUDRATE); while(!Serial) { }; 135 Serial.println("? -> Help."); 136 Serial.print("[ARD LNRD/] > "); 137} /* End of "void setup()": ARDUINO LEONARDO */ 138 139 140/* Global Variables for Test Purposes: ARDUINO LEONARDO */ 141 142/* End of Global Variables for Test Purposes: ARDUINO LEONARDO */ 143 144 145/* Verified Global Variables: ARDUINO LEONARDO */ 146 int CommandFromDue_Section, CommandFromDue_SetValue; 147/* End of Verified Global Variables: ARDUINO LEONARDO */ 148 149 150void CommandPromptOverSerial(String _Prompt) { /* VERIFIED FUNCTION: ARDUINO LEONARDO */ 151 /* Reprompting and flushing serial for next input. 152 * This function will be used after each use of ReadUserInputOverSerial() 153 */ 154 Serial.print(_Prompt); Serial.end(); Serial.begin(_BAUDRATE); while(!Serial) { }; 155} /* End of "CommandPromtOverSerial()": ARDUINO LEONARDO */ 156 157 158String ReadUserInputOverSerial(){ /* VERIFIED FUNCTION: ARDUINO LEONARDO */ 159 /* This function will be used only "if (Serial.available())" */ 160 char UserInput[32]; 161 Serial.readBytes(UserInput,32); 162 /* Buffering and capitalizing User Input */ 163 String Buffer=UserInput; Buffer.toUpperCase(); 164 return(Buffer); 165} /* End of "ReadUserInputOverSerial()": ARDUINO LEONARDO */ 166 167 168void Read(String Tag) { /* VERIFIED FUNCTION: ARDUINO LEONARDO */ 169 /* Indicates value at input channel defined by "Tag" */ 170 if ( (Tag.substring(5,7)=="ZL") || (Tag.substring(5,7)=="ZH") || (Tag.substring(5,7)=="OK")) 171 Serial.println(Tag + " = " + String(digitalRead(IO_Channel(Tag)))); 172 else if (Tag.substring(5,7)=="TX") 173 Serial.println(Tag + " = " + String(analogRead(IO_Channel(Tag)))); 174 else Serial.println(Tag + "is not an input."); 175} /* End of "void Read()": ARDUINO LEONARDO */ 176 177 178void LoopTest() { /* VERIFIED FUNCTION: ARDUINO LEONARDO */ 179 String Command, User_Tag; 180 Serial.println("? -> Help, X -> Terminate"); 181 CommandPromptOverSerial("[ARD LNRD/LoopTest/] > "); 182 do 183 { 184 if (Serial.available()) 185 { 186 byte Command_to_Case=0; /* Command is invalid if remains as 0. */ 187 int User_Value=0, User_Value_Last=0, Reading=0, Reading_Last=-1; 188 String Buffer = ReadUserInputOverSerial(); 189 Serial.println(Buffer); Serial.end(); Serial.begin(_BAUDRATE); while(!Serial) { }; 190 191 /* Seperating Command and Tag*/ 192 Command = Buffer.substring(0,2); User_Tag = Buffer.substring(3,10); 193 194 /* Converting (string)Command to (byte)Command_to_Case */ 195 if (Command=="AI") Command_to_Case=11; 196 if (Command=="AO") Command_to_Case=12; 197 if (Command=="DI") Command_to_Case=21; 198 if (Command=="DO") Command_to_Case=22; 199 if (Command=="PO") Command_to_Case=32; 200 if (Command[0]=='?') Command_to_Case=98; // Display Help 201 if (Command[0]=='X') Command_to_Case=99; // Terminate Test 202 203 /* Checking whether tag is valid. */ 204 if ((Command_to_Case!=0)&&(Command_to_Case<90)&&(IO_Channel(User_Tag)<0)) 205 Command_to_Case=1; /* Redirecting to unrecognized tag case. */ 206 207 /* Taking respective actions and giving feedbacks for each command. */ 208 switch (Command_to_Case) { 209 case 0 : Serial.println("Invalid command."); 210 CommandPromptOverSerial("[ARD LNRD/LoopTest/] > "); 211 break; 212 case 1 : Serial.println("Tag not exists: " + User_Tag); 213 CommandPromptOverSerial("[ARD LNRD/LoopTest/] > "); 214 break; 215 case 11: Serial.println("AI-Test('X'->Terminate): " + User_Tag); 216 Reading_Last = -1; 217 do 218 { 219 Buffer = ReadUserInputOverSerial(); 220 Reading = analogRead(IO_Channel(User_Tag)); 221 if (Reading != Reading_Last) 222 { 223 Serial.println(User_Tag + " = " + String(Reading)); 224 Reading_Last = Reading; delay(1000); 225 /* 1s delay is used to avoid fast updates on Serial Monitor */ 226 } 227 } 228 while (Buffer[0]!='X'); 229 Serial.println(); 230 CommandPromptOverSerial("[ARD LNRD/LoopTest/] > "); 231 break; 232 case 12: Serial.println("AO-Test('X'->Terminate): " + User_Tag); 233 Serial.println("Integer Value(0-1023): "); 234 Buffer=""; 235 do 236 { 237 if (Serial.available()) 238 { 239 Buffer = ReadUserInputOverSerial(); 240 if (Buffer[0]=='X') User_Value=0; else User_Value=Buffer.substring(0).toInt(); 241 Serial.println(String(User_Value) + " -> " + User_Tag); 242 if (User_Value!=User_Value_Last) 243 { 244 analogWrite(IO_Channel(User_Tag),User_Value); 245 User_Value_Last = User_Value; 246 } 247 else analogWrite(IO_Channel(User_Tag),User_Value_Last); 248 Serial.print("Integer Value(0-1023): "); 249 Serial.end(); Serial.begin(_BAUDRATE); while(!Serial) { }; 250 } /* End of "if (Serial.available())..." */ 251 } 252 while (Buffer[0]!='X'); 253 Serial.println(); 254 CommandPromptOverSerial("[ARD LNRD/LoopTest/] > "); 255 break; 256 case 21: Serial.println("DI-Test('X'->Terminate): " + User_Tag); 257 Reading_Last = -1; 258 do 259 { 260 Buffer = ReadUserInputOverSerial(); 261 Reading = digitalRead(IO_Channel(User_Tag)); 262 if (Reading != Reading_Last) 263 { 264 Serial.println(User_Tag + " = " + String(Reading)); 265 Reading_Last = Reading; 266 } 267 } 268 while (Buffer[0]!='X'); 269 Serial.println(); 270 CommandPromptOverSerial("[ARD LNRD/LoopTest/] > "); 271 break; 272 case 22: Serial.println("DO-Test('X'->Terminate): " + User_Tag); 273 Serial.print("Enter 0 or 1: "); 274 Buffer=""; 275 do 276 { 277 if (Serial.available()) 278 { 279 Buffer = ReadUserInputOverSerial(); 280 if (Buffer[0]=='X') User_Value=0; else User_Value=Buffer.substring(0).toInt(); 281 Serial.println(String(User_Value) + " -> " + User_Tag); 282 if (((User_Value==0)||(User_Value==1))&&(User_Value!=User_Value_Last)) 283 { 284 285 digitalWrite(IO_Channel(User_Tag),User_Value); 286 User_Value_Last = User_Value; 287 } 288 else digitalWrite(IO_Channel(User_Tag),User_Value_Last); 289 Serial.print("Enter 0 or 1: "); 290 Serial.end(); Serial.begin(_BAUDRATE); while(!Serial) { }; 291 } /* End of "if (Serial.available())..." */ 292 } 293 while (Buffer[0]!='X'); 294 Serial.println(); 295 CommandPromptOverSerial("[ARD LNRD/LoopTest/] > "); 296 break; 297 case 32: Serial.println("PWM-Test('X'->Terminate): " + User_Tag); 298 CommandToServo(User_Tag,90); /* Sending "stop" to selected tag */ 299 Serial.print("Integer Value(0-180): "); 300 Buffer=""; 301 do 302 { 303 if (Serial.available()) 304 { 305 Buffer = ReadUserInputOverSerial(); 306 if (Buffer[0]=='X') User_Value=90; else User_Value=Buffer.substring(0).toInt(); 307 Serial.println(String(User_Value) + " -> " + User_Tag); 308 if (((User_Value>=0)||(User_Value<=180))&&(User_Value!=User_Value_Last)) 309 { CommandToServo(User_Tag,User_Value); User_Value_Last = User_Value; } 310 else CommandToServo(User_Tag,User_Value); 311 Serial.print("Integer Value(0-180): "); 312 Serial.end(); Serial.begin(_BAUDRATE); while(!Serial) { }; 313 } /* End of "if (Serial.available())..." */ 314 } 315 while (Buffer[0]!='X'); 316 Serial.println(); 317 CommandPromptOverSerial("[ARD LNRD/LoopTest/] > "); 318 break; 319 case 98: Serial.println("Usage: '[CC] [Tag]'"); 320 Serial.println(" [CC]: Channel Type"); 321 Serial.println(" AI, AO: Analog Input/Output"); 322 Serial.println(" DI, DO: Digital Input/Output"); 323 Serial.println(" PO: PWM Output for Servo Motor"); 324 Serial.println("Other Commands:"); 325 Serial.println(" ? : Help (this display)"); 326 Serial.println(" X : Exit from Loop Testing"); 327 CommandPromptOverSerial("[ARD LNRD/LoopTest/] > "); 328 break; 329 case 99: Serial.println("Loop Test is terminated."); 330 break; 331 } /* End of "switch (Command_to_Case)..." */ 332 } /* End of "if (Serial.available())..." */ 333 } 334 while (Command[0]!='X'); 335 336} /* End of "void LoopTest()": ARDUINO LEONARDO */ 337 338 339void MoveToPosition /* VERIFIED FUNCTION: ARDUINO LEONARDO */ 340 (String Section, int Speed, int SetValue) { 341 /* This function is used to operate a "Section" at a determined 342 * "Speed" to required position given by "SetValue". 343 * ("Section" is the first 5 characters of any I/O tag.) 344 */ 345 String PWM_Tag=Section+"CM", TX_Tag=Section+"TX"; 346 347 boolean Actuated=0, 348 Flag_SPMatched=0, 349 Flag_LL=0, Flag_L=0, Flag_H=0, Flag_HH=0, 350 Alarmed_1=0, Alarmed_2=0; 351 352 byte Section_to_Case=0; 353 354 int IncreaseDirection=0, Direction=0, 355 TX_RawMin, TX_RawMax, 356 Alarm_LL, Alarm_L, Alarm_H, Alarm_HH, 357 CurrentTxReading; 358 359 if (Section=="_11FR") { Section_to_Case=1; IncreaseDirection=_CCW; } 360 else if (Section=="_11FU") { Section_to_Case=2; IncreaseDirection=_CCW; } 361 else if (Section=="_21UA") { Section_to_Case=3; IncreaseDirection=_CW; } 362 else if (Section=="_31SH") { Section_to_Case=4; IncreaseDirection=_CW; } 363 364 /* Scale and Alarm values corresponding to Sections should be adjusted 365 * depending on tests. 366 * Direction equations should be adjusted by multiplying -1 depending on 367 * mechanical configuration. 368 */ 369 switch (Section_to_Case) { 370 case 0 : Serial.println("Invalid section."); 371 break; 372 case 1 : /* Customizations for _11FR Section */ 373 TX_RawMin=0; TX_RawMax=1023; 374 Alarm_LL=10; Alarm_L=20; Alarm_H=80; Alarm_HH=90; 375 break; 376 case 2 : /* Customizations for _11FU Section */ 377 TX_RawMin=690; TX_RawMax=230; 378 Alarm_LL=5; Alarm_L=10; Alarm_H=80; Alarm_HH=90; 379 break; 380 case 3 : /* Customizations for _21UA Section - Completed */ 381 TX_RawMin=775; TX_RawMax=520; 382 Alarm_LL=10; Alarm_L=20; Alarm_H=80; Alarm_HH=90; 383 break; 384 case 4 : /* Customizations for _31SH Section - Completed */ 385 TX_RawMin=250; TX_RawMax=760; 386 Alarm_LL=10; Alarm_L=20; Alarm_H=80; Alarm_HH=90; 387 break; 388 } 389 390 CurrentTxReading=map( analogRead(IO_Channel(TX_Tag)), TX_RawMin, TX_RawMax, 0, 100 ); 391 392 if (Section_to_Case!=0 && Speed!=0) 393 { 394 Serial.print("Actuate " + PWM_Tag + ", Pos.Tx:" + TX_Tag + "[=" + String(CurrentTxReading) + "%], "); 395 if (IncreaseDirection==_CCW) Direction = (int)( (SetValue-CurrentTxReading)/abs(SetValue-CurrentTxReading) ); 396 else Direction = (int)( (-1)*(SetValue-CurrentTxReading)/abs(SetValue-CurrentTxReading) ); 397 switch (Direction) { 398 case _CCW: Serial.print("CCW, "); 399 break; 400 case _CW : Serial.print("CW, "); 401 break; 402 case 0 : Serial.print("STOP, "); 403 break; 404 } 405 Serial.print("Speed="+String(Speed)+", "); 406 Serial.println("Set Value="+String(SetValue)+"%"); 407 while ( !( Flag_SPMatched || Flag_LL || Flag_HH ) ) 408 { 409 CurrentTxReading=map( analogRead(IO_Channel(TX_Tag)), TX_RawMin, TX_RawMax, 0, 100 ); 410 if (IncreaseDirection==_CCW) 411 { 412 if ( (Direction==_CW) && (CurrentTxReading<=Alarm_L) ) Flag_L = 1; 413 if ( (Direction==_CW) && (CurrentTxReading<=Alarm_LL) ) Flag_LL = 1; 414 if ( (Direction==_CCW) && (CurrentTxReading>=Alarm_H) ) Flag_H = 1; 415 if ( (Direction==_CCW) && (CurrentTxReading>=Alarm_HH) ) Flag_HH = 1; 416 if ( ( (Direction==_CCW) && (CurrentTxReading>=SetValue) ) || 417 ( (Direction==_CW) && (CurrentTxReading<=SetValue) ) ) Flag_SPMatched = 1; 418 } 419 if (IncreaseDirection==_CW) 420 { 421 if ( (Direction==_CCW) && (CurrentTxReading<=Alarm_L) ) Flag_L = 1; 422 if ( (Direction==_CCW) && (CurrentTxReading<=Alarm_LL) ) Flag_LL = 1; 423 if ( (Direction==_CW) && (CurrentTxReading>=Alarm_H) ) Flag_H = 1; 424 if ( (Direction==_CW) && (CurrentTxReading>=Alarm_HH) ) Flag_HH = 1; 425 if ( ( (Direction==_CW) && (CurrentTxReading>=SetValue) ) || 426 ( (Direction==_CCW) && (CurrentTxReading<=SetValue) ) ) Flag_SPMatched = 1; 427 } 428 429 if (Flag_L || Flag_H) 430 if (!Alarmed_1) 431 { 432 if ( Flag_L ) Serial.println("Alarm : "+ String(Alarm_L) + "% is reached."); 433 if ( Flag_H ) Serial.println("Alarm : "+ String(Alarm_H) + "% is reached."); 434 Alarmed_1 = 1; 435 } 436 437 if (Flag_LL || Flag_HH) 438 { 439 CommandToServo(PWM_Tag, 90); /* Interlocked */ 440 if (!Alarmed_2) 441 { 442 if ( Flag_LL ) Serial.println("Interlocked: "+ String(Alarm_LL) + "% is reached."); 443 if ( Flag_HH ) Serial.println("Interlocked: "+ String(Alarm_HH) + "% is reached."); 444 Alarmed_2 = 1; 445 } 446 } 447 else if (! Actuated ) 448 { CommandToServo(PWM_Tag, 90+Direction*Speed); Actuated = 1; } 449 450 } /* Waits until reaching to setpoint */ 451 CommandToServo(PWM_Tag, 90); /* Stop command to Servo */ 452 Serial.println(PWM_Tag + " is stopped at " + String(CurrentTxReading) + "%."); 453 } /* End of "if (Section_to_Case!=0)" */ 454} /* End of "void MoveToPosition()" */ 455 456 457void CommandFromDue() { /* VERIFIED FUNCTION: ARDUINO LEONARDO */ 458 CommandFromDue_Section = analogRead(_00XXCM); 459 if ( ((_FA_Radius-_ICDB)<=CommandFromDue_Section) && (CommandFromDue_Section<=(_FA_Radius+_ICDB)) ) 460 CommandFromDue_Section=_FA_Radius; 461 else if ( ((_FA_Ulna-_ICDB)<=CommandFromDue_Section) && (CommandFromDue_Section<=(_FA_Ulna+_ICDB)) ) 462 CommandFromDue_Section=_FA_Ulna; 463 else if ( ((_UpperArm-_ICDB)<=CommandFromDue_Section) && (CommandFromDue_Section<=(_UpperArm+_ICDB)) ) 464 CommandFromDue_Section=_UpperArm; 465 else if ( ((_Shoulder-_ICDB)<=CommandFromDue_Section) && (CommandFromDue_Section<=(_Shoulder+_ICDB)) ) 466 CommandFromDue_Section=_Shoulder; 467 else 468 CommandFromDue_Section=0; 469 470 CommandFromDue_SetValue = map(analogRead(_00XXSP),168,846,0,100); 471 /* Due generates DAC outputs between 0.55-2.75 V and it corresponds 472 * (int) 168-846 on Arduino Leonardo. 473 * Here analog reading is converted to Percentage. 474 */ 475 476 boolean CommandFromDue_Execute = digitalRead(_00XXXC); 477 String Section = ""; 478 int Speed = 0; 479 if ((CommandFromDue_Section!=0) && CommandFromDue_Execute) 480 { 481 Serial.println(); 482 Serial.print("Command by ARD DUE: "+ String(CommandFromDue_SetValue) + "% -> "); 483 switch(CommandFromDue_Section){ 484 case _FA_Radius: Serial.println("Front Arm-Radius"); 485 Section = "_11FR"; 486 Speed = 20; /* to be adjusted depending on tests */ 487 break; 488 case _FA_Ulna : Serial.println("Front Arm-Ulna"); 489 Section = "_11FU"; 490 Speed = 20; /* to be adjusted depending on tests */ 491 break; 492 case _UpperArm : Serial.println("Upper Arm"); 493 Section = "_21UA"; 494 Speed = 90; /* to be adjusted depending on tests */ 495 break; 496 case _Shoulder : Serial.println("Shoulder"); 497 Section = "_31SH"; 498 Speed = 30; /* to be adjusted depending on tests */ 499 break; 500 } 501 Serial.println(" -action-"); 502 MoveToPosition(Section, Speed, CommandFromDue_SetValue); 503 Serial.println("OK -> ARD DUE."); 504 CommandPromptOverSerial("[ARD LNRD/] > "); 505 digitalWrite(_00XXOK, HIGH); delay(100); digitalWrite(_00XXOK, LOW); // OK Feedback to Due 506 } /* End of "if (Section_Num!=0)" */ 507} /* End of "void CommandToLeonardo()" */ 508 509 510/* "void loop(): ARDUINO LEONARDO */ 511void loop() { 512 /* TEST CODES: */ 513 CommandFromDue(); 514 /* VERIFIED CODES: */ 515 /* Commands and Actions */ 516 /* All recognized commands give a feedback in all cases. 517 * If you don't see any feedback on Serial Monitor, check your input 518 * for any typewriting errors. 519 * Example: "XYZ" command does not exist, so will result as follows: 520 * [ARD LNRD/] > XYZ 521 * [ARD LNRD/] > 522 */ 523 if (Serial.available()) 524 { 525 String Command=ReadUserInputOverSerial(); Serial.println(Command); 526 if (Command.substring(0,1)=="?") /* Help */ 527 { 528 Serial.println("Commands:"); 529 Serial.println(" CFD"); 530 Serial.println(" Reads Command from ARD DUE."); 531 Serial.println(" LT"); 532 Serial.println(" Starts Loop Test subroutine."); 533 Serial.println(" MTP [Section] [Speed (0-90)] [SetValue]"); 534 Serial.println(" Move To Position - Section: First 5 characters of PWM Tag (e.g. _10WH for _10WHCM)."); 535 Serial.println(" RD [IO_Tag]"); 536 Serial.println(" Reads value on a single input channel."); 537 /* This function is used to operate a "section" given by "Tag" to "Direction" 538 * at specified "Speed" and during determined "Milliseconds", unless "LimitSw" 539 * is activated (Limit Switches give 0 when activated). 540 */ 541 Serial.end(); Serial.begin(_BAUDRATE); while(!Serial) { }; 542 } 543 544 if (Command.substring(0,3)=="CFD") 545 CommandFromDue(); 546 547 if (Command.substring(0,2)=="LT") 548 LoopTest(); 549 550 if (Command.substring(0,3)=="MTP") 551 MoveToPosition(Command.substring(4,9), Command.substring(10,12).toInt(), Command.substring(13).toInt()); 552 /* MoveToPosition(Section, Speed, Set Value) */ 553 554 if (Command.substring(0,2)=="RD") 555 Read(Command.substring(3,10)); 556 /* Read(Tag) */ 557 558 559 CommandPromptOverSerial("[ARD LNRD/] > "); 560 } /* End of "if (Serial.available())..." */ 561} /* End of "void loop()": ARDUINO LEONARDO */ 562
Application Program - Arduino Due
arduino
This program shall be loaded onto Due
1#include <Servo.h> 2 3/* Serial Comm.Baudrate Setting: ARDUINO DUE */ 4 const int _BAUDRATE = 4800; 5/* End of Serial Comm.Baudrate Setting: ARDUINO DUE */ 6 7/* Tag-Channel(Pin) Assignments and IO_Channel() Function: ARDUINO DUE */ 8 const int _01MCZL = 22; // 01: Thumb Finger Metacarpal ZL 9 const int _01MCZH = 23; // 01: Thumb Finger Metacarpal ZH 10 const int _01PDZL = 24; // 01: Thumb Finger Proximal+Distal ZL 11 const int _01PDZH = 25; // 01: Thumb Finger Proximal+Distal ZH 12 const int _01MCCM = 2; // 01: Thumb Finger Metacarpal CMD 13 const int _01PDCM = 3; // 01: Thumb Finger Distal CMD 14 15 const int _02PXZL = 26; // 02: Index Finger Proximal ZL 16 const int _02PXZH = 27; // 02: Index Finger Proximal ZH 17 const int _02MDZL = 28; // 02: Index Finger Medial+Distal ZL 18 const int _02MDZH = 29; // 02: Index Finger Medial+Distal ZH 19 const int _02PXCM = 4; // 02: Index Finger Proximal CMD 20 const int _02MDCM = 5; // 02: Index Finger Distal CMD 21 22 const int _03PXZL = 30; // 03: Middle Finger Proximal ZL 23 const int _03PXZH = 31; // 03: Middle Finger Proximal ZH 24 const int _03MDZL = 32; // 03: Middle Finger Medial+Distal ZL 25 const int _03MDZH = 33; // 03: Middle Finger Medial+Distal ZH 26 const int _03PXCM = 6; // 03: Middle Finger Proximal CMD 27 const int _03MDCM = 7; // 03: Middle Finger Distal CMD 28 29 const int _04PXZL = 34; // 04: Ring Finger Proximal ZL 30 const int _04PXZH = 35; // 04: Ring Finger Proximal ZH 31 const int _04MDZL = 36; // 04: Ring Finger Medial+Distal ZL 32 const int _04MDZH = 37; // 04: Ring Finger Medial+Distal ZH 33 const int _04PXCM = 8; // 04: Ring Finger Proximal CMD 34 const int _04MDCM = 9; // 04: Ring Finger Distal CMD 35 36 const int _05PXZL = 38; // 05: Pinky Finger Proximal ZL 37 const int _05PXZH = 39; // 05: Pinky Finger Proximal ZH 38 const int _05MDZL = 40; // 05: Pinky Finger Medial+Distal ZL 39 const int _05MDZH = 41; // 05: Pinky Finger Medial+Distal ZH 40 const int _05PXCM = 10; // 05: Pinky Finger Proximal CMD 41 const int _05MDCM = 11; // 05: Pinky Finger Distal CMD 42 43 const int _10WHZL = 42; // 10: Wrist Horizontal ZL 44 const int _10WHZH = 43; // 10: Wrist Horizontal ZH 45 const int _10WHTX = A0; // 10: Wrist Horizontal Actual Pos 46 const int _10WHCM = 12; // 10: Wrist Horizontal CMD 47 48 const int _10WVZL = 44; // 10: Wrist Vertical ZL 49 const int _10WVZH = 45; // 10: Wrist Vertical ZH 50 const int _10WVTX = A1; // 10: Wrist Vertical Actual Pos 51 const int _10WVCM = 13; // 10: Wrist Vertical CMD 52 53 const int _11FRTX = A2; // 11: Front Arm Radius Actual Pos 54 const int _11FUTX = A3; // 11: Front Arm Ulna Actual Pos 55 const int _21UATX = A4; // 21: Upper Arm Actual Pos 56 const int _31SHTX = A5; // 31: Shoulder Actual Pos 57 58 const int _00XXXC = 50; // Execute Command (0: Stop/Idle, 1: Execute) 59 const int _00XXSP = DAC0; // Setpoint Register 60 const int _00XXCM = DAC1; // Command Selector (*1) 61 const int _00XXOK = 51; // Command Executed by Arduino Leonardo (*2-IMPORTANT) 62 63 int IO_Channel(String Tag) { /* VERIFIED FUNCTION: ARDUINO DUE */ 64 /* This function is used to validate IO Channel for DI/DO/AO/PWM-O. 65 * If given tag is valid, respective channel number will be returned. 66 * Otherwise -1 will be returned as "error". 67 */ 68 if (Tag=="_01MCZL") return(_01MCZL); 69 if (Tag=="_01MCZH") return(_01MCZH); 70 if (Tag=="_01PDZL") return(_01PDZL); 71 if (Tag=="_01PDZH") return(_01PDZH); 72 if (Tag=="_01MCCM") return(_01MCCM); 73 if (Tag=="_01PDCM") return(_01PDCM); 74 if (Tag=="_02PXZL") return(_02PXZL); 75 if (Tag=="_02PXZH") return(_02PXZH); 76 if (Tag=="_02MDZL") return(_02MDZL); 77 if (Tag=="_02MDZH") return(_02MDZH); 78 if (Tag=="_02PXCM") return(_02PXCM); 79 if (Tag=="_02MDCM") return(_02MDCM); 80 if (Tag=="_03PXZL") return(_03PXZL); 81 if (Tag=="_03PXZH") return(_03PXZH); 82 if (Tag=="_03MDZL") return(_03MDZL); 83 if (Tag=="_03MDZH") return(_03MDZH); 84 if (Tag=="_03PXCM") return(_03PXCM); 85 if (Tag=="_03MDCM") return(_03MDCM); 86 if (Tag=="_04PXZL") return(_04PXZL); 87 if (Tag=="_04PXZH") return(_04PXZH); 88 if (Tag=="_04MDZL") return(_04MDZL); 89 if (Tag=="_04MDZH") return(_04MDZH); 90 if (Tag=="_04PXCM") return(_04PXCM); 91 if (Tag=="_04MDCM") return(_04MDCM); 92 if (Tag=="_05PXZL") return(_05PXZL); 93 if (Tag=="_05PXZH") return(_05PXZH); 94 if (Tag=="_05MDZL") return(_05MDZL); 95 if (Tag=="_05MDZH") return(_05MDZH); 96 if (Tag=="_05PXCM") return(_05PXCM); 97 if (Tag=="_05MDCM") return(_05MDCM); 98 if (Tag=="_10WHZL") return(_10WHZL); 99 if (Tag=="_10WHZH") return(_10WHZH); 100 if (Tag=="_10WHTX") return(_10WHTX); 101 if (Tag=="_10WHCM") return(_10WHCM); 102 if (Tag=="_10WVZL") return(_10WVZL); 103 if (Tag=="_10WVZH") return(_10WVZH); 104 if (Tag=="_10WVTX") return(_10WVTX); 105 if (Tag=="_10WVCM") return(_10WVCM); 106 if (Tag=="_11FRTX") return(_11FRTX); 107 if (Tag=="_11FUTX") return(_11FUTX); 108 if (Tag=="_21UATX") return(_21UATX); 109 if (Tag=="_31SHTX") return(_31SHTX); 110 if (Tag=="_00XXXC") return(_00XXXC); 111 if (Tag=="_00XXSP") return(_00XXSP); 112 if (Tag=="_00XXCM") return(_00XXCM); 113 if (Tag=="_00XXOK") return(_00XXOK); 114 return(-1); 115} /* End of "IO_Channel(String Tag)": ARDUINO DUE */ 116/* End of Tag-Channel(Pin) Assignments and IO_Channel() Function: ARDUINO DUE */ 117 118 119/* Servo _Definitions + CommandToServo() function: ARDUINO DUE */ 120 Servo _01MC_Servo; 121 Servo _01PD_Servo; 122 Servo _02PX_Servo; 123 Servo _02MD_Servo; 124 Servo _03PX_Servo; 125 Servo _03MD_Servo; 126 Servo _04PX_Servo; 127 Servo _04MD_Servo; 128 Servo _05PX_Servo; 129 Servo _05MD_Servo; 130 Servo _10WH_Servo; 131 Servo _10WV_Servo; 132 133 void CommandToServo(String Tag, int Value) { /* VERIFIED FUNCTION: ARDUINO DUE */ 134 /* This function is used to actuate PWM outputs to Servo Motors. */ 135 if (Tag=="_01MCCM") _01MC_Servo.write(Value); 136 if (Tag=="_01PDCM") _01PD_Servo.write(Value); 137 if (Tag=="_02PXCM") _02PX_Servo.write(Value); 138 if (Tag=="_02MDCM") _02MD_Servo.write(Value); 139 if (Tag=="_03PXCM") _03PX_Servo.write(Value); 140 if (Tag=="_03MDCM") _03MD_Servo.write(Value); 141 if (Tag=="_04PXCM") _04PX_Servo.write(Value); 142 if (Tag=="_04MDCM") _04MD_Servo.write(Value); 143 if (Tag=="_05PXCM") _05PX_Servo.write(Value); 144 if (Tag=="_05MDCM") _05MD_Servo.write(Value); 145 if (Tag=="_10WHCM") _10WH_Servo.write(Value); 146 if (Tag=="_10WVCM") _10WV_Servo.write(Value); 147 } /* End of "CommandToServo()": ARDUINO DUE */ 148/* End of Servo _Definitions + CommandToServo() function: ARDUINO DUE */ 149 150 151/* Constant Parameters, floatMap() Function: ARDUINO DUE */ 152 const float _DBPercent = 1.0; // % Deadband 153 const int _CCW = 1; // Servo Direction for Counter Clockwise 154 const int _CW = -1; // Servo Direction for Counter Clockwise 155 156 float floatMap /* VERIFIED FUNCTION: ARDUINO DUE */ 157 (int InputValue, int fromLow, int fromHigh, int toLow, int toHigh) { 158 /* This function has exacly same functionality with Arduino's 159 * map(value, fromLow, fromHigh, toLow, toHigh) 160 * function. However, original map function truncates fractions and return 161 * just integers. So this one is programmed to return exactly scaled values. 162 */ 163 return( (float)toLow + (float)((float)((InputValue-fromLow)*(toHigh-toLow))/(float)(fromHigh-fromLow)) ); 164 } /* End of "floatMap()" */ 165 166/* End of Constant Parameters, floatMap() Function: ARDUINO DUE */ 167 168 169/* Constant Parameters for Intercommunication: ARDUINO DUE */ 170 const int _FA_Radius = 840; // Front Arm Radius Section 171 const int _FA_Ulna = 1530; // Front Arm Ulna Section 172 const int _UpperArm = 2220; // Upper Arm Section 173 const int _Shoulder = 2910; // Front Arm Radius Section 174/* End of Constant Parameters for Intercommunication: ARDUINO DUE */ 175 176 177/* "void setup()": ARDUINO DUE */ 178void setup() { 179 // TEST CODES: 180 181 // VERIFIED CODES: 182 pinMode(_01MCZL, INPUT_PULLUP); 183 pinMode(_01MCZH, INPUT_PULLUP); 184 pinMode(_01PDZL, INPUT_PULLUP); 185 pinMode(_01PDZH, INPUT_PULLUP); 186 pinMode(_02PXZL, INPUT_PULLUP); 187 pinMode(_02PXZH, INPUT_PULLUP); 188 pinMode(_02MDZL, INPUT_PULLUP); 189 pinMode(_02MDZH, INPUT_PULLUP); 190 pinMode(_03PXZL, INPUT_PULLUP); 191 pinMode(_03PXZH, INPUT_PULLUP); 192 pinMode(_03MDZL, INPUT_PULLUP); 193 pinMode(_03MDZH, INPUT_PULLUP); 194 pinMode(_04PXZL, INPUT_PULLUP); 195 pinMode(_04PXZH, INPUT_PULLUP); 196 pinMode(_04MDZL, INPUT_PULLUP); 197 pinMode(_04MDZH, INPUT_PULLUP); 198 pinMode(_05PXZL, INPUT_PULLUP); 199 pinMode(_05PXZH, INPUT_PULLUP); 200 pinMode(_05MDZL, INPUT_PULLUP); 201 pinMode(_05MDZH, INPUT_PULLUP); 202 pinMode(_10WHZL, INPUT_PULLUP); 203 pinMode(_10WHZH, INPUT_PULLUP); 204 pinMode(_10WVZL, INPUT_PULLUP); 205 pinMode(_10WVZH, INPUT_PULLUP); 206 pinMode(_00XXOK, INPUT); 207 208 _01MC_Servo.attach(_01MCCM); 209 _01PD_Servo.attach(_01PDCM); 210 _02PX_Servo.attach(_02PXCM); 211 _02MD_Servo.attach(_02MDCM); 212 _03PX_Servo.attach(_03PXCM); 213 _03MD_Servo.attach(_03MDCM); 214 _04PX_Servo.attach(_04PXCM); 215 _04MD_Servo.attach(_04MDCM); 216 _05PX_Servo.attach(_05PXCM); 217 _05MD_Servo.attach(_05MDCM); 218 _10WH_Servo.attach(_10WHCM); 219 _10WV_Servo.attach(_10WVCM); 220 221 /* This part is used to send initial "neutral position/stop" 222 * to all PWM outputs assigned to Servo Motors. 223 */ 224 _01MC_Servo.write(90); 225 _01PD_Servo.write(90); 226 _02PX_Servo.write(90); 227 _02MD_Servo.write(90); 228 _03PX_Servo.write(90); 229 _03MD_Servo.write(90); 230 _04PX_Servo.write(90); 231 _04MD_Servo.write(90); 232 _05PX_Servo.write(90); 233 _05MD_Servo.write(90); 234 _10WH_Servo.write(90); 235 _10WV_Servo.write(90); 236 237 analogReadResolution(12); /* Analog Reading Scale: 0-4095 for ARDUINO DUE*/ 238 239 analogWriteResolution(12); /* Analog Writing Scale: 0-4095 for ARDUINO DUE*/ 240 /* DAC Initialize 241 * DAC initialization is needed at first start, otherwise DAC of Due 242 * generates incorrect voltage levels. 243 * Note that 0 and 4095 have no functions on Leonardo. 244 */ 245 analogWrite(_00XXSP,0); delay(50); analogWrite(_00XXSP,4095); delay(50); analogWrite(_00XXSP,0); 246 analogWrite(_00XXCM,0); delay(50); analogWrite(_00XXCM,4095); delay(50); analogWrite(_00XXCM,0); 247 248 digitalWrite(_00XXXC,LOW); 249 250 Serial.begin(_BAUDRATE); while(!Serial) { }; 251 Serial.println("? -> Help."); 252 Serial.print("[ARD DUE/] > "); 253} /* End of "void setup()": ARDUINO DUE */ 254 255 256/* Global Variables for Test Purposes: ARDUINO DUE */ 257 258/* End of Global Variables for Test Purposes: ARDUINO DUE */ 259 260 261/* Verified Global Variables: ARDUINO DUE */ 262 263/* End of Verified Global Variables: ARDUINO DUE */ 264 265 266void CommandPromptOverSerial(String _Prompt) { /* VERIFIED FUNCTION: ARDUINO DUE */ 267 /* Reprompting and flushing serial for next input. 268 * This function will be used after each use of ReadUserInputOverSerial() 269 */ 270 Serial.print(_Prompt); Serial.end(); Serial.begin(_BAUDRATE); while(!Serial) { }; 271} /* End of "CommandPromtOverSerial()": ARDUINO DUE */ 272 273 274String ReadUserInputOverSerial(){ /* VERIFIED FUNCTION: ARDUINO DUE */ 275 /* This function will be used only "if (Serial.available())" */ 276 char UserInput[32]; 277 Serial.readBytes(UserInput,32); 278 /* Buffering and capitalizing User Input */ 279 String Buffer=UserInput; Buffer.toUpperCase(); 280 return(Buffer); 281} /* End of "ReadUserInputOverSerial()": ARDUINO DUE */ 282 283 284void Read(String Tag) { /* VERIFIED FUNCTION: ARDUINO DUE */ 285 /* Indicates value at input channel defined by "Tag" */ 286 if ( (Tag.substring(5,7)=="ZL") || (Tag.substring(5,7)=="ZH") || (Tag.substring(5,7)=="OK")) 287 Serial.println(Tag + " = " + String(digitalRead(IO_Channel(Tag)))); 288 else if (Tag.substring(5,7)=="TX") 289 Serial.println(Tag + " = " + String(analogRead(IO_Channel(Tag)))); 290 else Serial.println(Tag + "is not an input."); 291} /* End of "void Read()": ARDUINO DUE */ 292 293 294void LoopTest() { /* VERIFIED FUNCTION: ARDUINO DUE */ 295 String Command, User_Tag; 296 Serial.println("? -> Help, X -> Terminate"); 297 CommandPromptOverSerial("[ARD DUE/LoopTest/] > "); 298 do 299 { 300 if (Serial.available()) 301 { 302 byte Command_to_Case=0; /* Command is invalid if remains as 0. */ 303 int User_Value=0, User_Value_Last=0, Reading=0, Reading_Last=-1; 304 String Buffer = ReadUserInputOverSerial(); 305 Serial.println(Buffer); Serial.end(); Serial.begin(_BAUDRATE); while(!Serial) { }; 306 307 /* Seperating Command and Tag*/ 308 Command = Buffer.substring(0,2); User_Tag = Buffer.substring(3,10); 309 310 /* Converting (string)Command to (byte)Command_to_Case */ 311 if (Command=="AI") Command_to_Case=11; 312 if (Command=="AO") Command_to_Case=12; 313 if (Command=="DI") Command_to_Case=21; 314 if (Command=="DO") Command_to_Case=22; 315 if (Command=="PO") Command_to_Case=32; 316 if (Command[0]=='?') Command_to_Case=98; // Display Help 317 if (Command[0]=='X') Command_to_Case=99; // Terminate Test 318 319 /* Checking whether tag is valid. */ 320 if ((Command_to_Case!=0)&&(Command_to_Case<90)&&(IO_Channel(User_Tag)<0)) 321 Command_to_Case=1; /* Redirecting to unrecognized tag case. */ 322 323 /* Taking respective actions and giving feedbacks for each command. */ 324 switch (Command_to_Case) { 325 case 0 : Serial.println("Invalid command."); 326 CommandPromptOverSerial("[ARD DUE/LoopTest/] > "); 327 break; 328 case 1 : Serial.println("Tag not exists: " + User_Tag); 329 CommandPromptOverSerial("[ARD DUE/LoopTest/] > "); 330 break; 331 case 11: Serial.println("AI-Test('X'->Terminate): " + User_Tag); 332 Reading_Last = -1; 333 do 334 { 335 Buffer = ReadUserInputOverSerial(); 336 Reading = analogRead(IO_Channel(User_Tag)); 337 if (Reading != Reading_Last) 338 { 339 Serial.println(User_Tag + " = " + String(Reading)); 340 Reading_Last = Reading; delay(1000); 341 /* 1s delay is used to avoid fast updates on Serial Monitor */ 342 } 343 } 344 while (Buffer[0]!='X'); 345 Serial.println(); 346 CommandPromptOverSerial("[ARD DUE/LoopTest/] > "); 347 break; 348 case 12: Serial.println("AO-Test('X'->Terminate): " + User_Tag); 349 Serial.println("Integer Value(0-4095): "); 350 Buffer=""; 351 do 352 { 353 if (Serial.available()) 354 { 355 Buffer = ReadUserInputOverSerial(); 356 if (Buffer[0]=='X') User_Value=0; else User_Value=Buffer.substring(0).toInt(); 357 Serial.println(String(User_Value) + " -> " + User_Tag); 358 if (User_Value!=User_Value_Last) 359 { 360 analogWrite(IO_Channel(User_Tag),User_Value); 361 User_Value_Last = User_Value; 362 } 363 else analogWrite(IO_Channel(User_Tag),User_Value_Last); 364 Serial.println("Integer Value(0-4095): "); 365 Serial.end(); Serial.begin(_BAUDRATE); while(!Serial) { }; 366 } /* End of "if (Serial.available())..." */ 367 } 368 while (Buffer[0]!='X'); 369 Serial.println(); 370 CommandPromptOverSerial("[ARD DUE/LoopTest/] > "); 371 break; 372 case 21: Serial.println("DI-Test('X'->Terminate): " + User_Tag); 373 Reading_Last = -1; 374 do 375 { 376 Buffer = ReadUserInputOverSerial(); 377 Reading = digitalRead(IO_Channel(User_Tag)); 378 if (Reading != Reading_Last) 379 { 380 Serial.println(User_Tag + " = " + String(Reading)); 381 Reading_Last = Reading; 382 } 383 } 384 while (Buffer[0]!='X'); 385 Serial.println(); 386 CommandPromptOverSerial("[ARD DUE/LoopTest/] > "); 387 break; 388 case 22: Serial.println("DO-Test('X'->Terminate): " + User_Tag); 389 Serial.print("Enter 0 or 1: "); 390 Buffer=""; 391 do 392 { 393 if (Serial.available()) 394 { 395 Buffer = ReadUserInputOverSerial(); 396 if (Buffer[0]=='X') User_Value=0; else User_Value=Buffer.substring(0).toInt(); 397 Serial.println(String(User_Value) + " -> " + User_Tag); 398 if (((User_Value==0)||(User_Value==1))&&(User_Value!=User_Value_Last)) 399 { 400 401 digitalWrite(IO_Channel(User_Tag),User_Value); 402 User_Value_Last = User_Value; 403 } 404 else digitalWrite(IO_Channel(User_Tag),User_Value_Last); 405 Serial.print("Enter 0 or 1: "); 406 Serial.end(); Serial.begin(_BAUDRATE); while(!Serial) { }; 407 } /* End of "if (Serial.available())..." */ 408 } 409 while (Buffer[0]!='X'); 410 CommandPromptOverSerial("[ARD DUE/LoopTest/] > "); 411 break; 412 case 32: Serial.println("PWM-Test('X'->Terminate): " + User_Tag); 413 CommandToServo(User_Tag,90); /* Sending "stop" to selected tag */ 414 Serial.print("Integer Value(0-180): "); 415 Buffer=""; 416 do 417 { 418 if (Serial.available()) 419 { 420 Buffer = ReadUserInputOverSerial(); 421 if (Buffer[0]=='X') User_Value=90; else User_Value=Buffer.substring(0).toInt(); 422 Serial.println(String(User_Value) + " -> " + User_Tag); 423 if (((User_Value>=0)||(User_Value<=180))&&(User_Value!=User_Value_Last)) 424 { CommandToServo(User_Tag,User_Value); User_Value_Last = User_Value; } 425 else CommandToServo(User_Tag,User_Value); 426 Serial.print("Integer Value(0-180): "); 427 Serial.end(); Serial.begin(_BAUDRATE); while(!Serial) { }; 428 } /* End of "if (Serial.available())..." */ 429 } 430 while (Buffer[0]!='X'); 431 Serial.println(); 432 CommandPromptOverSerial("[ARD DUE/LoopTest/] > "); 433 break; 434 case 98: Serial.println("Usage: '[CC] [Tag]'"); 435 Serial.println(" [CC]: Channel Type"); 436 Serial.println(" AI, AO: Analog Input/Output"); 437 Serial.println(" DI, DO: Digital Input/Output"); 438 Serial.println(" PO: PWM Output for Servo Motor"); 439 Serial.println("Other Commands:"); 440 Serial.println(" ? : Help (this display)"); 441 Serial.println(" X : Exit from Loop Testing"); 442 CommandPromptOverSerial("[ARD DUE/LoopTest/] > "); 443 break; 444 case 99: Serial.println("Loop Test is terminated."); 445 break; 446 } /* End of "switch (Command_to_Case)..." */ 447 } /* End of "if (Serial.available())..." */ 448 } 449 while (Command[0]!='X'); 450} /* End of "void LoopTest()": ARDUINO DUE */ 451 452 453void FullTravelDurationMeasurement( /* VERIFIED FUNCTION: ARDUINO DUE */ 454 String PWM_Tag, String LimitSw_CCW, String LimitSw_CW, int Speed) { 455 /* This function is used to measure full travel time of a "section" 456 * equipped with Servo:User_Tag and Limit Switches. 457 */ 458 int LimitSw_CCW_Ch = IO_Channel(LimitSw_CCW), 459 LimitSw_CW_Ch = IO_Channel(LimitSw_CW), 460 _LimitSwCCW_Ch = 0, /* Corrected Limit Sw.Ch.for CCW-direction */ 461 _LimitSwCW_Ch = 0, /* Corrected Limit Sw.Ch.for CW-direction */ 462 Direction = _CCW, 463 Counter = 0; 464 unsigned long StartTime=0, Duration=0; 465 Serial.println("Full Travel Duration Test"); 466 if ((IO_Channel(PWM_Tag)*LimitSw_CCW_Ch*LimitSw_CW_Ch)!=-1) 467 { 468 Serial.println(" PWM : " + PWM_Tag + " [Ch. " + String(IO_Channel(PWM_Tag)) + "]"); 469 Serial.println(" Limit Switch.1: " + LimitSw_CCW + " [Ch. " + String(LimitSw_CCW_Ch) + "]"); 470 Serial.println(" Limit Switch.2: " + LimitSw_CW + " [Ch. " + String(LimitSw_CW_Ch) + "]"); 471 472 /* Matching Limit Switches with Directions 473 * Servo is set to turn CCW direction at first. Then reached limit switch is 474 * marked as the one corresponding to CCW-end, while the other is for CW-end. 475 * Notes: 476 * - Limit switches gives 0 when reached. 477 * - (90+Speed) -> Counter Clockwise turn (CCW) 478 * - (90-Speed) -> Clockwise turn (CW) 479 */ 480 CommandToServo(PWM_Tag, 90 + Direction*Speed); 481 while ( digitalRead(LimitSw_CCW_Ch) && digitalRead(LimitSw_CW_Ch) ) { } 482 CommandToServo(PWM_Tag, 90); 483 if ( ! digitalRead(LimitSw_CCW_Ch) ) 484 { _LimitSwCCW_Ch = LimitSw_CCW_Ch; _LimitSwCW_Ch = LimitSw_CW_Ch; } 485 else 486 { _LimitSwCCW_Ch = LimitSw_CW_Ch; _LimitSwCW_Ch = LimitSw_CCW_Ch; } 487 Serial.println(" DI-Channel matches CCW-end: Ch. " + String(_LimitSwCCW_Ch)); 488 Serial.println(" DI-Channel matches CW-end : Ch. " + String(_LimitSwCW_Ch)); 489 490 /* Main Test, 5 steps */ 491 for (Counter=0; Counter<=4; Counter++) 492 { 493 /* Direction must be reverted when limit is reached */ 494 if ( ! digitalRead(_LimitSwCCW_Ch) ) Direction = _CW; 495 else if ( ! digitalRead(_LimitSwCW_Ch) ) Direction = _CCW; 496 else Direction = 0; 497 498 /* Starting time is stored and Servo is actuated */ 499 StartTime = millis(); CommandToServo(PWM_Tag, 90 + Direction*Speed); 500 501 /* Wait until reaching to limit at direction */ 502 if (Direction==-1) while ( digitalRead(_LimitSwCW_Ch) ) { } 503 else if (Direction==1) while ( digitalRead(_LimitSwCCW_Ch) ) { } 504 505 CommandToServo(PWM_Tag, 90); Duration = millis() - StartTime; Direction *= -1; 506 Serial.println(" Duration.#" + String(Counter) + ": " + String(Duration) + " ms"); 507 } 508 } 509 else 510 Serial.println("Error: Unrecognized tag(s)."); 511} /* End of "void FullTravelDurationMeasurement()": ARDUINO DUE */ 512 513 514void MoveDuringMillis /* VERIFIED FUNCTION: ARDUINO DUE */ 515 (String Tag, int Direction, int Speed, unsigned long Milliseconds) { 516 /* This function is used to operate a "section" given by "Tag" to "Direction" 517 * at specified "Speed" and during determined "Milliseconds", unless "LimitSw" 518 * is activated (Limit Switches give 0 when activated). 519 */ 520 unsigned long StartTime=0, EndTime=0; 521 Serial.print("Actuate " + Tag + ", "); 522 switch (Direction) { 523 case _CCW: Serial.print("Counter Clockwise, "); break; 524 case _CW : Serial.print("Clockwise, "); break; 525 case 0 : Serial.print("STOP, "); break; 526 } 527 Serial.print("Speed="+String(Speed)+", "); 528 Serial.println("Time="+String(Milliseconds)+"ms"); 529 if (IO_Channel(Tag)==-1) Serial.println("Error: Recheck given tag."); 530 else 531 { 532 StartTime=millis(); 533 CommandToServo(Tag, 90 + Direction*Speed); 534 while ( (millis()-StartTime)<Milliseconds ) 535 { /* Waits until time is over */ }; 536 CommandToServo(Tag, 90); EndTime=millis(); /* Stop command to Servo */ 537 Serial.println(Tag + " STOP: Time reached - " + String(EndTime-StartTime) + "ms."); 538 } 539} /* End of "void MoveDuringMillis()": ARDUINO DUE */ 540 541 542void MoveToPosition /* VERIFIED FUNCTION: ARDUINO DUE */ 543 (String Section, int Speed, float SetValue) { 544 /* This function is used to operate a "Section" at a determined 545 * "Speed" to required position given by "SetValue". 546 * ("Section" is the first 5 characters of any I/O tag.) 547 */ 548 String PWM_Tag=Section+"CM", TX_Tag=Section+"TX"; 549 550 boolean Actuated=0, 551 Flag_SPMatched=0, 552 Flag_LL=0, Flag_L=0, Flag_H=0, Flag_HH=0, 553 Alarmed_1=0, Alarmed_2=0; 554 555 byte Section_to_Case=0; 556 557 int IncreaseDirection=0, Direction=0, 558 TX_RawMin, TX_RawMax, 559 Alarm_LL, Alarm_L, Alarm_H, Alarm_HH, 560 CurrentTxReading; 561 562 if (Section=="_10WH") { Section_to_Case=1; IncreaseDirection=_CCW; } 563 else if (Section=="_10WV") { Section_to_Case=2; IncreaseDirection=_CW; } 564 565 /* Scale and Alarm values corresponding to Sections should be adjusted 566 * depending on tests. 567 * Direction equations should be adjusted by multiplying -1 depending on 568 * mechanical configuration. 569 */ 570 switch (Section_to_Case) { 571 case 0 : Serial.println("Invalid section."); 572 break; 573 case 1 : /* Customizations for _10WH Section */ 574 TX_RawMin=0; TX_RawMax=4095; 575 Alarm_LL=10; Alarm_L=20; Alarm_H=80; Alarm_HH=90; 576 break; 577 case 2 : /* Customizations for _10WL Section */ 578 TX_RawMin=0; TX_RawMax=4095; 579 Alarm_LL=10; Alarm_L=20; Alarm_H=80; Alarm_HH=90; 580 break; 581 } 582 583 CurrentTxReading=map( analogRead(IO_Channel(TX_Tag)), TX_RawMin, TX_RawMax, 0, 100 ); 584 585 if (Section_to_Case!=0 && Speed!=0) 586 { 587 Serial.print("Actuate " + PWM_Tag + ", Pos.Tx:" + TX_Tag + "[=" + String(CurrentTxReading) + "%], "); 588 if (IncreaseDirection==_CCW) Direction = (int)( (SetValue-CurrentTxReading)/abs(SetValue-CurrentTxReading) ); 589 else Direction = (int)( (-1)*(SetValue-CurrentTxReading)/abs(SetValue-CurrentTxReading) ); 590 switch (Direction) { 591 case _CCW: Serial.print("CCW, "); 592 break; 593 case _CW : Serial.print("CW, "); 594 break; 595 case 0 : Serial.print("STOP, "); 596 break; 597 } 598 Serial.print("Speed="+String(Speed)+", "); 599 Serial.println("Set Value="+String(SetValue)+"%"); 600 while ( !( Flag_SPMatched || Flag_LL || Flag_HH ) ) 601 { 602 CurrentTxReading=map( analogRead(IO_Channel(TX_Tag)), TX_RawMin, TX_RawMax, 0, 100 ); 603 if (IncreaseDirection==_CCW) 604 { 605 if ( (Direction==_CW) && (CurrentTxReading<=Alarm_L) ) Flag_L = 1; 606 if ( (Direction==_CW) && (CurrentTxReading<=Alarm_LL) ) Flag_LL = 1; 607 if ( (Direction==_CCW) && (CurrentTxReading>=Alarm_H) ) Flag_H = 1; 608 if ( (Direction==_CCW) && (CurrentTxReading>=Alarm_HH) ) Flag_HH = 1; 609 if ( ( (Direction==_CCW) && (CurrentTxReading>=SetValue) ) || 610 ( (Direction==_CW) && (CurrentTxReading<=SetValue) ) ) Flag_SPMatched = 1; 611 } 612 if (IncreaseDirection==_CW) 613 { 614 if ( (Direction==_CCW) && (CurrentTxReading<=Alarm_L) ) Flag_L = 1; 615 if ( (Direction==_CCW) && (CurrentTxReading<=Alarm_LL) ) Flag_LL = 1; 616 if ( (Direction==_CW) && (CurrentTxReading>=Alarm_H) ) Flag_H = 1; 617 if ( (Direction==_CW) && (CurrentTxReading>=Alarm_HH) ) Flag_HH = 1; 618 if ( ( (Direction==_CW) && (CurrentTxReading>=SetValue) ) || 619 ( (Direction==_CCW) && (CurrentTxReading<=SetValue) ) ) Flag_SPMatched = 1; 620 } 621 622 if (Flag_L || Flag_H) 623 if (!Alarmed_1) 624 { 625 if ( Flag_L ) Serial.println("Alarm : "+ String(Alarm_L) + "% is reached."); 626 if ( Flag_H ) Serial.println("Alarm : "+ String(Alarm_H) + "% is reached."); 627 Alarmed_1 = 1; 628 } 629 630 if (Flag_LL || Flag_HH) 631 { 632 CommandToServo(PWM_Tag, 90); /* Interlocked */ 633 if (!Alarmed_2) 634 { 635 if ( Flag_LL ) Serial.println("Interlocked: "+ String(Alarm_LL) + "% is reached."); 636 if ( Flag_HH ) Serial.println("Interlocked: "+ String(Alarm_HH) + "% is reached."); 637 Alarmed_2 = 1; 638 } 639 } 640 else if (! Actuated ) 641 { CommandToServo(PWM_Tag, 90+Direction*Speed); Actuated = 1; } 642 643 } /* Waits until reaching to setpoint */ 644 CommandToServo(PWM_Tag, 90); /* Stop command to Servo */ 645 Serial.println(PWM_Tag + " is stopped at " + String(CurrentTxReading) + "%."); 646 } /* End of "if (Section_to_Case!=0)" */ 647} /* End of "void MoveToPosition()" */ 648 649 650void CommandToLeonardo(String Section, int SetValue) { /* VERIFIED FUNCTION: ARDUINO DUE */ 651 /* SetValue is given as Percentage */ 652 int Section_Num=0; 653 if (Section=="_11FR") Section_Num = _FA_Radius; 654 else if (Section=="_11FU") Section_Num = _FA_Ulna; 655 else if (Section=="_21UA") Section_Num = _UpperArm; 656 else if (Section=="_31SH") Section_Num = _Shoulder; 657 if (Section_Num==0) Serial.println("Section cannot be recognized."); 658 if ((SetValue<0) || (SetValue>100)) Serial.println("Set Value should be between 0-100)."); 659 if ((Section_Num!=0) && (SetValue>=0) && (SetValue<=100) ) 660 { 661 Serial.print(String(SetValue) + "% -> "); 662 switch(Section_Num){ 663 case _FA_Radius: Serial.println("Front Arm-Radius"); break; 664 case _FA_Ulna : Serial.println("Front Arm-Ulna"); break; 665 case _UpperArm : Serial.println("Upper Arm"); break; 666 case _Shoulder : Serial.println("Shoulder"); break; 667 } 668 669 analogWrite (_00XXCM, Section_Num); 670 analogWrite (_00XXSP, map(SetValue, 0, 100, 0, 4095)); 671 delay(50); // Delay for stabilization of outputs 672 digitalWrite(_00XXXC, HIGH); // Execute Command 673 do { } while(!digitalRead(_00XXOK)); // Wait until getting OK from Leonardo 674 Serial.println("ARD LNRD = OK."); 675 analogWrite (_00XXCM, 0); 676 analogWrite (_00XXSP, 0); // Resetting Section and Set Value outputs 677 delay(50); // Delay for stabilization of outputs 678 digitalWrite(_00XXXC, LOW); // Resetting Command 679 } /* End of "if (Section_Num!=0)" */ 680} /* End of "void CommandToLeonardo()" */ 681 682 683void Fingers_Hold() { 684 /* CurrentPos and SetValue are given as Percentage and multipliers of 10.*/ 685 /* Finger#0 is 01-Metacarpal and #1 is 01-Proximal+Distal. 686 * The rest are only for Medial+Distal Sections of respective fingers. 687 * All timing values are determined by Speed = 20 ( PWM = 90+/-20 ) 688 * Some sections should be released in 2 steps, indicated as (x+y). 689 * Finger#.............: _01MCCM _01PDCM _02MDCM _03MDCM _04MDCM _05MDCM 690 * Tigth Direction.....: CCW CCW CCW CCW CW CW 691 * Full Tight Time...ms: 2000 2500 4000 2500 2500 2500 692 * Full Release Time.ms: 1500+500 1500 2000+1000 2000 2000 2000 693 */ 694 695 /* Tighting Order of Fingers: 03-01(MD+PD)-05-02-04 696 * 10% tight on each step. 697 */ 698 for (int i=1; i<=5; i++) { 699 _03MD_Servo.write(120); delay(500); _03MD_Servo.write(90); 700 _01MC_Servo.write(120); delay(200); _01MC_Servo.write(90); 701 _01PD_Servo.write(120); delay(250); _01PD_Servo.write(90); 702 _05MD_Servo.write(70); delay(250); _05MD_Servo.write(90); 703 _02MD_Servo.write(120); delay(400); _02MD_Servo.write(90); 704 _04MD_Servo.write(70); delay(250); _04MD_Servo.write(90); 705 } 706} /* End of "int Fingers_Hold()" */ 707 708 709void Fingers_Release() { 710 /* CurrentPos and SetValue are given as Percentage and multipliers of 10.*/ 711 /* Finger#0 is 01-Metacarpal and #1 is 01-Proximal+Distal. 712 * The rest are only for Medial+Distal Sections of respective fingers. 713 * All timing values are determined by Speed = 20 ( PWM = 90+/-20 ) 714 * Some sections should be released in 2 steps, indicated as (x+y). 715 * Finger#.............: _01MCCM _01PDCM _02MDCM _03MDCM _04MDCM _05MDCM 716 * Tigth Direction.....: CCW CCW CCW CCW CW CW 717 * Full Tight Time...ms: 2000 2500 4000 2500 2500 2500 718 * Full Release Time.ms: 1500+500 1500 2000+1000 2000 2000 2000 719 */ 720 721 /* Relesing Order of Fingers: 03-01(MD+PD)-05-02-04 722 * 10% tight on each step. 723 */ 724 for (int i=1; i<=5; i++) { 725 _03MD_Servo.write(60); delay(200); _03MD_Servo.write(90); 726 _01MC_Servo.write(70); delay(200); _01MC_Servo.write(90); 727 _01PD_Servo.write(70); delay(150); _01PD_Servo.write(90); 728 _05MD_Servo.write(110); delay(200); _05MD_Servo.write(90); 729 _02MD_Servo.write(60); delay(300); _02MD_Servo.write(90); 730 _04MD_Servo.write(110); delay(200); _04MD_Servo.write(90); 731 } 732} /* End of "int Fingers_Release()" */ 733 734 735void Final_Demo(){ 736 CommandToLeonardo("_31SH", 0); 737 CommandToLeonardo("_11FU", 0); 738 CommandToLeonardo("_21UA", 0); 739 740 for (int i=0;i<=100;i+=10) { 741 CommandToLeonardo("_31SH", i); 742 if (i<=50) CommandToLeonardo("_11FU", i); 743 else CommandToLeonardo("_11FU", (i-50)); 744 CommandToLeonardo("_21UA", i); 745 } 746 /* CommandToLeonardo(String Section, int SetValue) */ 747 CommandToLeonardo("_31SH", 0); 748 CommandToLeonardo("_11FU", 0); 749 CommandToLeonardo("_21UA", 0); 750} 751 752 753/* "void loop(): ARDUINO DUE */ 754void loop() { 755 int User_Int1=0; 756 /* TEST CODES: */ 757 758 /* VERIFIED CODES: */ 759 /* Commands and Actions */ 760 /* All recognized commands give a feedback in all cases. 761 * If you don't see any feedback on Serial Monitor, check your input 762 * for any typewriting errors. 763 * Example: "XYZ" command does not exist, so will result as follows: 764 * [ARD DUE/] > XYZ 765 * [ARD DUE/] > 766 */ 767 if (Serial.available()) 768 { 769 String Command=ReadUserInputOverSerial(); Serial.println(Command); 770 if (Command.substring(0,1)=="?") /* Help */ 771 { 772 Serial.println("Commands:"); 773 Serial.println(" CTL [Section] [SetValue (0-100)]"); 774 Serial.println(" CommandToLeonardo - Section: First 5 characters of PWM Tag (e.g. _10WH for _10WHCM)."); 775 Serial.println(" FD"); 776 Serial.println(" Final Demo."); 777 Serial.println(" FTD [ServoTag] [LimitSwitch1Tag] [LimitSwitch2Tag] [Speed (0-90)]"); 778 Serial.println(" Full Travel Duration(Test)."); 779 Serial.println(" HLD"); 780 Serial.println(" Hold (by Fingers)."); 781 Serial.println(" LT"); 782 Serial.println(" Starts Loop Test subroutine."); 783 Serial.println(" MDM [ServoTag] [Direction] [Speed (0-90)] [Milliseconds]"); 784 Serial.println(" Move During Millis-Actuate a Servo during a determined time."); 785 Serial.println(" Direction : Either CC(=CounterClockwise) or CW(=Clockwise)"); 786 Serial.println(" WARNING:"); 787 Serial.println(" This function does not check whether limit switch is matching with direction."); 788 Serial.println(" Incorrect usage can result damages. Ensure applicable durations by using"); 789 Serial.println(" FullTravelDurationMeasurement() in advance."); 790 Serial.println(" MTP [Section] [Speed (0-90)] [SetValue]"); 791 Serial.println(" Move To Position - Section: First 5 characters of PWM Tag (e.g. _10WH for _10WHCM)."); 792 Serial.println(" RD [IO_Tag]"); 793 Serial.println(" Reads value on a single input channel."); 794 Serial.println(" REL"); 795 Serial.println(" Release (by Fingers)."); 796 Serial.end(); Serial.begin(_BAUDRATE); while(!Serial) { }; 797 } 798 799 if (Command.substring(0,3)=="CTL") 800 CommandToLeonardo(Command.substring(4,9), Command.substring(10).toInt()); 801 /* CommandToLeonardo(String Section, int SetValue) */ 802 803 if (Command.substring(0,2)=="FD") 804 Final_Demo(); 805 806 if (Command.substring(0,3)=="FTD") 807 FullTravelDurationMeasurement(Command.substring(4,11), Command.substring(12,19), Command.substring(20,27), Command.substring(28,30).toInt()); 808 /* FullTravelDurationMeasurement(PWM-Output, Limit Switch.1, Limit Switch.2 Tag, Speed) */ 809 810 if (Command.substring(0,3)=="HLD") 811 Fingers_Hold(); 812 if (Command.substring(0,3)=="REL") 813 Fingers_Release(); 814 815 if (Command.substring(0,2)=="LT") 816 LoopTest(); 817 818 if (Command.substring(0,3)=="MDM") 819 { 820 if (Command.substring(12,14)=="CC") User_Int1=_CCW; 821 else if (Command.substring(12,14)=="CW") User_Int1=_CW; 822 else User_Int1=0; /* Direction */ 823 MoveDuringMillis(Command.substring(4,11), User_Int1, 824 Command.substring(15,17).toInt(), (unsigned long)Command.substring(18,22).toInt()); 825 /* MoveDuringMillis(PWM-Output, Direction, Speed, Milliseconds) */ 826 } 827 828 if (Command.substring(0,3)=="MTP") 829 MoveToPosition(Command.substring(4,9), Command.substring(10,12).toInt(), Command.substring(13).toFloat()); 830 /* MoveToPosition(Section, Speed, Set Value) */ 831 832 if (Command.substring(0,2)=="RD") 833 Read(Command.substring(3,10)); 834 /* Read(Tag) */ 835 836 CommandPromptOverSerial("[ARD DUE/] > "); 837 } /* End of "if (Serial.available())..." */ 838} /* End of "void loop()": ARDUINO DUE */ 839
Application Program - Arduino Due
arduino
This program shall be loaded onto Due
1#include <Servo.h> 2 3/* Serial Comm.Baudrate Setting: ARDUINO DUE 4 */ 5 const int _BAUDRATE = 4800; 6/* End of Serial Comm.Baudrate Setting: 7 ARDUINO DUE */ 8 9/* Tag-Channel(Pin) Assignments and IO_Channel() Function: 10 ARDUINO DUE */ 11 const int _01MCZL = 22; // 01: Thumb Finger Metacarpal ZL 12 13 const int _01MCZH = 23; // 01: Thumb Finger Metacarpal ZH 14 const int 15 _01PDZL = 24; // 01: Thumb Finger Proximal+Distal ZL 16 const int _01PDZH 17 = 25; // 01: Thumb Finger Proximal+Distal ZH 18 const int _01MCCM = 2; // 19 01: Thumb Finger Metacarpal CMD 20 const int _01PDCM = 3; // 01: Thumb Finger 21 Distal CMD 22 23 const int _02PXZL = 26; // 02: Index Finger Proximal ZL 24 25 const int _02PXZH = 27; // 02: Index Finger Proximal ZH 26 const int _02MDZL 27 = 28; // 02: Index Finger Medial+Distal ZL 28 const int _02MDZH = 29; // 29 02: Index Finger Medial+Distal ZH 30 const int _02PXCM = 4; // 02: Index 31 Finger Proximal CMD 32 const int _02MDCM = 5; // 02: Index Finger Distal 33 CMD 34 35 const int _03PXZL = 30; // 03: Middle Finger Proximal ZL 36 const 37 int _03PXZH = 31; // 03: Middle Finger Proximal ZH 38 const int _03MDZL = 39 32; // 03: Middle Finger Medial+Distal ZL 40 const int _03MDZH = 33; // 41 03: Middle Finger Medial+Distal ZH 42 const int _03PXCM = 6; // 03: Middle 43 Finger Proximal CMD 44 const int _03MDCM = 7; // 03: Middle Finger Distal 45 CMD 46 47 const int _04PXZL = 34; // 04: Ring Finger Proximal ZL 48 const 49 int _04PXZH = 35; // 04: Ring Finger Proximal ZH 50 const int _04MDZL = 36; 51 // 04: Ring Finger Medial+Distal ZL 52 const int _04MDZH = 37; // 04: Ring 53 Finger Medial+Distal ZH 54 const int _04PXCM = 8; // 04: Ring Finger Proximal 55 CMD 56 const int _04MDCM = 9; // 04: Ring Finger Distal CMD 57 58 const 59 int _05PXZL = 38; // 05: Pinky Finger Proximal ZL 60 const int _05PXZH = 39; 61 // 05: Pinky Finger Proximal ZH 62 const int _05MDZL = 40; // 05: Pinky 63 Finger Medial+Distal ZL 64 const int _05MDZH = 41; // 05: Pinky Finger Medial+Distal 65 ZH 66 const int _05PXCM = 10; // 05: Pinky Finger Proximal CMD 67 const 68 int _05MDCM = 11; // 05: Pinky Finger Distal CMD 69 70 const int _10WHZL 71 = 42; // 10: Wrist Horizontal ZL 72 const int _10WHZH = 43; // 10: Wrist 73 Horizontal ZH 74 const int _10WHTX = A0; // 10: Wrist Horizontal Actual Pos 75 76 const int _10WHCM = 12; // 10: Wrist Horizontal CMD 77 78 const int _10WVZL 79 = 44; // 10: Wrist Vertical ZL 80 const int _10WVZH = 45; // 10: Wrist Vertical 81 ZH 82 const int _10WVTX = A1; // 10: Wrist Vertical Actual Pos 83 const 84 int _10WVCM = 13; // 10: Wrist Vertical CMD 85 86 const int _11FRTX = A2; 87 // 11: Front Arm Radius Actual Pos 88 const int _11FUTX = A3; // 11: Front 89 Arm Ulna Actual Pos 90 const int _21UATX = A4; // 21: Upper Arm Actual Pos 91 92 const int _31SHTX = A5; // 31: Shoulder Actual Pos 93 94 const int _00XXXC 95 = 50; // Execute Command (0: Stop/Idle, 1: Execute) 96 const int _00XXSP = 97 DAC0; // Setpoint Register 98 const int _00XXCM = DAC1; // Command Selector 99 (*1) 100 const int _00XXOK = 51; // Command Executed by Arduino Leonardo (*2-IMPORTANT) 101 102 103 int IO_Channel(String Tag) { /* VERIFIED FUNCTION: ARDUINO DUE */ 104 /* 105 This function is used to validate IO Channel for DI/DO/AO/PWM-O. 106 * If 107 given tag is valid, respective channel number will be returned. 108 * Otherwise 109 -1 will be returned as "error". 110 */ 111 if (Tag=="_01MCZL") 112 return(_01MCZL); 113 if (Tag=="_01MCZH") return(_01MCZH); 114 if (Tag=="_01PDZL") 115 return(_01PDZL); 116 if (Tag=="_01PDZH") return(_01PDZH); 117 if (Tag=="_01MCCM") 118 return(_01MCCM); 119 if (Tag=="_01PDCM") return(_01PDCM); 120 if (Tag=="_02PXZL") 121 return(_02PXZL); 122 if (Tag=="_02PXZH") return(_02PXZH); 123 if (Tag=="_02MDZL") 124 return(_02MDZL); 125 if (Tag=="_02MDZH") return(_02MDZH); 126 if (Tag=="_02PXCM") 127 return(_02PXCM); 128 if (Tag=="_02MDCM") return(_02MDCM); 129 if (Tag=="_03PXZL") 130 return(_03PXZL); 131 if (Tag=="_03PXZH") return(_03PXZH); 132 if (Tag=="_03MDZL") 133 return(_03MDZL); 134 if (Tag=="_03MDZH") return(_03MDZH); 135 if (Tag=="_03PXCM") 136 return(_03PXCM); 137 if (Tag=="_03MDCM") return(_03MDCM); 138 if (Tag=="_04PXZL") 139 return(_04PXZL); 140 if (Tag=="_04PXZH") return(_04PXZH); 141 if (Tag=="_04MDZL") 142 return(_04MDZL); 143 if (Tag=="_04MDZH") return(_04MDZH); 144 if (Tag=="_04PXCM") 145 return(_04PXCM); 146 if (Tag=="_04MDCM") return(_04MDCM); 147 if (Tag=="_05PXZL") 148 return(_05PXZL); 149 if (Tag=="_05PXZH") return(_05PXZH); 150 if (Tag=="_05MDZL") 151 return(_05MDZL); 152 if (Tag=="_05MDZH") return(_05MDZH); 153 if (Tag=="_05PXCM") 154 return(_05PXCM); 155 if (Tag=="_05MDCM") return(_05MDCM); 156 if (Tag=="_10WHZL") 157 return(_10WHZL); 158 if (Tag=="_10WHZH") return(_10WHZH); 159 if (Tag=="_10WHTX") 160 return(_10WHTX); 161 if (Tag=="_10WHCM") return(_10WHCM); 162 if (Tag=="_10WVZL") 163 return(_10WVZL); 164 if (Tag=="_10WVZH") return(_10WVZH); 165 if (Tag=="_10WVTX") 166 return(_10WVTX); 167 if (Tag=="_10WVCM") return(_10WVCM); 168 if (Tag=="_11FRTX") 169 return(_11FRTX); 170 if (Tag=="_11FUTX") return(_11FUTX); 171 if (Tag=="_21UATX") 172 return(_21UATX); 173 if (Tag=="_31SHTX") return(_31SHTX); 174 if (Tag=="_00XXXC") 175 return(_00XXXC); 176 if (Tag=="_00XXSP") return(_00XXSP); 177 if (Tag=="_00XXCM") 178 return(_00XXCM); 179 if (Tag=="_00XXOK") return(_00XXOK); 180 return(-1); 181} 182 /* End of "IO_Channel(String Tag)": ARDUINO DUE */ 183/* End of Tag-Channel(Pin) 184 Assignments and IO_Channel() Function: ARDUINO DUE */ 185 186 187/* Servo _Definitions 188 + CommandToServo() function: ARDUINO DUE */ 189 Servo _01MC_Servo; 190 Servo 191 _01PD_Servo; 192 Servo _02PX_Servo; 193 Servo _02MD_Servo; 194 Servo _03PX_Servo; 195 196 Servo _03MD_Servo; 197 Servo _04PX_Servo; 198 Servo _04MD_Servo; 199 Servo 200 _05PX_Servo; 201 Servo _05MD_Servo; 202 Servo _10WH_Servo; 203 Servo _10WV_Servo; 204 205 206 void CommandToServo(String Tag, int Value) { /* VERIFIED FUNCTION: ARDUINO DUE 207 */ 208 /* This function is used to actuate PWM outputs to Servo Motors. */ 209 210 if (Tag=="_01MCCM") _01MC_Servo.write(Value); 211 if (Tag=="_01PDCM") 212 _01PD_Servo.write(Value); 213 if (Tag=="_02PXCM") _02PX_Servo.write(Value); 214 215 if (Tag=="_02MDCM") _02MD_Servo.write(Value); 216 if (Tag=="_03PXCM") 217 _03PX_Servo.write(Value); 218 if (Tag=="_03MDCM") _03MD_Servo.write(Value); 219 220 if (Tag=="_04PXCM") _04PX_Servo.write(Value); 221 if (Tag=="_04MDCM") 222 _04MD_Servo.write(Value); 223 if (Tag=="_05PXCM") _05PX_Servo.write(Value); 224 225 if (Tag=="_05MDCM") _05MD_Servo.write(Value); 226 if (Tag=="_10WHCM") 227 _10WH_Servo.write(Value); 228 if (Tag=="_10WVCM") _10WV_Servo.write(Value); 229 230 } /* End of "CommandToServo()": ARDUINO DUE */ 231/* End of Servo _Definitions 232 + CommandToServo() function: ARDUINO DUE */ 233 234 235/* Constant Parameters, floatMap() 236 Function: ARDUINO DUE */ 237 const float _DBPercent = 1.0; // % Deadband 238 239 const int _CCW = 1; // Servo Direction for Counter Clockwise 240 241 const int _CW = -1; // Servo Direction for Counter Clockwise 242 243 244 float floatMap /* VERIFIED FUNCTION: ARDUINO DUE */ 245 (int 246 InputValue, int fromLow, int fromHigh, int toLow, int toHigh) { 247 /* This 248 function has exacly same functionality with Arduino's 249 * map(value, fromLow, 250 fromHigh, toLow, toHigh) 251 * function. However, original map function truncates 252 fractions and return 253 * just integers. So this one is programmed to return 254 exactly scaled values. 255 */ 256 return( (float)toLow + (float)((float)((InputValue-fromLow)*(toHigh-toLow))/(float)(fromHigh-fromLow)) 257 ); 258 } /* End of "floatMap()" */ 259 260/* End of Constant Parameters, 261 floatMap() Function: ARDUINO DUE */ 262 263 264/* Constant Parameters for Intercommunication: 265 ARDUINO DUE */ 266 const int _FA_Radius = 840; // Front Arm Radius Section 267 268 const int _FA_Ulna = 1530; // Front Arm Ulna Section 269 const int 270 _UpperArm = 2220; // Upper Arm Section 271 const int _Shoulder = 272 2910; // Front Arm Radius Section 273/* End of Constant Parameters for Intercommunication: 274 ARDUINO DUE */ 275 276 277/* "void setup()": ARDUINO DUE */ 278void setup() { 279 280 // TEST CODES: 281 282 // VERIFIED CODES: 283 pinMode(_01MCZL, INPUT_PULLUP); 284 285 pinMode(_01MCZH, INPUT_PULLUP); 286 pinMode(_01PDZL, INPUT_PULLUP); 287 pinMode(_01PDZH, 288 INPUT_PULLUP); 289 pinMode(_02PXZL, INPUT_PULLUP); 290 pinMode(_02PXZH, INPUT_PULLUP); 291 292 pinMode(_02MDZL, INPUT_PULLUP); 293 pinMode(_02MDZH, INPUT_PULLUP); 294 pinMode(_03PXZL, 295 INPUT_PULLUP); 296 pinMode(_03PXZH, INPUT_PULLUP); 297 pinMode(_03MDZL, INPUT_PULLUP); 298 299 pinMode(_03MDZH, INPUT_PULLUP); 300 pinMode(_04PXZL, INPUT_PULLUP); 301 pinMode(_04PXZH, 302 INPUT_PULLUP); 303 pinMode(_04MDZL, INPUT_PULLUP); 304 pinMode(_04MDZH, INPUT_PULLUP); 305 306 pinMode(_05PXZL, INPUT_PULLUP); 307 pinMode(_05PXZH, INPUT_PULLUP); 308 pinMode(_05MDZL, 309 INPUT_PULLUP); 310 pinMode(_05MDZH, INPUT_PULLUP); 311 pinMode(_10WHZL, INPUT_PULLUP); 312 313 pinMode(_10WHZH, INPUT_PULLUP); 314 pinMode(_10WVZL, INPUT_PULLUP); 315 pinMode(_10WVZH, 316 INPUT_PULLUP); 317 pinMode(_00XXOK, INPUT); 318 319 _01MC_Servo.attach(_01MCCM); 320 321 _01PD_Servo.attach(_01PDCM); 322 _02PX_Servo.attach(_02PXCM); 323 _02MD_Servo.attach(_02MDCM); 324 325 _03PX_Servo.attach(_03PXCM); 326 _03MD_Servo.attach(_03MDCM); 327 _04PX_Servo.attach(_04PXCM); 328 329 _04MD_Servo.attach(_04MDCM); 330 _05PX_Servo.attach(_05PXCM); 331 _05MD_Servo.attach(_05MDCM); 332 333 _10WH_Servo.attach(_10WHCM); 334 _10WV_Servo.attach(_10WVCM); 335 336 /* 337 This part is used to send initial "neutral position/stop" 338 * to all PWM 339 outputs assigned to Servo Motors. 340 */ 341 _01MC_Servo.write(90); 342 _01PD_Servo.write(90); 343 344 _02PX_Servo.write(90); 345 _02MD_Servo.write(90); 346 _03PX_Servo.write(90); 347 348 _03MD_Servo.write(90); 349 _04PX_Servo.write(90); 350 _04MD_Servo.write(90); 351 352 _05PX_Servo.write(90); 353 _05MD_Servo.write(90); 354 _10WH_Servo.write(90); 355 356 _10WV_Servo.write(90); 357 358 analogReadResolution(12); /* Analog Reading 359 Scale: 0-4095 for ARDUINO DUE*/ 360 361 analogWriteResolution(12); /* Analog Writing 362 Scale: 0-4095 for ARDUINO DUE*/ 363 /* DAC Initialize 364 * DAC initialization 365 is needed at first start, otherwise DAC of Due 366 * generates incorrect voltage 367 levels. 368 * Note that 0 and 4095 have no functions on Leonardo. 369 */ 370 371 analogWrite(_00XXSP,0); delay(50); analogWrite(_00XXSP,4095); delay(50); analogWrite(_00XXSP,0); 372 373 analogWrite(_00XXCM,0); delay(50); analogWrite(_00XXCM,4095); delay(50); analogWrite(_00XXCM,0); 374 375 376 digitalWrite(_00XXXC,LOW); 377 378 Serial.begin(_BAUDRATE); while(!Serial) 379 { }; 380 Serial.println("? -> Help."); 381 Serial.print("[ARD DUE/] > "); 382} 383 /* End of "void setup()": ARDUINO DUE */ 384 385 386/* Global Variables for Test 387 Purposes: ARDUINO DUE */ 388 389/* End of Global Variables for Test Purposes: ARDUINO 390 DUE */ 391 392 393/* Verified Global Variables: ARDUINO DUE */ 394 395/* End of 396 Verified Global Variables: ARDUINO DUE */ 397 398 399void CommandPromptOverSerial(String 400 _Prompt) { /* VERIFIED FUNCTION: ARDUINO DUE */ 401 /* Reprompting and flushing 402 serial for next input. 403 * This function will be used after each use of ReadUserInputOverSerial() 404 405 */ 406 Serial.print(_Prompt); Serial.end(); Serial.begin(_BAUDRATE); while(!Serial) 407 { }; 408} /* End of "CommandPromtOverSerial()": ARDUINO DUE */ 409 410 411String 412 ReadUserInputOverSerial(){ /* VERIFIED FUNCTION: ARDUINO DUE */ 413 /* This function 414 will be used only "if (Serial.available())" */ 415 char UserInput[32]; 416 417 Serial.readBytes(UserInput,32); 418 /* Buffering and capitalizing User Input 419 */ 420 String Buffer=UserInput; Buffer.toUpperCase(); 421 return(Buffer); 422} 423 /* End of "ReadUserInputOverSerial()": ARDUINO DUE */ 424 425 426void Read(String 427 Tag) { /* VERIFIED FUNCTION: ARDUINO DUE */ 428 /* Indicates value at input channel 429 defined by "Tag" */ 430 if ( (Tag.substring(5,7)=="ZL") || (Tag.substring(5,7)=="ZH") 431 || (Tag.substring(5,7)=="OK")) 432 Serial.println(Tag + " = " + String(digitalRead(IO_Channel(Tag)))); 433 434 else if (Tag.substring(5,7)=="TX") 435 Serial.println(Tag + " = " 436 + String(analogRead(IO_Channel(Tag)))); 437 else Serial.println(Tag + "is 438 not an input."); 439} /* End of "void Read()": ARDUINO DUE */ 440 441 442void 443 LoopTest() { /* VERIFIED FUNCTION: ARDUINO DUE */ 444 String Command, User_Tag; 445 446 Serial.println("? -> Help, X -> Terminate"); 447 CommandPromptOverSerial("[ARD 448 DUE/LoopTest/] > "); 449 do 450 { 451 if (Serial.available()) 452 { 453 454 byte Command_to_Case=0; /* Command is invalid if remains as 0. */ 455 456 int User_Value=0, User_Value_Last=0, Reading=0, Reading_Last=-1; 457 458 String Buffer = ReadUserInputOverSerial(); 459 Serial.println(Buffer); 460 Serial.end(); Serial.begin(_BAUDRATE); while(!Serial) { }; 461 462 /* 463 Seperating Command and Tag*/ 464 Command = Buffer.substring(0,2); User_Tag 465 = Buffer.substring(3,10); 466 467 /* Converting (string)Command 468 to (byte)Command_to_Case */ 469 if (Command=="AI") Command_to_Case=11; 470 471 if (Command=="AO") Command_to_Case=12; 472 if (Command=="DI") 473 Command_to_Case=21; 474 if (Command=="DO") Command_to_Case=22; 475 if 476 (Command=="PO") Command_to_Case=32; 477 if (Command[0]=='?') Command_to_Case=98; 478 // Display Help 479 if (Command[0]=='X') Command_to_Case=99; // Terminate 480 Test 481 482 /* Checking whether tag is valid. */ 483 if 484 ((Command_to_Case!=0)&&(Command_to_Case<90)&&(IO_Channel(User_Tag)<0)) 485 Command_to_Case=1; 486 /* Redirecting to unrecognized tag case. */ 487 488 /* Taking respective 489 actions and giving feedbacks for each command. */ 490 switch (Command_to_Case) 491 { 492 case 0 : Serial.println("Invalid command."); 493 CommandPromptOverSerial("[ARD 494 DUE/LoopTest/] > "); 495 break; 496 case 1 497 : Serial.println("Tag not exists: " + User_Tag); 498 CommandPromptOverSerial("[ARD 499 DUE/LoopTest/] > "); 500 break; 501 case 11: 502 Serial.println("AI-Test('X'->Terminate): " + User_Tag); 503 Reading_Last 504 = -1; 505 do 506 { 507 Buffer 508 = ReadUserInputOverSerial(); 509 Reading = analogRead(IO_Channel(User_Tag)); 510 511 if (Reading != Reading_Last) 512 { 513 514 Serial.println(User_Tag + " = " + String(Reading)); 515 516 Reading_Last = Reading; delay(1000); 517 /* 518 1s delay is used to avoid fast updates on Serial Monitor */ 519 } 520 521 } 522 while (Buffer[0]!='X'); 523 524 Serial.println(); 525 CommandPromptOverSerial("[ARD 526 DUE/LoopTest/] > "); 527 break; 528 case 12: 529 Serial.println("AO-Test('X'->Terminate): " + User_Tag); 530 Serial.println("Integer 531 Value(0-4095): "); 532 Buffer=""; 533 do 534 535 { 536 if (Serial.available()) 537 538 { 539 Buffer = ReadUserInputOverSerial(); 540 541 if (Buffer[0]=='X') User_Value=0; else User_Value=Buffer.substring(0).toInt(); 542 543 Serial.println(String(User_Value) + " -> " + User_Tag); 544 545 if (User_Value!=User_Value_Last) 546 { 547 548 analogWrite(IO_Channel(User_Tag),User_Value); 549 550 User_Value_Last = User_Value; 551 } 552 553 else analogWrite(IO_Channel(User_Tag),User_Value_Last); 554 555 Serial.println("Integer Value(0-4095): "); 556 557 Serial.end(); Serial.begin(_BAUDRATE); while(!Serial) 558 { }; 559 } /* End of "if (Serial.available())..." 560 */ 561 } 562 while (Buffer[0]!='X'); 563 564 Serial.println(); 565 CommandPromptOverSerial("[ARD 566 DUE/LoopTest/] > "); 567 break; 568 case 21: 569 Serial.println("DI-Test('X'->Terminate): " + User_Tag); 570 Reading_Last 571 = -1; 572 do 573 { 574 Buffer 575 = ReadUserInputOverSerial(); 576 Reading = digitalRead(IO_Channel(User_Tag)); 577 578 if (Reading != Reading_Last) 579 { 580 581 Serial.println(User_Tag + " = " + String(Reading)); 582 583 Reading_Last = Reading; 584 } 585 586 } 587 while (Buffer[0]!='X'); 588 589 Serial.println(); 590 CommandPromptOverSerial("[ARD 591 DUE/LoopTest/] > "); 592 break; 593 case 22: 594 Serial.println("DO-Test('X'->Terminate): " + User_Tag); 595 Serial.print("Enter 596 0 or 1: "); 597 Buffer=""; 598 do 599 600 { 601 if (Serial.available()) 602 603 { 604 Buffer = ReadUserInputOverSerial(); 605 606 if (Buffer[0]=='X') User_Value=0; else User_Value=Buffer.substring(0).toInt(); 607 608 Serial.println(String(User_Value) + " -> " + User_Tag); 609 610 if (((User_Value==0)||(User_Value==1))&&(User_Value!=User_Value_Last)) 611 612 { 613 614 615 digitalWrite(IO_Channel(User_Tag),User_Value); 616 617 User_Value_Last = User_Value; 618 } 619 620 else digitalWrite(IO_Channel(User_Tag),User_Value_Last); 621 622 Serial.print("Enter 0 or 1: "); 623 Serial.end(); 624 Serial.begin(_BAUDRATE); while(!Serial) { }; 625 } 626 /* End of "if (Serial.available())..." */ 627 } 628 while 629 (Buffer[0]!='X'); 630 CommandPromptOverSerial("[ARD DUE/LoopTest/] 631 > "); 632 break; 633 case 32: Serial.println("PWM-Test('X'->Terminate): 634 " + User_Tag); 635 CommandToServo(User_Tag,90); /* Sending 636 "stop" to selected tag */ 637 Serial.print("Integer Value(0-180): 638 "); 639 Buffer=""; 640 do 641 { 642 643 if (Serial.available()) 644 { 645 646 Buffer = ReadUserInputOverSerial(); 647 if 648 (Buffer[0]=='X') User_Value=90; else User_Value=Buffer.substring(0).toInt(); 649 650 Serial.println(String(User_Value) + " -> " + User_Tag); 651 652 if (((User_Value>=0)||(User_Value<=180))&&(User_Value!=User_Value_Last)) 653 654 { CommandToServo(User_Tag,User_Value); User_Value_Last 655 = User_Value; } 656 else CommandToServo(User_Tag,User_Value); 657 658 Serial.print("Integer Value(0-180): "); 659 Serial.end(); 660 Serial.begin(_BAUDRATE); while(!Serial) { }; 661 } 662 /* End of "if (Serial.available())..." */ 663 } 664 while 665 (Buffer[0]!='X'); 666 Serial.println(); 667 CommandPromptOverSerial("[ARD 668 DUE/LoopTest/] > "); 669 break; 670 case 98: 671 Serial.println("Usage: '[CC] [Tag]'"); 672 Serial.println(" 673 [CC]: Channel Type"); 674 Serial.println(" AI, 675 AO: Analog Input/Output"); 676 Serial.println(" DI, 677 DO: Digital Input/Output"); 678 Serial.println(" PO: 679 PWM Output for Servo Motor"); 680 Serial.println("Other 681 Commands:"); 682 Serial.println(" ? : Help (this display)"); 683 684 Serial.println(" X : Exit from Loop Testing"); 685 686 CommandPromptOverSerial("[ARD DUE/LoopTest/] > "); 687 688 break; 689 case 99: 690 Serial.println("Loop Test is terminated."); 691 break; 692 693 } /* End of "switch (Command_to_Case)..." 694 */ 695 } /* End of "if (Serial.available())..." */ 696 } 697 while (Command[0]!='X'); 698} 699 /* End of "void LoopTest()": ARDUINO DUE */ 700 701 702void FullTravelDurationMeasurement( 703 /* VERIFIED FUNCTION: ARDUINO DUE */ 704 String PWM_Tag, String LimitSw_CCW, 705 String LimitSw_CW, int Speed) { 706 /* This function is used to measure full travel 707 time of a "section" 708 * equipped with Servo:User_Tag and Limit Switches. 709 710 */ 711 int LimitSw_CCW_Ch = IO_Channel(LimitSw_CCW), 712 LimitSw_CW_Ch 713 = IO_Channel(LimitSw_CW), 714 _LimitSwCCW_Ch = 0, /* Corrected 715 Limit Sw.Ch.for CCW-direction */ 716 _LimitSwCW_Ch = 0, /* Corrected 717 Limit Sw.Ch.for CW-direction */ 718 Direction = _CCW, 719 Counter 720 = 0; 721 unsigned long StartTime=0, Duration=0; 722 Serial.println("Full 723 Travel Duration Test"); 724 if ((IO_Channel(PWM_Tag)*LimitSw_CCW_Ch*LimitSw_CW_Ch)!=-1) 725 726 { 727 Serial.println(" PWM : " + PWM_Tag + " [Ch. " 728 + String(IO_Channel(PWM_Tag)) + "]"); 729 Serial.println(" Limit Switch.1: 730 " + LimitSw_CCW + " [Ch. " + String(LimitSw_CCW_Ch) + "]"); 731 Serial.println(" 732 Limit Switch.2: " + LimitSw_CW + " [Ch. " + String(LimitSw_CW_Ch) + "]"); 733 734 735 /* Matching Limit Switches with Directions 736 * Servo 737 is set to turn CCW direction at first. Then reached limit switch is 738 * 739 marked as the one corresponding to CCW-end, while the other is for CW-end. 740 * 741 Notes: 742 * - Limit switches gives 0 when reached. 743 * - 744 (90+Speed) -> Counter Clockwise turn (CCW) 745 * - (90-Speed) -> Clockwise 746 turn (CW) 747 */ 748 CommandToServo(PWM_Tag, 90 + Direction*Speed); 749 750 while ( digitalRead(LimitSw_CCW_Ch) && digitalRead(LimitSw_CW_Ch) ) { } 751 752 CommandToServo(PWM_Tag, 90); 753 if ( ! digitalRead(LimitSw_CCW_Ch) 754 ) 755 { _LimitSwCCW_Ch = LimitSw_CCW_Ch; _LimitSwCW_Ch = LimitSw_CW_Ch; 756 } 757 else 758 { _LimitSwCCW_Ch = LimitSw_CW_Ch; _LimitSwCW_Ch 759 = LimitSw_CCW_Ch; } 760 Serial.println(" DI-Channel matches CCW-end: 761 Ch. " + String(_LimitSwCCW_Ch)); 762 Serial.println(" DI-Channel matches 763 CW-end : Ch. " + String(_LimitSwCW_Ch)); 764 765 /* Main Test, 5 steps */ 766 767 for (Counter=0; Counter<=4; Counter++) 768 { 769 /* 770 Direction must be reverted when limit is reached */ 771 if ( ! digitalRead(_LimitSwCCW_Ch) 772 ) Direction = _CW; 773 else if ( ! digitalRead(_LimitSwCW_Ch) ) Direction 774 = _CCW; 775 else Direction = 0; 776 777 /* 778 Starting time is stored and Servo is actuated */ 779 StartTime = millis(); 780 CommandToServo(PWM_Tag, 90 + Direction*Speed); 781 782 /* Wait until 783 reaching to limit at direction */ 784 if (Direction==-1) while ( digitalRead(_LimitSwCW_Ch) 785 ) { } 786 else if (Direction==1) while ( digitalRead(_LimitSwCCW_Ch) 787 ) { } 788 789 CommandToServo(PWM_Tag, 90); Duration = millis() 790 - StartTime; Direction *= -1; 791 Serial.println(" Duration.#" 792 + String(Counter) + ": " + String(Duration) + " ms"); 793 } 794 } 795 796 else 797 Serial.println("Error: Unrecognized tag(s)."); 798} /* End 799 of "void FullTravelDurationMeasurement()": ARDUINO DUE */ 800 801 802void MoveDuringMillis 803 /* VERIFIED FUNCTION: ARDUINO DUE */ 804 (String Tag, int Direction, int Speed, 805 unsigned long Milliseconds) { 806 /* This function is used to operate a "section" 807 given by "Tag" to "Direction" 808 * at specified "Speed" and during determined 809 "Milliseconds", unless "LimitSw" 810 * is activated (Limit Switches give 0 811 when activated). 812 */ 813 unsigned long StartTime=0, EndTime=0; 814 Serial.print("Actuate 815 " + Tag + ", "); 816 switch (Direction) { 817 case _CCW: Serial.print("Counter 818 Clockwise, "); break; 819 case _CW : Serial.print("Clockwise, "); break; 820 821 case 0 : Serial.print("STOP, "); break; 822 } 823 Serial.print("Speed="+String(Speed)+", 824 "); 825 Serial.println("Time="+String(Milliseconds)+"ms"); 826 if (IO_Channel(Tag)==-1) 827 Serial.println("Error: Recheck given tag."); 828 else 829 { 830 StartTime=millis(); 831 832 CommandToServo(Tag, 90 + Direction*Speed); 833 while ( (millis()-StartTime)<Milliseconds 834 ) 835 { /* Waits until time is over */ }; 836 CommandToServo(Tag, 837 90); EndTime=millis(); /* Stop command to Servo */ 838 Serial.println(Tag 839 + " STOP: Time reached - " + String(EndTime-StartTime) + "ms."); 840 } 841} 842 /* End of "void MoveDuringMillis()": ARDUINO DUE */ 843 844 845void MoveToPosition 846 /* VERIFIED FUNCTION: ARDUINO DUE */ 847 (String Section, int Speed, float SetValue) 848 { 849 /* This function is used to operate a "Section" at a determined 850 * 851 "Speed" to required position given by "SetValue". 852 * ("Section" is the 853 first 5 characters of any I/O tag.) 854 */ 855 String PWM_Tag=Section+"CM", 856 TX_Tag=Section+"TX"; 857 858 boolean Actuated=0, 859 Flag_SPMatched=0, 860 861 Flag_LL=0, Flag_L=0, Flag_H=0, Flag_HH=0, 862 Alarmed_1=0, Alarmed_2=0; 863 864 865 byte Section_to_Case=0; 866 867 int IncreaseDirection=0, Direction=0, 868 869 TX_RawMin, TX_RawMax, 870 Alarm_LL, Alarm_L, Alarm_H, Alarm_HH, 871 872 CurrentTxReading; 873 874 if (Section=="_10WH") { Section_to_Case=1; 875 IncreaseDirection=_CCW; } 876 else if (Section=="_10WV") { Section_to_Case=2; 877 IncreaseDirection=_CW; } 878 879 /* Scale and Alarm values corresponding to Sections 880 should be adjusted 881 * depending on tests. 882 * Direction equations should 883 be adjusted by multiplying -1 depending on 884 * mechanical configuration. 885 886 */ 887 switch (Section_to_Case) { 888 case 0 : Serial.println("Invalid 889 section."); 890 break; 891 case 1 : /* Customizations 892 for _10WH Section */ 893 TX_RawMin=0; TX_RawMax=4095; 894 Alarm_LL=10; 895 Alarm_L=20; Alarm_H=80; Alarm_HH=90; 896 break; 897 case 898 2 : /* Customizations for _10WL Section */ 899 TX_RawMin=0; TX_RawMax=4095; 900 901 Alarm_LL=10; Alarm_L=20; Alarm_H=80; Alarm_HH=90; 902 break; 903 904 } 905 906 CurrentTxReading=map( analogRead(IO_Channel(TX_Tag)), TX_RawMin, TX_RawMax, 907 0, 100 ); 908 909 if (Section_to_Case!=0 && Speed!=0) 910 { 911 Serial.print("Actuate 912 " + PWM_Tag + ", Pos.Tx:" + TX_Tag + "[=" + String(CurrentTxReading) + "%], 913 "); 914 if (IncreaseDirection==_CCW) Direction = (int)( (SetValue-CurrentTxReading)/abs(SetValue-CurrentTxReading) 915 ); 916 else Direction = (int)( (-1)*(SetValue-CurrentTxReading)/abs(SetValue-CurrentTxReading) 917 ); 918 switch (Direction) { 919 case _CCW: Serial.print("CCW, 920 "); 921 break; 922 case _CW : Serial.print("CW, 923 "); 924 break; 925 case 0 : Serial.print("STOP, 926 "); 927 break; 928 } 929 Serial.print("Speed="+String(Speed)+", 930 "); 931 Serial.println("Set Value="+String(SetValue)+"%"); 932 while 933 ( !( Flag_SPMatched || Flag_LL || Flag_HH ) ) 934 { 935 CurrentTxReading=map( 936 analogRead(IO_Channel(TX_Tag)), TX_RawMin, TX_RawMax, 0, 100 ); 937 if 938 (IncreaseDirection==_CCW) 939 { 940 if ( (Direction==_CW) 941 && (CurrentTxReading<=Alarm_L) ) Flag_L = 1; 942 if ( (Direction==_CW) 943 && (CurrentTxReading<=Alarm_LL) ) Flag_LL = 1; 944 if ( (Direction==_CCW) 945 && (CurrentTxReading>=Alarm_H) ) Flag_H = 1; 946 if ( (Direction==_CCW) 947 && (CurrentTxReading>=Alarm_HH) ) Flag_HH = 1; 948 if ( ( (Direction==_CCW) 949 && (CurrentTxReading>=SetValue) ) || 950 ( (Direction==_CW) 951 && (CurrentTxReading<=SetValue) ) ) Flag_SPMatched = 1; 952 } 953 954 if (IncreaseDirection==_CW) 955 { 956 if 957 ( (Direction==_CCW) && (CurrentTxReading<=Alarm_L) ) Flag_L = 1; 958 if 959 ( (Direction==_CCW) && (CurrentTxReading<=Alarm_LL) ) Flag_LL = 1; 960 if 961 ( (Direction==_CW) && (CurrentTxReading>=Alarm_H) ) Flag_H = 1; 962 if 963 ( (Direction==_CW) && (CurrentTxReading>=Alarm_HH) ) Flag_HH = 1; 964 if 965 ( ( (Direction==_CW) && (CurrentTxReading>=SetValue) ) || 966 ( 967 (Direction==_CCW) && (CurrentTxReading<=SetValue) ) ) Flag_SPMatched = 1; 968 } 969 970 971 if (Flag_L || Flag_H) 972 if (!Alarmed_1) 973 { 974 975 if ( Flag_L ) Serial.println("Alarm : "+ String(Alarm_L) 976 + "% is reached."); 977 if ( Flag_H ) Serial.println("Alarm 978 : "+ String(Alarm_H) + "% is reached."); 979 Alarmed_1 980 = 1; 981 } 982 983 if (Flag_LL || Flag_HH) 984 { 985 986 CommandToServo(PWM_Tag, 90); /* Interlocked */ 987 if 988 (!Alarmed_2) 989 { 990 if ( Flag_LL ) Serial.println("Interlocked: 991 "+ String(Alarm_LL) + "% is reached."); 992 if ( Flag_HH ) 993 Serial.println("Interlocked: "+ String(Alarm_HH) + "% is reached."); 994 Alarmed_2 995 = 1; 996 } 997 } 998 else if (! 999 Actuated ) 1000 { CommandToServo(PWM_Tag, 90+Direction*Speed); 1001 Actuated = 1; } 1002 1003 } /* Waits until reaching to setpoint 1004 */ 1005 CommandToServo(PWM_Tag, 90); /* Stop command to Servo */ 1006 Serial.println(PWM_Tag 1007 + " is stopped at " + String(CurrentTxReading) + "%."); 1008 } /* End of 1009 "if (Section_to_Case!=0)" */ 1010} /* End of "void MoveToPosition()" */ 1011 1012 1013void 1014 CommandToLeonardo(String Section, int SetValue) { /* VERIFIED FUNCTION: ARDUINO 1015 DUE */ 1016 /* SetValue is given as Percentage */ 1017 int Section_Num=0; 1018 1019 if (Section=="_11FR") Section_Num = _FA_Radius; 1020 else if (Section=="_11FU") 1021 Section_Num = _FA_Ulna; 1022 else if (Section=="_21UA") Section_Num 1023 = _UpperArm; 1024 else if (Section=="_31SH") Section_Num 1025 = _Shoulder; 1026 if (Section_Num==0) Serial.println("Section cannot be recognized."); 1027 1028 if ((SetValue<0) || (SetValue>100)) Serial.println("Set Value should be between 1029 0-100)."); 1030 if ((Section_Num!=0) && (SetValue>=0) && (SetValue<=100) ) 1031 1032 { 1033 Serial.print(String(SetValue) + "% -> "); 1034 switch(Section_Num){ 1035 1036 case _FA_Radius: Serial.println("Front Arm-Radius"); break; 1037 1038 case _FA_Ulna : Serial.println("Front Arm-Ulna"); break; 1039 case 1040 _UpperArm : Serial.println("Upper Arm"); break; 1041 case _Shoulder 1042 : Serial.println("Shoulder"); break; 1043 } 1044 1045 analogWrite (_00XXCM, 1046 Section_Num); 1047 analogWrite (_00XXSP, map(SetValue, 0, 100, 0, 4095)); 1048 1049 delay(50); // Delay for stabilization of outputs 1050 1051 digitalWrite(_00XXXC, HIGH); // Execute Command 1052 do { 1053 } while(!digitalRead(_00XXOK)); // Wait until getting OK from Leonardo 1054 Serial.println("ARD 1055 LNRD = OK."); 1056 analogWrite (_00XXCM, 0); 1057 analogWrite (_00XXSP, 1058 0); // Resetting Section and Set Value outputs 1059 delay(50); 1060 // Delay for stabilization of outputs 1061 digitalWrite(_00XXXC, 1062 LOW); // Resetting Command 1063 } /* End of "if (Section_Num!=0)" 1064 */ 1065} /* End of "void CommandToLeonardo()" */ 1066 1067 1068void Fingers_Hold() 1069 { 1070 /* CurrentPos and SetValue are given as Percentage and multipliers of 1071 10.*/ 1072 /* Finger#0 is 01-Metacarpal and #1 is 01-Proximal+Distal. 1073 * 1074 The rest are only for Medial+Distal Sections of respective fingers. 1075 * All 1076 timing values are determined by Speed = 20 ( PWM = 90+/-20 ) 1077 * Some sections 1078 should be released in 2 steps, indicated as (x+y). 1079 * Finger#.............: 1080 _01MCCM _01PDCM _02MDCM _03MDCM _04MDCM _05MDCM 1081 * Tigth Direction.....: 1082 CCW CCW CCW CCW CW CW 1083 * Full Tight Time...ms: 1084 2000 2500 4000 2500 2500 2500 1085 * Full Release Time.ms: 1086 1500+500 1500 2000+1000 2000 2000 2000 1087 */ 1088 1089 /* Tighting 1090 Order of Fingers: 03-01(MD+PD)-05-02-04 1091 * 10% tight on each step. 1092 */ 1093 1094 for (int i=1; i<=5; i++) { 1095 _03MD_Servo.write(120); delay(500); _03MD_Servo.write(90); 1096 1097 _01MC_Servo.write(120); delay(200); _01MC_Servo.write(90); 1098 _01PD_Servo.write(120); 1099 delay(250); _01PD_Servo.write(90); 1100 _05MD_Servo.write(70); delay(250); 1101 _05MD_Servo.write(90); 1102 _02MD_Servo.write(120); delay(400); _02MD_Servo.write(90); 1103 1104 _04MD_Servo.write(70); delay(250); _04MD_Servo.write(90); 1105 } 1106} 1107 /* End of "int Fingers_Hold()" */ 1108 1109 1110void Fingers_Release() { 1111 /* 1112 CurrentPos and SetValue are given as Percentage and multipliers of 10.*/ 1113 /* 1114 Finger#0 is 01-Metacarpal and #1 is 01-Proximal+Distal. 1115 * The rest are only 1116 for Medial+Distal Sections of respective fingers. 1117 * All timing values are 1118 determined by Speed = 20 ( PWM = 90+/-20 ) 1119 * Some sections should be released 1120 in 2 steps, indicated as (x+y). 1121 * Finger#.............: _01MCCM _01PDCM 1122 _02MDCM _03MDCM _04MDCM _05MDCM 1123 * Tigth Direction.....: CCW CCW 1124 CCW CCW CW CW 1125 * Full Tight Time...ms: 2000 2500 1126 4000 2500 2500 2500 1127 * Full Release Time.ms: 1500+500 1500 1128 2000+1000 2000 2000 2000 1129 */ 1130 1131 /* Relesing Order of Fingers: 1132 03-01(MD+PD)-05-02-04 1133 * 10% tight on each step. 1134 */ 1135 for (int 1136 i=1; i<=5; i++) { 1137 _03MD_Servo.write(60); delay(200); _03MD_Servo.write(90); 1138 1139 _01MC_Servo.write(70); delay(200); _01MC_Servo.write(90); 1140 _01PD_Servo.write(70); 1141 delay(150); _01PD_Servo.write(90); 1142 _05MD_Servo.write(110); delay(200); 1143 _05MD_Servo.write(90); 1144 _02MD_Servo.write(60); delay(300); _02MD_Servo.write(90); 1145 1146 _04MD_Servo.write(110); delay(200); _04MD_Servo.write(90); 1147 } 1148} 1149 /* End of "int Fingers_Release()" */ 1150 1151 1152void Final_Demo(){ 1153 CommandToLeonardo("_31SH", 1154 0); 1155 CommandToLeonardo("_11FU", 0); 1156 CommandToLeonardo("_21UA", 0); 1157 1158 1159 for (int i=0;i<=100;i+=10) { 1160 CommandToLeonardo("_31SH", i); 1161 1162 if (i<=50) CommandToLeonardo("_11FU", i); 1163 else CommandToLeonardo("_11FU", 1164 (i-50)); 1165 CommandToLeonardo("_21UA", i); 1166 } 1167 /* CommandToLeonardo(String 1168 Section, int SetValue) */ 1169 CommandToLeonardo("_31SH", 0); 1170 CommandToLeonardo("_11FU", 1171 0); 1172 CommandToLeonardo("_21UA", 0); 1173} 1174 1175 1176/* "void loop(): ARDUINO 1177 DUE */ 1178void loop() { 1179 int User_Int1=0; 1180 /* TEST CODES: */ 1181 1182 1183 /* VERIFIED CODES: */ 1184 /* Commands and Actions */ 1185 /* All 1186 recognized commands give a feedback in all cases. 1187 * If you don't see any 1188 feedback on Serial Monitor, check your input 1189 * for any typewriting errors. 1190 1191 * Example: "XYZ" command does not exist, so will result as follows: 1192 1193 * [ARD DUE/] > XYZ 1194 * [ARD DUE/] > 1195 */ 1196 if 1197 (Serial.available()) 1198 { 1199 String Command=ReadUserInputOverSerial(); 1200 Serial.println(Command); 1201 if (Command.substring(0,1)=="?") /* Help 1202 */ 1203 { 1204 Serial.println("Commands:"); 1205 Serial.println(" 1206 CTL [Section] [SetValue (0-100)]"); 1207 Serial.println(" CommandToLeonardo 1208 - Section: First 5 characters of PWM Tag (e.g. _10WH for _10WHCM)."); 1209 Serial.println(" 1210 FD"); 1211 Serial.println(" Final Demo."); 1212 Serial.println(" 1213 FTD [ServoTag] [LimitSwitch1Tag] [LimitSwitch2Tag] [Speed (0-90)]"); 1214 Serial.println(" 1215 Full Travel Duration(Test)."); 1216 Serial.println(" HLD"); 1217 1218 Serial.println(" Hold (by Fingers)."); 1219 Serial.println(" 1220 LT"); 1221 Serial.println(" Starts Loop Test subroutine."); 1222 1223 Serial.println(" MDM [ServoTag] [Direction] [Speed (0-90)] [Milliseconds]"); 1224 1225 Serial.println(" Move During Millis-Actuate a Servo during a 1226 determined time."); 1227 Serial.println(" Direction : Either 1228 CC(=CounterClockwise) or CW(=Clockwise)"); 1229 Serial.println(" WARNING:"); 1230 1231 Serial.println(" This function does not check whether limit switch 1232 is matching with direction."); 1233 Serial.println(" Incorrect 1234 usage can result damages. Ensure applicable durations by using"); 1235 Serial.println(" 1236 FullTravelDurationMeasurement() in advance."); 1237 Serial.println(" 1238 MTP [Section] [Speed (0-90)] [SetValue]"); 1239 Serial.println(" 1240 Move To Position - Section: First 5 characters of PWM Tag (e.g. _10WH for 1241 _10WHCM)."); 1242 Serial.println(" RD [IO_Tag]"); 1243 Serial.println(" 1244 Reads value on a single input channel."); 1245 Serial.println(" 1246 REL"); 1247 Serial.println(" Release (by Fingers)."); 1248 Serial.end(); 1249 Serial.begin(_BAUDRATE); while(!Serial) { }; 1250 } 1251 1252 1253 if (Command.substring(0,3)=="CTL") 1254 CommandToLeonardo(Command.substring(4,9), 1255 Command.substring(10).toInt()); 1256 /* CommandToLeonardo(String Section, 1257 int SetValue) */ 1258 1259 if (Command.substring(0,2)=="FD") 1260 Final_Demo(); 1261 1262 1263 if (Command.substring(0,3)=="FTD") 1264 FullTravelDurationMeasurement(Command.substring(4,11), 1265 Command.substring(12,19), Command.substring(20,27), Command.substring(28,30).toInt()); 1266 1267 /* FullTravelDurationMeasurement(PWM-Output, Limit Switch.1, Limit Switch.2 1268 Tag, Speed) */ 1269 1270 if (Command.substring(0,3)=="HLD") 1271 Fingers_Hold(); 1272 1273 if (Command.substring(0,3)=="REL") 1274 Fingers_Release(); 1275 1276 1277 if (Command.substring(0,2)=="LT") 1278 LoopTest(); 1279 1280 1281 if (Command.substring(0,3)=="MDM") 1282 { 1283 if 1284 (Command.substring(12,14)=="CC") User_Int1=_CCW; 1285 else if (Command.substring(12,14)=="CW") 1286 User_Int1=_CW; 1287 else User_Int1=0; /* 1288 Direction */ 1289 MoveDuringMillis(Command.substring(4,11), User_Int1, 1290 1291 Command.substring(15,17).toInt(), (unsigned long)Command.substring(18,22).toInt()); 1292 1293 /* MoveDuringMillis(PWM-Output, Direction, Speed, Milliseconds) */ 1294 1295 } 1296 1297 if (Command.substring(0,3)=="MTP") 1298 MoveToPosition(Command.substring(4,9), 1299 Command.substring(10,12).toInt(), Command.substring(13).toFloat()); 1300 /* 1301 MoveToPosition(Section, Speed, Set Value) */ 1302 1303 if (Command.substring(0,2)=="RD") 1304 1305 Read(Command.substring(3,10)); 1306 /* Read(Tag) */ 1307 1308 1309 CommandPromptOverSerial("[ARD DUE/] > "); 1310 } /* End of "if 1311 (Serial.available())..." */ 1312} /* End of "void loop()": ARDUINO DUE */ 1313
Application Program - Arduino Leonardo
arduino
This one shall be loaded to Leonardo
1#include <Servo.h> 2 3/* Serial Comm.Baudrate Setting: ARDUINO LEONARDO 4 */ 5 const int _BAUDRATE = 4800; 6/* End of Serial Comm.Baudrate Setting: 7 ARDUINO LEONARDO */ 8 9/* Tag-Channel(Pin) Assignments and IO_Channel() Function: 10 ARDUINO LEONARDO */ 11 const int _11FRZL = 0; // 11: Front Arm Radius ZL 12 13 const int _11FRZH = 1; // 11: Front Arm Radius ZH 14 const int _11FRTX 15 = A2; // 11: Front Arm Radius Actual Pos 16 const int _11FRCM = 9; // 11: 17 Front Arm Radius CMD 18 19 const int _11FUZL = 2; // 11: Front Arm Ulna 20 ZL 21 const int _11FUZH = 3; // 11: Front Arm Ulna ZH 22 const int _11FUTX 23 = A3; // 11: Front Arm Ulna Actual Pos 24 const int _11FUCM = 10; // 11: 25 Front Arm Ulna CMD 26 27 const int _21UAZL = 4; // 21: Upper Arm ZL 28 29 const int _21UAZH = 5; // 21: Upper Arm ZH 30 const int _21UATX = A4; 31 // 21: Upper Arm Actual Pos 32 const int _21UACM = 11; // 21: Upper Arm 33 CMD 34 35 const int _31SHZL = 6; // 31: Shoulder ZL 36 const int _31SHZH 37 = 7; // 31: Shoulder ZH 38 const int _31SHTX = A5; // 31: Shoulder Actual 39 Pos 40 const int _31SHCM = 13; // 21: Shoulder CMD 41 42 const int _00XXXC 43 = 8; // Execute Command (0: Stop/Idle, 1: Execute) 44 const int _00XXSP = 45 A0; // Setpoint Register 46 const int _00XXCM = A1; // Command Selector 47 (*1) 48 const int _00XXOK = 12; // Command Executed by Arduino Leonardo (*2-IMPORTANT) 49 50 51 int IO_Channel(String Tag) { /* VERIFIED FUNCTION: ARDUINO LEONARDO */ 52 /* 53 This function is used to validate IO Channel for DI/DO/AO/PWM-O. 54 * If 55 given tag is valid, respective channel number will be returned. 56 * Otherwise 57 -1 will be returned as "error". 58 */ 59 if (Tag=="_11FRZL") 60 return(_11FRZL); 61 if (Tag=="_11FRZH") return(_11FRZH); 62 if (Tag=="_11FRTX") 63 return(_11FRTX); 64 if (Tag=="_11FRCM") return(_11FRCM); 65 if (Tag=="_11FUZL") 66 return(_11FUZL); 67 if (Tag=="_11FUZH") return(_11FUZH); 68 if (Tag=="_11FUTX") 69 return(_11FUTX); 70 if (Tag=="_11FUCM") return(_11FUCM); 71 if (Tag=="_21UAZL") 72 return(_21UAZL); 73 if (Tag=="_21UAZH") return(_21UAZH); 74 if (Tag=="_21UATX") 75 return(_21UATX); 76 if (Tag=="_21UACM") return(_21UACM); 77 if (Tag=="_31SHZL") 78 return(_31SHZL); 79 if (Tag=="_31SHZH") return(_31SHZH); 80 if (Tag=="_31SHTX") 81 return(_31SHTX); 82 if (Tag=="_31SHCM") return(_31SHCM); 83 if (Tag=="_00XXXC") 84 return(_00XXXC); 85 if (Tag=="_00XXSP") return(_00XXSP); 86 if (Tag=="_00XXCM") 87 return(_00XXCM); 88 if (Tag=="_00XXOK") return(_00XXOK); 89 return(-1); 90} 91 /* End of "IO_Channel(String Tag)": ARDUINO LEONARDO */ 92/* End of Tag-Channel(Pin) 93 Assignments and IO_Channel() Function: ARDUINO LEONARDO */ 94 95 96/* Servo _Definitions 97 + CommandToServo() function: ARDUINO LEONARDO */ 98 Servo _11FR_Servo; 99 Servo 100 _11FU_Servo; 101 Servo _21UA_Servo; 102 Servo _31SH_Servo; 103 104 void 105 CommandToServo(String Tag, int Value) { /* VERIFIED FUNCTION: ARDUINO LEONARDO */ 106 107 /* This function is used to actuate PWM outputs to Servo Motors. */ 108 if 109 (Tag=="_11FRCM") _11FR_Servo.write(Value); 110 if (Tag=="_11FUCM") _11FU_Servo.write(Value); 111 112 if (Tag=="_21UACM") _21UA_Servo.write(Value); 113 if (Tag=="_31SHCM") 114 _31SH_Servo.write(Value); 115 } /* End of "CommandToServo()": ARDUINO LEONARDO 116 */ 117/* End of Servo _Definitions + CommandToServo() function: ARDUINO LEONARDO 118 */ 119 120 121/* Constant Parameters: ARDUINO LEONARDO */ 122 const int _DBPercent 123 = 1; // % Deadband 124 const int _CCW = 1; // Servo Direction for Counter 125 Clockwise 126 const int _CW = -1; // Servo Direction for Counter Clockwise 127/* 128 End of Constant Parameters: ARDUINO LEONARDO */ 129 130 131/* Constant Parameters 132 for Intercommunication: ARDUINO LEONARDO */ 133 const int _FA_Radius = 310; 134 // Front Arm Radius Section 135 const int _FA_Ulna = 425; // Front 136 Arm Ulna Section 137 const int _UpperArm = 540; // Upper Arm Section 138 139 const int _Shoulder = 655; // Front Arm Radius Section 140 const int 141 _ICDB = 40; // Front Arm Radius Section 142 /* Since the signals 143 cannot generate exact integer values, Section 144 * shall be identified by 145 [ (SectionValue) +/- _ICDB ] reading. 146 */ 147/* End of Constant Parameters 148 for Intercommunication: ARDUINO LEONARDO */ 149 150 151/* "void setup()": ARDUINO 152 LEONARDO */ 153void setup() { 154 // TEST CODES: 155 156 // VERIFIED CODES: 157 158 pinMode(_11FRZL, INPUT_PULLUP); 159 pinMode(_11FRZH, INPUT_PULLUP); 160 pinMode(_11FUZL, 161 INPUT_PULLUP); 162 pinMode(_11FUZH, INPUT_PULLUP); 163 pinMode(_21UAZL, INPUT_PULLUP); 164 165 pinMode(_21UAZH, INPUT_PULLUP); 166 pinMode(_31SHZL, INPUT_PULLUP); 167 pinMode(_31SHZH, 168 INPUT_PULLUP); 169 pinMode(_00XXXC, INPUT_PULLUP); 170 pinMode(_00XXOK, OUTPUT); 171 172 173 _11FR_Servo.attach(_11FRCM); 174 _11FU_Servo.attach(_11FUCM); 175 _21UA_Servo.attach(_21UACM); 176 177 _31SH_Servo.attach(_31SHCM); 178 179 /* This part is used to send initial "neutral 180 position/stop" 181 * to all PWM outputs assigned to Servo Motors. 182 */ 183 184 _11FR_Servo.write(90); 185 _11FU_Servo.write(90); 186 _21UA_Servo.write(90); 187 188 _31SH_Servo.write(90); 189 190 /* Initial Feedback to Due */ 191 digitalWrite(_00XXOK, 192 LOW); 193 194 /* analogReadResolution(10); Analog Reading Scale: 0-1023 for 195 ARDUINO LEONARDO*/ 196 197 /* analogWriteResolution(10); Analog Writing Scale: 198 0-1023 for ARDUINO LEONARDO*/ 199 200 Serial.begin(_BAUDRATE); while(!Serial) 201 { }; 202 Serial.println("? -> Help."); 203 Serial.print("[ARD LNRD/] > 204 "); 205} /* End of "void setup()": ARDUINO LEONARDO */ 206 207 208/* Global Variables 209 for Test Purposes: ARDUINO LEONARDO */ 210 211/* End of Global Variables for Test 212 Purposes: ARDUINO LEONARDO */ 213 214 215/* Verified Global Variables: ARDUINO LEONARDO 216 */ 217 int CommandFromDue_Section, CommandFromDue_SetValue; 218/* End of Verified 219 Global Variables: ARDUINO LEONARDO */ 220 221 222void CommandPromptOverSerial(String 223 _Prompt) { /* VERIFIED FUNCTION: ARDUINO LEONARDO */ 224 /* Reprompting and flushing 225 serial for next input. 226 * This function will be used after each use of ReadUserInputOverSerial() 227 228 */ 229 Serial.print(_Prompt); Serial.end(); Serial.begin(_BAUDRATE); while(!Serial) 230 { }; 231} /* End of "CommandPromtOverSerial()": ARDUINO LEONARDO */ 232 233 234String 235 ReadUserInputOverSerial(){ /* VERIFIED FUNCTION: ARDUINO LEONARDO */ 236 /* This 237 function will be used only "if (Serial.available())" */ 238 char UserInput[32]; 239 240 Serial.readBytes(UserInput,32); 241 /* Buffering and capitalizing User Input 242 */ 243 String Buffer=UserInput; Buffer.toUpperCase(); 244 return(Buffer); 245} 246 /* End of "ReadUserInputOverSerial()": ARDUINO LEONARDO */ 247 248 249void Read(String 250 Tag) { /* VERIFIED FUNCTION: ARDUINO LEONARDO */ 251 /* Indicates value at input 252 channel defined by "Tag" */ 253 if ( (Tag.substring(5,7)=="ZL") || (Tag.substring(5,7)=="ZH") 254 || (Tag.substring(5,7)=="OK")) 255 Serial.println(Tag + " = " + String(digitalRead(IO_Channel(Tag)))); 256 257 else if (Tag.substring(5,7)=="TX") 258 Serial.println(Tag + " = " 259 + String(analogRead(IO_Channel(Tag)))); 260 else Serial.println(Tag + "is 261 not an input."); 262} /* End of "void Read()": ARDUINO LEONARDO */ 263 264 265void 266 LoopTest() { /* VERIFIED FUNCTION: ARDUINO LEONARDO */ 267 String Command, User_Tag; 268 269 Serial.println("? -> Help, X -> Terminate"); 270 CommandPromptOverSerial("[ARD 271 LNRD/LoopTest/] > "); 272 do 273 { 274 if (Serial.available()) 275 { 276 277 byte Command_to_Case=0; /* Command is invalid if remains as 0. */ 278 279 int User_Value=0, User_Value_Last=0, Reading=0, Reading_Last=-1; 280 281 String Buffer = ReadUserInputOverSerial(); 282 Serial.println(Buffer); 283 Serial.end(); Serial.begin(_BAUDRATE); while(!Serial) { }; 284 285 /* 286 Seperating Command and Tag*/ 287 Command = Buffer.substring(0,2); User_Tag 288 = Buffer.substring(3,10); 289 290 /* Converting (string)Command 291 to (byte)Command_to_Case */ 292 if (Command=="AI") Command_to_Case=11; 293 294 if (Command=="AO") Command_to_Case=12; 295 if (Command=="DI") 296 Command_to_Case=21; 297 if (Command=="DO") Command_to_Case=22; 298 if 299 (Command=="PO") Command_to_Case=32; 300 if (Command[0]=='?') Command_to_Case=98; 301 // Display Help 302 if (Command[0]=='X') Command_to_Case=99; // Terminate 303 Test 304 305 /* Checking whether tag is valid. */ 306 if 307 ((Command_to_Case!=0)&&(Command_to_Case<90)&&(IO_Channel(User_Tag)<0)) 308 Command_to_Case=1; 309 /* Redirecting to unrecognized tag case. */ 310 311 /* Taking respective 312 actions and giving feedbacks for each command. */ 313 switch (Command_to_Case) 314 { 315 case 0 : Serial.println("Invalid command."); 316 CommandPromptOverSerial("[ARD 317 LNRD/LoopTest/] > "); 318 break; 319 case 320 1 : Serial.println("Tag not exists: " + User_Tag); 321 CommandPromptOverSerial("[ARD 322 LNRD/LoopTest/] > "); 323 break; 324 case 325 11: Serial.println("AI-Test('X'->Terminate): " + User_Tag); 326 Reading_Last 327 = -1; 328 do 329 { 330 Buffer 331 = ReadUserInputOverSerial(); 332 Reading = analogRead(IO_Channel(User_Tag)); 333 334 if (Reading != Reading_Last) 335 { 336 337 Serial.println(User_Tag + " = " + String(Reading)); 338 339 Reading_Last = Reading; delay(1000); 340 /* 341 1s delay is used to avoid fast updates on Serial Monitor */ 342 } 343 344 } 345 while (Buffer[0]!='X'); 346 347 Serial.println(); 348 CommandPromptOverSerial("[ARD 349 LNRD/LoopTest/] > "); 350 break; 351 case 352 12: Serial.println("AO-Test('X'->Terminate): " + User_Tag); 353 Serial.println("Integer 354 Value(0-1023): "); 355 Buffer=""; 356 do 357 358 { 359 if (Serial.available()) 360 361 { 362 Buffer = ReadUserInputOverSerial(); 363 364 if (Buffer[0]=='X') User_Value=0; else User_Value=Buffer.substring(0).toInt(); 365 366 Serial.println(String(User_Value) + " -> " + User_Tag); 367 368 if (User_Value!=User_Value_Last) 369 { 370 371 analogWrite(IO_Channel(User_Tag),User_Value); 372 373 User_Value_Last = User_Value; 374 } 375 376 else analogWrite(IO_Channel(User_Tag),User_Value_Last); 377 378 Serial.print("Integer Value(0-1023): "); 379 Serial.end(); 380 Serial.begin(_BAUDRATE); while(!Serial) { }; 381 } 382 /* End of "if (Serial.available())..." */ 383 } 384 while 385 (Buffer[0]!='X'); 386 Serial.println(); 387 CommandPromptOverSerial("[ARD 388 LNRD/LoopTest/] > "); 389 break; 390 case 391 21: Serial.println("DI-Test('X'->Terminate): " + User_Tag); 392 Reading_Last 393 = -1; 394 do 395 { 396 Buffer 397 = ReadUserInputOverSerial(); 398 Reading = digitalRead(IO_Channel(User_Tag)); 399 400 if (Reading != Reading_Last) 401 { 402 403 Serial.println(User_Tag + " = " + String(Reading)); 404 405 Reading_Last = Reading; 406 } 407 408 } 409 while (Buffer[0]!='X'); 410 411 Serial.println(); 412 CommandPromptOverSerial("[ARD 413 LNRD/LoopTest/] > "); 414 break; 415 case 416 22: Serial.println("DO-Test('X'->Terminate): " + User_Tag); 417 Serial.print("Enter 418 0 or 1: "); 419 Buffer=""; 420 do 421 422 { 423 if (Serial.available()) 424 425 { 426 Buffer = ReadUserInputOverSerial(); 427 428 if (Buffer[0]=='X') User_Value=0; else User_Value=Buffer.substring(0).toInt(); 429 430 Serial.println(String(User_Value) + " -> " + User_Tag); 431 432 if (((User_Value==0)||(User_Value==1))&&(User_Value!=User_Value_Last)) 433 434 { 435 436 437 digitalWrite(IO_Channel(User_Tag),User_Value); 438 439 User_Value_Last = User_Value; 440 } 441 442 else digitalWrite(IO_Channel(User_Tag),User_Value_Last); 443 444 Serial.print("Enter 0 or 1: "); 445 Serial.end(); 446 Serial.begin(_BAUDRATE); while(!Serial) { }; 447 } 448 /* End of "if (Serial.available())..." */ 449 } 450 while 451 (Buffer[0]!='X'); 452 Serial.println(); 453 CommandPromptOverSerial("[ARD 454 LNRD/LoopTest/] > "); 455 break; 456 case 457 32: Serial.println("PWM-Test('X'->Terminate): " + User_Tag); 458 CommandToServo(User_Tag,90); 459 /* Sending "stop" to selected tag */ 460 Serial.print("Integer 461 Value(0-180): "); 462 Buffer=""; 463 do 464 465 { 466 if (Serial.available()) 467 468 { 469 Buffer = ReadUserInputOverSerial(); 470 471 if (Buffer[0]=='X') User_Value=90; else User_Value=Buffer.substring(0).toInt(); 472 473 Serial.println(String(User_Value) + " -> " + User_Tag); 474 475 if (((User_Value>=0)||(User_Value<=180))&&(User_Value!=User_Value_Last)) 476 477 { CommandToServo(User_Tag,User_Value); User_Value_Last 478 = User_Value; } 479 else CommandToServo(User_Tag,User_Value); 480 481 Serial.print("Integer Value(0-180): "); 482 Serial.end(); 483 Serial.begin(_BAUDRATE); while(!Serial) { }; 484 } 485 /* End of "if (Serial.available())..." */ 486 } 487 while 488 (Buffer[0]!='X'); 489 Serial.println(); 490 CommandPromptOverSerial("[ARD 491 LNRD/LoopTest/] > "); 492 break; 493 case 494 98: Serial.println("Usage: '[CC] [Tag]'"); 495 Serial.println(" 496 [CC]: Channel Type"); 497 Serial.println(" AI, 498 AO: Analog Input/Output"); 499 Serial.println(" DI, 500 DO: Digital Input/Output"); 501 Serial.println(" PO: 502 PWM Output for Servo Motor"); 503 Serial.println("Other 504 Commands:"); 505 Serial.println(" ? : Help (this display)"); 506 507 Serial.println(" X : Exit from Loop Testing"); 508 509 CommandPromptOverSerial("[ARD LNRD/LoopTest/] > "); 510 511 break; 512 case 99: 513 Serial.println("Loop Test is terminated."); 514 break; 515 516 } /* End of "switch (Command_to_Case)..." 517 */ 518 } /* End of "if (Serial.available())..." */ 519 } 520 while (Command[0]!='X'); 521 522} 523 /* End of "void LoopTest()": ARDUINO LEONARDO */ 524 525 526void MoveToPosition 527 /* VERIFIED FUNCTION: ARDUINO LEONARDO */ 528 (String Section, int Speed, int 529 SetValue) { 530 /* This function is used to operate a "Section" at a determined 531 532 * "Speed" to required position given by "SetValue". 533 * ("Section" 534 is the first 5 characters of any I/O tag.) 535 */ 536 String PWM_Tag=Section+"CM", 537 TX_Tag=Section+"TX"; 538 539 boolean Actuated=0, 540 Flag_SPMatched=0, 541 542 Flag_LL=0, Flag_L=0, Flag_H=0, Flag_HH=0, 543 Alarmed_1=0, Alarmed_2=0; 544 545 546 byte Section_to_Case=0; 547 548 int IncreaseDirection=0, Direction=0, 549 550 TX_RawMin, TX_RawMax, 551 Alarm_LL, Alarm_L, Alarm_H, Alarm_HH, 552 553 CurrentTxReading; 554 555 if (Section=="_11FR") { Section_to_Case=1; 556 IncreaseDirection=_CCW; } 557 else if (Section=="_11FU") { Section_to_Case=2; 558 IncreaseDirection=_CCW; } 559 else if (Section=="_21UA") { Section_to_Case=3; 560 IncreaseDirection=_CW; } 561 else if (Section=="_31SH") { 562 Section_to_Case=4; IncreaseDirection=_CW; } 563 564 /* Scale and Alarm values 565 corresponding to Sections should be adjusted 566 * depending on tests. 567 * 568 Direction equations should be adjusted by multiplying -1 depending on 569 * mechanical 570 configuration. 571 */ 572 switch (Section_to_Case) { 573 case 0 : Serial.println("Invalid 574 section."); 575 break; 576 case 1 : /* Customizations 577 for _11FR Section */ 578 TX_RawMin=0; TX_RawMax=1023; 579 Alarm_LL=10; 580 Alarm_L=20; Alarm_H=80; Alarm_HH=90; 581 break; 582 case 583 2 : /* Customizations for _11FU Section */ 584 TX_RawMin=690; TX_RawMax=230; 585 586 Alarm_LL=5; Alarm_L=10; Alarm_H=80; Alarm_HH=90; 587 break; 588 589 case 3 : /* Customizations for _21UA Section - Completed */ 590 TX_RawMin=775; 591 TX_RawMax=520; 592 Alarm_LL=10; Alarm_L=20; Alarm_H=80; Alarm_HH=90; 593 594 break; 595 case 4 : /* Customizations for _31SH Section 596 - Completed */ 597 TX_RawMin=250; TX_RawMax=760; 598 Alarm_LL=10; 599 Alarm_L=20; Alarm_H=80; Alarm_HH=90; 600 break; 601 } 602 603 604 CurrentTxReading=map( analogRead(IO_Channel(TX_Tag)), TX_RawMin, TX_RawMax, 0, 605 100 ); 606 607 if (Section_to_Case!=0 && Speed!=0) 608 { 609 Serial.print("Actuate 610 " + PWM_Tag + ", Pos.Tx:" + TX_Tag + "[=" + String(CurrentTxReading) + "%], 611 "); 612 if (IncreaseDirection==_CCW) Direction = (int)( (SetValue-CurrentTxReading)/abs(SetValue-CurrentTxReading) 613 ); 614 else Direction = (int)( (-1)*(SetValue-CurrentTxReading)/abs(SetValue-CurrentTxReading) 615 ); 616 switch (Direction) { 617 case _CCW: Serial.print("CCW, 618 "); 619 break; 620 case _CW : Serial.print("CW, 621 "); 622 break; 623 case 0 : Serial.print("STOP, 624 "); 625 break; 626 } 627 Serial.print("Speed="+String(Speed)+", 628 "); 629 Serial.println("Set Value="+String(SetValue)+"%"); 630 while 631 ( !( Flag_SPMatched || Flag_LL || Flag_HH ) ) 632 { 633 CurrentTxReading=map( 634 analogRead(IO_Channel(TX_Tag)), TX_RawMin, TX_RawMax, 0, 100 ); 635 if 636 (IncreaseDirection==_CCW) 637 { 638 if ( (Direction==_CW) 639 && (CurrentTxReading<=Alarm_L) ) Flag_L = 1; 640 if ( (Direction==_CW) 641 && (CurrentTxReading<=Alarm_LL) ) Flag_LL = 1; 642 if ( (Direction==_CCW) 643 && (CurrentTxReading>=Alarm_H) ) Flag_H = 1; 644 if ( (Direction==_CCW) 645 && (CurrentTxReading>=Alarm_HH) ) Flag_HH = 1; 646 if ( ( (Direction==_CCW) 647 && (CurrentTxReading>=SetValue) ) || 648 ( (Direction==_CW) 649 && (CurrentTxReading<=SetValue) ) ) Flag_SPMatched = 1; 650 } 651 652 if (IncreaseDirection==_CW) 653 { 654 if 655 ( (Direction==_CCW) && (CurrentTxReading<=Alarm_L) ) Flag_L = 1; 656 if 657 ( (Direction==_CCW) && (CurrentTxReading<=Alarm_LL) ) Flag_LL = 1; 658 if 659 ( (Direction==_CW) && (CurrentTxReading>=Alarm_H) ) Flag_H = 1; 660 if 661 ( (Direction==_CW) && (CurrentTxReading>=Alarm_HH) ) Flag_HH = 1; 662 if 663 ( ( (Direction==_CW) && (CurrentTxReading>=SetValue) ) || 664 ( 665 (Direction==_CCW) && (CurrentTxReading<=SetValue) ) ) Flag_SPMatched = 1; 666 } 667 668 669 if (Flag_L || Flag_H) 670 if (!Alarmed_1) 671 { 672 673 if ( Flag_L ) Serial.println("Alarm : "+ String(Alarm_L) 674 + "% is reached."); 675 if ( Flag_H ) Serial.println("Alarm 676 : "+ String(Alarm_H) + "% is reached."); 677 Alarmed_1 678 = 1; 679 } 680 681 if (Flag_LL || Flag_HH) 682 { 683 684 CommandToServo(PWM_Tag, 90); /* Interlocked */ 685 if 686 (!Alarmed_2) 687 { 688 if ( Flag_LL ) Serial.println("Interlocked: 689 "+ String(Alarm_LL) + "% is reached."); 690 if ( Flag_HH ) 691 Serial.println("Interlocked: "+ String(Alarm_HH) + "% is reached."); 692 Alarmed_2 693 = 1; 694 } 695 } 696 else if (! 697 Actuated ) 698 { CommandToServo(PWM_Tag, 90+Direction*Speed); 699 Actuated = 1; } 700 701 } /* Waits until reaching to setpoint 702 */ 703 CommandToServo(PWM_Tag, 90); /* Stop command to Servo */ 704 Serial.println(PWM_Tag 705 + " is stopped at " + String(CurrentTxReading) + "%."); 706 } /* End of 707 "if (Section_to_Case!=0)" */ 708} /* End of "void MoveToPosition()" */ 709 710 711void 712 CommandFromDue() { /* VERIFIED FUNCTION: ARDUINO LEONARDO */ 713 CommandFromDue_Section 714 = analogRead(_00XXCM); 715 if ( ((_FA_Radius-_ICDB)<=CommandFromDue_Section) 716 && (CommandFromDue_Section<=(_FA_Radius+_ICDB)) ) 717 CommandFromDue_Section=_FA_Radius; 718 719 else if ( ((_FA_Ulna-_ICDB)<=CommandFromDue_Section) && (CommandFromDue_Section<=(_FA_Ulna+_ICDB)) 720 ) 721 CommandFromDue_Section=_FA_Ulna; 722 else 723 if ( ((_UpperArm-_ICDB)<=CommandFromDue_Section) && (CommandFromDue_Section<=(_UpperArm+_ICDB)) 724 ) 725 CommandFromDue_Section=_UpperArm; 726 else 727 if ( ((_Shoulder-_ICDB)<=CommandFromDue_Section) && (CommandFromDue_Section<=(_Shoulder+_ICDB)) 728 ) 729 CommandFromDue_Section=_Shoulder; 730 else 731 732 CommandFromDue_Section=0; 733 734 CommandFromDue_SetValue 735 = map(analogRead(_00XXSP),168,846,0,100); 736 /* Due generates DAC outputs between 737 0.55-2.75 V and it corresponds 738 * (int) 168-846 on Arduino Leonardo. 739 740 * Here analog reading is converted to Percentage. 741 */ 742 743 boolean 744 CommandFromDue_Execute = digitalRead(_00XXXC); 745 String Section = ""; 746 747 int Speed = 0; 748 if ((CommandFromDue_Section!=0) && CommandFromDue_Execute) 749 750 { 751 Serial.println(); 752 Serial.print("Command by ARD DUE: 753 "+ String(CommandFromDue_SetValue) + "% -> "); 754 switch(CommandFromDue_Section){ 755 756 case _FA_Radius: Serial.println("Front Arm-Radius"); 757 Section 758 = "_11FR"; 759 Speed = 20; /* to be adjusted depending 760 on tests */ 761 break; 762 case _FA_Ulna 763 : Serial.println("Front Arm-Ulna"); 764 Section 765 = "_11FU"; 766 Speed = 20; /* to be adjusted depending 767 on tests */ 768 break; 769 case _UpperArm 770 : Serial.println("Upper Arm"); 771 Section = "_21UA"; 772 773 Speed = 90; /* to be adjusted depending on tests 774 */ 775 break; 776 case _Shoulder : Serial.println("Shoulder"); 777 778 Section = "_31SH"; 779 Speed 780 = 30; /* to be adjusted depending on tests */ 781 break; 782 783 } 784 Serial.println(" -action-"); 785 MoveToPosition(Section, 786 Speed, CommandFromDue_SetValue); 787 Serial.println("OK -> ARD DUE."); 788 789 CommandPromptOverSerial("[ARD LNRD/] > "); 790 digitalWrite(_00XXOK, 791 HIGH); delay(100); digitalWrite(_00XXOK, LOW); // OK Feedback to Due 792 } 793 /* End of "if (Section_Num!=0)" */ 794} /* End of "void CommandToLeonardo()" 795 */ 796 797 798/* "void loop(): ARDUINO LEONARDO */ 799void loop() { 800 /* TEST 801 CODES: */ 802 CommandFromDue(); 803 /* VERIFIED CODES: */ 804 /* Commands 805 and Actions */ 806 /* All recognized commands give a feedback in all cases. 807 808 * If you don't see any feedback on Serial Monitor, check your input 809 * 810 for any typewriting errors. 811 * Example: "XYZ" command does not exist, 812 so will result as follows: 813 * [ARD LNRD/] > XYZ 814 * [ARD 815 LNRD/] > 816 */ 817 if (Serial.available()) 818 { 819 String 820 Command=ReadUserInputOverSerial(); Serial.println(Command); 821 if (Command.substring(0,1)=="?") 822 /* Help */ 823 { 824 Serial.println("Commands:"); 825 Serial.println(" 826 CFD"); 827 Serial.println(" Reads Command from ARD DUE."); 828 829 Serial.println(" LT"); 830 Serial.println(" Starts 831 Loop Test subroutine."); 832 Serial.println(" MTP [Section] [Speed 833 (0-90)] [SetValue]"); 834 Serial.println(" Move To Position - 835 Section: First 5 characters of PWM Tag (e.g. _10WH for _10WHCM)."); 836 Serial.println(" 837 RD [IO_Tag]"); 838 Serial.println(" Reads value on a single 839 input channel."); 840 /* This function is used to operate a "section" 841 given by "Tag" to "Direction" 842 * at specified "Speed" and 843 during determined "Milliseconds", unless "LimitSw" 844 * is activated 845 (Limit Switches give 0 when activated). 846 */ 847 Serial.end(); 848 Serial.begin(_BAUDRATE); while(!Serial) { }; 849 } 850 851 852 if (Command.substring(0,3)=="CFD") 853 CommandFromDue(); 854 855 856 if (Command.substring(0,2)=="LT") 857 LoopTest(); 858 859 860 if (Command.substring(0,3)=="MTP") 861 MoveToPosition(Command.substring(4,9), 862 Command.substring(10,12).toInt(), Command.substring(13).toInt()); 863 /* 864 MoveToPosition(Section, Speed, Set Value) */ 865 866 if (Command.substring(0,2)=="RD") 867 868 Read(Command.substring(3,10)); 869 /* Read(Tag) */ 870 871 872 873 CommandPromptOverSerial("[ARD LNRD/] > "); 874 } /* End of "if 875 (Serial.available())..." */ 876} /* End of "void loop()": ARDUINO LEONARDO */ 877 878
Downloadable files
Instr.List, I/O List, Terminal Board Layout, Wiring Diagram
Instr.List, I/O List, Terminal Board Layout, Wiring Diagram
Comments
Only logged in users can leave comments
tolgadurudogan
0 Followers
•0 Projects
Table of contents
Intro
4
0