ClarkeRubber

Arduino - Dual Servo Control

Dec 17th, 2012
118
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 2.51 KB | None | 0 0
  1. #include <Servo.h>
  2.  
  3. Servo servo_base, servo_joint;
  4.  
  5. int servo_base_output = 120;
  6. int servo_joint_output = 90;
  7.  
  8. int servo_pin_base = 10;
  9. int servo_pin_joint = 11;
  10.  
  11. String stream, stream_base, stream_joint;
  12. char charBuf[50];
  13.  
  14. char *i;
  15.  
  16. char ascii_table[] = {' ', '!', '"', '#', '$', '%', '&', '\'', '(', ')', '*',
  17.                     '+', ',', '-', '.', '/', '0', '1', '2', '3', '4', '5',
  18.                     '6', '7', '8', '9', ':', ';', '<', '=', '>', '?', '@',
  19.                     'A', 'B', 'C', 'D', 'E', 'F', 'G','H', 'I', 'J', 'K', 'L',
  20.                     'M', 'N', 'O', 'P', 'Q', 'R', 'S', 'T', 'U', 'V', 'W', 'X',
  21.                     'Y', 'Z', '[', '\\', ']', '^', '_', '`', 'a', 'b', 'c', 'd',
  22.                     'e', 'f', 'g', 'h', 'i', 'j', 'k', 'l', 'm', 'n', 'o',
  23.                     'p', 'q', 'r', 's', 't', 'u', 'v', 'w', 'x', 'y', 'z', '{',
  24.                     '|', '}', '~'};
  25.  
  26. char ascii_to_char(int ascii){
  27.   return ascii_table[ascii - 32];
  28. }
  29.  
  30. String capture_stream(){
  31.   int input;
  32.   String output;
  33.   boolean flag = false;
  34.   while((input = Serial.read()) != -1){
  35.     output += String(ascii_to_char(input));
  36.     flag = true;
  37.   }
  38.   if(flag){
  39.     Serial.println(String(output));
  40.     return output;
  41.   }else{
  42.     Serial.println(String("-1"));
  43.     return "-1";
  44.   }
  45. }
  46.  
  47. void setup(){
  48.   Serial.begin(9600);
  49.   Serial.println("starting up");
  50.   servo_base.attach(servo_pin_base);
  51.   servo_joint.attach(servo_pin_joint);  
  52. }
  53.  
  54. void loop(){
  55.   if((stream = capture_stream()) != "-1"){
  56.     stream.toCharArray(charBuf, 50);
  57.     stream_base = strtok_r(charBuf, ":", &i);
  58.     stream_joint = strtok_r(NULL, ":", &i);
  59.    
  60.     if(stream_base == "-") stream_base = String(servo_base_output);
  61.     if(stream_joint == "-") stream_joint = String(servo_joint_output);
  62.    
  63.     servo_base_output = stream_base.toInt();
  64.     servo_joint_output = stream_joint.toInt();
  65.    
  66.     Serial.println("Base Servo:  " + String(servo_base_output));
  67.     Serial.println("Joint Servo: " + String(servo_joint_output));
  68.    
  69.     if(servo_base_output != -1){
  70.       if(servo_base_output > 180){
  71.         servo_base_output = 180;
  72.       }else if(servo_base_output < 0){
  73.         servo_base_output = 0;
  74.       }
  75.       servo_base.write(servo_base_output);
  76.     }
  77.    
  78.     if(servo_joint_output != -1){
  79.       if(servo_joint_output > 180){
  80.         servo_joint_output = 180;
  81.       }else if(servo_joint_output < 0){
  82.         servo_joint_output = 0;
  83.       }
  84.       servo_joint.write(servo_joint_output);
  85.     }
  86.   }
  87.   delay(500);
  88. }
Advertisement
Add Comment
Please, Sign In to add comment