Components and supplies
PVC Pipe 40mm Diameter
Arduino Leonardo
Arduino Due
SM-4303 Standard Servo Motor
Jumper wires (generic)
Hand Tools (Screwdrivers, Cutters, etc.)
Header Pins
CAD Application
Packaging Rubber Bands
PCB 8x2 cm
PVC Tube, 6mm diameter
PVC Pipe 25mm Diameter
Glass Nails
Paper Clips
SG90 Micro-servo motor
Staple Pins
Plastic Cable Ties
Balsa Wood parts
Crafting Tools (Drills and Accessories)
Flat Washer 1/4" Screw Size
Fishing Ropes
Apps and platforms
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