daily pastebin goal
65%
SHARE
TWEET

Untitled

a guest Jan 22nd, 2019 69 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <string.h>
  2. #include <ServoShield.h>
  3.  
  4. // Define global variables
  5. #define NUMBER_OF_SERVOS 16  // The total number of robot's servos
  6.  
  7. ServoShield shieldServos; // ServoShield interfacing object
  8.  
  9. int servominpos[NUMBER_OF_SERVOS] = {1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000, 1000};      // Servos Minimum Positions
  10. int servomaxpos[NUMBER_OF_SERVOS] = {2000, 2000, 2000, 2000, 2000, 2000, 2000, 2000, 2000, 2000, 2000, 2000, 2000, 2000, 2000};  // Servos Maximum Positions
  11. int servomidpos[NUMBER_OF_SERVOS] = {1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500, 1500};  // Servos Initial Positions
  12.  
  13.  
  14. void setup()
  15. {
  16.  
  17.   Serial.begin(19200);
  18.   delay(1000);
  19.  
  20.   InitializeAllServos();
  21.  
  22.   if (shieldServos.start())
  23.   {
  24.     Serial.println("Servo Shield started succefully...");
  25.   }
  26.   else
  27.   {
  28.     Serial.println("Servo Shield failed to start, DO NOT send any servo command, check your Servo shield connectivity...");
  29.   }
  30.  
  31.   delay(2000);
  32. }
  33.  
  34.  
  35. void loop()
  36. {
  37.   char servoCommand[256];
  38.   char delim[] = ",";
  39.   char *motion = NULL;
  40.  
  41.   while (1)
  42.   {  
  43.  
  44.     *motion = NULL;
  45.  
  46.     // Read the servo command from the serial port, Servo Command Format: SxxPyyyy,SxxPyyyy,SxxPyyyy,.... example S01P1000,S02P0500,S03P20,...
  47.     // Notes: DO NOT OMMIT the leading zeroes e.g. S1P1000 --> Wrong, S01P1000 --> Correct, S02P500-->Wrong, S02P0500--> Correct
  48.     // You can use any number of your servos in any order, just use the comma for separator e.g. S01P2000,S03P0600,S02P1750,S11P2300
  49.     GetStringFromSerial(servoCommand, sizeof(servoCommand));
  50.  
  51.     //If we send 'x' instead of servo command the programm will end
  52.     //if (servoCommand[0] =='x')
  53.     //{
  54.       //break;
  55.     //}
  56.    
  57.     Serial.println(servoCommand);
  58.  
  59.     // Start the Servo Command parsing
  60.     motion = strtok( servoCommand, delim);
  61.  
  62.     while( motion != NULL )
  63.     {
  64.       //extract motion data (servo id and position) from servo command
  65.       String extractedMotion = motion;        
  66.       char servoid[extractedMotion.substring(1,3).length() + 1];
  67.       char servopos[extractedMotion.substring(4,8).length() + 1];
  68.       extractedMotion.substring(1,3).toCharArray(servoid, sizeof(servoid));
  69.       extractedMotion.substring(4,8).toCharArray(servopos, sizeof(servopos));
  70.  
  71.       // Convert extracted strings to integers
  72.       int sid = atoi(servoid);
  73.       int spos = atoi(servopos);
  74.  
  75.       //Print some debug info
  76.       char debugMsg[256];
  77.       sprintf(debugMsg,"Motion string: %s   Servo Id: %d   Servo Position: %d", motion, sid, spos);
  78.       Serial.println(debugMsg);
  79.  
  80.       // Move the
  81.       shieldServos.setposition(sid , spos);
  82.       delay(2000);
  83.  
  84.      
  85.       // Continue parsing
  86.       motion= strtok( NULL, delim );
  87.     }
  88.  
  89.     delay(1);
  90.   }
  91.  
  92. }
  93.  
  94.  
  95. void InitializeAllServos()
  96. {  
  97.   // Initialize all servos and set the max and min and starting positions
  98.   for (int i=0;i<NUMBER_OF_SERVOS;i++)                                                                  
  99.   {
  100.     shieldServos.setbounds(i, servominpos[i], servomaxpos[i]);  //Set the minimum and maximum pulse duration of the servo
  101.     delay(150);
  102.     shieldServos.setposition(i, servomidpos[i]);      //Set the initial position of the servo
  103.     delay(150);
  104.   }
  105. }
  106.  
  107.  
  108. void GetStringFromSerial(char *buf, int bufsize)
  109. {
  110.   int i;
  111.   for (i=0; i<bufsize - 1; ++i)
  112.   {
  113.     while (Serial.available() == 0); // wait for character to arrive
  114.     buf[i] = Serial.read();
  115.     if (buf[i] == 10 || buf[i] == 13) // if LF or CR character found (end of string) break
  116.       break;
  117.   }
  118.   buf[i] = 0; // 0 string terminator just in case
  119. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top