Advertisement
snakerdlk

Arduino servo control via serial(pc)

Oct 19th, 2011
59
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 3.21 KB | None | 0 0
  1. #define verPin 3
  2. #define horPin 5
  3.  
  4. #include <string.h>
  5. #include <Servo.h>
  6.  
  7. unsigned minHorDegree = 45;
  8. unsigned maxHorDegree = 135;
  9.  
  10. unsigned minVerDegree = 45;
  11. unsigned maxVerDegree = 135;
  12.  
  13. Servo srvHor;
  14. Servo srvVer;
  15.  
  16. unsigned pos;
  17.  
  18. char message[20] = "";
  19. unsigned index = 0;
  20.  
  21. char aux[4] = "";
  22.  
  23. int horValue = (maxHorDegree-minHorDegree)/2;
  24. int verValue = (maxVerDegree-minVerDegree)/2;
  25.  
  26. int curHorValue = horValue;
  27. int curVerValue = verValue;
  28.  
  29. int horMaxStep = 5;
  30. int verMaxStep = 5;
  31.  
  32. void setup(){
  33.   Serial.begin(9600);
  34.   Serial.println("setup");
  35.  
  36.   srvHor.attach(horPin);
  37.   srvVer.attach(verPin);
  38.   Serial.println("servos attached");
  39. }
  40.  
  41. void loop(){
  42.   if(Serial.available() > 0){
  43.     message[index] = Serial.read();
  44.     if(message[index] != '\n'){
  45.       Serial.print(message[index]);
  46.       Serial.println(" is not null");
  47.       index++;
  48.     }else{
  49.       Serial.println("Got enter. EOM");
  50.       message[index] = '\0';
  51.       index = 0;
  52.       unsigned len = strlen(message);
  53.      
  54.       if(message[1] != ':'){
  55.         Serial.print("error(");
  56.         Serial.print(len);
  57.         Serial.println(")");
  58.       }else{
  59.         Serial.print("ack(");
  60.         Serial.print(len);
  61.         Serial.println(")");
  62.        
  63.        
  64.         if(message[0] == 'a'){ //a:...
  65.           //ignore
  66.         }else if(message[0] == 's'){ //s:XXX:XXX  
  67.           aux[0] = message[2];
  68.           aux[1] = message[3];
  69.           aux[2] = message[4];
  70.           aux[3] = '\0';
  71.           horValue = atoi(aux);
  72.           if(horValue > maxHorDegree){
  73.             horValue = maxHorDegree;
  74.           }else if(horValue < minHorDegree){
  75.              horValue = minHorDegree;
  76.           }
  77.          
  78.           aux[6] = message[4];
  79.           aux[7] = message[5];
  80.           aux[8] = message[6];
  81.           aux[3] = '\0';
  82.           verValue = atoi(aux);
  83.          
  84.           if(verValue > maxVerDegree){
  85.             verValue = maxVerDegree;
  86.           }else if(horValue < minHorDegree){
  87.              verValue = minVerDegree;
  88.           }
  89.          
  90.           Serial.print(horValue);
  91.           Serial.print(":");
  92.           Serial.print(verValue);
  93.           Serial.println(".");
  94.         }else{
  95.           Serial.println("unknown");
  96.         }
  97.       }
  98.     }
  99.   }
  100.    
  101.   if(curVerValue != verValue){
  102.     int diff = verValue-curVerValue;
  103.     Serial.print("verDiff should be: ");
  104.     Serial.println(diff);
  105.     if(diff > 0 && diff > verMaxStep){
  106.       Serial.println("diff set to 5");
  107.       diff = 5;
  108.     }
  109.    
  110.     if(diff < 0 && diff < verMaxStep){
  111.       Serial.println("diff set to -5");
  112.       diff = -5;
  113.     }
  114.     curVerValue += diff;
  115.    
  116.     srvVer.write(curVerValue);
  117.     Serial.print("ver: from ");
  118.     Serial.print(curVerValue-diff);
  119.     Serial.print(" to ");
  120.     Serial.print(curVerValue);
  121.     Serial.print(" until ");
  122.     Serial.println(verValue);
  123.   }
  124.  
  125.   if(0){//if(curHorValue != horValue){
  126.     int diff = horValue - curHorValue;
  127.     if(diff > horMaxStep){
  128.       diff = horMaxStep;
  129.     }
  130.     if(diff < (-1)*horMaxStep){
  131.       diff = (-1)*horMaxStep;
  132.     }
  133.     curHorValue += diff;
  134.    
  135.     srvHor.write(curHorValue);
  136.     Serial.print("hor:set to ");
  137.     Serial.println(curHorValue);
  138.   }
  139. }
  140.  
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement